diff --git a/src/main.cpp b/src/main.cpp
index d19bba0..871c103 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,237 +1,226 @@
-#include <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 <visualization_msgs/MarkerArray.h>
-#include <visualization_msgs/Marker.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/PointCloud2.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
-    int stateDim=4;// [x,y,v_x,v_y]//,w,h]
-    int measDim=2;// [z_x,z_y,z_w,z_h]
-    int ctrlDim=0;
-    cv::KalmanFilter KF0(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF1(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF2(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF3(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF4(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF5(stateDim,measDim,ctrlDim,CV_32F);
+// KF init
+int stateDim = 4; // [x,y,v_x,v_y]//,w,h]
+int measDim = 2;  // [z_x,z_y,z_w,z_h]
+int ctrlDim = 0;
+cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F);
 
-    ros::Publisher pub_cluster0;
-    ros::Publisher pub_cluster1;
-    ros::Publisher pub_cluster2;
-    ros::Publisher pub_cluster3;
-    ros::Publisher pub_cluster4;
-    ros::Publisher pub_cluster5;
+ros::Publisher pub_cluster0;
+ros::Publisher pub_cluster1;
+ros::Publisher pub_cluster2;
+ros::Publisher pub_cluster3;
+ros::Publisher pub_cluster4;
+ros::Publisher pub_cluster5;
 
-    ros::Publisher markerPub;
+ros::Publisher markerPub;
 
-    std::vector<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);
-    cv::Mat_<float> measurement(2,1); 
+std::vector<int> objID; // Output of the data association using KF
+                        // measurement.setTo(Scalar(0));
 
-    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)
-  {
-    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)
-{
-    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)
-            {
-                minEl=distMat[i][j];
-                minIndex=std::make_pair(i,j);
-
-            }
-
-        }
-    cout<<"minIndex="<<minIndex.first<<","<<minIndex.second<<"\n";
-    return minIndex;
-}
-void KFT(const std_msgs::Float32MultiArray ccs)
-{
-
-
-
-    // First predict, to update the internal statePre variable
-
-    std::vector<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";
-
-    // Get measurements
-    // Extract the position of the clusters forom the multiArray. To check if the data
-    // coming in, check the .z (every third) coordinate and that will be 0.0
-    std::vector<geometry_msgs::Point> clusterCenters;//clusterCenters
-   
-    int i=0;
-    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);
-       
+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) {
+        minEl = distMat[i][j];
+        minIndex = std::make_pair(i, j);
+      }
     }
+  cout << "minIndex=" << minIndex.first << "," << minIndex.second << "\n";
+  return minIndex;
+}
+void KFT(const std_msgs::Float32MultiArray ccs) {
+
+  // First predict, to update the internal statePre variable
+
+  std::vector<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";
+
+  // Get measurements
+  // Extract the position of the clusters forom the multiArray. To check if the
+  // data coming in, check the .z (every third) coordinate and that will be 0.0
+  std::vector<geometry_msgs::Point> clusterCenters; // clusterCenters
+
+  int i = 0;
+  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++)
-    {
-        geometry_msgs::Point pt;
-        pt.x=(*it).at<float>(0);
-        pt.y=(*it).at<float>(1);
-        pt.z=(*it).at<float>(2);
+  std::vector<geometry_msgs::Point> KFpredictions;
+  i = 0;
+  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);
-        
-    }
+    KFpredictions.push_back(pt);
+  }
   // cout<<"Got predictions"<<"\n";
 
-    
-    
-    // Find the cluster that is more probable to be belonging to a given KF.
-    objID.clear();//Clear the objID vector
-    objID.resize(6);//Allocate default elements so that [i] doesnt segfault. Should be done better
-    // Copy clusterCentres for modifying it and preventing multiple assignments of the same ID
-    std::vector<geometry_msgs::Point> copyOfClusterCenters(clusterCenters);
-    std::vector<std::vector<float> > distMat;
-
-    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]));
-        }
-
-        distMat.push_back(distVec);
-      /*// Based on distVec instead of distMat (global min). Has problems with the person's leg going out of scope 
-       int ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()));
-       //cout<<"finterlN="<<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].z=10000;
-      */
-     cout<<"filterN="<<filterN<<"\n";
-
+  // Find the cluster that is more probable to be belonging to a given KF.
+  objID.clear();   // Clear the objID vector
+  objID.resize(6); // Allocate default elements so that [i] doesnt segfault.
+                   // Should be done better
+  // Copy clusterCentres for modifying it and preventing multiple assignments of
+  // the same ID
+  std::vector<geometry_msgs::Point> copyOfClusterCenters(clusterCenters);
+  std::vector<std::vector<float>> distMat;
 
+  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]));
     }
 
-    cout<<"distMat.size()"<<distMat.size()<<"\n";
-    cout<<"distMat[0].size()"<<distMat.at(0).size()<<"\n";
-    // DEBUG: print the distMat
-    for ( const auto &row : distMat )
+    distMat.push_back(distVec);
+    /*// Based on distVec instead of distMat (global min). Has problems with the
+    person's leg going out of scope int
+    ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()));
+     //cout<<"finterlN="<<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].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 << ' ';
+    std::cout << std::endl;
+  }
+
+  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";
+    // 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
     {
-       for ( const auto &s : row ) std::cout << s << ' ';
-       std::cout << std::endl;
+      distMat[row][minIndex.second] = 10000.0;
     }
+    // 4. if(counter<6) got to 1.
+    cout << "clusterCount=" << clusterCount << "\n";
+  }
 
+  // cout<<"Got object IDs"<<"\n";
+  // countIDs(objID);// for verif/corner cases
 
-
-    for(int clusterCount=0;clusterCount<6;clusterCount++)
-    {
-        // 1. Find min(distMax)==> (i,j);
-        std::pair<int,int> minIndex(findIndexOfMin(distMat));
-         cout<<"Received minIndex="<<minIndex.first<<","<<minIndex.second<<"\n";
-        // 2. objID[i]=clusterCenters[j]; counter++
-        objID[minIndex.first]=minIndex.second;
-    
-        // 3. distMat[i,:]=10000; distMat[:,j]=10000
-        distMat[minIndex.first]=std::vector<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";
-    //countIDs(objID);// for verif/corner cases
-
-    //display objIDs
+  // display objIDs
   /* DEBUG
     cout<<"objID= ";
     for(auto it=objID.begin();it!=objID.end();it++)
@@ -239,125 +228,123 @@ void KFT(const std_msgs::Float32MultiArray ccs)
     cout<<"\n";
     */
 
-    visualization_msgs::MarkerArray clusterMarkers;
+  visualization_msgs::MarkerArray clusterMarkers;
 
