v2. Unsupervised clustering is incorporated into the same node (tracker).

src/main.cpp

@@ -18,9 +18,24 @@
 #include <std_msgs/Float32MultiArray.h>
 #include <std_msgs/Int32MultiArray.h>
 
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.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/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+
 using namespace std;
 using namespace cv;
 
+
 ros::Publisher objID_pub;
 
     // KF init
@@ -34,13 +49,23 @@ ros::Publisher objID_pub;
     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;
+
 
     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));
 
+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)
   {
     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));
@@ -65,19 +90,12 @@ int countIDs(vector<int> v)
 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::ConstPtr& ccs)
+void KFT(const std_msgs::Float32MultiArray ccs)
 {
 
 
 
     // First predict, to update the internal statePre variable
-   /* Mat prediction0 = KF0.predict();
-    Mat prediction1 = KF1.predict();
-    Mat prediction2 = KF2.predict();
-    Mat prediction3 = KF3.predict();
-    Mat prediction4 = KF4.predict();
-    Mat prediction5 = KF5.predict();
-    */
 
     std::vector<cv::Mat> pred{KF0.predict(),KF1.predict(),KF2.predict(),KF3.predict(),KF4.predict(),KF5.predict()};
  //cout<<"Pred successfull\n";
@@ -91,7 +109,7 @@ void KFT(const std_msgs::Float32MultiArray::ConstPtr& ccs)
     std::vector<geometry_msgs::Point> clusterCenters;//clusterCenters
    
     int i=0;
-    for (std::vector<float>::const_iterator it=ccs->data.begin();it!=ccs->data.end();it+=3)
+    for (std::vector<float>::const_iterator it=ccs.data.begin();it!=ccs.data.end();it+=3)
     {
         geometry_msgs::Point pt;
         pt.x=*it;
@@ -117,10 +135,10 @@ void KFT(const std_msgs::Float32MultiArray::ConstPtr& ccs)
     }
    // cout<<"Got predictions"<<"\n";
 
-    std::vector<int> objID;
+    
     
     // 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<float> distVec;
@@ -184,6 +202,7 @@ void KFT(const std_msgs::Float32MultiArray::ConstPtr& ccs)
     Mat estimated4 = KF0.correct(meas4Mat);
     Mat estimated5 = KF0.correct(meas5Mat);
  
+    // Publish the point clouds belonging to each clusters
 
 
    // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
@@ -191,18 +210,25 @@ void KFT(const std_msgs::Float32MultiArray::ConstPtr& ccs)
 //cout<<"DONE KF_TRACKER\n";
    
 }
+void publish_cloud(ros::Publisher& pub, pcl::PointCloud<pcl::PointXYZ>::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);
+
+}
 
 
-int main(int argc, char** argv)
+void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
+
 {
-    // ROS init
+    cout<<"IF firstFrame="<<firstFrame<<"\n";
-    ros::init (argc,argv,"KFTracker");
+    // If this is the first frame, initialize kalman filters for the clustered objects
-    ros::NodeHandle nh;
+if (firstFrame)
-
+{   
-   
+  // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset. 
-    // Publishers to publish the state of the objects (pos and vel)
+  // Could be made generic by creating a Kalman Filter only when a new object is detected  
-    //objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
-
     KF0.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
     KF1.transitionMatrix = *(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);
@@ -237,47 +263,293 @@ int main(int argc, char** argv)
      cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(1e-1));
      cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1));
 
+// Process the point cloud
+     pcl::PointCloud<pcl::PointXYZ>::Ptr input_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*/
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+
+  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<int>. 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<pcl::PointIndices> cluster_indices;
+  pcl::EuclideanClusterExtraction<pcl::PointXYZ> 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<PointIndices> 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<pcl::PointIndices>::const_iterator it;
+  std::vector<int>::const_iterator pit;
+  // Vector of cluster pointclouds
+  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
+
+  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
+          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
+          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<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);
+    }
+    
+
      // Set initial state
