1.0.4

Remove topic slash prefix

CHANGELOG.rst

@@ -2,6 +2,30 @@
 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_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
+set(CMAKE_CXX_STANDARD 14)
 ## 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,6 +34,9 @@ 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.2</version>
+  <version>1.0.4</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,47 +1,46 @@
-#include <iostream>
-#include <string.h>
-#include <fstream>
-#include <algorithm>
-#include <iterator>
-#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 <algorithm>
+#include <fstream>
+#include <geometry_msgs/Point.h>
+#include <iostream>
+#include <iterator>
 #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 "pcl_ros/point_cloud.h"
-#include <geometry_msgs/Point.h>
+#include <ros/ros.h>
 #include <std_msgs/Float32MultiArray.h>
 #include <std_msgs/Int32MultiArray.h>
+#include <string.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/centroid.h>
 #include <pcl/common/geometry.h>
+#include <pcl/features/normal_3d.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/point_cloud.h>
+#include <pcl/point_types.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>
-#include <pcl/common/centroid.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/PointCloud2.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
@@ -66,7 +65,6 @@ ros::Publisher objID_pub;
 
 std::vector<geometry_msgs::Point> prevClusterCenters;
 
-
 cv::Mat state(stateDim, 1, CV_32F);
 cv::Mat_<float> measurement(2, 1);
 
@@ -76,156 +74,147 @@ ros::Publisher objID_pub;
 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<int> v)
+//Count unique object IDs. just to make sure same ID has not been assigned to
+two KF_Trackers. int countIDs(vector<int> 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<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++)
-        {
-            if( distMat[i][j]<minEl)
-            {
+    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)
-{
-
-
+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(),KF3.predict(),KF4.predict(),KF5.predict()};
+  std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.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 ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
+  // cout<<"Prediction 1
+  // ="<<prediction.at<float>(0)<<","<<prediction.at<float>(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
+  // 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<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;
     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. Should be done better
-    // Copy clusterCentres for modifying it and preventing multiple assignments of the same ID
+  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<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++)
-        {
-            distVec.push_back(euclidean_distance(KFpredictions[filterN],copyOfClusterCenters[n]));
+    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()));
+    /*// 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="<<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 not assigned to another cluster
-       copyOfClusterCenters[ID].y=10000;
+     copyOfClusterCenters[ID].x=100000;// A large value so that this center is
+    not assigned to another cluster 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 &s : row ) std::cout << s << ' ';
+  for (const auto &row : distMat) {
+    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<<"\n";
+    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<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
+    distMat[minIndex.first] =
+        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
     {
       distMat[row][minIndex.second] = 10000.0;
     }
     // 4. if(counter<6) got to 1.
     cout << "clusterCount=" << clusterCount << "\n";
-
   }
 
   // cout<<"Got object IDs"<<"\n";
@@ -241,14 +230,15 @@ 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.scale.x=0.3;         m.scale.y=0.3;         m.scale.z=0.3;
+    m.header.frame_id = "map";
+    m.scale.x = 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;
@@ -268,9 +258,6 @@ 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);
@@ -278,8 +265,7 @@ 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);
@@ -295,8 +281,6 @@ 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);
@@ -319,45 +303,48 @@ if (!(meas4[0]==0.0f || meas4[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
 
-
   // 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, pcl::PointCloud<pcl::PointXYZ>::Ptr cluster){
+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.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 objects
-if (firstFrame)
-{   
+  // 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  
+    // Could be made generic by creating a Kalman Filter only when a new object
+    // 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,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);
-    KF2.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);
+    KF0.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,
+                            dvx, 0, 0, 0, 0, dvy);
+    KF2.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);
@@ -389,16 +376,18 @@ if (firstFrame)
     cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));
 
     // 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>);
+    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::search::KdTree<pcl::PointXYZ>::Ptr tree(
+        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);
@@ -409,7 +398,6 @@ if (firstFrame)
     /* 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
@@ -419,24 +407,24 @@ if (firstFrame)
 
     for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
 
-         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-         float x=0.0; float y=0.0;
+      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
+          new pcl::PointCloud<pcl::PointXYZ>);
+      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);
+        // mindist_this_cluster = std::min(dist_this_point,
+        // mindist_this_cluster);
       }
 
-         
       pcl::PointXYZ centroid;
       centroid.x = x / numPts;
       centroid.y = y / numPts;
@@ -446,19 +434,17 @@ if (firstFrame)
 
       // 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 (new pcl::PointCloud<pcl::PointXYZ>);
+      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);
     }
 
-        while (clusterCentroids.size()<6)
-    {
+    while (clusterCentroids.size() < 6) {
       pcl::PointXYZ centroid;
       centroid.x = 0.0;
       centroid.y = 0.0;
@@ -467,7 +453,6 @@ if (firstFrame)
       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;
@@ -486,7 +471,6 @@ if (firstFrame)
     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;
@@ -507,8 +491,7 @@ if (firstFrame)
 
     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;
@@ -522,27 +505,30 @@ if (firstFrame)
       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
+      //cin.ignore();// To be able to see the printed initial state of the
+      KalmanFilter
       */
   }
 
