CHANGELOG.rst

@@ -2,30 +2,6 @@
 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)
 ------------------
 * Added link to wiki pages

CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 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
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages

README.md

@@ -34,9 +34,6 @@ The input point-clouds can be from:
 3. A point cloud dataset or 
 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
 
 If you use the code or snippets from this repository in your work, please cite:

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <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>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 

src/main.cpp

@@ -1,46 +1,47 @@
-#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 <string.h>
+#include <fstream>
+#include <algorithm>
 #include <iterator>
+#include "kf_tracker/featureDetection.h"
+#include "kf_tracker/CKalmanFilter.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/video.hpp>
+#include "opencv2/video/tracking.hpp"
+#include <ros/ros.h>
 #include <pcl/io/pcd_io.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/Int32MultiArray.h>
-#include <string.h>
 
-#include <pcl/common/centroid.h>
+#include <sensor_msgs/PointCloud2.h>
-#include <pcl/common/geometry.h>
+#include <pcl_conversions/pcl_conversions.h>
-#include <pcl/features/normal_3d.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_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/extract_clusters.h>
 #include <pcl/segmentation/sac_segmentation.h>
-#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/segmentation/extract_clusters.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <pcl/common/centroid.h>
  
+#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/Marker.h>
 #include <limits>
 #include <utility>
-#include <visualization_msgs/Marker.h>
-#include <visualization_msgs/MarkerArray.h>
 
 using namespace std;
 using namespace cv;
 
+
 ros::Publisher objID_pub;
 
     // KF init
@@ -65,6 +66,7 @@ ros::Publisher markerPub;
 
     std::vector<geometry_msgs::Point> prevClusterCenters;
 
+
     cv::Mat state(stateDim,1,CV_32F);
     cv::Mat_<float> measurement(2,1); 
 
@@ -74,147 +76,156 @@ std::vector<int> objID; // Output of the data association using KF
 bool firstFrame=true;
 
 // 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
-    // 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.
 */
 