-     KF0.statePre.at<float>(0)=0.0;//initial x pos of the cluster
+     KF0.statePre.at<float>(0)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].x;
-     KF0.statePre.at<float>(1)=0.0;//initial y pos of the cluster
+     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
-     KF1.statePre.at<float>(0)=0.0;//initial x pos of the cluster
+     KF1.statePre.at<float>(0)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].x;
-     KF1.statePre.at<float>(1)=0.0;//initial y pos of the cluster
+     KF1.statePre.at<float>(1)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].y;
      KF1.statePre.at<float>(2)=0;// initial v_x
      KF1.statePre.at<float>(3)=0;//initial v_y
 
      // Set initial state
-     KF2.statePre.at<float>(0)=0.0;//initial x pos of the cluster
+     KF2.statePre.at<float>(0)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].x;
-     KF2.statePre.at<float>(1)=0.0;//initial y pos of the cluster
+     KF2.statePre.at<float>(1)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].y;
      KF2.statePre.at<float>(2)=0;// initial v_x
      KF2.statePre.at<float>(3)=0;//initial v_y
 
 
      // Set initial state
-     KF3.statePre.at<float>(0)=0.0;//initial x pos of the cluster
+     KF3.statePre.at<float>(0)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].x;
-     KF3.statePre.at<float>(1)=0.0;//initial y pos of the cluster
+     KF3.statePre.at<float>(1)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].y;
      KF3.statePre.at<float>(2)=0;// initial v_x
      KF3.statePre.at<float>(3)=0;//initial v_y
 
      // Set initial state
-     KF4.statePre.at<float>(0)=0.0;//initial x pos of the cluster
+     KF4.statePre.at<float>(0)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].x;
-     KF4.statePre.at<float>(1)=0.0;//initial y pos of the cluster
+     KF4.statePre.at<float>(1)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].y;
      KF4.statePre.at<float>(2)=0;// initial v_x
      KF4.statePre.at<float>(3)=0;//initial v_y
 
      // Set initial state
-     KF5.statePre.at<float>(0)=0.0;//initial x pos of the cluster
+     KF5.statePre.at<float>(0)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].x;
-     KF5.statePre.at<float>(1)=0.0;//initial y pos of the cluster
+     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
 
-cout<<"About to setup callback";
+     firstFrame=false;
+
+     // Print the initial state of the kalman filter for debugging
+     cout<<"KF0.satePre="<<KF0.statePre.at<float>(0)<<","<<KF0.statePre.at<float>(1)<<"\n";
+     cout<<"KF1.satePre="<<KF1.statePre.at<float>(0)<<","<<KF1.statePre.at<float>(1)<<"\n";
+     cout<<"KF2.satePre="<<KF2.statePre.at<float>(0)<<","<<KF2.statePre.at<float>(1)<<"\n";
+     cout<<"KF3.satePre="<<KF3.statePre.at<float>(0)<<","<<KF3.statePre.at<float>(1)<<"\n";
+     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
+}
+
+ 
+else
+{ 
+  cout<<"ELSE firstFrame="<<firstFrame<<"\n";
+  pcl::PointCloud<pcl::PointXYZ>::Ptr input_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*/
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+
+  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<int>. 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<pcl::PointIndices> cluster_indices;
+  pcl::EuclideanClusterExtraction<pcl::PointXYZ> 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<PointIndices> 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<pcl::PointIndices>::const_iterator it;
+  std::vector<int>::const_iterator pit;
+  // Vector of cluster pointclouds
+  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
+
+  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
+          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
+          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<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);
+    }
+    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);
+
+    }
+    cout<<"6 clusters initialized\n";
+
+    //cc_pos.publish(cc);// Publish cluster mid-points.
+    KFT(cc);
+    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<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);
+  // Create a ROS publisher for the output point cloud
+  pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_0", 1);
+  pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_1", 1);
+  pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_2", 1);
+  pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_3", 1);
+  pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_4", 1);
+  pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_5", 1);
       // 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);
+/* Point cloud clustering
+*/
+    
+  //cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
+
+
+/* Point cloud clustering
+*/    
+
 
     ros::spin();