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_naive.cpp

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