-std::pair<int, int> findIndexOfMin(std::vector<std::vector<float>> distMat) {
+std::pair<int,int> findIndexOfMin(std::vector<std::vector<float> > distMat)
+{
     cout<<"findIndexOfMin cALLED\n";
     std::pair<int,int>minIndex;
     float minEl=std::numeric_limits<float>::max();
     cout<<"minEl="<<minEl<<"\n";
     for (int i=0; i<distMat.size();i++)
-    for (int j = 0; j < distMat.at(0).size(); j++) {
+        for(int j=0;j<distMat.at(0).size();j++)
-      if (distMat[i][j] < minEl) {
+        {
+            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) {
+void KFT(const std_msgs::Float32MultiArray ccs)
+{
+
+
 
     // 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";
 
     //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
-  // 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
    
     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;
         pt.x=*it;
         pt.y=*(it+1);
         pt.z=*(it+2);
 
         clusterCenters.push_back(pt);
+       
     }
 
   //  cout<<"CLusterCenters Obtained"<<"\n";
     std::vector<geometry_msgs::Point> KFpredictions;
     i=0;
-  for (auto it = pred.begin(); it != pred.end(); it++) {
+    for (auto it=pred.begin();it!=pred.end();it++)
+    {
         geometry_msgs::Point pt;
         pt.x=(*it).at<float>(0);
         pt.y=(*it).at<float>(1);
         pt.z=(*it).at<float>(2);
 
         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.
+    objID.resize(6);//Allocate default elements so that [i] doesnt segfault. Should be done better
-                   // Should be done better
+    // Copy clusterCentres for modifying it and preventing multiple assignments of the same ID
-  // Copy clusterCentres for modifying it and preventing multiple assignments of
-  // the same ID
     std::vector<geometry_msgs::Point> copyOfClusterCenters(clusterCenters);
     std::vector<std::vector<float> > distMat;
 
-  for (int filterN = 0; filterN < 6; filterN++) {
+    for(int filterN=0;filterN<6;filterN++)
+    {
         std::vector<float> distVec;
-    for (int n = 0; n < 6; n++) {
+        for(int n=0;n<6;n++)
-      distVec.push_back(
+        {
-          euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[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
+      /*// Based on distVec instead of distMat (global min). Has problems with the person's leg going out of scope 
-    person's leg going out of scope int
+       int ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()));
-    ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()));
        //cout<<"finterlN="<<filterN<<"   minID="<<ID
        objID.push_back(ID);
       // Prevent assignment of the same object ID to multiple clusters
-     copyOfClusterCenters[ID].x=100000;// A large value so that this center is
+       copyOfClusterCenters[ID].x=100000;// A large value so that this center is not assigned to another cluster
-    not assigned to another cluster copyOfClusterCenters[ID].y=10000;
+       copyOfClusterCenters[ID].y=10000;
        copyOfClusterCenters[ID].z=10000;
       */
      cout<<"filterN="<<filterN<<"\n";
+
+
     }
 
     cout<<"distMat.size()"<<distMat.size()<<"\n";
     cout<<"distMat[0].size()"<<distMat.at(0).size()<<"\n";
     // DEBUG: print the distMat
-  for (const auto &row : distMat) {
+    for ( const auto &row : distMat )
-    for (const auto &s : row)
+    {
-      std::cout << s << ' ';
+       for ( const auto &s : row ) std::cout << s << ' ';
        std::cout << std::endl;
     }
 
-  for (int clusterCount = 0; clusterCount < 6; clusterCount++) {
+
+
+    for(int clusterCount=0;clusterCount<6;clusterCount++)
+    {
         // 1. Find min(distMax)==> (i,j);
         std::pair<int,int> minIndex(findIndexOfMin(distMat));
-    cout << "Received minIndex=" << minIndex.first << "," << minIndex.second
+         cout<<"Received minIndex="<<minIndex.first<<","<<minIndex.second<<"\n";
-         << "\n";
         // 2. objID[i]=clusterCenters[j]; counter++
         objID[minIndex.first]=minIndex.second;
     
         // 3. distMat[i,:]=10000; distMat[:,j]=10000
-    distMat[minIndex.first] =
+        distMat[minIndex.first]=std::vector<float>(6,10000.0);// Set the row to a high number.
-        std::vector<float>(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 (int row = 0; row < distMat.size();
-         row++) // set the column to a high number
         {
             distMat[row][minIndex.second]=10000.0;
         }
         // 4. if(counter<6) got to 1.
         cout<<"clusterCount="<<clusterCount<<"\n";
+
     }
 
    // cout<<"Got object IDs"<<"\n";
@@ -230,15 +241,14 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
 
     visualization_msgs::MarkerArray clusterMarkers;
 
-  for (int i = 0; i < 6; i++) {
+     for (int i=0;i<6;i++)
+     {
         visualization_msgs::Marker m;
 
         m.id=i;
         m.type=visualization_msgs::Marker::CUBE;
-    m.header.frame_id = "map";
+        m.header.frame_id="/map";
-    m.scale.x = 0.3;
+        m.scale.x=0.3;         m.scale.y=0.3;         m.scale.z=0.3;
-    m.scale.y = 0.3;
-    m.scale.z = 0.3;
         m.action=visualization_msgs::Marker::ADD;
         m.color.a=1.0;
         m.color.r=i%2?1:0;
@@ -258,6 +268,9 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
 
      markerPub.publish(clusterMarkers);
 
+
+
+
     std_msgs::Int32MultiArray obj_id;
     for(auto it=objID.begin();it!=objID.end();it++)
         obj_id.data.push_back(*it);
@@ -265,7 +278,8 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
     objID_pub.publish(obj_id);
     // convert clusterCenters from geometry_msgs::Point to floats
     std::vector<std::vector<float> > cc;
-  for (int i = 0; i < 6; i++) {
+    for (int i=0;i<6;i++)
+    {
         vector<float> pt;
         pt.push_back(clusterCenters[objID[i]].x);
         pt.push_back(clusterCenters[objID[i]].y);
@@ -281,6 +295,8 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
     float meas4[2]={cc[4].at(0),cc[4].at(1)};
     float meas5[2]={cc[5].at(0),cc[5].at(1)};
 
+
+
     // The update phase 
     cv::Mat meas0Mat=cv::Mat(2,1,CV_32F,meas0);
     cv::Mat meas1Mat=cv::Mat(2,1,CV_32F,meas1);
@@ -303,48 +319,45 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
 if (!(meas5[0]==0.0f || meas5[1]==0.0f))
     Mat estimated5 = KF5.correct(meas5Mat);
     
+ 
     // Publish the point clouds belonging to each clusters
 
+
    // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
    // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
 //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);
   pcl::toROSMsg (*cluster , *clustermsg);
-  clustermsg->header.frame_id = "map";
+  clustermsg->header.frame_id = "/map";
   clustermsg->header.stamp = ros::Time::now();
   pub.publish (*clustermsg);
+
 }
 
+
 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
 
 {
    // 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. 
-    // 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
 
     float dvx=0.01f; //1.0
     float dvy=0.01f;//1.0
     float dx=1.0f;
     float dy=1.0f;
-    KF0.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+    KF0.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
-                            dvx, 0, 0, 0, 0, dvy);
+    KF1.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
-    KF1.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+    KF2.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
-                            dvx, 0, 0, 0, 0, dvy);
+    KF3.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
-    KF2.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+    KF4.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
-                            dvx, 0, 0, 0, 0, dvy);
+    KF5.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
-    KF3.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
-                            dvx, 0, 0, 0, 0, dvy);
-    KF4.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
-                            dvx, 0, 0, 0, 0, dvy);
-    KF5.transitionMatrix = (Mat_<float>(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);
@@ -376,18 +389,16 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
      cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));
 
 // 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*/
-    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);
 
   tree->setInputCloud (input_cloud);
 
+
   std::vector<pcl::PointIndices> cluster_indices;
   pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
   ec.setClusterTolerance (0.08); 
@@ -398,6 +409,7 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
   /* Extract the clusters out of pc and save indices in cluster_indices.*/
   ec.extract (cluster_indices);
 
+  
   std::vector<pcl::PointIndices>::const_iterator it;
   std::vector<int>::const_iterator pit;
   // Vector of cluster pointclouds
@@ -407,24 +419,24 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
 
   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>);
+         float x=0.0; float y=0.0;
-      float x = 0.0;
-      float y = 0.0;
          int numPts=0;
-      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]);
                   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 = std::min(dist_this_point, mindist_this_cluster);
-        // mindist_this_cluster);
           }
 
+         
       pcl::PointXYZ centroid;
       centroid.x=x/numPts;
       centroid.y=y/numPts;
@@ -434,17 +446,19 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
 
       //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<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));
       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;
@@ -453,6 +467,7 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
        clusterCentroids.push_back(centroid);
     }
     
+
      // Set initial state
      KF0.statePre.at<float>(0)=clusterCentroids.at(0).x;
      KF0.statePre.at<float>(1)=clusterCentroids.at(0).y;
@@ -471,6 +486,7 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
      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)=clusterCentroids.at(3).x;
      KF3.statePre.at<float>(1)=clusterCentroids.at(3).y;
@@ -491,7 +507,8 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
 
      firstFrame=false;
 
-    for (int i = 0; i < 6; i++) {
+    for (int i=0;i<6;i++)
+     {
         geometry_msgs::Point pt;
         pt.x=clusterCentroids.at(i).x;
         pt.y=clusterCentroids.at(i).y;
@@ -505,30 +522,27 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
      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
+     //cin.ignore();// To be able to see the printed initial state of the KalmanFilter
-      KalmanFilter
      */
 }
 
-  else {
+ 
+else
+{ 
   //cout<<"ELSE firstFrame="<<firstFrame<<"\n";
-    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*/
-    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);
 
   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;
   pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
@@ -557,24 +571,26 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
      // Cluster centroids
   std::vector<pcl::PointXYZ> clusterCentroids;
 
-    for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
+  
-      float x = 0.0;
+
-      float y = 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<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]);
 
+
                   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 = std::min(dist_this_point, mindist_this_cluster);
-        // mindist_this_cluster);
           }
 
     pcl::PointXYZ centroid;
@@ -586,18 +602,19 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
 
       //Get the centroid of the cluster
       clusterCentroids.push_back(centroid);
+
     }
    // 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(
+      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));
       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;
@@ -606,11 +623,14 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
        clusterCentroids.push_back(centroid);
     }
 
+
     std_msgs::Float32MultiArray cc;
-    for (int i = 0; i < 6; i++) {
+    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";
 
@@ -618,69 +638,80 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
     KFT(cc);
     int i=0;
     bool publishedCluster[6];
-    for (auto it = objID.begin(); it != objID.end();
+    for(auto it=objID.begin();it!=objID.end();it++)
-         it++) { // cout<<"Inside the for loop\n";
+        { //cout<<"Inside the for loop\n";
             
-      switch (i) {
+        
+            switch(i)
+            {
                 cout<<"Inside the switch case\n";
                 case 0: { 
                             publish_cloud(pub_cluster0,cluster_vec[*it]);
-        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++;
                             break;
+
                         }
                 case 1: {
                             publish_cloud(pub_cluster1,cluster_vec[*it]);
-        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++;
                             break;
+
                         }
                 case 2: {
                             publish_cloud(pub_cluster2,cluster_vec[*it]);
-        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++;
                             break;
+
                         }
                 case 3: {
                             publish_cloud(pub_cluster3,cluster_vec[*it]);
-        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++;
                             break;
+
                         }
                 case 4: {
                             publish_cloud(pub_cluster4,cluster_vec[*it]);
-        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++;
                             break;
+
                         }
 
                 case 5: {
                             publish_cloud(pub_cluster5,cluster_vec[*it]);
-        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++;
                             break;
+
                         }
-      default:
+                default: break;
-        break;
-      }
-    }
-  }
             }
         
-int main(int argc, char **argv) {
+        }
+
+} 
+
+}   
+
+
+  
+
+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<geometry_msgs::Twist> ("obj_1",1);
 
