12345qiupeng
/
multiple-object-tracking-lidar
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: 12345qiupeng:27db548ea51d8faa498c9a2492b172219d6a56fb
Branches Tags
12345qiupeng:master
..
compare: 12345qiupeng:959f7244906d37d80c19fa7ea8fed1c62b2678b1
Branches Tags
12345qiupeng:master

No commits in common. "27db548ea51d8faa498c9a2492b172219d6a56fb" and "959f7244906d37d80c19fa7ea8fed1c62b2678b1" have entirely different histories.
27db548ea5 ... 959f724490

6 changed files with 1023 additions and 1021 deletions
Show Stats Expand all files Collapse all files

24
CHANGELOG.rst
View File

@ -2,30 +2,6 @@
Changelog for package multi_object_tracking_lidar Changelog for package multi_object_tracking_lidar
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.0.4 (2021-08-29)
------------------
* Merge pull request `#46 <https://github.com/praveen-palanisamy/multiple-object-tracking-lidar/issues/46>`_ from praveen-palanisamy/rm-topic-slash-prefix
Remove topic slash prefix
* Apply clang-format-10
* Rm slash prefix (deprecated in TF2)
* Add note to filter NaNs in input point clouds
* Contributors: Praveen Palanisamy
1.0.3 (2020-06-27)
------------------
* Merge pull request #26 from artursg/noetic-devel
C++11 --> C++14 to allow compiling with later versions of PCL, ROS Neotic
* Compiles under ROS Noetic
* Merge pull request #25 from praveen-palanisamy/add-license-1
Add MIT LICENSE
* Add LICENSE
* Merge pull request #24 from mzahran001/patch-1
Fix broken hyperlink to wiki page in README
* Fixing link error
* Updated README to make clustering approach for 3D vs 2D clear #21
* Added DOI and citing info
* Contributors: Artur Sagitov, Mohamed Zahran, Praveen Palanisamy
1.0.2 (2019-12-01) 1.0.2 (2019-12-01)
------------------ ------------------
* Added link to wiki pages * Added link to wiki pages

2
CMakeLists.txt
View File

@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 2.8.3)
project(multi_object_tracking_lidar) project(multi_object_tracking_lidar)
set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
## Find catkin macros and libraries ## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages ## is used, also find other catkin packages

3
README.md
View File