-     for (int i=0;i<6;i++)
-     {
-        visualization_msgs::Marker m;
+  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.action=visualization_msgs::Marker::ADD;
-        m.color.a=1.0;
-        m.color.r=i%2?1:0;
-        m.color.g=i%3?1:0;
-        m.color.b=i%4?1:0;
+    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.action = visualization_msgs::Marker::ADD;
+    m.color.a = 1.0;
+    m.color.r = i % 2 ? 1 : 0;
+    m.color.g = i % 3 ? 1 : 0;
+    m.color.b = i % 4 ? 1 : 0;
 
-       //geometry_msgs::Point clusterC(clusterCenters.at(objID[i]));
-        geometry_msgs::Point clusterC(KFpredictions[i]);
-       m.pose.position.x=clusterC.x;
-       m.pose.position.y=clusterC.y;
-       m.pose.position.z=clusterC.z;
+    // geometry_msgs::Point clusterC(clusterCenters.at(objID[i]));
+    geometry_msgs::Point clusterC(KFpredictions[i]);
+    m.pose.position.x = clusterC.x;
+    m.pose.position.y = clusterC.y;
+    m.pose.position.z = clusterC.z;
 
-       clusterMarkers.markers.push_back(m);
-     }
+    clusterMarkers.markers.push_back(m);
+  }
 
-    prevClusterCenters=clusterCenters;
+  prevClusterCenters = clusterCenters;
 
-     markerPub.publish(clusterMarkers);
+  markerPub.publish(clusterMarkers);
 
+  std_msgs::Int32MultiArray obj_id;
+  for (auto it = objID.begin(); it != objID.end(); it++)
+    obj_id.data.push_back(*it);
+  // Publish the object IDs
+  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++) {
+    vector<float> pt;
+    pt.push_back(clusterCenters[objID[i]].x);
+    pt.push_back(clusterCenters[objID[i]].y);
+    pt.push_back(clusterCenters[objID[i]].z);
 
+    cc.push_back(pt);
+  }
+  // cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
+  float meas0[2] = {cc[0].at(0), cc[0].at(1)};
+  float meas1[2] = {cc[1].at(0), cc[1].at(1)};
+  float meas2[2] = {cc[2].at(0), cc[2].at(1)};
+  float meas3[2] = {cc[3].at(0), cc[3].at(1)};
+  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);
+  cv::Mat meas2Mat = cv::Mat(2, 1, CV_32F, meas2);
+  cv::Mat meas3Mat = cv::Mat(2, 1, CV_32F, meas3);
+  cv::Mat meas4Mat = cv::Mat(2, 1, CV_32F, meas4);
+  cv::Mat meas5Mat = cv::Mat(2, 1, CV_32F, meas5);
 
-    std_msgs::Int32MultiArray obj_id;
-    for(auto it=objID.begin();it!=objID.end();it++)
-        obj_id.data.push_back(*it);
-    // Publish the object IDs
-    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++)
-    {
-        vector<float> pt;
-        pt.push_back(clusterCenters[objID[i]].x);
-        pt.push_back(clusterCenters[objID[i]].y);
-        pt.push_back(clusterCenters[objID[i]].z);
-        
-        cc.push_back(pt);
-    }
-    //cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
-    float meas0[2]={cc[0].at(0),cc[0].at(1)};
-    float meas1[2]={cc[1].at(0),cc[1].at(1)};
-    float meas2[2]={cc[2].at(0),cc[2].at(1)};
-    float meas3[2]={cc[3].at(0),cc[3].at(1)};
-    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);
-    cv::Mat meas2Mat=cv::Mat(2,1,CV_32F,meas2);
-    cv::Mat meas3Mat=cv::Mat(2,1,CV_32F,meas3);
-    cv::Mat meas4Mat=cv::Mat(2,1,CV_32F,meas4);
-    cv::Mat meas5Mat=cv::Mat(2,1,CV_32F,meas5);
-
-//cout<<"meas0Mat"<<meas0Mat<<"\n";
-if (!(meas0Mat.at<float>(0,0)==0.0f || meas0Mat.at<float>(1,0)==0.0f))
+  // cout<<"meas0Mat"<<meas0Mat<<"\n";
+  if (!(meas0Mat.at<float>(0, 0) == 0.0f || meas0Mat.at<float>(1, 0) == 0.0f))
     Mat estimated0 = KF0.correct(meas0Mat);
-if (!(meas1[0]==0.0f || meas1[1]==0.0f))
+  if (!(meas1[0] == 0.0f || meas1[1] == 0.0f))
     Mat estimated1 = KF1.correct(meas1Mat);
-if (!(meas2[0]==0.0f || meas2[1]==0.0f))
+  if (!(meas2[0] == 0.0f || meas2[1] == 0.0f))
     Mat estimated2 = KF2.correct(meas2Mat);
-if (!(meas3[0]==0.0f || meas3[1]==0.0f))
+  if (!(meas3[0] == 0.0f || meas3[1] == 0.0f))
     Mat estimated3 = KF3.correct(meas3Mat);
-if (!(meas4[0]==0.0f || meas4[1]==0.0f))
+  if (!(meas4[0] == 0.0f || meas4[1] == 0.0f))
     Mat estimated4 = KF4.correct(meas4Mat);
-if (!(meas5[0]==0.0f || meas5[1]==0.0f))
+  if (!(meas5[0] == 0.0f || meas5[1] == 0.0f))
     Mat estimated5 = KF5.correct(meas5Mat);
-    
- 
-    // Publish the point clouds belonging to each clusters
 
+  // Publish the point clouds belonging to each clusters
 
-   // cout<<"estimate="<<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<<"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){
-  sensor_msgs::PointCloud2::Ptr clustermsg (new sensor_msgs::PointCloud2);
-  pcl::toROSMsg (*cluster , *clustermsg);
+void publish_cloud(ros::Publisher &pub,
+                   pcl::PointCloud<pcl::PointXYZ>::Ptr cluster) {
+  sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2);
+  pcl::toROSMsg(*cluster, *clustermsg);
   clustermsg->header.frame_id = "map";
   clustermsg->header.stamp = ros::Time::now();
-  pub.publish (*clustermsg);
-
+  pub.publish(*clustermsg);
 }
 
-
-void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
+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)
-{   
-  // 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  
+  // cout<<"IF firstFrame="<<firstFrame<<"\n";
+  // If this is the first frame, initialize kalman filters for the clustered
+  // objects
+  if (firstFrame) {
+    // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
+    // Could be made generic by creating a Kalman Filter only when a new object
+    // is detected
 
-    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);
+    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);
 
     cv::setIdentity(KF0.measurementMatrix);
     cv::setIdentity(KF1.measurementMatrix);