- 
-else
-{ 
+  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>);
+    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::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.
+    /* 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;
@@ -571,26 +557,24 @@ else
     // 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 (new pcl::PointCloud<pcl::PointXYZ>);
-          for(pit = it->indices.begin(); pit != it->indices.end(); pit++) 
-          {
+      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]);
 
-
         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);
+        // mindist_this_cluster = std::min(dist_this_point,
+        // mindist_this_cluster);
       }
 
       pcl::PointXYZ centroid;
@@ -602,19 +586,18 @@ else
 
       // 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 (new pcl::PointCloud<pcl::PointXYZ>);
+      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);
     }
 
-     while (clusterCentroids.size()<6)
-    {
+    while (clusterCentroids.size() < 6) {
       pcl::PointXYZ centroid;
       centroid.x = 0.0;
       centroid.y = 0.0;
@@ -623,14 +606,11 @@ else
       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";
 
@@ -638,80 +618,69 @@ else
     KFT(cc);
     int i = 0;
     bool publishedCluster[6];
-    for(auto it=objID.begin();it!=objID.end();it++)
-        { //cout<<"Inside the for loop\n";
+    for (auto it = objID.begin(); it != objID.end();
+         it++) { // 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]=true;//Use this flag to publish only once for a given obj ID
+        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
+        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
+        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
+        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
+        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
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
         i++;
         break;
-
       }
-                default: break;
+      default:
+        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
@@ -735,8 +704,5 @@ cout<<"About to setup callback\n";
   /* Point cloud clustering
    */
 
-
   ros::spin();
-
-
 }

src/main_naive.cpp

@@ -1,41 +1,40 @@
-#include <iostream>
-#include <string.h>
-#include <fstream>
-#include <algorithm>
-#include <iterator>
-#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 <algorithm>
+#include <fstream>
+#include <geometry_msgs/Point.h>
+#include <iostream>
+#include <iterator>
 #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 "pcl_ros/point_cloud.h"
-#include <geometry_msgs/Point.h>
+#include <ros/ros.h>
 #include <std_msgs/Float32MultiArray.h>
 #include <std_msgs/Int32MultiArray.h>
+#include <string.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/features/normal_3d.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/point_cloud.h>
+#include <pcl/point_types.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>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/PointCloud2.h>
 
 using namespace std;
 using namespace cv;
 
-
 ros::Publisher objID_pub;
 
 // KF init
@@ -58,7 +57,6 @@ ros::Publisher objID_pub;
 
 std::vector<geometry_msgs::Point> prevClusterCenters;
 
-
 cv::Mat state(stateDim, 1, CV_32F);
 cv::Mat_<float> measurement(2, 1);
 
@@ -68,72 +66,71 @@ ros::Publisher objID_pub;
 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<int> v)