+
+
 cout<<"About to setup callback\n";
 
 // Create a ROS subscriber for the input point cloud
@@ -704,5 +735,8 @@ int main(int argc, char **argv) {
 /* Point cloud clustering
 */    
 
+
     ros::spin();
+
+
 }

src/main_naive.cpp

@@ -1,40 +1,41 @@
-#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 <string.h>
+#include <fstream>
+#include <algorithm>
 #include <iterator>
+#include "kf_tracker/featureDetection.h"
+#include "kf_tracker/CKalmanFilter.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/video.hpp>
+#include "opencv2/video/tracking.hpp"
+#include <ros/ros.h>
 #include <pcl/io/pcd_io.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/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_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/extract_clusters.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 cv;
 
+
 ros::Publisher objID_pub;
 
     // KF init
@@ -57,6 +58,7 @@ ros::Publisher pub_cluster5;
 
     std::vector<geometry_msgs::Point> prevClusterCenters;
 
+
     cv::Mat state(stateDim,1,CV_32F);
     cv::Mat_<float> measurement(2,1); 
 
@@ -66,71 +68,72 @@ std::vector<int> objID; // Output of the data association using KF
 bool firstFrame=true;
 
 // 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
-    // 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
 
-  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";
 
     //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
-  // 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
    
     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;
         pt.x=*it;
         pt.y=*(it+1);
         pt.z=*(it+2);
 
         clusterCenters.push_back(pt);