@@ -372,8 +359,8 @@ if (firstFrame)
     // [ 0  0  0    1 Ev_y 0 ]
     //// [ 0  0  0    0 1    Ew ]
     //// [ 0  0  0    0 0    Eh ]
-    float sigmaP=0.01;
-    float sigmaQ=0.1;
+    float sigmaP = 0.01;
+    float sigmaQ = 0.1;
     setIdentity(KF0.processNoiseCov, Scalar::all(sigmaP));
     setIdentity(KF1.processNoiseCov, Scalar::all(sigmaP));
     setIdentity(KF2.processNoiseCov, Scalar::all(sigmaP));
@@ -381,362 +368,341 @@ if (firstFrame)
     setIdentity(KF4.processNoiseCov, Scalar::all(sigmaP));
     setIdentity(KF5.processNoiseCov, Scalar::all(sigmaP));
     // Meas noise cov matrix R
-     cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ));//1e-1
-     cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ));
-     cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ));
-     cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ));
-     cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ));
-     cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));
+    cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1
+    cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ));
+    cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ));
+    cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ));
+    cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ));
+    cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));
 
-// Process the point cloud
-     pcl::PointCloud<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>);
+    // Process the point cloud
+    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    /* Creating the KdTree from input point cloud*/
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
+        new pcl::search::KdTree<pcl::PointXYZ>);
 
-  pcl::fromROSMsg (*input, *input_cloud);
+    pcl::fromROSMsg(*input, *input_cloud);
 
-  tree->setInputCloud (input_cloud);
+    tree->setInputCloud(input_cloud);
 
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+    ec.setClusterTolerance(0.08);
+    ec.setMinClusterSize(10);
+    ec.setMaxClusterSize(600);
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(input_cloud);
+    /* Extract the clusters out of pc and save indices in cluster_indices.*/
+    ec.extract(cluster_indices);
 
-  std::vector<pcl::PointIndices> cluster_indices;
-  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
-  ec.setClusterTolerance (0.08); 
-  ec.setMinClusterSize (10);
-  ec.setMaxClusterSize (600);
-  ec.setSearchMethod (tree);
-  ec.setInputCloud (input_cloud);
-  /* Extract the clusters out of pc and save indices in cluster_indices.*/
-  ec.extract (cluster_indices);
-
-  
-  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::PointIndices>::const_iterator it;
+    std::vector<int>::const_iterator pit;
+    // Vector of cluster pointclouds
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec;
     // Cluster centroids
-  std::vector<pcl::PointXYZ> clusterCentroids;
+    std::vector<pcl::PointXYZ> clusterCentroids;
 
-  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;
-         int numPts=0;
-          for(pit = it->indices.begin(); pit != it->indices.end(); pit++) 
-          {
-          
-                  cloud_cluster->points.push_back(input_cloud->points[*pit]);
-                  x+=input_cloud->points[*pit].x;
-                  y+=input_cloud->points[*pit].y;
-                  numPts++;
+    for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
 
+      pcl::PointCloud<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++) {
 
-                  //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
-                  //                                          origin);
-                  //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster);
-          }
+        cloud_cluster->points.push_back(input_cloud->points[*pit]);
+        x += input_cloud->points[*pit].x;
+        y += input_cloud->points[*pit].y;
+        numPts++;
+
+        // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
+        //                                          origin);
+        // mindist_this_cluster = std::min(dist_this_point,
+        // mindist_this_cluster);
+      }
 
-         
       pcl::PointXYZ centroid;
-      centroid.x=x/numPts;
-      centroid.y=y/numPts;
-      centroid.z=0.0;
-      
+      centroid.x = x / numPts;
+      centroid.y = y / numPts;
+      centroid.z = 0.0;
+
       cluster_vec.push_back(cloud_cluster);
 
-      //Get the centroid of the cluster
+      // Get the centroid of the cluster
       clusterCentroids.push_back(centroid);
-
-
     }
 
-    //Ensure at least 6 clusters exist to publish (later clusters may be empty)
-    while (cluster_vec.size() < 6){
-      pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-      empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
+    // Ensure at least 6 clusters exist to publish (later clusters may be empty)
+    while (cluster_vec.size() < 6) {
+      pcl::PointCloud<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;
-      centroid.z=0.0;
-      
-       clusterCentroids.push_back(centroid);
+      centroid.x = 0.0;
+      centroid.y = 0.0;
+      centroid.z = 0.0;
+
+      clusterCentroids.push_back(centroid);
     }
-    
 
-     // Set initial state
-     KF0.statePre.at<float>(0)=clusterCentroids.at(0).x;
-     KF0.statePre.at<float>(1)=clusterCentroids.at(0).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) = clusterCentroids.at(0).x;
+    KF0.statePre.at<float>(1) = clusterCentroids.at(0).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)=clusterCentroids.at(1).x;
-     KF1.statePre.at<float>(1)=clusterCentroids.at(1).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) = clusterCentroids.at(1).x;
+    KF1.statePre.at<float>(1) = clusterCentroids.at(1).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)=clusterCentroids.at(2).x;
-     KF2.statePre.at<float>(1)=clusterCentroids.at(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) = clusterCentroids.at(2).x;
+    KF2.statePre.at<float>(1) = clusterCentroids.at(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) = clusterCentroids.at(3).x;
+    KF3.statePre.at<float>(1) = clusterCentroids.at(3).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)=clusterCentroids.at(3).x;
-     KF3.statePre.at<float>(1)=clusterCentroids.at(3).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) = clusterCentroids.at(4).x;
+    KF4.statePre.at<float>(1) = clusterCentroids.at(4).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)=clusterCentroids.at(4).x;
-     KF4.statePre.at<float>(1)=clusterCentroids.at(4).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) = clusterCentroids.at(5).x;
+    KF5.statePre.at<float>(1) = clusterCentroids.at(5).y;
+    KF5.statePre.at<float>(2) = 0; // initial v_x
+    KF5.statePre.at<float>(3) = 0; // initial v_y
 
-     // Set initial state
-     KF5.statePre.at<float>(0)=clusterCentroids.at(5).x;
-     KF5.statePre.at<float>(1)=clusterCentroids.at(5).y;
-     KF5.statePre.at<float>(2)=0;// initial v_x
-     KF5.statePre.at<float>(3)=0;//initial v_y
+    firstFrame = false;
 
-     firstFrame=false;
+    for (int i = 0; i < 6; i++) {
+      geometry_msgs::Point pt;
+      pt.x = clusterCentroids.at(i).x;
+      pt.y = clusterCentroids.at(i).y;
+      prevClusterCenters.push_back(pt);
+    }
+    /*  // Print the initial state of the kalman filter for debugging
+      cout<<"KF0.satePre="<<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";
 
-    for (int i=0;i<6;i++)
-     {
-        geometry_msgs::Point pt;
-        pt.x=clusterCentroids.at(i).x;
-        pt.y=clusterCentroids.at(i).y;
-        prevClusterCenters.push_back(pt);
-     }
-   /*  // Print the initial state of the kalman filter for debugging
-     cout<<"KF0.satePre="<<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
+      */
+  }
 