@ -34,9 +34,6 @@ The input point-clouds can be from:
3. A point cloud dataset or 3. A point cloud dataset or
4. Any other data source that produces point clouds 4. Any other data source that produces point clouds
**Note:** This package expects valid point cloud data as input. The point clouds you publish to the "`filtered_cloud`" is **not** expected to contain NaNs. The point cloud filtering is somewhat task and application dependent and therefore it is not done by this module.
PCL library provides `pcl::removeNaNFromPointCloud (...)` method to filter out NaN points. You can refer to [this example code snippet](https://github.com/praveen-palanisamy/multiple-object-tracking-lidar/issues/29#issuecomment-672098760) to easily filter out NaN points in your point cloud.
## Citing ## Citing
If you use the code or snippets from this repository in your work, please cite: If you use the code or snippets from this repository in your work, please cite:

2
package.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package> <package>
<name>multi_object_tracking_lidar</name> <name>multi_object_tracking_lidar</name>
<version>1.0.4</version> <version>1.0.2</version>
<description>ROS package for Multiple objects detection, tracking and classification from LIDAR scans/point-clouds</description> <description>ROS package for Multiple objects detection, tracking and classification from LIDAR scans/point-clouds</description>
<!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- One maintainer tag required, multiple allowed, one person per tag -->

776
src/main.cpp
View File

File diff suppressed because it is too large Load Diff

645
src/main_naive.cpp
View File

@ -1,136 +1,139 @@
#include "kf_tracker/CKalmanFilter.h"
#include "kf_tracker/featureDetection.h"
#include "opencv2/video/tracking.hpp"
#include "pcl_ros/point_cloud.h"
#include <algorithm>
#include <fstream>
#include <geometry_msgs/Point.h>
#include <iostream> #include <iostream>
#include <string.h>
#include <fstream>
#include <algorithm>
#include <iterator> #include <iterator>
#include "kf_tracker/featureDetection.h"
#include "kf_tracker/CKalmanFilter.h"
#include <opencv2/core/core.hpp> #include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp> #include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp> #include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/video.hpp> #include <opencv2/video/video.hpp>
#include "opencv2/video/tracking.hpp"
#include <ros/ros.h>
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <ros/ros.h> #include "pcl_ros/point_cloud.h"
#include <geometry_msgs/Point.h>
#include <std_msgs/Float32MultiArray.h> #include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int32MultiArray.h> #include <std_msgs/Int32MultiArray.h>
#include <string.h>
#include <pcl/common/geometry.h> #include <sensor_msgs/PointCloud2.h>
#include <pcl/features/normal_3d.h> #include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/point_cloud.h> #include <pcl/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/common/geometry.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h> #include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h> #include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h> #include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl/segmentation/extract_clusters.h>
#include <sensor_msgs/PointCloud2.h>
using namespace std; using namespace std;
using namespace cv; using namespace cv;
ros::Publisher objID_pub; ros::Publisher objID_pub;
// KF init // KF init
int stateDim = 4; // [x,y,v_x,v_y]//,w,h] int stateDim=4;// [x,y,v_x,v_y]//,w,h]
int measDim = 2; // [z_x,z_y,z_w,z_h] int measDim=2;// [z_x,z_y,z_w,z_h]
int ctrlDim = 0; int ctrlDim=0;
cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F); cv::KalmanFilter KF0(stateDim,measDim,ctrlDim,CV_32F);
cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F); cv::KalmanFilter KF1(stateDim,measDim,ctrlDim,CV_32F);
cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F); cv::KalmanFilter KF2(stateDim,measDim,ctrlDim,CV_32F);
cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F); cv::KalmanFilter KF3(stateDim,measDim,ctrlDim,CV_32F);
cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F); cv::KalmanFilter KF4(stateDim,measDim,ctrlDim,CV_32F);
cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F); cv::KalmanFilter KF5(stateDim,measDim,ctrlDim,CV_32F);
ros::Publisher pub_cluster0; ros::Publisher pub_cluster0;
ros::Publisher pub_cluster1; ros::Publisher pub_cluster1;
ros::Publisher pub_cluster2; ros::Publisher pub_cluster2;
ros::Publisher pub_cluster3; ros::Publisher pub_cluster3;
ros::Publisher pub_cluster4; ros::Publisher pub_cluster4;
ros::Publisher pub_cluster5; ros::Publisher pub_cluster5;
std::vector<geometry_msgs::Point> prevClusterCenters; std::vector<geometry_msgs::Point> prevClusterCenters;
cv::Mat state(stateDim, 1, CV_32F);
cv::Mat_<float> measurement(2, 1);
std::vector<int> objID; // Output of the data association using KF cv::Mat state(stateDim,1,CV_32F);
cv::Mat_<float> measurement(2,1);
std::vector<int> objID;// Output of the data association using KF
// measurement.setTo(Scalar(0)); // measurement.setTo(Scalar(0));
bool firstFrame = true; bool firstFrame=true;
// calculate euclidean distance of two points // calculate euclidean distance of two points
double euclidean_distance(geometry_msgs::Point &p1, geometry_msgs::Point &p2) { double euclidean_distance(geometry_msgs::Point& p1, geometry_msgs::Point& p2)
return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + {
(p1.z - p2.z) * (p1.z - p2.z)); return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z));
} }
/* /*
//Count unique object IDs. just to make sure same ID has not been assigned to //Count unique object IDs. just to make sure same ID has not been assigned to two KF_Trackers.
two KF_Trackers. int countIDs(vector<int> v) int countIDs(vector<int> v)
{ {
transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = distance(v.end(), v.begin())
distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log sort(v.begin(), v.end()); // Average case O(n log n), worst case O(n^2) (usually implemented as quicksort.
n), worst case O(n^2) (usually implemented as quicksort. // To guarantee worst case O(n log n) replace with make_heap, then sort_heap.
// To guarantee worst case O(n log n) replace with make_heap, then
sort_heap.
// Unique will take a sorted range, and move things around to get duplicated // Unique will take a sorted range, and move things around to get duplicated
// items to the back and returns an iterator to the end of the unique // items to the back and returns an iterator to the end of the unique section of the range
section of the range auto unique_end = unique(v.begin(), v.end()); // Again n auto unique_end = unique(v.begin(), v.end()); // Again n comparisons
comparisons return distance(unique_end, v.begin()); // Constant time for random return distance(unique_end, v.begin()); // Constant time for random access iterators (like vector's)
access iterators (like vector's)
} }
*/ */
/* /*
objID: vector containing the IDs of the clusters that should be associated with objID: vector containing the IDs of the clusters that should be associated with each KF_Tracker
each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc. objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
*/ */
void KFT(const std_msgs::Float32MultiArray ccs) { void KFT(const std_msgs::Float32MultiArray ccs)
{
// First predict, to update the internal statePre variable // First predict, to update the internal statePre variable
std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.predict(), std::vector<cv::Mat> pred{KF0.predict(),KF1.predict(),KF2.predict(),KF3.predict(),KF4.predict(),KF5.predict()};
KF3.predict(), KF4.predict(), KF5.predict()}; //cout<<"Pred successfull\n";
// cout<<"Pred successfull\n";
// cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1)); //cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
// cout<<"Prediction 1 // cout<<"Prediction 1 ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
// ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
// Get measurements // Get measurements
// Extract the position of the clusters forom the multiArray. To check if the // Extract the position of the clusters forom the multiArray. To check if the data
// data coming in, check the .z (every third) coordinate and that will be 0.0 // coming in, check the .z (every third) coordinate and that will be 0.0
std::vector<geometry_msgs::Point> clusterCenters; // clusterCenters std::vector<geometry_msgs::Point> clusterCenters;//clusterCenters
int i = 0; int i=0;
for (std::vector<float>::const_iterator it = ccs.data.begin(); for (std::vector<float>::const_iterator it=ccs.data.begin();it!=ccs.data.end();it+=3)
it != ccs.data.end(); it += 3) { {
geometry_msgs::Point pt; geometry_msgs::Point pt;
pt.x = *it; pt.x=*it;
pt.y = *(it + 1); pt.y=*(it+1);
pt.z = *(it + 2); pt.z=*(it+2);
clusterCenters.push_back(pt); clusterCenters.push_back(pt);
} }
// cout<<"CLusterCenters Obtained"<<"\n"; // cout<<"CLusterCenters Obtained"<<"\n";
std::vector<geometry_msgs::Point> KFpredictions; std::vector<geometry_msgs::Point> KFpredictions;
i = 0; i=0;
for (auto it = pred.begin(); it != pred.end(); it++) { for (auto it=pred.begin();it!=pred.end();it++)
{
geometry_msgs::Point pt; geometry_msgs::Point pt;
pt.x = (*it).at<float>(0); pt.x=(*it).at<float>(0);
pt.y = (*it).at<float>(1); pt.y=(*it).at<float>(1);
pt.z = (*it).at<float>(2); pt.z=(*it).at<float>(2);
KFpredictions.push_back(pt); KFpredictions.push_back(pt);
} }
// cout<<"Got predictions"<<"\n"; // cout<<"Got predictions"<<"\n";
@ -144,11 +147,9 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
for(int n=0;n<6;n++) for(int n=0;n<6;n++)
distVec.push_back(euclidean_distance(KFpredictions[filterN],clusterCenters[n])); distVec.push_back(euclidean_distance(KFpredictions[filterN],clusterCenters[n]));
// // cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n";
cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n";
objID.push_back(std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()))); objID.push_back(std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())));
// cout<<"MinD for // cout<<"MinD for filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
} }
@ -156,7 +157,7 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
//countIDs(objID);// for verif/corner cases //countIDs(objID);// for verif/corner cases
Original version using kalman filter prediction Original version using kalman filter prediction
*/ */
// display objIDs //display objIDs
/* DEBUG /* DEBUG
cout<<"objID= "; cout<<"objID= ";
for(auto it=objID.begin();it!=objID.end();it++) for(auto it=objID.begin();it!=objID.end();it++)
@ -164,38 +165,41 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
cout<<"\n"; cout<<"\n";
*/ */
/* Naive version without using kalman filter */ /* Naive version without using kalman filter */
objID.clear(); // Clear the objID vector objID.clear();//Clear the objID vector
for (int filterN = 0; filterN < 6; filterN++) { for(int filterN=0;filterN<6;filterN++)
{
std::vector<float> distVec; std::vector<float> distVec;
for (int n = 0; n < 6; n++) for(int n=0;n<6;n++)
distVec.push_back( distVec.push_back(euclidean_distance(prevClusterCenters[n],clusterCenters[n]));
euclidean_distance(prevClusterCenters[n], clusterCenters[n]));
// cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n"; // cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n";
objID.push_back(std::distance(distVec.begin(), objID.push_back(std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())));
min_element(distVec.begin(), distVec.end()))); // cout<<"MinD for filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
// cout<<"MinD for
// filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
} }
/* Naive version without kalman filter */ /* Naive version without kalman filter */
// prevClusterCenters.clear(); //prevClusterCenters.clear();
// Set the current associated clusters to the prevClusterCenters // Set the current associated clusters to the prevClusterCenters
for (int i = 0; i < 6; i++) { for (int i=0;i<6;i++)
prevClusterCenters[objID.at(i)] = clusterCenters.at(i); {
prevClusterCenters[objID.at(i)]=clusterCenters.at(i);
} }
/* Naive version without kalman filter */ /* Naive version without kalman filter */
/* Naive version without using kalman filter */ /* Naive version without using kalman filter */
std_msgs::Int32MultiArray obj_id; std_msgs::Int32MultiArray obj_id;
for (auto it = objID.begin(); it != objID.end(); it++) for(auto it=objID.begin();it!=objID.end();it++)
obj_id.data.push_back(*it); obj_id.data.push_back(*it);
objID_pub.publish(obj_id); objID_pub.publish(obj_id);
// convert clusterCenters from geometry_msgs::Point to floats // convert clusterCenters from geometry_msgs::Point to floats
std::vector<std::vector<float>> cc; std::vector<std::vector<float> > cc;
for (int i = 0; i < clusterCenters.size(); i++) { for (int i=0;i<clusterCenters.size();i++)
{
vector<float> pt; vector<float> pt;
pt.push_back(clusterCenters[i].x); pt.push_back(clusterCenters[i].x);
pt.push_back(clusterCenters[i].y); pt.push_back(clusterCenters[i].y);
@ -203,23 +207,25 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
cc.push_back(pt); cc.push_back(pt);
} }
// cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n"; //cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
float meas0[3] = {cc[0].at(0), cc[0].at(1)}; float meas0[3]={cc[0].at(0),cc[0].at(1)};
float meas1[3] = {cc[1].at(0), cc[1].at(1)}; float meas1[3]={cc[1].at(0),cc[1].at(1)};
float meas2[3] = {cc[2].at(0), cc[2].at(1)}; float meas2[3]={cc[2].at(0),cc[2].at(1)};
float meas3[3] = {cc[3].at(0), cc[3].at(1)}; float meas3[3]={cc[3].at(0),cc[3].at(1)};
float meas4[3] = {cc[4].at(0), cc[4].at(1)}; float meas4[3]={cc[4].at(0),cc[4].at(1)};
float meas5[3] = {cc[5].at(0), cc[5].at(1)}; float meas5[3]={cc[5].at(0),cc[5].at(1)};
// The update phase // The update phase
cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0); cv::Mat meas0Mat=cv::Mat(2,1,CV_32F,meas0);
cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1); cv::Mat meas1Mat=cv::Mat(2,1,CV_32F,meas1);
cv::Mat meas2Mat = cv::Mat(2, 1, CV_32F, meas2); cv::Mat meas2Mat=cv::Mat(2,1,CV_32F,meas2);
cv::Mat meas3Mat = cv::Mat(2, 1, CV_32F, meas3); cv::Mat meas3Mat=cv::Mat(2,1,CV_32F,meas3);
cv::Mat meas4Mat = cv::Mat(2, 1, CV_32F, meas4); cv::Mat meas4Mat=cv::Mat(2,1,CV_32F,meas4);
cv::Mat meas5Mat = cv::Mat(2, 1, CV_32F, meas5); cv::Mat meas5Mat=cv::Mat(2,1,CV_32F,meas5);
// cout<<"meas0Mat"<<meas0Mat<<"\n"; //cout<<"meas0Mat"<<meas0Mat<<"\n";
Mat estimated0 = KF0.correct(meas0Mat); Mat estimated0 = KF0.correct(meas0Mat);
Mat estimated1 = KF0.correct(meas1Mat); Mat estimated1 = KF0.correct(meas1Mat);
@ -230,41 +236,37 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
// Publish the point clouds belonging to each clusters // Publish the point clouds belonging to each clusters
// cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n"; // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
// Point statePt(estimated.at<float>(0),estimated.at<float>(1)); // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
// cout<<"DONE KF_TRACKER\n"; //cout<<"DONE KF_TRACKER\n";
} }
void publish_cloud(ros::Publisher &pub, void publish_cloud(ros::Publisher& pub, pcl::PointCloud<pcl::PointXYZ>::Ptr cluster){
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster) { sensor_msgs::PointCloud2::Ptr clustermsg (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2); pcl::toROSMsg (*cluster , *clustermsg);
pcl::toROSMsg(*cluster, *clustermsg); clustermsg->header.frame_id = "/map";
clustermsg->header.frame_id = "map";
clustermsg->header.stamp = ros::Time::now(); clustermsg->header.stamp = ros::Time::now();
pub.publish(*clustermsg); pub.publish (*clustermsg);
} }
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{ {
cout << "IF firstFrame=" << firstFrame << "\n"; cout<<"IF firstFrame="<<firstFrame<<"\n";
// If this is the first frame, initialize kalman filters for the clustered // If this is the first frame, initialize kalman filters for the clustered objects
// objects if (firstFrame)
if (firstFrame) { {
// Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset. // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
// Could be made generic by creating a Kalman Filter only when a new object // Could be made generic by creating a Kalman Filter only when a new object is detected
// is detected KF0.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
KF0.transitionMatrix = KF1.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); KF2.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
KF1.transitionMatrix = KF3.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); KF4.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
KF2.transitionMatrix = KF5.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
KF3.transitionMatrix =
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
KF4.transitionMatrix =
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
KF5.transitionMatrix =
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
cv::setIdentity(KF0.measurementMatrix); cv::setIdentity(KF0.measurementMatrix);
cv::setIdentity(KF1.measurementMatrix); cv::setIdentity(KF1.measurementMatrix);
@ -293,316 +295,309 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(1e-1)); cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(1e-1));
cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1)); cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1));
// Process the point cloud // Process the point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud( pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
/* Creating the KdTree from input point cloud*/ /* Creating the KdTree from input point cloud*/
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree( pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *input_cloud); pcl::fromROSMsg (*input, *input_cloud);
tree->setInputCloud(input_cloud); tree->setInputCloud (input_cloud);
/* Here we are creating a vector of PointIndices, which contains the actual /* Here we are creating a vector of PointIndices, which contains the actual index
* index information in a vector<int>. The indices of each detected cluster * information in a vector<int>. The indices of each detected cluster are saved here.
* are saved here. Cluster_indices is a vector containing one instance of * Cluster_indices is a vector containing one instance of PointIndices for each detected
* PointIndices for each detected cluster. Cluster_indices[0] contain all * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
* indices of the first cluster in input point cloud.
*/ */
std::vector<pcl::PointIndices> cluster_indices; std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.08); ec.setClusterTolerance (0.08);
ec.setMinClusterSize(10); ec.setMinClusterSize (10);
ec.setMaxClusterSize(600); ec.setMaxClusterSize (600);
ec.setSearchMethod(tree); ec.setSearchMethod (tree);
ec.setInputCloud(input_cloud); ec.setInputCloud (input_cloud);
/* Extract the clusters out of pc and save indices in cluster_indices.*/ /* Extract the clusters out of pc and save indices in cluster_indices.*/
ec.extract(cluster_indices); ec.extract (cluster_indices);
/* To separate each cluster out of the vector<PointIndices> we have to /* To separate each cluster out of the vector<PointIndices> we have to
* iterate through cluster_indices, create a new PointCloud for each * iterate through cluster_indices, create a new PointCloud for each
* entry and write all points of the current cluster in the PointCloud. * entry and write all points of the current cluster in the PointCloud.
*/ */
// pcl::PointXYZ origin (0,0,0); //pcl::PointXYZ origin (0,0,0);
// float mindist_this_cluster = 1000; //float mindist_this_cluster = 1000;
// float dist_this_point = 1000; //float dist_this_point = 1000;
std::vector<pcl::PointIndices>::const_iterator it; std::vector<pcl::PointIndices>::const_iterator it;
std::vector<int>::const_iterator pit; std::vector<int>::const_iterator pit;
// Vector of cluster pointclouds // Vector of cluster pointclouds
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec; std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
new pcl::PointCloud<pcl::PointXYZ>); for(pit = it->indices.begin(); pit != it->indices.end(); pit++)
for (pit = it->indices.begin(); pit != it->indices.end(); pit++) { {
cloud_cluster->points.push_back(input_cloud->points[*pit]); cloud_cluster->points.push_back(input_cloud->points[*pit]);
// dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
// origin); // origin);
// mindist_this_cluster = std::min(dist_this_point, //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster);
// mindist_this_cluster);
} }
cluster_vec.push_back(cloud_cluster); cluster_vec.push_back(cloud_cluster);
} }
// Ensure at least 6 clusters exist to publish (later clusters may be empty) //Ensure at least 6 clusters exist to publish (later clusters may be empty)
while (cluster_vec.size() < 6) { while (cluster_vec.size() < 6){
pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster( pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
new pcl::PointCloud<pcl::PointXYZ>); empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
cluster_vec.push_back(empty_cluster); cluster_vec.push_back(empty_cluster);
} }
// Set initial state
KF0.statePre.at<float>(0) =
cluster_vec[0]->points[cluster_vec[0]->points.size() / 2].x;
KF0.statePre.at<float>(1) =
cluster_vec[0]->points[cluster_vec[0]->points.size() / 2].y;
KF0.statePre.at<float>(2) = 0; // initial v_x
KF0.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF1.statePre.at<float>(0) = KF0.statePre.at<float>(0)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].x;
cluster_vec[1]->points[cluster_vec[1]->points.size() / 2].x;
KF1.statePre.at<float>(1) = KF0.statePre.at<float>(1)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].y;
cluster_vec[1]->points[cluster_vec[1]->points.size() / 2].y; KF0.statePre.at<float>(2)=0;// initial v_x
KF1.statePre.at<float>(2) = 0; // initial v_x KF0.statePre.at<float>(3)=0;//initial v_y
KF1.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF2.statePre.at<float>(0) = KF1.statePre.at<float>(0)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].x;
cluster_vec[2]->points[cluster_vec[2]->points.size() / 2].x; KF1.statePre.at<float>(1)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].y;
KF2.statePre.at<float>(1) = KF1.statePre.at<float>(2)=0;// initial v_x
cluster_vec[2]->points[cluster_vec[2]->points.size() / 2].y; KF1.statePre.at<float>(3)=0;//initial v_y
KF2.statePre.at<float>(2) = 0; // initial v_x
KF2.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF3.statePre.at<float>(0) = KF2.statePre.at<float>(0)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].x;
cluster_vec[3]->points[cluster_vec[3]->points.size() / 2].x; KF2.statePre.at<float>(1)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].y;
KF3.statePre.at<float>(1) = KF2.statePre.at<float>(2)=0;// initial v_x
cluster_vec[3]->points[cluster_vec[3]->points.size() / 2].y; KF2.statePre.at<float>(3)=0;//initial v_y
KF3.statePre.at<float>(2) = 0; // initial v_x
KF3.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF4.statePre.at<float>(0) = KF3.statePre.at<float>(0)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].x;
cluster_vec[4]->points[cluster_vec[4]->points.size() / 2].x; KF3.statePre.at<float>(1)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].y;
KF4.statePre.at<float>(1) = KF3.statePre.at<float>(2)=0;// initial v_x
cluster_vec[4]->points[cluster_vec[4]->points.size() / 2].y; KF3.statePre.at<float>(3)=0;//initial v_y
KF4.statePre.at<float>(2) = 0; // initial v_x
KF4.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF5.statePre.at<float>(0) = KF4.statePre.at<float>(0)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].x;
cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].x; KF4.statePre.at<float>(1)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].y;
KF5.statePre.at<float>(1) = KF4.statePre.at<float>(2)=0;// initial v_x
cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].y; KF4.statePre.at<float>(3)=0;//initial v_y
KF5.statePre.at<float>(2) = 0; // initial v_x
KF5.statePre.at<float>(3) = 0; // initial v_y
firstFrame = false; // Set initial state
KF5.statePre.at<float>(0)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].x;
KF5.statePre.at<float>(1)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].y;
KF5.statePre.at<float>(2)=0;// initial v_x
KF5.statePre.at<float>(3)=0;//initial v_y
firstFrame=false;
// Print the initial state of the kalman filter for debugging // Print the initial state of the kalman filter for debugging
cout << "KF0.satePre=" << KF0.statePre.at<float>(0) << "," cout<<"KF0.satePre="<<KF0.statePre.at<float>(0)<<","<<KF0.statePre.at<float>(1)<<"\n";
<< KF0.statePre.at<float>(1) << "\n"; cout<<"KF1.satePre="<<KF1.statePre.at<float>(0)<<","<<KF1.statePre.at<float>(1)<<"\n";
cout << "KF1.satePre=" << KF1.statePre.at<float>(0) << "," cout<<"KF2.satePre="<<KF2.statePre.at<float>(0)<<","<<KF2.statePre.at<float>(1)<<"\n";
<< KF1.statePre.at<float>(1) << "\n"; cout<<"KF3.satePre="<<KF3.statePre.at<float>(0)<<","<<KF3.statePre.at<float>(1)<<"\n";
cout << "KF2.satePre=" << KF2.statePre.at<float>(0) << "," cout<<"KF4.satePre="<<KF4.statePre.at<float>(0)<<","<<KF4.statePre.at<float>(1)<<"\n";
<< KF2.statePre.at<float>(1) << "\n"; cout<<"KF5.satePre="<<KF5.statePre.at<float>(0)<<","<<KF5.statePre.at<float>(1)<<"\n";
cout << "KF3.satePre=" << KF3.statePre.at<float>(0) << ","
<< KF3.statePre.at<float>(1) << "\n"; //cin.ignore();// To be able to see the printed initial state of the KalmanFilter
cout << "KF4.satePre=" << KF4.statePre.at<float>(0) << ","
<< KF4.statePre.at<float>(1) << "\n";
cout << "KF5.satePre=" << KF5.statePre.at<float>(0) << ","
<< KF5.statePre.at<float>(1) << "\n";
// cin.ignore();// To be able to see the printed initial state of the
// KalmanFilter
/* Naive version without kalman filter */ /* Naive version without kalman filter */
for (int i = 0; i < 6; i++) { for (int i=0;i<6;i++)
{
geometry_msgs::Point pt; geometry_msgs::Point pt;
pt.x = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].x; pt.x=cluster_vec[i]->points[cluster_vec[i]->points.size()/2].x;
pt.y = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].y; pt.y=cluster_vec[i]->points[cluster_vec[i]->points.size()/2].y;
prevClusterCenters.push_back(pt); prevClusterCenters.push_back(pt);
} }
/* Naive version without kalman filter */ /* Naive version without kalman filter */
} }
else {
cout << "ELSE firstFrame=" << firstFrame << "\n"; else
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud( {
new pcl::PointCloud<pcl::PointXYZ>); cout<<"ELSE firstFrame="<<firstFrame<<"\n";
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud( pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
/* Creating the KdTree from input point cloud*/ /* Creating the KdTree from input point cloud*/
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree( pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *input_cloud); pcl::fromROSMsg (*input, *input_cloud);
tree->setInputCloud(input_cloud); tree->setInputCloud (input_cloud);
/* Here we are creating a vector of PointIndices, which contains the actual /* Here we are creating a vector of PointIndices, which contains the actual index
* index information in a vector<int>. The indices of each detected cluster * information in a vector<int>. The indices of each detected cluster are saved here.
* are saved here. Cluster_indices is a vector containing one instance of * Cluster_indices is a vector containing one instance of PointIndices for each detected
* PointIndices for each detected cluster. Cluster_indices[0] contain all * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
* indices of the first cluster in input point cloud.
*/ */
std::vector<pcl::PointIndices> cluster_indices; std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.08); ec.setClusterTolerance (0.08);
ec.setMinClusterSize(10); ec.setMinClusterSize (10);
ec.setMaxClusterSize(600); ec.setMaxClusterSize (600);
ec.setSearchMethod(tree); ec.setSearchMethod (tree);
ec.setInputCloud(input_cloud); ec.setInputCloud (input_cloud);
cout << "PCL init successfull\n"; cout<<"PCL init successfull\n";
/* Extract the clusters out of pc and save indices in cluster_indices.*/ /* Extract the clusters out of pc and save indices in cluster_indices.*/
ec.extract(cluster_indices); ec.extract (cluster_indices);
cout << "PCL extract successfull\n"; cout<<"PCL extract successfull\n";
/* To separate each cluster out of the vector<PointIndices> we have to /* To separate each cluster out of the vector<PointIndices> we have to
* iterate through cluster_indices, create a new PointCloud for each * iterate through cluster_indices, create a new PointCloud for each
* entry and write all points of the current cluster in the PointCloud. * entry and write all points of the current cluster in the PointCloud.
*/ */
// pcl::PointXYZ origin (0,0,0); //pcl::PointXYZ origin (0,0,0);
// float mindist_this_cluster = 1000; //float mindist_this_cluster = 1000;
// float dist_this_point = 1000; //float dist_this_point = 1000;
std::vector<pcl::PointIndices>::const_iterator it; std::vector<pcl::PointIndices>::const_iterator it;
std::vector<int>::const_iterator pit; std::vector<int>::const_iterator pit;
// Vector of cluster pointclouds // Vector of cluster pointclouds
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec; std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
new pcl::PointCloud<pcl::PointXYZ>); for(pit = it->indices.begin(); pit != it->indices.end(); pit++)
for (pit = it->indices.begin(); pit != it->indices.end(); pit++) { {
cloud_cluster->points.push_back(input_cloud->points[*pit]); cloud_cluster->points.push_back(input_cloud->points[*pit]);
// dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
// origin); // origin);
// mindist_this_cluster = std::min(dist_this_point, //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster);
// mindist_this_cluster);
} }
cluster_vec.push_back(cloud_cluster); cluster_vec.push_back(cloud_cluster);
}
// cout<<"cluster_vec got some clusters\n";
// Ensure at least 6 clusters exist to publish (later clusters may be empty) }
while (cluster_vec.size() < 6) { //cout<<"cluster_vec got some clusters\n";
pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
new pcl::PointCloud<pcl::PointXYZ>); //Ensure at least 6 clusters exist to publish (later clusters may be empty)
empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0)); while (cluster_vec.size() < 6){
pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
cluster_vec.push_back(empty_cluster); cluster_vec.push_back(empty_cluster);
} }
std_msgs::Float32MultiArray cc; std_msgs::Float32MultiArray cc;
for (int i = 0; i < 6; i++) { for(int i=0;i<6;i++)
cc.data.push_back( {
cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].x); cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].x);
cc.data.push_back( cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].y);
cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].y); cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].z);
cc.data.push_back(
cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].z);
} }
cout << "6 clusters initialized\n"; cout<<"6 clusters initialized\n";
// cc_pos.publish(cc);// Publish cluster mid-points. //cc_pos.publish(cc);// Publish cluster mid-points.
KFT(cc); KFT(cc);
int i = 0; int i=0;
bool publishedCluster[6]; bool publishedCluster[6];
for (auto it = objID.begin(); it != objID.end(); it++) { for(auto it=objID.begin();it!=objID.end();it++)
cout << "Inside the for loop\n"; { cout<<"Inside the for loop\n";
switch (*it) { switch(*it)
cout << "Inside the switch case\n"; {
cout<<"Inside the switch case\n";
case 0: { case 0: {
publish_cloud(pub_cluster0, cluster_vec[i]); publish_cloud(pub_cluster0,cluster_vec[i]);
publishedCluster[i] = publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 1: { case 1: {
publish_cloud(pub_cluster1, cluster_vec[i]); publish_cloud(pub_cluster1,cluster_vec[i]);
publishedCluster[i] = publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 2: { case 2: {
publish_cloud(pub_cluster2, cluster_vec[i]); publish_cloud(pub_cluster2,cluster_vec[i]);
publishedCluster[i] = publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 3: { case 3: {
publish_cloud(pub_cluster3, cluster_vec[i]); publish_cloud(pub_cluster3,cluster_vec[i]);
publishedCluster[i] = publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 4: { case 4: {
publish_cloud(pub_cluster4, cluster_vec[i]); publish_cloud(pub_cluster4,cluster_vec[i]);
publishedCluster[i] = publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 5: { case 5: {
publish_cloud(pub_cluster5, cluster_vec[i]); publish_cloud(pub_cluster5,cluster_vec[i]);
publishedCluster[i] = publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
default: default: break;
break;
}
} }
} }
} }
int main(int argc, char **argv) { }
int main(int argc, char** argv)
{
// ROS init // ROS init
ros::init(argc, argv, "KFTracker"); ros::init (argc,argv,"KFTracker");
ros::NodeHandle nh; ros::NodeHandle nh;
// Publishers to publish the state of the objects (pos and vel) // Publishers to publish the state of the objects (pos and vel)
// objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1); //objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
cout << "About to setup callback\n";
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe("filtered_cloud", 1, cloud_cb); cout<<"About to setup callback\n";
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("filtered_cloud", 1, cloud_cb);
// Create a ROS publisher for the output point cloud // Create a ROS publisher for the output point cloud
pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2>("cluster_0", 1); pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_0", 1);
pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2>("cluster_1", 1); pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_1", 1);
pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2>("cluster_2", 1); pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_2", 1);
pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2>("cluster_3", 1); pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_3", 1);
pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2>("cluster_4", 1); pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_4", 1);
pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2>("cluster_5", 1); pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_5", 1);
// Subscribe to the clustered pointclouds // Subscribe to the clustered pointclouds
// ros::Subscriber c1=nh.subscribe("ccs",100,KFT); //ros::Subscriber c1=nh.subscribe("ccs",100,KFT);
objID_pub = nh.advertise<std_msgs::Int32MultiArray>("obj_id", 1); objID_pub = nh.advertise<std_msgs::Int32MultiArray>("obj_id", 1);
/* Point cloud clustering /* Point cloud clustering
*/ */
// cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1 //cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
/* Point cloud clustering
*/
/* Point cloud clustering
*/
ros::spin(); ros::spin();
} }