+       
     }
 
   //  cout<<"CLusterCenters Obtained"<<"\n";
     std::vector<geometry_msgs::Point> KFpredictions;
     i=0;
-  for (auto it = pred.begin(); it != pred.end(); it++) {
+    for (auto it=pred.begin();it!=pred.end();it++)
+    {
         geometry_msgs::Point pt;
         pt.x=(*it).at<float>(0);
         pt.y=(*it).at<float>(1);
         pt.z=(*it).at<float>(2);
 
         KFpredictions.push_back(pt);
+        
     }
    // cout<<"Got predictions"<<"\n";
 
@@ -144,11 +147,9 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
         for(int n=0;n<6;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())));
-     // cout<<"MinD for
+       // cout<<"MinD for filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
- filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
     
     }
 
@@ -166,36 +167,39 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
 
 /* Naive version without using kalman filter */
 objID.clear();//Clear the objID vector
-  for (int filterN = 0; filterN < 6; filterN++) {
+    for(int filterN=0;filterN<6;filterN++)
+    {
         std::vector<float> distVec;
         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";
-    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 */
     //prevClusterCenters.clear();
     // 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);
+       
      }
 
      /* Naive version without kalman filter */
 
 /* Naive version without using kalman filter */    
 
+
     std_msgs::Int32MultiArray obj_id;
     for(auto it=objID.begin();it!=objID.end();it++)
         obj_id.data.push_back(*it);
     objID_pub.publish(obj_id);
     // convert clusterCenters from geometry_msgs::Point to floats
     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;
         pt.push_back(clusterCenters[i].x);
         pt.push_back(clusterCenters[i].y);
@@ -211,6 +215,8 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
     float meas4[3]={cc[4].at(0),cc[4].at(1)};
     float meas5[3]={cc[5].at(0),cc[5].at(1)};
 
+
+
     // The update phase 
     cv::Mat meas0Mat=cv::Mat(2,1,CV_32F,meas0);
     cv::Mat meas1Mat=cv::Mat(2,1,CV_32F,meas1);
@@ -230,41 +236,37 @@ void KFT(const std_msgs::Float32MultiArray ccs) {
  
     // Publish the point clouds belonging to each clusters
 
+
    // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
    // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
 //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);
   pcl::toROSMsg (*cluster , *clustermsg);
-  clustermsg->header.frame_id = "map";
+  clustermsg->header.frame_id = "/map";
   clustermsg->header.stamp = ros::Time::now();
   pub.publish (*clustermsg);
+
 }
 
+
 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
 
 {
     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. 
-    // 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(KF1.measurementMatrix);
@@ -294,23 +296,19 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
      cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1));
 
 // 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*/