+//Count unique object IDs. just to make sure same ID has not been assigned to
+two KF_Trackers. int countIDs(vector<int> 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<cv::Mat> pred{KF0.predict(),KF1.predict(),KF2.predict(),KF3.predict(),KF4.predict(),KF5.predict()};
+  std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.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 ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
+  // cout<<"Prediction 1
+  // ="<<prediction.at<float>(0)<<","<<prediction.at<float>(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
+  // 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<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;
     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";
 
@@ -147,9 +144,11 @@ 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 filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
+     // cout<<"MinD for
+ filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
 
   }
 
@@ -167,39 +166,36 @@ 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(euclidean_distance(prevClusterCenters[n],clusterCenters[n]));
+      distVec.push_back(
+          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(),min_element(distVec.begin(),distVec.end())));
-       // cout<<"MinD for filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
-    
+    objID.push_back(std::distance(distVec.begin(),
+                                  min_element(distVec.begin(), distVec.end())));
+    // 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);
@@ -215,8 +211,6 @@ objID.clear();//Clear the objID vector
   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);
@@ -236,37 +230,41 @@ objID.clear();//Clear the objID vector
 
   // 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, pcl::PointCloud<pcl::PointXYZ>::Ptr cluster){
+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.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 objects
-if (firstFrame)
-{   
+  // 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_<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);
-    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);
+    // Could be made generic by creating a Kalman Filter only when a new object
+    // is detected
+    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);
+    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);
@@ -296,19 +294,23 @@ if (firstFrame)
     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>);
+    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::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.
+    /* 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;
@@ -334,84 +336,98 @@ if (firstFrame)
     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++) 
-          {
+      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);
+        // 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>);
+      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)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].x;
+    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>(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)=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>(0) =
+        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>(2) = 0; // initial v_x
     KF1.statePre.at<float>(3) = 0; // initial v_y
 
     // Set initial state
-     KF2.statePre.at<float>(0)=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>(0) =
+        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>(2) = 0; // initial v_x
     KF2.statePre.at<float>(3) = 0; // initial v_y
 
-
     // Set initial state
-     KF3.statePre.at<float>(0)=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>(0) =
+        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>(2) = 0; // initial v_x
     KF3.statePre.at<float>(3) = 0; // initial v_y
 
     // Set initial state
-     KF4.statePre.at<float>(0)=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>(0) =
+        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>(2) = 0; // initial v_x
     KF4.statePre.at<float>(3) = 0; // initial v_y
 
     // Set initial state
-     KF5.statePre.at<float>(0)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].x;
-     KF5.statePre.at<float>(1)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].y;
+    KF5.statePre.at<float>(0) =
+        cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].x;
+    KF5.statePre.at<float>(1) =
+        cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].y;
     KF5.statePre.at<float>(2) = 0; // initial v_x
     KF5.statePre.at<float>(3) = 0; // initial v_y
 
     firstFrame = false;
 
     // Print the initial state of the kalman filter for debugging
-     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
-
+    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
 
     /* 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;
@@ -421,23 +437,25 @@ if (firstFrame)
     /* Naive version without kalman filter */
   }
 
- 
-else
-{ 
+  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>);
+    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::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.
+    /* 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;
@@ -464,35 +482,36 @@ cout<<"PCL extract successfull\n";
     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++) 
-          {
+      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);
+        // 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>);
+      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);
-
+    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";
 
@@ -500,79 +519,69 @@ cout<<"PCL extract successfull\n";
     KFT(cc);
     int i = 0;
     bool publishedCluster[6];
-    for(auto it=objID.begin();it!=objID.end();it++)
-        { cout<<"Inside the for loop\n";
+    for (auto it = objID.begin(); it != objID.end(); it++) {
+      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]=true;//Use this flag to publish only once for a given obj ID
+        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
+        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
+        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
+        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
+        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
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
         i++;
         break;
-
       }
-                default: break;
+      default:
+        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
@@ -592,12 +601,8 @@ cout<<"About to setup callback\n";
 
   // cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
 
-
   /* Point cloud clustering
    */
 
-
   ros::spin();
-
-
 }