-     //cin.ignore();// To be able to see the printed initial state of the KalmanFilter
+  else {
+    // cout<<"ELSE firstFrame="<<firstFrame<<"\n";
+    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    /* Creating the KdTree from input point cloud*/
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
+        new pcl::search::KdTree<pcl::PointXYZ>);
+
+    pcl::fromROSMsg(*input, *input_cloud);
+
+    tree->setInputCloud(input_cloud);
+
+    /* Here we are creating a vector of PointIndices, which contains the actual
+     * index information in a vector<int>. The indices of each detected cluster
+     * are saved here. Cluster_indices is a vector containing one instance of
+     * PointIndices for each detected cluster. Cluster_indices[0] contain all
+     * indices of the first cluster in input point cloud.
      */
-}
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+    ec.setClusterTolerance(0.3);
+    ec.setMinClusterSize(10);
+    ec.setMaxClusterSize(600);
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(input_cloud);
+    // cout<<"PCL init successfull\n";
+    /* Extract the clusters out of pc and save indices in cluster_indices.*/
+    ec.extract(cluster_indices);
+    // cout<<"PCL extract successfull\n";
+    /* To separate each cluster out of the vector<PointIndices> we have to
+     * iterate through cluster_indices, create a new PointCloud for each
+     * entry and write all points of the current cluster in the PointCloud.
+     */
+    // pcl::PointXYZ origin (0,0,0);
+    // float mindist_this_cluster = 1000;
+    // float dist_this_point = 1000;
 
- 
-else
-{ 
-  //cout<<"ELSE firstFrame="<<firstFrame<<"\n";
-  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
-  pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
-      /* Creating the KdTree from input point cloud*/
-  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+    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;
 
-  pcl::fromROSMsg (*input, *input_cloud);
+    // Cluster centroids
+    std::vector<pcl::PointXYZ> clusterCentroids;
 
-  tree->setInputCloud (input_cloud);
+    for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
+      float x = 0.0;
+      float y = 0.0;
+      int numPts = 0;
+      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
+          new pcl::PointCloud<pcl::PointXYZ>);
+      for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
 
-  /* Here we are creating a vector of PointIndices, which contains the actual index
-   * information in a vector<int>. The indices of each detected cluster are saved here.
-   * Cluster_indices is a vector containing one instance of PointIndices for each detected 
-   * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
-   */
-  std::vector<pcl::PointIndices> cluster_indices;
-  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
-  ec.setClusterTolerance (0.3);
-  ec.setMinClusterSize (10);
-  ec.setMaxClusterSize (600);
-  ec.setSearchMethod (tree);
-  ec.setInputCloud (input_cloud);
-//cout<<"PCL init successfull\n";
-  /* Extract the clusters out of pc and save indices in cluster_indices.*/
-  ec.extract (cluster_indices);
-//cout<<"PCL extract successfull\n";
-  /* To separate each cluster out of the vector<PointIndices> we have to 
-   * iterate through cluster_indices, create a new PointCloud for each 
-   * entry and write all points of the current cluster in the PointCloud. 
-   */
-  //pcl::PointXYZ origin (0,0,0);
-  //float mindist_this_cluster = 1000;
-  //float dist_this_point = 1000;
+        cloud_cluster->points.push_back(input_cloud->points[*pit]);
 
-  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;
+        x += input_cloud->points[*pit].x;
+        y += input_cloud->points[*pit].y;
+        numPts++;
 
-     // Cluster centroids
-  std::vector<pcl::PointXYZ> clusterCentroids;
+        // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
+        //                                          origin);
+        // mindist_this_cluster = std::min(dist_this_point,
+        // mindist_this_cluster);
+      }
 
-  
+      pcl::PointXYZ centroid;
+      centroid.x = x / numPts;
+      centroid.y = y / numPts;
+      centroid.z = 0.0;
 
-  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
-   {
-        float x=0.0; float y=0.0;
-         int numPts=0;
-          pcl::PointCloud<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);
-          }
-
-    pcl::PointXYZ centroid;
-      centroid.x=x/numPts;
-      centroid.y=y/numPts;
-      centroid.z=0.0;
-      
       cluster_vec.push_back(cloud_cluster);
 
-      //Get the centroid of the cluster
+      // Get the centroid of the cluster
       clusterCentroids.push_back(centroid);
-
     }
-   // cout<<"cluster_vec got some clusters\n";
+    // cout<<"cluster_vec got some clusters\n";
 
-    //Ensure at least 6 clusters exist to publish (later clusters may be empty)
-    while (cluster_vec.size() < 6){
-      pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-      empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
+    // Ensure at least 6 clusters exist to publish (later clusters may be empty)
+    while (cluster_vec.size() < 6) {
+      pcl::PointCloud<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;
-      centroid.z=0.0;
-      
-       clusterCentroids.push_back(centroid);
-    }
+      centroid.x = 0.0;
+      centroid.y = 0.0;
+      centroid.z = 0.0;
 
+      clusterCentroids.push_back(centroid);
+    }
 
     std_msgs::Float32MultiArray cc;