-    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);
 
   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;
   pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
@@ -336,98 +334,84 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
   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(
+          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]);
                   //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
                   //                                          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);
+
     }
 
     //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(
+      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));
       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) =
+     // Set initial state
-        cluster_vec[0]->points[cluster_vec[0]->points.size() / 2].y;
+     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
-    KF1.statePre.at<float>(0) =
+     KF1.statePre.at<float>(0)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].x;
-        cluster_vec[1]->points[cluster_vec[1]->points.size() / 2].x;
+     KF1.statePre.at<float>(1)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].y;
-    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) =
+     KF2.statePre.at<float>(0)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].x;
-        cluster_vec[2]->points[cluster_vec[2]->points.size() / 2].x;
+     KF2.statePre.at<float>(1)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].y;
-    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) =
+     KF3.statePre.at<float>(0)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].x;
-        cluster_vec[3]->points[cluster_vec[3]->points.size() / 2].x;
+     KF3.statePre.at<float>(1)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].y;
-    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) =
+     KF4.statePre.at<float>(0)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].x;
-        cluster_vec[4]->points[cluster_vec[4]->points.size() / 2].x;
+     KF4.statePre.at<float>(1)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].y;
-    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) =
+     KF5.statePre.at<float>(0)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].x;
-        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>(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
-    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 */
-    for (int i = 0; i < 6; i++) {
+     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;
@@ -437,25 +421,23 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
      /* Naive version without kalman filter */
 }
 
-  else {
+ 
+else
+{ 
   cout<<"ELSE firstFrame="<<firstFrame<<"\n";
-    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*/
-    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);
 
   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;
   pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
@@ -482,36 +464,35 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
   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(
+          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]);
                   //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
                   //                                          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);
+
     }
     //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(
+      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));
       cluster_vec.push_back(empty_cluster);
     }
     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";
 
@@ -519,69 +500,79 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
     KFT(cc);
     int i=0;
     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";
                 case 0: { 
                             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++;
                             break;
+
                         }
                 case 1: {
                             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++;
                             break;
+
                         }
                 case 2: {
                             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++;
                             break;
+
                         }
                 case 3: {
                             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++;
                             break;
+
                         }
                 case 4: {
                             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++;
                             break;
+
                         }
 
                 case 5: {
                             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++;
                             break;
+
                         }
-      default:
+                default: break;
-        break;
-      }
-    }
-  }
             }
     
-int main(int argc, char **argv) {
+        }
+
+} 
+
+}   
+
+
+  
+
+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
@@ -601,8 +592,12 @@ int main(int argc, char **argv) {
     
   //cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
 
+
 /* Point cloud clustering
 */    
 
+
     ros::spin();
+
+
 }