From a9aa364d334933a099abf998d3f4f061f1949d00 Mon Sep 17 00:00:00 2001 From: Praveen Palanisamy Date: Sat, 28 Aug 2021 22:53:40 -0700 Subject: [PATCH] Apply clang-format-10 --- src/main.cpp | 1110 +++++++++++++++++++++----------------------- src/main_naive.cpp | 947 ++++++++++++++++++------------------- 2 files changed, 1014 insertions(+), 1043 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index d19bba0..871c103 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,237 +1,226 @@ -#include -#include -#include -#include -#include -#include "kf_tracker/featureDetection.h" #include "kf_tracker/CKalmanFilter.h" +#include "kf_tracker/featureDetection.h" +#include "opencv2/video/tracking.hpp" +#include "pcl_ros/point_cloud.h" +#include +#include +#include +#include +#include #include #include #include #include -#include "opencv2/video/tracking.hpp" -#include #include #include -#include "pcl_ros/point_cloud.h" -#include +#include #include #include +#include -#include -#include -#include -#include +#include #include +#include #include #include -#include #include +#include +#include #include #include -#include #include -#include - -#include -#include +#include +#include +#include + #include #include +#include +#include using namespace std; using namespace cv; - ros::Publisher objID_pub; - // KF init - int stateDim=4;// [x,y,v_x,v_y]//,w,h] - int measDim=2;// [z_x,z_y,z_w,z_h] - int ctrlDim=0; - cv::KalmanFilter KF0(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF1(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF2(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF3(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF4(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF5(stateDim,measDim,ctrlDim,CV_32F); +// KF init +int stateDim = 4; // [x,y,v_x,v_y]//,w,h] +int measDim = 2; // [z_x,z_y,z_w,z_h] +int ctrlDim = 0; +cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F); - ros::Publisher pub_cluster0; - ros::Publisher pub_cluster1; - ros::Publisher pub_cluster2; - ros::Publisher pub_cluster3; - ros::Publisher pub_cluster4; - ros::Publisher pub_cluster5; +ros::Publisher pub_cluster0; +ros::Publisher pub_cluster1; +ros::Publisher pub_cluster2; +ros::Publisher pub_cluster3; +ros::Publisher pub_cluster4; +ros::Publisher pub_cluster5; - ros::Publisher markerPub; +ros::Publisher markerPub; - std::vector prevClusterCenters; +std::vector prevClusterCenters; +cv::Mat state(stateDim, 1, CV_32F); +cv::Mat_ measurement(2, 1); - cv::Mat state(stateDim,1,CV_32F); - cv::Mat_ measurement(2,1); +std::vector objID; // Output of the data association using KF + // measurement.setTo(Scalar(0)); - std::vector objID;// Output of the data association using KF - // measurement.setTo(Scalar(0)); - -bool firstFrame=true; +bool firstFrame = true; // calculate euclidean distance of two points - 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)); - } +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)); +} /* -//Count unique object IDs. just to make sure same ID has not been assigned to two KF_Trackers. -int countIDs(vector v) +//Count unique object IDs. just to make sure same ID has not been assigned to +two KF_Trackers. int countIDs(vector v) { - transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = distance(v.end(), v.begin()) - sort(v.begin(), v.end()); // Average case O(n log 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. + transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = +distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log +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. // 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 section of the range - auto unique_end = unique(v.begin(), v.end()); // Again n comparisons - return distance(unique_end, v.begin()); // Constant time for random access iterators (like vector's) + // items to the back and returns an iterator to the end of the unique +section of the range auto unique_end = unique(v.begin(), v.end()); // Again n +comparisons return distance(unique_end, v.begin()); // Constant time for random +access iterators (like vector's) } */ /* -objID: vector containing the IDs of the clusters that should be associated with each KF_Tracker -objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc. +objID: vector containing the IDs of the clusters that should be associated with +each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc. */ -std::pair findIndexOfMin(std::vector > distMat) -{ - cout<<"findIndexOfMin cALLED\n"; - std::pairminIndex; - float minEl=std::numeric_limits::max(); - cout<<"minEl="< pred{KF0.predict(),KF1.predict(),KF2.predict(),KF3.predict(),KF4.predict(),KF5.predict()}; - //cout<<"Pred successfull\n"; - - //cv::Point predictPt(prediction.at(0),prediction.at(1)); - // cout<<"Prediction 1 ="<(0)<<","<(1)<<"\n"; - - // Get measurements - // Extract the position of the clusters forom the multiArray. To check if the data - // coming in, check the .z (every third) coordinate and that will be 0.0 - std::vector clusterCenters;//clusterCenters - - int i=0; - for (std::vector::const_iterator it=ccs.data.begin();it!=ccs.data.end();it+=3) - { - geometry_msgs::Point pt; - pt.x=*it; - pt.y=*(it+1); - pt.z=*(it+2); - - clusterCenters.push_back(pt); - +std::pair findIndexOfMin(std::vector> distMat) { + cout << "findIndexOfMin cALLED\n"; + std::pair minIndex; + float minEl = std::numeric_limits::max(); + cout << "minEl=" << minEl << "\n"; + for (int i = 0; i < distMat.size(); i++) + for (int j = 0; j < distMat.at(0).size(); j++) { + if (distMat[i][j] < minEl) { + minEl = distMat[i][j]; + minIndex = std::make_pair(i, j); + } } + cout << "minIndex=" << minIndex.first << "," << minIndex.second << "\n"; + return minIndex; +} +void KFT(const std_msgs::Float32MultiArray ccs) { + + // First predict, to update the internal statePre variable + + std::vector pred{KF0.predict(), KF1.predict(), KF2.predict(), + KF3.predict(), KF4.predict(), KF5.predict()}; + // cout<<"Pred successfull\n"; + + // cv::Point predictPt(prediction.at(0),prediction.at(1)); + // cout<<"Prediction 1 + // ="<(0)<<","<(1)<<"\n"; + + // Get measurements + // Extract the position of the clusters forom the multiArray. To check if the + // data coming in, check the .z (every third) coordinate and that will be 0.0 + std::vector clusterCenters; // clusterCenters + + int i = 0; + for (std::vector::const_iterator it = ccs.data.begin(); + it != ccs.data.end(); it += 3) { + geometry_msgs::Point pt; + pt.x = *it; + pt.y = *(it + 1); + pt.z = *(it + 2); + + clusterCenters.push_back(pt); + } // cout<<"CLusterCenters Obtained"<<"\n"; - std::vector KFpredictions; - i=0; - for (auto it=pred.begin();it!=pred.end();it++) - { - geometry_msgs::Point pt; - pt.x=(*it).at(0); - pt.y=(*it).at(1); - pt.z=(*it).at(2); + std::vector KFpredictions; + i = 0; + for (auto it = pred.begin(); it != pred.end(); it++) { + geometry_msgs::Point pt; + pt.x = (*it).at(0); + pt.y = (*it).at(1); + pt.z = (*it).at(2); - KFpredictions.push_back(pt); - - } + KFpredictions.push_back(pt); + } // cout<<"Got predictions"<<"\n"; - - - // Find the cluster that is more probable to be belonging to a given KF. - objID.clear();//Clear the objID vector - objID.resize(6);//Allocate default elements so that [i] doesnt segfault. Should be done better - // Copy clusterCentres for modifying it and preventing multiple assignments of the same ID - std::vector copyOfClusterCenters(clusterCenters); - std::vector > distMat; - - for(int filterN=0;filterN<6;filterN++) - { - std::vector distVec; - for(int n=0;n<6;n++) - { - distVec.push_back(euclidean_distance(KFpredictions[filterN],copyOfClusterCenters[n])); - } - - distMat.push_back(distVec); - /*// Based on distVec instead of distMat (global min). Has problems with the person's leg going out of scope - int ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())); - //cout<<"finterlN="< copyOfClusterCenters(clusterCenters); + std::vector> distMat; + for (int filterN = 0; filterN < 6; filterN++) { + std::vector distVec; + for (int n = 0; n < 6; n++) { + distVec.push_back( + euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n])); } - cout<<"distMat.size()"< (i,j); + std::pair minIndex(findIndexOfMin(distMat)); + cout << "Received minIndex=" << minIndex.first << "," << minIndex.second + << "\n"; + // 2. objID[i]=clusterCenters[j]; counter++ + objID[minIndex.first] = minIndex.second; + + // 3. distMat[i,:]=10000; distMat[:,j]=10000 + distMat[minIndex.first] = + std::vector(6, 10000.0); // Set the row to a high number. + for (int row = 0; row < distMat.size(); + row++) // set the column to a high number { - for ( const auto &s : row ) std::cout << s << ' '; - std::cout << std::endl; + distMat[row][minIndex.second] = 10000.0; } + // 4. if(counter<6) got to 1. + cout << "clusterCount=" << clusterCount << "\n"; + } + // cout<<"Got object IDs"<<"\n"; + // countIDs(objID);// for verif/corner cases - - for(int clusterCount=0;clusterCount<6;clusterCount++) - { - // 1. Find min(distMax)==> (i,j); - std::pair minIndex(findIndexOfMin(distMat)); - cout<<"Received minIndex="<(6,10000.0);// Set the row to a high number. - for(int row=0;row> cc; + for (int i = 0; i < 6; i++) { + vector pt; + pt.push_back(clusterCenters[objID[i]].x); + pt.push_back(clusterCenters[objID[i]].y); + pt.push_back(clusterCenters[objID[i]].z); + cc.push_back(pt); + } + // cout<<"cc[5][0]="< > cc; - for (int i=0;i<6;i++) - { - vector pt; - pt.push_back(clusterCenters[objID[i]].x); - pt.push_back(clusterCenters[objID[i]].y); - pt.push_back(clusterCenters[objID[i]].z); - - cc.push_back(pt); - } - //cout<<"cc[5][0]="<(0,0)==0.0f || meas0Mat.at(1,0)==0.0f)) + // cout<<"meas0Mat"<(0, 0) == 0.0f || meas0Mat.at(1, 0) == 0.0f)) Mat estimated0 = KF0.correct(meas0Mat); -if (!(meas1[0]==0.0f || meas1[1]==0.0f)) + if (!(meas1[0] == 0.0f || meas1[1] == 0.0f)) Mat estimated1 = KF1.correct(meas1Mat); -if (!(meas2[0]==0.0f || meas2[1]==0.0f)) + if (!(meas2[0] == 0.0f || meas2[1] == 0.0f)) Mat estimated2 = KF2.correct(meas2Mat); -if (!(meas3[0]==0.0f || meas3[1]==0.0f)) + if (!(meas3[0] == 0.0f || meas3[1] == 0.0f)) Mat estimated3 = KF3.correct(meas3Mat); -if (!(meas4[0]==0.0f || meas4[1]==0.0f)) + if (!(meas4[0] == 0.0f || meas4[1] == 0.0f)) Mat estimated4 = KF4.correct(meas4Mat); -if (!(meas5[0]==0.0f || meas5[1]==0.0f)) + if (!(meas5[0] == 0.0f || meas5[1] == 0.0f)) Mat estimated5 = KF5.correct(meas5Mat); - - - // Publish the point clouds belonging to each clusters + // Publish the point clouds belonging to each clusters - // cout<<"estimate="<(0)<<","<(1)<<"\n"; - // Point statePt(estimated.at(0),estimated.at(1)); -//cout<<"DONE KF_TRACKER\n"; - + // cout<<"estimate="<(0)<<","<(1)<<"\n"; + // Point statePt(estimated.at(0),estimated.at(1)); + // cout<<"DONE KF_TRACKER\n"; } -void publish_cloud(ros::Publisher& pub, pcl::PointCloud::Ptr cluster){ - sensor_msgs::PointCloud2::Ptr clustermsg (new sensor_msgs::PointCloud2); - pcl::toROSMsg (*cluster , *clustermsg); +void publish_cloud(ros::Publisher &pub, + pcl::PointCloud::Ptr cluster) { + sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(*cluster, *clustermsg); clustermsg->header.frame_id = "map"; 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="<(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); - KF1.transitionMatrix = (Mat_(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); - KF2.transitionMatrix = (Mat_(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); - KF3.transitionMatrix = (Mat_(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); - KF4.transitionMatrix = (Mat_(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); - KF5.transitionMatrix = (Mat_(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); + float dvx = 0.01f; // 1.0 + float dvy = 0.01f; // 1.0 + float dx = 1.0f; + float dy = 1.0f; + KF0.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF1.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF2.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF3.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF4.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF5.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); cv::setIdentity(KF0.measurementMatrix); cv::setIdentity(KF1.measurementMatrix); @@ -372,8 +359,8 @@ if (firstFrame) // [ 0 0 0 1 Ev_y 0 ] //// [ 0 0 0 0 1 Ew ] //// [ 0 0 0 0 0 Eh ] - float sigmaP=0.01; - float sigmaQ=0.1; + float sigmaP = 0.01; + float sigmaQ = 0.1; setIdentity(KF0.processNoiseCov, Scalar::all(sigmaP)); setIdentity(KF1.processNoiseCov, Scalar::all(sigmaP)); setIdentity(KF2.processNoiseCov, Scalar::all(sigmaP)); @@ -381,362 +368,341 @@ if (firstFrame) setIdentity(KF4.processNoiseCov, Scalar::all(sigmaP)); setIdentity(KF5.processNoiseCov, Scalar::all(sigmaP)); // Meas noise cov matrix R - cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ));//1e-1 - cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ)); - cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ)); - cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ)); - cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ)); - cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ)); + cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1 + cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ)); + cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ)); + cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ)); + cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ)); + cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ)); -// Process the point cloud - pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr clustered_cloud (new pcl::PointCloud); - /* Creating the KdTree from input point cloud*/ - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + // Process the point cloud + pcl::PointCloud::Ptr input_cloud( + new pcl::PointCloud); + pcl::PointCloud::Ptr clustered_cloud( + new pcl::PointCloud); + /* Creating the KdTree from input point cloud*/ + pcl::search::KdTree::Ptr tree( + new pcl::search::KdTree); - pcl::fromROSMsg (*input, *input_cloud); + pcl::fromROSMsg(*input, *input_cloud); - tree->setInputCloud (input_cloud); + tree->setInputCloud(input_cloud); + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(0.08); + ec.setMinClusterSize(10); + ec.setMaxClusterSize(600); + ec.setSearchMethod(tree); + ec.setInputCloud(input_cloud); + /* Extract the clusters out of pc and save indices in cluster_indices.*/ + ec.extract(cluster_indices); - std::vector cluster_indices; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance (0.08); - ec.setMinClusterSize (10); - ec.setMaxClusterSize (600); - ec.setSearchMethod (tree); - ec.setInputCloud (input_cloud); - /* Extract the clusters out of pc and save indices in cluster_indices.*/ - ec.extract (cluster_indices); - - - std::vector::const_iterator it; - std::vector::const_iterator pit; - // Vector of cluster pointclouds - std::vector::Ptr > cluster_vec; + std::vector::const_iterator it; + std::vector::const_iterator pit; + // Vector of cluster pointclouds + std::vector::Ptr> cluster_vec; // Cluster centroids - std::vector clusterCentroids; + std::vector clusterCentroids; - for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { - - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - float x=0.0; float y=0.0; - int numPts=0; - for(pit = it->indices.begin(); pit != it->indices.end(); pit++) - { - - cloud_cluster->points.push_back(input_cloud->points[*pit]); - x+=input_cloud->points[*pit].x; - y+=input_cloud->points[*pit].y; - numPts++; + for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { + pcl::PointCloud::Ptr cloud_cluster( + new pcl::PointCloud); + float x = 0.0; + float y = 0.0; + int numPts = 0; + for (pit = it->indices.begin(); pit != it->indices.end(); pit++) { - //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], - // origin); - //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster); - } + cloud_cluster->points.push_back(input_cloud->points[*pit]); + x += input_cloud->points[*pit].x; + y += input_cloud->points[*pit].y; + numPts++; + + // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], + // origin); + // mindist_this_cluster = std::min(dist_this_point, + // mindist_this_cluster); + } - pcl::PointXYZ centroid; - centroid.x=x/numPts; - centroid.y=y/numPts; - centroid.z=0.0; - + centroid.x = x / numPts; + centroid.y = y / numPts; + centroid.z = 0.0; + cluster_vec.push_back(cloud_cluster); - //Get the centroid of the cluster + // Get the centroid of the cluster clusterCentroids.push_back(centroid); - - } - //Ensure at least 6 clusters exist to publish (later clusters may be empty) - while (cluster_vec.size() < 6){ - pcl::PointCloud::Ptr empty_cluster (new pcl::PointCloud); - empty_cluster->points.push_back(pcl::PointXYZ(0,0,0)); + // Ensure at least 6 clusters exist to publish (later clusters may be empty) + while (cluster_vec.size() < 6) { + pcl::PointCloud::Ptr empty_cluster( + new pcl::PointCloud); + empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0)); cluster_vec.push_back(empty_cluster); } - while (clusterCentroids.size()<6) - { + while (clusterCentroids.size() < 6) { pcl::PointXYZ centroid; - centroid.x=0.0; - centroid.y=0.0; - centroid.z=0.0; - - clusterCentroids.push_back(centroid); + centroid.x = 0.0; + centroid.y = 0.0; + centroid.z = 0.0; + + clusterCentroids.push_back(centroid); } - - // Set initial state - KF0.statePre.at(0)=clusterCentroids.at(0).x; - KF0.statePre.at(1)=clusterCentroids.at(0).y; - KF0.statePre.at(2)=0;// initial v_x - KF0.statePre.at(3)=0;//initial v_y + // Set initial state + KF0.statePre.at(0) = clusterCentroids.at(0).x; + KF0.statePre.at(1) = clusterCentroids.at(0).y; + KF0.statePre.at(2) = 0; // initial v_x + KF0.statePre.at(3) = 0; // initial v_y - // Set initial state - KF1.statePre.at(0)=clusterCentroids.at(1).x; - KF1.statePre.at(1)=clusterCentroids.at(1).y; - KF1.statePre.at(2)=0;// initial v_x - KF1.statePre.at(3)=0;//initial v_y + // Set initial state + KF1.statePre.at(0) = clusterCentroids.at(1).x; + KF1.statePre.at(1) = clusterCentroids.at(1).y; + KF1.statePre.at(2) = 0; // initial v_x + KF1.statePre.at(3) = 0; // initial v_y - // Set initial state - KF2.statePre.at(0)=clusterCentroids.at(2).x; - KF2.statePre.at(1)=clusterCentroids.at(2).y; - KF2.statePre.at(2)=0;// initial v_x - KF2.statePre.at(3)=0;//initial v_y + // Set initial state + KF2.statePre.at(0) = clusterCentroids.at(2).x; + KF2.statePre.at(1) = clusterCentroids.at(2).y; + KF2.statePre.at(2) = 0; // initial v_x + KF2.statePre.at(3) = 0; // initial v_y + // Set initial state + KF3.statePre.at(0) = clusterCentroids.at(3).x; + KF3.statePre.at(1) = clusterCentroids.at(3).y; + KF3.statePre.at(2) = 0; // initial v_x + KF3.statePre.at(3) = 0; // initial v_y - // Set initial state - KF3.statePre.at(0)=clusterCentroids.at(3).x; - KF3.statePre.at(1)=clusterCentroids.at(3).y; - KF3.statePre.at(2)=0;// initial v_x - KF3.statePre.at(3)=0;//initial v_y + // Set initial state + KF4.statePre.at(0) = clusterCentroids.at(4).x; + KF4.statePre.at(1) = clusterCentroids.at(4).y; + KF4.statePre.at(2) = 0; // initial v_x + KF4.statePre.at(3) = 0; // initial v_y - // Set initial state - KF4.statePre.at(0)=clusterCentroids.at(4).x; - KF4.statePre.at(1)=clusterCentroids.at(4).y; - KF4.statePre.at(2)=0;// initial v_x - KF4.statePre.at(3)=0;//initial v_y + // Set initial state + KF5.statePre.at(0) = clusterCentroids.at(5).x; + KF5.statePre.at(1) = clusterCentroids.at(5).y; + KF5.statePre.at(2) = 0; // initial v_x + KF5.statePre.at(3) = 0; // initial v_y - // Set initial state - KF5.statePre.at(0)=clusterCentroids.at(5).x; - KF5.statePre.at(1)=clusterCentroids.at(5).y; - KF5.statePre.at(2)=0;// initial v_x - KF5.statePre.at(3)=0;//initial v_y + firstFrame = false; - firstFrame=false; + for (int i = 0; i < 6; i++) { + geometry_msgs::Point pt; + pt.x = clusterCentroids.at(i).x; + pt.y = clusterCentroids.at(i).y; + prevClusterCenters.push_back(pt); + } + /* // Print the initial state of the kalman filter for debugging + cout<<"KF0.satePre="<(0)<<","<(1)<<"\n"; + cout<<"KF1.satePre="<(0)<<","<(1)<<"\n"; + cout<<"KF2.satePre="<(0)<<","<(1)<<"\n"; + cout<<"KF3.satePre="<(0)<<","<(1)<<"\n"; + cout<<"KF4.satePre="<(0)<<","<(1)<<"\n"; + cout<<"KF5.satePre="<(0)<<","<(1)<<"\n"; - for (int i=0;i<6;i++) - { - geometry_msgs::Point pt; - pt.x=clusterCentroids.at(i).x; - pt.y=clusterCentroids.at(i).y; - prevClusterCenters.push_back(pt); - } - /* // Print the initial state of the kalman filter for debugging - cout<<"KF0.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF1.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF2.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF3.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF4.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF5.satePre="<(0)<<","<(1)<<"\n"; + //cin.ignore();// To be able to see the printed initial state of the + KalmanFilter + */ + } - //cin.ignore();// To be able to see the printed initial state of the KalmanFilter + else { + // cout<<"ELSE firstFrame="<::Ptr input_cloud( + new pcl::PointCloud); + pcl::PointCloud::Ptr clustered_cloud( + new pcl::PointCloud); + /* Creating the KdTree from input point cloud*/ + pcl::search::KdTree::Ptr tree( + new pcl::search::KdTree); + + pcl::fromROSMsg(*input, *input_cloud); + + tree->setInputCloud(input_cloud); + + /* Here we are creating a vector of PointIndices, which contains the actual + * index information in a vector. The indices of each detected cluster + * are saved here. Cluster_indices is a vector containing one instance of + * PointIndices for each detected cluster. Cluster_indices[0] contain all + * indices of the first cluster in input point cloud. */ -} + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(0.3); + ec.setMinClusterSize(10); + ec.setMaxClusterSize(600); + ec.setSearchMethod(tree); + ec.setInputCloud(input_cloud); + // cout<<"PCL init successfull\n"; + /* Extract the clusters out of pc and save indices in cluster_indices.*/ + ec.extract(cluster_indices); + // cout<<"PCL extract successfull\n"; + /* To separate each cluster out of the vector we have to + * iterate through cluster_indices, create a new PointCloud for each + * entry and write all points of the current cluster in the PointCloud. + */ + // pcl::PointXYZ origin (0,0,0); + // float mindist_this_cluster = 1000; + // float dist_this_point = 1000; - -else -{ - //cout<<"ELSE firstFrame="<::Ptr input_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr clustered_cloud (new pcl::PointCloud); - /* Creating the KdTree from input point cloud*/ - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + std::vector::const_iterator it; + std::vector::const_iterator pit; + // Vector of cluster pointclouds + std::vector::Ptr> cluster_vec; - pcl::fromROSMsg (*input, *input_cloud); + // Cluster centroids + std::vector clusterCentroids; - tree->setInputCloud (input_cloud); + for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { + float x = 0.0; + float y = 0.0; + int numPts = 0; + pcl::PointCloud::Ptr cloud_cluster( + new pcl::PointCloud); + for (pit = it->indices.begin(); pit != it->indices.end(); pit++) { - /* Here we are creating a vector of PointIndices, which contains the actual index - * information in a vector. The indices of each detected cluster are saved here. - * Cluster_indices is a vector containing one instance of PointIndices for each detected - * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud. - */ - std::vector cluster_indices; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance (0.3); - ec.setMinClusterSize (10); - ec.setMaxClusterSize (600); - ec.setSearchMethod (tree); - ec.setInputCloud (input_cloud); -//cout<<"PCL init successfull\n"; - /* Extract the clusters out of pc and save indices in cluster_indices.*/ - ec.extract (cluster_indices); -//cout<<"PCL extract successfull\n"; - /* To separate each cluster out of the vector we have to - * iterate through cluster_indices, create a new PointCloud for each - * entry and write all points of the current cluster in the PointCloud. - */ - //pcl::PointXYZ origin (0,0,0); - //float mindist_this_cluster = 1000; - //float dist_this_point = 1000; + cloud_cluster->points.push_back(input_cloud->points[*pit]); - std::vector::const_iterator it; - std::vector::const_iterator pit; - // Vector of cluster pointclouds - std::vector::Ptr > cluster_vec; + x += input_cloud->points[*pit].x; + y += input_cloud->points[*pit].y; + numPts++; - // Cluster centroids - std::vector clusterCentroids; + // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], + // origin); + // mindist_this_cluster = std::min(dist_this_point, + // mindist_this_cluster); + } - + pcl::PointXYZ centroid; + centroid.x = x / numPts; + centroid.y = y / numPts; + centroid.z = 0.0; - for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) - { - float x=0.0; float y=0.0; - int numPts=0; - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - for(pit = it->indices.begin(); pit != it->indices.end(); pit++) - { - - cloud_cluster->points.push_back(input_cloud->points[*pit]); - - - x+=input_cloud->points[*pit].x; - y+=input_cloud->points[*pit].y; - numPts++; - - //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], - // origin); - //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster); - } - - pcl::PointXYZ centroid; - centroid.x=x/numPts; - centroid.y=y/numPts; - centroid.z=0.0; - cluster_vec.push_back(cloud_cluster); - //Get the centroid of the cluster + // Get the centroid of the cluster clusterCentroids.push_back(centroid); - } - // cout<<"cluster_vec got some clusters\n"; + // 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){ - pcl::PointCloud::Ptr empty_cluster (new pcl::PointCloud); - empty_cluster->points.push_back(pcl::PointXYZ(0,0,0)); + // Ensure at least 6 clusters exist to publish (later clusters may be empty) + while (cluster_vec.size() < 6) { + pcl::PointCloud::Ptr empty_cluster( + new pcl::PointCloud); + empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0)); cluster_vec.push_back(empty_cluster); } - while (clusterCentroids.size()<6) - { + while (clusterCentroids.size() < 6) { pcl::PointXYZ centroid; - centroid.x=0.0; - centroid.y=0.0; - centroid.z=0.0; - - clusterCentroids.push_back(centroid); - } + centroid.x = 0.0; + centroid.y = 0.0; + centroid.z = 0.0; + clusterCentroids.push_back(centroid); + } std_msgs::Float32MultiArray cc; - for(int i=0;i<6;i++) - { - cc.data.push_back(clusterCentroids.at(i).x); - cc.data.push_back(clusterCentroids.at(i).y); - cc.data.push_back(clusterCentroids.at(i).z); - + for (int i = 0; i < 6; i++) { + cc.data.push_back(clusterCentroids.at(i).x); + cc.data.push_back(clusterCentroids.at(i).y); + cc.data.push_back(clusterCentroids.at(i).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); - int i=0; + int i = 0; bool publishedCluster[6]; - for(auto it=objID.begin();it!=objID.end();it++) - { //cout<<"Inside the for loop\n"; - - - switch(i) - { - cout<<"Inside the switch case\n"; - case 0: { - publish_cloud(pub_cluster0,cluster_vec[*it]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - case 1: { - publish_cloud(pub_cluster1,cluster_vec[*it]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - case 2: { - publish_cloud(pub_cluster2,cluster_vec[*it]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - case 3: { - publish_cloud(pub_cluster3,cluster_vec[*it]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - case 4: { - publish_cloud(pub_cluster4,cluster_vec[*it]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - - case 5: { - publish_cloud(pub_cluster5,cluster_vec[*it]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - default: break; - } - - } - -} - -} - - - - -int main(int argc, char** argv) -{ - // ROS init - ros::init (argc,argv,"kf_tracker"); - ros::NodeHandle nh; - - - // Publishers to publish the state of the objects (pos and vel) - //objState1=nh.advertise ("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); - // Create a ROS publisher for the output point cloud - pub_cluster0 = nh.advertise ("cluster_0", 1); - pub_cluster1 = nh.advertise ("cluster_1", 1); - pub_cluster2 = nh.advertise ("cluster_2", 1); - pub_cluster3 = nh.advertise ("cluster_3", 1); - pub_cluster4 = nh.advertise ("cluster_4", 1); - pub_cluster5 = nh.advertise ("cluster_5", 1); - // Subscribe to the clustered pointclouds - //ros::Subscriber c1=nh.subscribe("ccs",100,KFT); - objID_pub = nh.advertise("obj_id", 1); -/* Point cloud clustering -*/ - - //cc_pos=nh.advertise("ccs",100);//clusterCenter1 - markerPub= nh.advertise ("viz",1); - -/* Point cloud clustering -*/ - - - ros::spin(); + for (auto it = objID.begin(); it != objID.end(); + it++) { // cout<<"Inside the for loop\n"; + switch (i) { + cout << "Inside the switch case\n"; + case 0: { + publish_cloud(pub_cluster0, cluster_vec[*it]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 1: { + publish_cloud(pub_cluster1, cluster_vec[*it]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 2: { + publish_cloud(pub_cluster2, cluster_vec[*it]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 3: { + publish_cloud(pub_cluster3, cluster_vec[*it]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 4: { + publish_cloud(pub_cluster4, cluster_vec[*it]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 5: { + publish_cloud(pub_cluster5, cluster_vec[*it]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + default: + break; + } + } + } +} + +int main(int argc, char **argv) { + // ROS init + ros::init(argc, argv, "kf_tracker"); + ros::NodeHandle nh; + + // Publishers to publish the state of the objects (pos and vel) + // objState1=nh.advertise ("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); + // Create a ROS publisher for the output point cloud + pub_cluster0 = nh.advertise("cluster_0", 1); + pub_cluster1 = nh.advertise("cluster_1", 1); + pub_cluster2 = nh.advertise("cluster_2", 1); + pub_cluster3 = nh.advertise("cluster_3", 1); + pub_cluster4 = nh.advertise("cluster_4", 1); + pub_cluster5 = nh.advertise("cluster_5", 1); + // Subscribe to the clustered pointclouds + // ros::Subscriber c1=nh.subscribe("ccs",100,KFT); + objID_pub = nh.advertise("obj_id", 1); + /* Point cloud clustering + */ + + // cc_pos=nh.advertise("ccs",100);//clusterCenter1 + markerPub = nh.advertise("viz", 1); + + /* Point cloud clustering + */ + + ros::spin(); } diff --git a/src/main_naive.cpp b/src/main_naive.cpp index 7be52e9..5495650 100644 --- a/src/main_naive.cpp +++ b/src/main_naive.cpp @@ -1,163 +1,162 @@ -#include -#include -#include -#include -#include -#include "kf_tracker/featureDetection.h" #include "kf_tracker/CKalmanFilter.h" +#include "kf_tracker/featureDetection.h" +#include "opencv2/video/tracking.hpp" +#include "pcl_ros/point_cloud.h" +#include +#include +#include +#include +#include #include #include #include #include -#include "opencv2/video/tracking.hpp" -#include #include #include -#include "pcl_ros/point_cloud.h" -#include +#include #include #include +#include -#include -#include -#include -#include #include +#include #include #include -#include #include +#include +#include #include #include -#include #include +#include +#include +#include using namespace std; using namespace cv; - ros::Publisher objID_pub; - // KF init - int stateDim=4;// [x,y,v_x,v_y]//,w,h] - int measDim=2;// [z_x,z_y,z_w,z_h] - int ctrlDim=0; - cv::KalmanFilter KF0(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF1(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF2(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF3(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF4(stateDim,measDim,ctrlDim,CV_32F); - cv::KalmanFilter KF5(stateDim,measDim,ctrlDim,CV_32F); +// KF init +int stateDim = 4; // [x,y,v_x,v_y]//,w,h] +int measDim = 2; // [z_x,z_y,z_w,z_h] +int ctrlDim = 0; +cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F); +cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F); - ros::Publisher pub_cluster0; - ros::Publisher pub_cluster1; - ros::Publisher pub_cluster2; - ros::Publisher pub_cluster3; - ros::Publisher pub_cluster4; - ros::Publisher pub_cluster5; +ros::Publisher pub_cluster0; +ros::Publisher pub_cluster1; +ros::Publisher pub_cluster2; +ros::Publisher pub_cluster3; +ros::Publisher pub_cluster4; +ros::Publisher pub_cluster5; - std::vector prevClusterCenters; +std::vector prevClusterCenters; +cv::Mat state(stateDim, 1, CV_32F); +cv::Mat_ measurement(2, 1); - cv::Mat state(stateDim,1,CV_32F); - cv::Mat_ measurement(2,1); +std::vector objID; // Output of the data association using KF + // measurement.setTo(Scalar(0)); - std::vector objID;// Output of the data association using KF - // measurement.setTo(Scalar(0)); - -bool firstFrame=true; +bool firstFrame = true; // calculate euclidean distance of two points - 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)); - } +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)); +} /* -//Count unique object IDs. just to make sure same ID has not been assigned to two KF_Trackers. -int countIDs(vector v) +//Count unique object IDs. just to make sure same ID has not been assigned to +two KF_Trackers. int countIDs(vector v) { - transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = distance(v.end(), v.begin()) - sort(v.begin(), v.end()); // Average case O(n log 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. + transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = +distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log +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. // 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 section of the range - auto unique_end = unique(v.begin(), v.end()); // Again n comparisons - return distance(unique_end, v.begin()); // Constant time for random access iterators (like vector's) + // items to the back and returns an iterator to the end of the unique +section of the range auto unique_end = unique(v.begin(), v.end()); // Again n +comparisons return distance(unique_end, v.begin()); // Constant time for random +access iterators (like vector's) } */ /* -objID: vector containing the IDs of the clusters that should be associated with each KF_Tracker -objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc. +objID: vector containing the IDs of the clusters that should be associated with +each KF_Tracker 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 + std::vector pred{KF0.predict(), KF1.predict(), KF2.predict(), + KF3.predict(), KF4.predict(), KF5.predict()}; + // cout<<"Pred successfull\n"; - // First predict, to update the internal statePre variable + // cv::Point predictPt(prediction.at(0),prediction.at(1)); + // cout<<"Prediction 1 + // ="<(0)<<","<(1)<<"\n"; - std::vector pred{KF0.predict(),KF1.predict(),KF2.predict(),KF3.predict(),KF4.predict(),KF5.predict()}; - //cout<<"Pred successfull\n"; + // Get measurements + // Extract the position of the clusters forom the multiArray. To check if the + // data coming in, check the .z (every third) coordinate and that will be 0.0 + std::vector clusterCenters; // clusterCenters - //cv::Point predictPt(prediction.at(0),prediction.at(1)); - // cout<<"Prediction 1 ="<(0)<<","<(1)<<"\n"; + int i = 0; + for (std::vector::const_iterator it = ccs.data.begin(); + it != ccs.data.end(); it += 3) { + geometry_msgs::Point pt; + pt.x = *it; + pt.y = *(it + 1); + pt.z = *(it + 2); - // Get measurements - // Extract the position of the clusters forom the multiArray. To check if the data - // coming in, check the .z (every third) coordinate and that will be 0.0 - std::vector clusterCenters;//clusterCenters - - int i=0; - for (std::vector::const_iterator it=ccs.data.begin();it!=ccs.data.end();it+=3) - { - geometry_msgs::Point pt; - pt.x=*it; - pt.y=*(it+1); - pt.z=*(it+2); - - clusterCenters.push_back(pt); - - } + clusterCenters.push_back(pt); + } // cout<<"CLusterCenters Obtained"<<"\n"; - std::vector KFpredictions; - i=0; - for (auto it=pred.begin();it!=pred.end();it++) - { - geometry_msgs::Point pt; - pt.x=(*it).at(0); - pt.y=(*it).at(1); - pt.z=(*it).at(2); + std::vector KFpredictions; + i = 0; + for (auto it = pred.begin(); it != pred.end(); it++) { + geometry_msgs::Point pt; + pt.x = (*it).at(0); + pt.y = (*it).at(1); + pt.z = (*it).at(2); - KFpredictions.push_back(pt); - - } - // cout<<"Got predictions"<<"\n"; + KFpredictions.push_back(pt); + } + // cout<<"Got predictions"<<"\n"; - /* Original Version using Kalman filter prediction - - // Find the cluster that is more probable to be belonging to a given KF. - objID.clear();//Clear the objID vector - for(int filterN=0;filterN<6;filterN++) - { - std::vector distVec; - for(int n=0;n<6;n++) - distVec.push_back(euclidean_distance(KFpredictions[filterN],clusterCenters[n])); - - // cout<<"distVec[="< distVec; + for(int n=0;n<6;n++) + distVec.push_back(euclidean_distance(KFpredictions[filterN],clusterCenters[n])); + + // + cout<<"distVec[="< distVec; - for(int n=0;n<6;n++) - distVec.push_back(euclidean_distance(prevClusterCenters[n],clusterCenters[n])); - - // cout<<"distVec[="< distVec; + for (int n = 0; n < 6; n++) + distVec.push_back( + euclidean_distance(prevClusterCenters[n], clusterCenters[n])); - /* Naive version without kalman filter */ + // cout<<"distVec[="< > cc; - for (int i=0;i pt; - pt.push_back(clusterCenters[i].x); - pt.push_back(clusterCenters[i].y); - pt.push_back(clusterCenters[i].z); - - cc.push_back(pt); - } - //cout<<"cc[5][0]="<> cc; + for (int i = 0; i < clusterCenters.size(); i++) { + vector pt; + pt.push_back(clusterCenters[i].x); + pt.push_back(clusterCenters[i].y); + pt.push_back(clusterCenters[i].z); + cc.push_back(pt); + } + // cout<<"cc[5][0]="<(1)<<"\n"; - // Point statePt(estimated.at(0),estimated.at(1)); -//cout<<"DONE KF_TRACKER\n"; - + // cout<<"estimate="<(0)<<","<(1)<<"\n"; + // Point statePt(estimated.at(0),estimated.at(1)); + // cout<<"DONE KF_TRACKER\n"; } -void publish_cloud(ros::Publisher& pub, pcl::PointCloud::Ptr cluster){ - sensor_msgs::PointCloud2::Ptr clustermsg (new sensor_msgs::PointCloud2); - pcl::toROSMsg (*cluster , *clustermsg); +void publish_cloud(ros::Publisher &pub, + pcl::PointCloud::Ptr cluster) { + sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(*cluster, *clustermsg); clustermsg->header.frame_id = "map"; 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="<(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); - KF1.transitionMatrix = *(Mat_(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); - KF2.transitionMatrix = *(Mat_(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); - KF3.transitionMatrix = *(Mat_(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); - KF4.transitionMatrix = *(Mat_(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); - KF5.transitionMatrix = *(Mat_(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); + cout << "IF firstFrame=" << firstFrame << "\n"; + // If this is the first frame, initialize kalman filters for the clustered + // objects + if (firstFrame) { + // 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 + // is detected + KF0.transitionMatrix = + *(Mat_(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); + KF1.transitionMatrix = + *(Mat_(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); + KF2.transitionMatrix = + *(Mat_(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); + KF3.transitionMatrix = + *(Mat_(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); + KF4.transitionMatrix = + *(Mat_(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); + KF5.transitionMatrix = + *(Mat_(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); cv::setIdentity(KF0.measurementMatrix); cv::setIdentity(KF1.measurementMatrix); @@ -288,316 +286,323 @@ if (firstFrame) setIdentity(KF4.processNoiseCov, Scalar::all(1e-4)); setIdentity(KF5.processNoiseCov, Scalar::all(1e-4)); // Meas noise cov matrix R - cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(1e-1)); - cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(1e-1)); - cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(1e-1)); - cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(1e-1)); - cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(1e-1)); - cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1)); + cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(1e-1)); + cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(1e-1)); + cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(1e-1)); + cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(1e-1)); + cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(1e-1)); + cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1)); -// Process the point cloud - pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr clustered_cloud (new pcl::PointCloud); - /* Creating the KdTree from input point cloud*/ - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + // Process the point cloud + pcl::PointCloud::Ptr input_cloud( + new pcl::PointCloud); + pcl::PointCloud::Ptr clustered_cloud( + new pcl::PointCloud); + /* Creating the KdTree from input point cloud*/ + pcl::search::KdTree::Ptr tree( + new pcl::search::KdTree); - 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 index - * information in a vector. The indices of each detected cluster are saved here. - * Cluster_indices is a vector containing one instance of PointIndices for each detected - * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud. - */ - std::vector cluster_indices; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance (0.08); - ec.setMinClusterSize (10); - ec.setMaxClusterSize (600); - ec.setSearchMethod (tree); - ec.setInputCloud (input_cloud); - /* Extract the clusters out of pc and save indices in cluster_indices.*/ - ec.extract (cluster_indices); + /* Here we are creating a vector of PointIndices, which contains the actual + * index information in a vector. The indices of each detected cluster + * are saved here. Cluster_indices is a vector containing one instance of + * PointIndices for each detected cluster. Cluster_indices[0] contain all + * indices of the first cluster in input point cloud. + */ + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(0.08); + ec.setMinClusterSize(10); + ec.setMaxClusterSize(600); + ec.setSearchMethod(tree); + ec.setInputCloud(input_cloud); + /* Extract the clusters out of pc and save indices in cluster_indices.*/ + ec.extract(cluster_indices); - /* To separate each cluster out of the vector we have to - * iterate through cluster_indices, create a new PointCloud for each - * entry and write all points of the current cluster in the PointCloud. - */ - //pcl::PointXYZ origin (0,0,0); - //float mindist_this_cluster = 1000; - //float dist_this_point = 1000; + /* To separate each cluster out of the vector we have to + * iterate through cluster_indices, create a new PointCloud for each + * entry and write all points of the current cluster in the PointCloud. + */ + // pcl::PointXYZ origin (0,0,0); + // float mindist_this_cluster = 1000; + // float dist_this_point = 1000; - std::vector::const_iterator it; - std::vector::const_iterator pit; - // Vector of cluster pointclouds - std::vector::Ptr > cluster_vec; + std::vector::const_iterator it; + std::vector::const_iterator pit; + // Vector of cluster pointclouds + std::vector::Ptr> cluster_vec; - for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - for(pit = it->indices.begin(); pit != it->indices.end(); pit++) - { - - cloud_cluster->points.push_back(input_cloud->points[*pit]); - //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], - // origin); - //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster); - } + for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { + pcl::PointCloud::Ptr cloud_cluster( + new pcl::PointCloud); + for (pit = it->indices.begin(); pit != it->indices.end(); pit++) { + + cloud_cluster->points.push_back(input_cloud->points[*pit]); + // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], + // origin); + // mindist_this_cluster = std::min(dist_this_point, + // mindist_this_cluster); + } - cluster_vec.push_back(cloud_cluster); - } - //Ensure at least 6 clusters exist to publish (later clusters may be empty) - while (cluster_vec.size() < 6){ - pcl::PointCloud::Ptr empty_cluster (new pcl::PointCloud); - empty_cluster->points.push_back(pcl::PointXYZ(0,0,0)); + // Ensure at least 6 clusters exist to publish (later clusters may be empty) + while (cluster_vec.size() < 6) { + pcl::PointCloud::Ptr empty_cluster( + new pcl::PointCloud); + empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0)); cluster_vec.push_back(empty_cluster); } - - // Set initial state - KF0.statePre.at(0)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].x; - - KF0.statePre.at(1)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].y; - KF0.statePre.at(2)=0;// initial v_x - KF0.statePre.at(3)=0;//initial v_y + // Set initial state + KF0.statePre.at(0) = + cluster_vec[0]->points[cluster_vec[0]->points.size() / 2].x; - // Set initial state - KF1.statePre.at(0)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].x; - KF1.statePre.at(1)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].y; - KF1.statePre.at(2)=0;// initial v_x - KF1.statePre.at(3)=0;//initial v_y + KF0.statePre.at(1) = + cluster_vec[0]->points[cluster_vec[0]->points.size() / 2].y; + KF0.statePre.at(2) = 0; // initial v_x + KF0.statePre.at(3) = 0; // initial v_y - // Set initial state - KF2.statePre.at(0)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].x; - KF2.statePre.at(1)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].y; - KF2.statePre.at(2)=0;// initial v_x - KF2.statePre.at(3)=0;//initial v_y + // Set initial state + KF1.statePre.at(0) = + cluster_vec[1]->points[cluster_vec[1]->points.size() / 2].x; + KF1.statePre.at(1) = + cluster_vec[1]->points[cluster_vec[1]->points.size() / 2].y; + KF1.statePre.at(2) = 0; // initial v_x + KF1.statePre.at(3) = 0; // initial v_y + // Set initial state + KF2.statePre.at(0) = + cluster_vec[2]->points[cluster_vec[2]->points.size() / 2].x; + KF2.statePre.at(1) = + cluster_vec[2]->points[cluster_vec[2]->points.size() / 2].y; + KF2.statePre.at(2) = 0; // initial v_x + KF2.statePre.at(3) = 0; // initial v_y - // Set initial state - KF3.statePre.at(0)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].x; - KF3.statePre.at(1)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].y; - KF3.statePre.at(2)=0;// initial v_x - KF3.statePre.at(3)=0;//initial v_y + // Set initial state + KF3.statePre.at(0) = + cluster_vec[3]->points[cluster_vec[3]->points.size() / 2].x; + KF3.statePre.at(1) = + cluster_vec[3]->points[cluster_vec[3]->points.size() / 2].y; + KF3.statePre.at(2) = 0; // initial v_x + KF3.statePre.at(3) = 0; // initial v_y - // Set initial state - KF4.statePre.at(0)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].x; - KF4.statePre.at(1)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].y; - KF4.statePre.at(2)=0;// initial v_x - KF4.statePre.at(3)=0;//initial v_y + // Set initial state + KF4.statePre.at(0) = + cluster_vec[4]->points[cluster_vec[4]->points.size() / 2].x; + KF4.statePre.at(1) = + cluster_vec[4]->points[cluster_vec[4]->points.size() / 2].y; + KF4.statePre.at(2) = 0; // initial v_x + KF4.statePre.at(3) = 0; // initial v_y - // Set initial state - KF5.statePre.at(0)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].x; - KF5.statePre.at(1)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].y; - KF5.statePre.at(2)=0;// initial v_x - KF5.statePre.at(3)=0;//initial v_y + // Set initial state + KF5.statePre.at(0) = + cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].x; + KF5.statePre.at(1) = + cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].y; + KF5.statePre.at(2) = 0; // initial v_x + KF5.statePre.at(3) = 0; // initial v_y - firstFrame=false; + firstFrame = false; - // Print the initial state of the kalman filter for debugging - cout<<"KF0.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF1.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF2.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF3.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF4.satePre="<(0)<<","<(1)<<"\n"; - cout<<"KF5.satePre="<(0)<<","<(1)<<"\n"; + // Print the initial state of the kalman filter for debugging + cout << "KF0.satePre=" << KF0.statePre.at(0) << "," + << KF0.statePre.at(1) << "\n"; + cout << "KF1.satePre=" << KF1.statePre.at(0) << "," + << KF1.statePre.at(1) << "\n"; + cout << "KF2.satePre=" << KF2.statePre.at(0) << "," + << KF2.statePre.at(1) << "\n"; + cout << "KF3.satePre=" << KF3.statePre.at(0) << "," + << KF3.statePre.at(1) << "\n"; + cout << "KF4.satePre=" << KF4.statePre.at(0) << "," + << KF4.statePre.at(1) << "\n"; + cout << "KF5.satePre=" << KF5.statePre.at(0) << "," + << KF5.statePre.at(1) << "\n"; - //cin.ignore();// To be able to see the printed initial state of the KalmanFilter - - - - /* Naive version without kalman filter */ - for (int i=0;i<6;i++) - { - geometry_msgs::Point pt; - 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; - prevClusterCenters.push_back(pt); - } - - /* Naive version without kalman filter */ -} - - -else -{ - cout<<"ELSE firstFrame="<::Ptr input_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr clustered_cloud (new pcl::PointCloud); - /* Creating the KdTree from input point cloud*/ - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - - pcl::fromROSMsg (*input, *input_cloud); - - tree->setInputCloud (input_cloud); - - /* Here we are creating a vector of PointIndices, which contains the actual index - * information in a vector. The indices of each detected cluster are saved here. - * Cluster_indices is a vector containing one instance of PointIndices for each detected - * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud. - */ - std::vector cluster_indices; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance (0.08); - ec.setMinClusterSize (10); - ec.setMaxClusterSize (600); - ec.setSearchMethod (tree); - ec.setInputCloud (input_cloud); -cout<<"PCL init successfull\n"; - /* Extract the clusters out of pc and save indices in cluster_indices.*/ - ec.extract (cluster_indices); -cout<<"PCL extract successfull\n"; - /* To separate each cluster out of the vector we have to - * iterate through cluster_indices, create a new PointCloud for each - * entry and write all points of the current cluster in the PointCloud. - */ - //pcl::PointXYZ origin (0,0,0); - //float mindist_this_cluster = 1000; - //float dist_this_point = 1000; - - std::vector::const_iterator it; - std::vector::const_iterator pit; - // Vector of cluster pointclouds - std::vector::Ptr > cluster_vec; - - for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - for(pit = it->indices.begin(); pit != it->indices.end(); pit++) - { - - cloud_cluster->points.push_back(input_cloud->points[*pit]); - //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], - // origin); - //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster); - } - - - cluster_vec.push_back(cloud_cluster); + // cin.ignore();// To be able to see the printed initial state of the + // KalmanFilter + /* Naive version without kalman filter */ + for (int i = 0; i < 6; i++) { + geometry_msgs::Point pt; + 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; + prevClusterCenters.push_back(pt); } - //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){ - pcl::PointCloud::Ptr empty_cluster (new pcl::PointCloud); - empty_cluster->points.push_back(pcl::PointXYZ(0,0,0)); + /* Naive version without kalman filter */ + } + + else { + cout << "ELSE firstFrame=" << firstFrame << "\n"; + pcl::PointCloud::Ptr input_cloud( + new pcl::PointCloud); + pcl::PointCloud::Ptr clustered_cloud( + new pcl::PointCloud); + /* Creating the KdTree from input point cloud*/ + pcl::search::KdTree::Ptr tree( + new pcl::search::KdTree); + + pcl::fromROSMsg(*input, *input_cloud); + + tree->setInputCloud(input_cloud); + + /* Here we are creating a vector of PointIndices, which contains the actual + * index information in a vector. The indices of each detected cluster + * are saved here. Cluster_indices is a vector containing one instance of + * PointIndices for each detected cluster. Cluster_indices[0] contain all + * indices of the first cluster in input point cloud. + */ + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(0.08); + ec.setMinClusterSize(10); + ec.setMaxClusterSize(600); + ec.setSearchMethod(tree); + ec.setInputCloud(input_cloud); + cout << "PCL init successfull\n"; + /* Extract the clusters out of pc and save indices in cluster_indices.*/ + ec.extract(cluster_indices); + cout << "PCL extract successfull\n"; + /* To separate each cluster out of the vector we have to + * iterate through cluster_indices, create a new PointCloud for each + * entry and write all points of the current cluster in the PointCloud. + */ + // pcl::PointXYZ origin (0,0,0); + // float mindist_this_cluster = 1000; + // float dist_this_point = 1000; + + std::vector::const_iterator it; + std::vector::const_iterator pit; + // Vector of cluster pointclouds + std::vector::Ptr> cluster_vec; + + for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { + pcl::PointCloud::Ptr cloud_cluster( + new pcl::PointCloud); + for (pit = it->indices.begin(); pit != it->indices.end(); pit++) { + + cloud_cluster->points.push_back(input_cloud->points[*pit]); + // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], + // origin); + // mindist_this_cluster = std::min(dist_this_point, + // mindist_this_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) { + pcl::PointCloud::Ptr empty_cluster( + new pcl::PointCloud); + empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0)); cluster_vec.push_back(empty_cluster); } std_msgs::Float32MultiArray cc; - 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].y); - cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].z); - + 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].y); + 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); - int i=0; + int i = 0; bool publishedCluster[6]; - for(auto it=objID.begin();it!=objID.end();it++) - { cout<<"Inside the for loop\n"; - - switch(*it) - { - cout<<"Inside the switch case\n"; - case 0: { - publish_cloud(pub_cluster0,cluster_vec[i]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - case 1: { - publish_cloud(pub_cluster1,cluster_vec[i]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - case 2: { - publish_cloud(pub_cluster2,cluster_vec[i]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - case 3: { - publish_cloud(pub_cluster3,cluster_vec[i]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - case 4: { - publish_cloud(pub_cluster4,cluster_vec[i]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - - case 5: { - publish_cloud(pub_cluster5,cluster_vec[i]); - publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID - i++; - break; - - } - default: break; - } - - } - -} - -} - - - - -int main(int argc, char** argv) -{ - // ROS init - ros::init (argc,argv,"KFTracker"); - ros::NodeHandle nh; - - - // Publishers to publish the state of the objects (pos and vel) - //objState1=nh.advertise ("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); - // Create a ROS publisher for the output point cloud - pub_cluster0 = nh.advertise ("cluster_0", 1); - pub_cluster1 = nh.advertise ("cluster_1", 1); - pub_cluster2 = nh.advertise ("cluster_2", 1); - pub_cluster3 = nh.advertise ("cluster_3", 1); - pub_cluster4 = nh.advertise ("cluster_4", 1); - pub_cluster5 = nh.advertise ("cluster_5", 1); - // Subscribe to the clustered pointclouds - //ros::Subscriber c1=nh.subscribe("ccs",100,KFT); - objID_pub = nh.advertise("obj_id", 1); -/* Point cloud clustering -*/ - - //cc_pos=nh.advertise("ccs",100);//clusterCenter1 - - -/* Point cloud clustering -*/ - - - ros::spin(); + for (auto it = objID.begin(); it != objID.end(); it++) { + cout << "Inside the for loop\n"; + switch (*it) { + cout << "Inside the switch case\n"; + case 0: { + publish_cloud(pub_cluster0, cluster_vec[i]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 1: { + publish_cloud(pub_cluster1, cluster_vec[i]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 2: { + publish_cloud(pub_cluster2, cluster_vec[i]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 3: { + publish_cloud(pub_cluster3, cluster_vec[i]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 4: { + publish_cloud(pub_cluster4, cluster_vec[i]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + case 5: { + publish_cloud(pub_cluster5, cluster_vec[i]); + publishedCluster[i] = + true; // Use this flag to publish only once for a given obj ID + i++; + break; + } + default: + break; + } + } + } +} + +int main(int argc, char **argv) { + // ROS init + ros::init(argc, argv, "KFTracker"); + ros::NodeHandle nh; + + // Publishers to publish the state of the objects (pos and vel) + // objState1=nh.advertise ("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); + // Create a ROS publisher for the output point cloud + pub_cluster0 = nh.advertise("cluster_0", 1); + pub_cluster1 = nh.advertise("cluster_1", 1); + pub_cluster2 = nh.advertise("cluster_2", 1); + pub_cluster3 = nh.advertise("cluster_3", 1); + pub_cluster4 = nh.advertise("cluster_4", 1); + pub_cluster5 = nh.advertise("cluster_5", 1); + // Subscribe to the clustered pointclouds + // ros::Subscriber c1=nh.subscribe("ccs",100,KFT); + objID_pub = nh.advertise("obj_id", 1); + /* Point cloud clustering + */ + + // cc_pos=nh.advertise("ccs",100);//clusterCenter1 + + /* Point cloud clustering + */ + + ros::spin(); }