-    for(int i=0;i<6;i++)
-    {
-        cc.data.push_back(clusterCentroids.at(i).x);
-        cc.data.push_back(clusterCentroids.at(i).y);
-        cc.data.push_back(clusterCentroids.at(i).z);    
-
+    for (int i = 0; i < 6; i++) {
+      cc.data.push_back(clusterCentroids.at(i).x);
+      cc.data.push_back(clusterCentroids.at(i).y);
+      cc.data.push_back(clusterCentroids.at(i).z);
     }
-   // cout<<"6 clusters initialized\n";
+    // cout<<"6 clusters initialized\n";
 
-    //cc_pos.publish(cc);// Publish cluster mid-points.
+    // cc_pos.publish(cc);// Publish cluster mid-points.
     KFT(cc);
-    int i=0;
+    int i = 0;
     bool publishedCluster[6];
-    for(auto it=objID.begin();it!=objID.end();it++)
-        { //cout<<"Inside the for loop\n";
-            
-        
-            switch(i)
-            {
-                cout<<"Inside the switch case\n";
-                case 0: { 
-                            publish_cloud(pub_cluster0,cluster_vec[*it]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                case 1: {
-                            publish_cloud(pub_cluster1,cluster_vec[*it]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                case 2: {
-                            publish_cloud(pub_cluster2,cluster_vec[*it]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                case 3: {
-                            publish_cloud(pub_cluster3,cluster_vec[*it]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                case 4: {
-                            publish_cloud(pub_cluster4,cluster_vec[*it]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-
-                case 5: {
-                            publish_cloud(pub_cluster5,cluster_vec[*it]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                default: break;
-            }
-        
-        }
-
-} 
-
-}   
-
-
-  
-
-int main(int argc, char** argv)
-{
-    // ROS init
-    ros::init (argc,argv,"kf_tracker");
-    ros::NodeHandle nh;
-
-   
-    // Publishers to publish the state of the objects (pos and vel)
-    //objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
-
-
-
-cout<<"About to setup callback\n";
-
-// Create a ROS subscriber for the input point cloud
-  ros::Subscriber sub = nh.subscribe ("filtered_cloud", 1, cloud_cb);
-  // Create a ROS publisher for the output point cloud
-  pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_0", 1);
-  pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_1", 1);
-  pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_2", 1);
-  pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_3", 1);
-  pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_4", 1);
-  pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_5", 1);
-      // Subscribe to the clustered pointclouds
-    //ros::Subscriber c1=nh.subscribe("ccs",100,KFT); 
-    objID_pub = nh.advertise<std_msgs::Int32MultiArray>("obj_id", 1);
-/* Point cloud clustering
-*/
-    
-  //cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
-  markerPub= nh.advertise<visualization_msgs::MarkerArray> ("viz",1);
-
-/* Point cloud clustering
-*/    
-
-
-    ros::spin();
+    for (auto it = objID.begin(); it != objID.end();
+         it++) { // cout<<"Inside the for loop\n";
 
+      switch (i) {
+        cout << "Inside the switch case\n";
+      case 0: {
+        publish_cloud(pub_cluster0, cluster_vec[*it]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      case 1: {
+        publish_cloud(pub_cluster1, cluster_vec[*it]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      case 2: {
+        publish_cloud(pub_cluster2, cluster_vec[*it]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      case 3: {
+        publish_cloud(pub_cluster3, cluster_vec[*it]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      case 4: {
+        publish_cloud(pub_cluster4, cluster_vec[*it]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
 
+      case 5: {
+        publish_cloud(pub_cluster5, cluster_vec[*it]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      default:
+        break;
+      }
+    }
+  }
+}
+
+int main(int argc, char **argv) {
+  // ROS init
+  ros::init(argc, argv, "kf_tracker");
+  ros::NodeHandle nh;
+
+  // Publishers to publish the state of the objects (pos and vel)
+  // objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
+
+  cout << "About to setup callback\n";
+
+  // Create a ROS subscriber for the input point cloud
+  ros::Subscriber sub = nh.subscribe("filtered_cloud", 1, cloud_cb);
+  // Create a ROS publisher for the output point cloud
+  pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2>("cluster_0", 1);
+  pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2>("cluster_1", 1);
+  pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2>("cluster_2", 1);
+  pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2>("cluster_3", 1);
+  pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2>("cluster_4", 1);
+  pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2>("cluster_5", 1);
+  // Subscribe to the clustered pointclouds
+  // ros::Subscriber c1=nh.subscribe("ccs",100,KFT);
+  objID_pub = nh.advertise<std_msgs::Int32MultiArray>("obj_id", 1);
+  /* Point cloud clustering
+   */
+
+  // cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
+  markerPub = nh.advertise<visualization_msgs::MarkerArray>("viz", 1);
+
+  /* Point cloud clustering
+   */
+
+  ros::spin();
 }
diff --git a/src/main_naive.cpp b/src/main_naive.cpp
index 7be52e9..5495650 100644
--- a/src/main_naive.cpp
+++ b/src/main_naive.cpp
@@ -1,163 +1,162 @@
-#include <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
-    int stateDim=4;// [x,y,v_x,v_y]//,w,h]
-    int measDim=2;// [z_x,z_y,z_w,z_h]
-    int ctrlDim=0;
-    cv::KalmanFilter KF0(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF1(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF2(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF3(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF4(stateDim,measDim,ctrlDim,CV_32F);
-    cv::KalmanFilter KF5(stateDim,measDim,ctrlDim,CV_32F);
+// KF init
+int stateDim = 4; // [x,y,v_x,v_y]//,w,h]
+int measDim = 2;  // [z_x,z_y,z_w,z_h]
+int ctrlDim = 0;
+cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F);
+cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F);
 
-    ros::Publisher pub_cluster0;
-    ros::Publisher pub_cluster1;
-    ros::Publisher pub_cluster2;
-    ros::Publisher pub_cluster3;
-    ros::Publisher pub_cluster4;
-    ros::Publisher pub_cluster5;
+ros::Publisher pub_cluster0;
+ros::Publisher pub_cluster1;
+ros::Publisher pub_cluster2;
+ros::Publisher pub_cluster3;
+ros::Publisher pub_cluster4;
+ros::Publisher pub_cluster5;
 
-    std::vector<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);
-    cv::Mat_<float> measurement(2,1); 
+std::vector<int> objID; // Output of the data association using KF
+                        // measurement.setTo(Scalar(0));
 
-    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)
-  {
-    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()};
+  // cout<<"Pred successfull\n";
 
-    // First predict, to update the internal statePre variable
+  // cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
+  // cout<<"Prediction 1
+  // ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
 
-    std::vector<cv::Mat> pred{KF0.predict(),KF1.predict(),KF2.predict(),KF3.predict(),KF4.predict(),KF5.predict()};
- //cout<<"Pred successfull\n";
+  // Get measurements
+  // Extract the position of the clusters forom the multiArray. To check if the
+  // data coming in, check the .z (every third) coordinate and that will be 0.0
+  std::vector<geometry_msgs::Point> clusterCenters; // clusterCenters
 
-    //cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
-   // cout<<"Prediction 1 ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
+  int i = 0;
+  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);
 
-    // Get measurements
-    // Extract the position of the clusters forom the multiArray. To check if the data
-    // coming in, check the .z (every third) coordinate and that will be 0.0
-    std::vector<geometry_msgs::Point> clusterCenters;//clusterCenters
-   
-    int i=0;
-    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);
-       
-    }
+    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++)
-    {
-        geometry_msgs::Point pt;
-        pt.x=(*it).at<float>(0);
-        pt.y=(*it).at<float>(1);
-        pt.z=(*it).at<float>(2);
+  std::vector<geometry_msgs::Point> KFpredictions;
+  i = 0;
+  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";
+    KFpredictions.push_back(pt);
+  }
+  // cout<<"Got predictions"<<"\n";
 
-    /* Original Version using Kalman filter prediction
-    
-    // Find the cluster that is more probable to be belonging to a given KF.
-    objID.clear();//Clear the objID vector
-    for(int filterN=0;filterN<6;filterN++)
-    {
-        std::vector<float> distVec;
-        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";
-        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";
-    
-    }
+  /* Original Version using Kalman filter prediction
 
-   // cout<<"Got object IDs"<<"\n";
-    //countIDs(objID);// for verif/corner cases
-      Original version using kalman filter prediction
-    */
-    //display objIDs
+  // Find the cluster that is more probable to be belonging to a given KF.
+  objID.clear();//Clear the objID vector
+  for(int filterN=0;filterN<6;filterN++)
+  {
+      std::vector<float> distVec;
+      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";
+      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<<"Got object IDs"<<"\n";
+  //countIDs(objID);// for verif/corner cases
+    Original version using kalman filter prediction
+  */
+  // display objIDs
   /* DEBUG
     cout<<"objID= ";
     for(auto it=objID.begin();it!=objID.end();it++)
@@ -165,108 +164,107 @@ void KFT(const std_msgs::Float32MultiArray ccs)
     cout<<"\n";
     */
 
-/* Naive version without using kalman filter */
-objID.clear();//Clear the objID vector
-    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]));
-       
-      // 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";
-    
-    }
-     /* Naive version without kalman filter */
-    //prevClusterCenters.clear();
-    // Set the current associated clusters to the prevClusterCenters
-     for (int i=0;i<6;i++)
-     {
-        prevClusterCenters[objID.at(i)]=clusterCenters.at(i);
-       
-     }
+  /* Naive version without using kalman filter */
+  objID.clear(); // Clear the objID vector
+  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]));
 
-     /* Naive version without kalman filter */
+    // 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";
+  }
+  /* Naive version without kalman filter */
+  // prevClusterCenters.clear();
+  // Set the current associated clusters to the prevClusterCenters
+  for (int i = 0; i < 6; i++) {
+    prevClusterCenters[objID.at(i)] = clusterCenters.at(i);
+  }
 
-/* Naive version without using kalman filter */    
+  /* 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++)
-    {
-        vector<float> pt;
-        pt.push_back(clusterCenters[i].x);
-        pt.push_back(clusterCenters[i].y);
-        pt.push_back(clusterCenters[i].z);
-        
-        cc.push_back(pt);
-    }
-    //cout<<"cc[5][0]="<<cc[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 meas1[3]={cc[1].at(0),cc[1].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 meas4[3]={cc[4].at(0),cc[4].at(1)};
-    float meas5[3]={cc[5].at(0),cc[5].at(1)};
+  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++) {
+    vector<float> pt;
+    pt.push_back(clusterCenters[i].x);
+    pt.push_back(clusterCenters[i].y);
+    pt.push_back(clusterCenters[i].z);
 
+    cc.push_back(pt);
+  }
+  // cout<<"cc[5][0]="<<cc[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 meas1[3] = {cc[1].at(0), cc[1].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 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);
+  cv::Mat meas2Mat = cv::Mat(2, 1, CV_32F, meas2);
+  cv::Mat meas3Mat = cv::Mat(2, 1, CV_32F, meas3);
+  cv::Mat meas4Mat = cv::Mat(2, 1, CV_32F, meas4);
+  cv::Mat meas5Mat = cv::Mat(2, 1, CV_32F, meas5);
 
-    // The update phase 
-    cv::Mat meas0Mat=cv::Mat(2,1,CV_32F,meas0);
-    cv::Mat meas1Mat=cv::Mat(2,1,CV_32F,meas1);
-    cv::Mat meas2Mat=cv::Mat(2,1,CV_32F,meas2);
-    cv::Mat meas3Mat=cv::Mat(2,1,CV_32F,meas3);
-    cv::Mat meas4Mat=cv::Mat(2,1,CV_32F,meas4);
-    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);
+  Mat estimated2 = KF0.correct(meas2Mat);
+  Mat estimated3 = KF0.correct(meas3Mat);
+  Mat estimated4 = KF0.correct(meas4Mat);
+  Mat estimated5 = KF0.correct(meas5Mat);
 
-    Mat estimated0 = KF0.correct(meas0Mat);
-    Mat estimated1 = KF0.correct(meas1Mat);
-    Mat estimated2 = KF0.correct(meas2Mat);
-    Mat estimated3 = KF0.correct(meas3Mat);
-    Mat estimated4 = KF0.correct(meas4Mat);
-    Mat estimated5 = KF0.correct(meas5Mat);
- 
-    // Publish the point clouds belonging to each clusters
+  // 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<<"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){
-  sensor_msgs::PointCloud2::Ptr clustermsg (new sensor_msgs::PointCloud2);
-  pcl::toROSMsg (*cluster , *clustermsg);
+void publish_cloud(ros::Publisher &pub,
+                   pcl::PointCloud<pcl::PointXYZ>::Ptr cluster) {
+  sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2);
+  pcl::toROSMsg(*cluster, *clustermsg);
   clustermsg->header.frame_id = "map";
   clustermsg->header.stamp = ros::Time::now();
-  pub.publish (*clustermsg);
-
+  pub.publish(*clustermsg);
 }
 
-
-void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
+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)
-{   
-  // 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);
+  cout << "IF firstFrame=" << firstFrame << "\n";
+  // If this is the first frame, initialize kalman filters for the clustered
+  // objects
+  if (firstFrame) {
+    // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
+    // Could be made generic by creating a Kalman Filter only when a new object
+    // is detected
+    KF0.transitionMatrix =
+        *(Mat_<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);
@@ -288,316 +286,323 @@ if (firstFrame)
     setIdentity(KF4.processNoiseCov, Scalar::all(1e-4));
     setIdentity(KF5.processNoiseCov, Scalar::all(1e-4));
     // Meas noise cov matrix R
-     cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(1e-1));
-     cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(1e-1));
-     cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(1e-1));
-     cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(1e-1));
-     cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(1e-1));
-     cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1));
+    cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(1e-1));
+    cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(1e-1));
+    cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(1e-1));
+    cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(1e-1));
+    cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(1e-1));
+    cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1));
 
-// Process the point cloud
-     pcl::PointCloud<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>);
+    // Process the point cloud
+    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    /* Creating the KdTree from input point cloud*/
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
+        new pcl::search::KdTree<pcl::PointXYZ>);
 
-  pcl::fromROSMsg (*input, *input_cloud);
+    pcl::fromROSMsg(*input, *input_cloud);
 
-  tree->setInputCloud (input_cloud);
+    tree->setInputCloud(input_cloud);
 
-  /* Here we are creating a vector of PointIndices, which contains the actual index
-   * information in a vector<int>. The indices of each detected cluster are saved here.
-   * Cluster_indices is a vector containing one instance of PointIndices for each detected 
-   * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
-   */
-  std::vector<pcl::PointIndices> cluster_indices;
-  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
-  ec.setClusterTolerance (0.08); 
-  ec.setMinClusterSize (10);
-  ec.setMaxClusterSize (600);
-  ec.setSearchMethod (tree);
-  ec.setInputCloud (input_cloud);
-  /* Extract the clusters out of pc and save indices in cluster_indices.*/
-  ec.extract (cluster_indices);
+    /* Here we are creating a vector of PointIndices, which contains the actual
+     * index information in a vector<int>. The indices of each detected cluster
+     * are saved here. Cluster_indices is a vector containing one instance of
+     * PointIndices for each detected cluster. Cluster_indices[0] contain all
+     * indices of the first cluster in input point cloud.
+     */
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+    ec.setClusterTolerance(0.08);
+    ec.setMinClusterSize(10);
+    ec.setMaxClusterSize(600);
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(input_cloud);
+    /* Extract the clusters out of pc and save indices in cluster_indices.*/
+    ec.extract(cluster_indices);
 
-  /* To separate each cluster out of the vector<PointIndices> we have to 
-   * iterate through cluster_indices, create a new PointCloud for each 
-   * entry and write all points of the current cluster in the PointCloud. 
-   */
-  //pcl::PointXYZ origin (0,0,0);
-  //float mindist_this_cluster = 1000;
-  //float dist_this_point = 1000;
+    /* To separate each cluster out of the vector<PointIndices> we have to
+     * iterate through cluster_indices, create a new PointCloud for each
+     * entry and write all points of the current cluster in the PointCloud.
+     */
+    // pcl::PointXYZ origin (0,0,0);
+    // float mindist_this_cluster = 1000;
+    // float dist_this_point = 1000;
 
-  std::vector<pcl::PointIndices>::const_iterator it;
-  std::vector<int>::const_iterator pit;
-  // Vector of cluster pointclouds
-  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
+    std::vector<pcl::PointIndices>::const_iterator it;
+    std::vector<int>::const_iterator pit;
+    // Vector of cluster pointclouds
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec;
 
-  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
-          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-          for(pit = it->indices.begin(); pit != it->indices.end(); pit++) 
-          {
-          
-                  cloud_cluster->points.push_back(input_cloud->points[*pit]);
-                  //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
-                  //                                          origin);
-                  //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster);
-          }
+    for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
+      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
+          new pcl::PointCloud<pcl::PointXYZ>);
+      for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
+
+        cloud_cluster->points.push_back(input_cloud->points[*pit]);
+        // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
+        //                                          origin);
+        // mindist_this_cluster = std::min(dist_this_point,
+        // mindist_this_cluster);
+      }
 
-         
       cluster_vec.push_back(cloud_cluster);
-
     }
 
-    //Ensure at least 6 clusters exist to publish (later clusters may be empty)
-    while (cluster_vec.size() < 6){
-      pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-      empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
+    // Ensure at least 6 clusters exist to publish (later clusters may be empty)
+    while (cluster_vec.size() < 6) {
+      pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
+          new pcl::PointCloud<pcl::PointXYZ>);
+      empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
       cluster_vec.push_back(empty_cluster);
     }
-    
 
-     // Set initial state
-     KF0.statePre.at<float>(0)=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;
 
-     // 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>(2)=0;// initial v_x
-     KF1.statePre.at<float>(3)=0;//initial v_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
-     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
+    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>(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>(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;
+    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>(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;
+    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>(2)=0;// initial v_x
-     KF5.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>(2) = 0; // initial v_x
+    KF5.statePre.at<float>(3) = 0; // initial v_y
 
-     firstFrame=false;
+    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";
+    // 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
-
-
-
-     /* Naive version without kalman filter */
-     for (int i=0;i<6;i++)
-     {
-        geometry_msgs::Point pt;
-        pt.x=cluster_vec[i]->points[cluster_vec[i]->points.size()/2].x;
-        pt.y=cluster_vec[i]->points[cluster_vec[i]->points.size()/2].y;
-        prevClusterCenters.push_back(pt);
-     }
-
-     /* Naive version without kalman filter */
-}
-
- 
-else
-{ 
-  cout<<"ELSE firstFrame="<<firstFrame<<"\n";
-  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
-  pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
-      /* Creating the KdTree from input point cloud*/
-  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
-
-  pcl::fromROSMsg (*input, *input_cloud);
-
-  tree->setInputCloud (input_cloud);
-
-  /* Here we are creating a vector of PointIndices, which contains the actual index
-   * information in a vector<int>. The indices of each detected cluster are saved here.
-   * Cluster_indices is a vector containing one instance of PointIndices for each detected 
-   * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
-   */
-  std::vector<pcl::PointIndices> cluster_indices;
-  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
-  ec.setClusterTolerance (0.08); 
-  ec.setMinClusterSize (10);
-  ec.setMaxClusterSize (600);
-  ec.setSearchMethod (tree);
-  ec.setInputCloud (input_cloud);
-cout<<"PCL init successfull\n";
-  /* Extract the clusters out of pc and save indices in cluster_indices.*/
-  ec.extract (cluster_indices);
-cout<<"PCL extract successfull\n";
-  /* To separate each cluster out of the vector<PointIndices> we have to 
-   * iterate through cluster_indices, create a new PointCloud for each 
-   * entry and write all points of the current cluster in the PointCloud. 
-   */
-  //pcl::PointXYZ origin (0,0,0);
-  //float mindist_this_cluster = 1000;
-  //float dist_this_point = 1000;
-
-  std::vector<pcl::PointIndices>::const_iterator it;
-  std::vector<int>::const_iterator pit;
-  // Vector of cluster pointclouds
-  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
-
-  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
-          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-          for(pit = it->indices.begin(); pit != it->indices.end(); pit++) 
-          {
-          
-                  cloud_cluster->points.push_back(input_cloud->points[*pit]);
-                  //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
-                  //                                          origin);
-                  //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster);
-          }
-
-         
-      cluster_vec.push_back(cloud_cluster);
+    // cin.ignore();// To be able to see the printed initial state of the
+    // KalmanFilter
 
+    /* Naive version without kalman filter */
+    for (int i = 0; i < 6; i++) {
+      geometry_msgs::Point pt;
+      pt.x = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].x;
+      pt.y = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].y;
+      prevClusterCenters.push_back(pt);
     }
-    //cout<<"cluster_vec got some clusters\n";
 
-    //Ensure at least 6 clusters exist to publish (later clusters may be empty)
-    while (cluster_vec.size() < 6){
-      pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-      empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
+    /* Naive version without kalman filter */
+  }
+
+  else {
+    cout << "ELSE firstFrame=" << firstFrame << "\n";
+    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    /* Creating the KdTree from input point cloud*/
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
+        new pcl::search::KdTree<pcl::PointXYZ>);
+
+    pcl::fromROSMsg(*input, *input_cloud);
+
+    tree->setInputCloud(input_cloud);
+
+    /* Here we are creating a vector of PointIndices, which contains the actual
+     * index information in a vector<int>. The indices of each detected cluster
+     * are saved here. Cluster_indices is a vector containing one instance of
+     * PointIndices for each detected cluster. Cluster_indices[0] contain all
+     * indices of the first cluster in input point cloud.
+     */
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+    ec.setClusterTolerance(0.08);
+    ec.setMinClusterSize(10);
+    ec.setMaxClusterSize(600);
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(input_cloud);
+    cout << "PCL init successfull\n";
+    /* Extract the clusters out of pc and save indices in cluster_indices.*/
+    ec.extract(cluster_indices);
+    cout << "PCL extract successfull\n";
+    /* To separate each cluster out of the vector<PointIndices> we have to
+     * iterate through cluster_indices, create a new PointCloud for each
+     * entry and write all points of the current cluster in the PointCloud.
+     */
+    // pcl::PointXYZ origin (0,0,0);
+    // float mindist_this_cluster = 1000;
+    // float dist_this_point = 1000;
+
+    std::vector<pcl::PointIndices>::const_iterator it;
+    std::vector<int>::const_iterator pit;
+    // Vector of cluster pointclouds
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec;
+
+    for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
+      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
+          new pcl::PointCloud<pcl::PointXYZ>);
+      for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
+
+        cloud_cluster->points.push_back(input_cloud->points[*pit]);
+        // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
+        //                                          origin);
+        // mindist_this_cluster = std::min(dist_this_point,
+        // mindist_this_cluster);
+      }
+
+      cluster_vec.push_back(cloud_cluster);
+    }
+    // cout<<"cluster_vec got some clusters\n";
+
+    // Ensure at least 6 clusters exist to publish (later clusters may be empty)
+    while (cluster_vec.size() < 6) {
+      pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
+          new pcl::PointCloud<pcl::PointXYZ>);
+      empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
       cluster_vec.push_back(empty_cluster);
     }
     std_msgs::Float32MultiArray cc;
-    for(int i=0;i<6;i++)
-    {
-        cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].x);
-        cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].y);
-        cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].z);
-
+    for (int i = 0; i < 6; i++) {
+      cc.data.push_back(
+          cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].x);
+      cc.data.push_back(
+          cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].y);
+      cc.data.push_back(
+          cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].z);
     }
-    cout<<"6 clusters initialized\n";
+    cout << "6 clusters initialized\n";
 
-    //cc_pos.publish(cc);// Publish cluster mid-points.
+    // cc_pos.publish(cc);// Publish cluster mid-points.
     KFT(cc);
-    int i=0;
+    int i = 0;
     bool publishedCluster[6];
-    for(auto it=objID.begin();it!=objID.end();it++)
-        { cout<<"Inside the for loop\n";
-            
-            switch(*it)
-            {
-                cout<<"Inside the switch case\n";
-                case 0: { 
-                            publish_cloud(pub_cluster0,cluster_vec[i]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                case 1: {
-                            publish_cloud(pub_cluster1,cluster_vec[i]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                case 2: {
-                            publish_cloud(pub_cluster2,cluster_vec[i]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                case 3: {
-                            publish_cloud(pub_cluster3,cluster_vec[i]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                case 4: {
-                            publish_cloud(pub_cluster4,cluster_vec[i]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-
-                case 5: {
-                            publish_cloud(pub_cluster5,cluster_vec[i]);
-                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
-                            i++;
-                            break;
-
-                        }
-                default: break;
-            }
-    
-        }
-
-} 
-
-}   
-
-
-  
-
-int main(int argc, char** argv)
-{
-    // ROS init
-    ros::init (argc,argv,"KFTracker");
-    ros::NodeHandle nh;
-
-   
-    // Publishers to publish the state of the objects (pos and vel)
-    //objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
-
-
-
-cout<<"About to setup callback\n";
-
-// Create a ROS subscriber for the input point cloud
-  ros::Subscriber sub = nh.subscribe ("filtered_cloud", 1, cloud_cb);
-  // Create a ROS publisher for the output point cloud
-  pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_0", 1);
-  pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_1", 1);
-  pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_2", 1);
-  pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_3", 1);
-  pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_4", 1);
-  pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_5", 1);
-      // Subscribe to the clustered pointclouds
-    //ros::Subscriber c1=nh.subscribe("ccs",100,KFT); 
-    objID_pub = nh.advertise<std_msgs::Int32MultiArray>("obj_id", 1);
-/* Point cloud clustering
-*/
-    
-  //cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
-
-
-/* Point cloud clustering
-*/    
-
-
-    ros::spin();
+    for (auto it = objID.begin(); it != objID.end(); it++) {
+      cout << "Inside the for loop\n";
 
+      switch (*it) {
+        cout << "Inside the switch case\n";
+      case 0: {
+        publish_cloud(pub_cluster0, cluster_vec[i]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      case 1: {
+        publish_cloud(pub_cluster1, cluster_vec[i]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      case 2: {
+        publish_cloud(pub_cluster2, cluster_vec[i]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      case 3: {
+        publish_cloud(pub_cluster3, cluster_vec[i]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      case 4: {
+        publish_cloud(pub_cluster4, cluster_vec[i]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
 
+      case 5: {
+        publish_cloud(pub_cluster5, cluster_vec[i]);
+        publishedCluster[i] =
+            true; // Use this flag to publish only once for a given obj ID
+        i++;
+        break;
+      }
+      default:
+        break;
+      }
+    }
+  }
+}
+
+int main(int argc, char **argv) {
+  // ROS init
+  ros::init(argc, argv, "KFTracker");
+  ros::NodeHandle nh;
+
+  // Publishers to publish the state of the objects (pos and vel)
+  // objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
+
+  cout << "About to setup callback\n";
+
+  // Create a ROS subscriber for the input point cloud
+  ros::Subscriber sub = nh.subscribe("filtered_cloud", 1, cloud_cb);
+  // Create a ROS publisher for the output point cloud
+  pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2>("cluster_0", 1);
+  pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2>("cluster_1", 1);
+  pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2>("cluster_2", 1);
+  pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2>("cluster_3", 1);
+  pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2>("cluster_4", 1);
+  pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2>("cluster_5", 1);
+  // Subscribe to the clustered pointclouds
+  // ros::Subscriber c1=nh.subscribe("ccs",100,KFT);
+  objID_pub = nh.advertise<std_msgs::Int32MultiArray>("obj_id", 1);
+  /* Point cloud clustering
+   */
+
+  // cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
+
+  /* Point cloud clustering
+   */
+
+  ros::spin();
 }