From 112a2d4e4cc39743e7a818acb5b04cb3549d4d28 Mon Sep 17 00:00:00 2001
From: Praveen Palanisamy <praveenofpersia@gmail.com>
Date: Thu, 10 Dec 2015 17:13:49 -0500
Subject: [PATCH] Working state of Multiple object stable tracking using Lidar
 scans with an extended Kalman Filter (rosrun kf_tracker tracker). A naive
 tracker is implemented in main_naive.cpp for comparison (rosrun kf_tracker
 naive_tracker).

---
 src/main.cpp       | 365 ++++++++++++++++++++-------
 src/main_naive.cpp | 603 +++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 878 insertions(+), 90 deletions(-)
 create mode 100644 src/main_naive.cpp

diff --git a/src/main.cpp b/src/main.cpp
index f7e9218..0bd35fd 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -31,6 +31,12 @@
 #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 <limits>
+#include <utility>
 
 using namespace std;
 using namespace cv;
@@ -56,6 +62,10 @@ ros::Publisher objID_pub;
     ros::Publisher pub_cluster4;
     ros::Publisher pub_cluster5;
 
+    ros::Publisher markerPub;
+
+    std::vector<geometry_msgs::Point> prevClusterCenters;
+
 
     cv::Mat state(stateDim,1,CV_32F);
     cv::Mat_<float> measurement(2,1); 
@@ -90,6 +100,27 @@ int countIDs(vector<int> v)
 objID: vector containing the IDs of the clusters that should be associated with each KF_Tracker
 objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
 */
+
+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)
 {
 
@@ -133,23 +164,70 @@ void KFT(const std_msgs::Float32MultiArray ccs)
         KFpredictions.push_back(pt);
         
     }
-   // cout<<"Got predictions"<<"\n";
+  // 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],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";
-    
+        {
+            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";
+
+
     }
+
+    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
+        {
+            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
 
@@ -160,28 +238,62 @@ void KFT(const std_msgs::Float32MultiArray ccs)
         cout<<*it<<" ,";
     cout<<"\n";
     */
+
+    visualization_msgs::MarkerArray clusterMarkers;
+
+     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;
+
+       //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);
+     }
+
+    prevClusterCenters=clusterCenters;
+
+     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<clusterCenters.size();i++)
+    for (int i=0;i<6;i++)
     {
         vector<float> pt;
-        pt.push_back(clusterCenters[i].x);
-        pt.push_back(clusterCenters[i].y);
-        pt.push_back(clusterCenters[i].z);
+        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[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)};
+    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)};
 
 
 
@@ -194,13 +306,19 @@ void KFT(const std_msgs::Float32MultiArray ccs)
     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))
     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);
+if (!(meas1[0]==0.0f || meas1[1]==0.0f))
+    Mat estimated1 = KF1.correct(meas1Mat);
+if (!(meas2[0]==0.0f || meas2[1]==0.0f))
+    Mat estimated2 = KF2.correct(meas2Mat);
+if (!(meas3[0]==0.0f || meas3[1]==0.0f))
+    Mat estimated3 = KF3.correct(meas3Mat);
+if (!(meas4[0]==0.0f || meas4[1]==0.0f))
+    Mat estimated4 = KF4.correct(meas4Mat);
+if (!(meas5[0]==0.0f || meas5[1]==0.0f))
+    Mat estimated5 = KF5.correct(meas5Mat);
+    
  
     // Publish the point clouds belonging to each clusters
 
@@ -223,18 +341,23 @@ void publish_cloud(ros::Publisher& pub, pcl::PointCloud<pcl::PointXYZ>::Ptr clus
 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
 
 {
-    cout<<"IF firstFrame="<<firstFrame<<"\n";
+   // cout<<"IF firstFrame="<<firstFrame<<"\n";
     // If this is the first frame, initialize kalman filters for the clustered objects
 if (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);
+
+    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);
@@ -249,19 +372,21 @@ if (firstFrame)
     // [ 0  0  0    1 Ev_y 0 ]
     //// [ 0  0  0    0 1    Ew ]
     //// [ 0  0  0    0 0    Eh ]
-    setIdentity(KF0.processNoiseCov, Scalar::all(1e-4));
-    setIdentity(KF1.processNoiseCov, Scalar::all(1e-4));
-    setIdentity(KF2.processNoiseCov, Scalar::all(1e-4));
-    setIdentity(KF3.processNoiseCov, Scalar::all(1e-4));
-    setIdentity(KF4.processNoiseCov, Scalar::all(1e-4));
-    setIdentity(KF5.processNoiseCov, Scalar::all(1e-4));
+    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));
+    setIdentity(KF3.processNoiseCov, Scalar::all(sigmaP));
+    setIdentity(KF4.processNoiseCov, Scalar::all(sigmaP));
+    setIdentity(KF5.processNoiseCov, Scalar::all(sigmaP));
     // 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(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>);
@@ -273,11 +398,7 @@ if (firstFrame)
 
   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); 
@@ -288,33 +409,45 @@ if (firstFrame)
   /* Extract the clusters out of pc and save indices in cluster_indices.*/
   ec.extract (cluster_indices);
 
-  /* To separate each cluster out of the vector<PointIndices> we have to 
-   * iterate through cluster_indices, create a new PointCloud for each 
-   * entry and write all points of the current cluster in the PointCloud. 
-   */
-  //pcl::PointXYZ origin (0,0,0);
-  //float mindist_this_cluster = 1000;
-  //float dist_this_point = 1000;
-
+  
   std::vector<pcl::PointIndices>::const_iterator it;
   std::vector<int>::const_iterator pit;
   // Vector of cluster pointclouds
   std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
+    // Cluster centroids
+  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>);
+          
+         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++;
+
+
                   //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
+      clusterCentroids.push_back(centroid);
+
+
     }
 
     //Ensure at least 6 clusters exist to publish (later clusters may be empty)
@@ -323,48 +456,65 @@ if (firstFrame)
       empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
       cluster_vec.push_back(empty_cluster);
     }
+
+        while (clusterCentroids.size()<6)
+    {
+      pcl::PointXYZ 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)=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>(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)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].x;
-     KF1.statePre.at<float>(1)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].y;
+     KF1.statePre.at<float>(0)=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)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].x;
-     KF2.statePre.at<float>(1)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].y;
+     KF2.statePre.at<float>(0)=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)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].x;
-     KF3.statePre.at<float>(1)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].y;
+     KF3.statePre.at<float>(0)=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)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].x;
-     KF4.statePre.at<float>(1)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].y;
+     KF4.statePre.at<float>(0)=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)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].x;
-     KF5.statePre.at<float>(1)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].y;
+     KF5.statePre.at<float>(0)=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;
 
-     // Print the initial state of the kalman filter for debugging
+    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";
@@ -373,12 +523,13 @@ if (firstFrame)
      cout<<"KF5.satePre="<<KF5.statePre.at<float>(0)<<","<<KF5.statePre.at<float>(1)<<"\n";
 
      //cin.ignore();// To be able to see the printed initial state of the KalmanFilter
+     */
 }
 
  
 else
 { 
-  cout<<"ELSE firstFrame="<<firstFrame<<"\n";
+  //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*/
@@ -395,15 +546,15 @@ else
    */
   std::vector<pcl::PointIndices> cluster_indices;
   pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
-  ec.setClusterTolerance (0.08); 
+  ec.setClusterTolerance (0.3);
   ec.setMinClusterSize (10);
   ec.setMaxClusterSize (600);
   ec.setSearchMethod (tree);
   ec.setInputCloud (input_cloud);
-cout<<"PCL init successfull\n";
+//cout<<"PCL init successfull\n";
   /* Extract the clusters out of pc and save indices in cluster_indices.*/
   ec.extract (cluster_indices);
-cout<<"PCL extract successfull\n";
+//cout<<"PCL extract successfull\n";
   /* To separate each cluster out of the vector<PointIndices> we have to 
    * iterate through cluster_indices, create a new PointCloud for each 
    * entry and write all points of the current cluster in the PointCloud. 
@@ -417,22 +568,43 @@ cout<<"PCL extract successfull\n";
   // Vector of cluster pointclouds
   std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
 
-  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
+     // Cluster centroids
+  std::vector<pcl::PointXYZ> clusterCentroids;
+
+  
+
+  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
+   {
+        float x=0.0; float y=0.0;
+         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
+      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){
@@ -440,56 +612,69 @@ cout<<"PCL extract successfull\n";
       empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
       cluster_vec.push_back(empty_cluster);
     }
+
+     while (clusterCentroids.size()<6)
+    {
+      pcl::PointXYZ 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(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);
+        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.
     KFT(cc);
     int i=0;
     bool publishedCluster[6];
     for(auto it=objID.begin();it!=objID.end();it++)
-        { cout<<"Inside the for loop\n";
+        { //cout<<"Inside the for loop\n";
             
-            switch(*it)
+        
+            switch(i)
             {
                 cout<<"Inside the switch case\n";
                 case 0: { 
-                            publish_cloud(pub_cluster0,cluster_vec[i]);
+                            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[i]);
+                            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[i]);
+                            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[i]);
+                            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[i]);
+                            publish_cloud(pub_cluster4,cluster_vec[*it]);
                             publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
                             i++;
                             break;
@@ -497,7 +682,7 @@ cout<<"PCL extract successfull\n";
                         }
 
                 case 5: {
-                            publish_cloud(pub_cluster5,cluster_vec[i]);
+                            publish_cloud(pub_cluster5,cluster_vec[*it]);
                             publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
                             i++;
                             break;
@@ -505,7 +690,7 @@ cout<<"PCL extract successfull\n";
                         }
                 default: break;
             }
-    
+        
         }
 
 } 
@@ -545,7 +730,7 @@ cout<<"About to setup callback\n";
 */
     
   //cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
-
+  markerPub= nh.advertise<visualization_msgs::MarkerArray> ("viz",1);
 
 /* Point cloud clustering
 */    
diff --git a/src/main_naive.cpp b/src/main_naive.cpp
new file mode 100644
index 0000000..0886d61
--- /dev/null
+++ b/src/main_naive.cpp
@@ -0,0 +1,603 @@
+#include <iostream>
+#include <string.h>
+#include <fstream>
+#include <algorithm>
+#include <iterator>
+#include "kf_tracker/featureDetection.h"
+#include "kf_tracker/CKalmanFilter.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/video/video.hpp>
+#include "opencv2/video/tracking.hpp"
+#include <ros/ros.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include "pcl_ros/point_cloud.h"
+#include <geometry_msgs/Point.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <std_msgs/Int32MultiArray.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/common/geometry.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+using namespace std;
+using namespace cv;
+
+
+ros::Publisher objID_pub;
+
+    // KF init
+    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;
+
+    std::vector<geometry_msgs::Point> prevClusterCenters;
+
+
+    cv::Mat state(stateDim,1,CV_32F);
+    cv::Mat_<float> measurement(2,1); 
+
+    std::vector<int> objID;// Output of the data association using KF
+   // measurement.setTo(Scalar(0));
+
+bool firstFrame=true;
+
+// calculate euclidean distance of two points
+  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)
+{
+    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)
+}
+*/
+
+/*
+
+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)
+{
+
+
+
+    // 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);
+
+        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";
+    
+    }
+
+   // 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++)
+        cout<<*it<<" ,";
+    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 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)};
+
+
+
+    // 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";
+
+    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
+
+
+   // 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);
+  clustermsg->header.frame_id = "/map";
+  clustermsg->header.stamp = ros::Time::now();
+  pub.publish (*clustermsg);
+
+}
+
+
+void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
+
+{
+    cout<<"IF firstFrame="<<firstFrame<<"\n";
+    // If this is the first frame, initialize kalman filters for the clustered objects
+if (firstFrame)
+{   
+  // 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);
+    cv::setIdentity(KF2.measurementMatrix);
+    cv::setIdentity(KF3.measurementMatrix);
+    cv::setIdentity(KF4.measurementMatrix);
+    cv::setIdentity(KF5.measurementMatrix);
+    // Process Noise Covariance Matrix Q
+    // [ Ex 0  0    0 0    0 ]
+    // [ 0  Ey 0    0 0    0 ]
+    // [ 0  0  Ev_x 0 0    0 ]
+    // [ 0  0  0    1 Ev_y 0 ]
+    //// [ 0  0  0    0 1    Ew ]
+    //// [ 0  0  0    0 0    Eh ]
+    setIdentity(KF0.processNoiseCov, Scalar::all(1e-4));
+    setIdentity(KF1.processNoiseCov, Scalar::all(1e-4));
+    setIdentity(KF2.processNoiseCov, Scalar::all(1e-4));
+    setIdentity(KF3.processNoiseCov, Scalar::all(1e-4));
+    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));
+
+// Process the point cloud
+     pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+      /* Creating the KdTree from input point cloud*/
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+
+  pcl::fromROSMsg (*input, *input_cloud);
+
+  tree->setInputCloud (input_cloud);
+
+  /* Here we are creating a vector of PointIndices, which contains the actual index
+   * information in a vector<int>. The indices of each detected cluster are saved here.
+   * Cluster_indices is a vector containing one instance of PointIndices for each detected 
+   * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
+   */
+  std::vector<pcl::PointIndices> cluster_indices;
+  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+  ec.setClusterTolerance (0.08); 
+  ec.setMinClusterSize (10);
+  ec.setMaxClusterSize (600);
+  ec.setSearchMethod (tree);
+  ec.setInputCloud (input_cloud);
+  /* Extract the clusters out of pc and save indices in cluster_indices.*/
+  ec.extract (cluster_indices);
+
+  /* To separate each cluster out of the vector<PointIndices> we have to 
+   * iterate through cluster_indices, create a new PointCloud for each 
+   * entry and write all points of the current cluster in the PointCloud. 
+   */
+  //pcl::PointXYZ origin (0,0,0);
+  //float mindist_this_cluster = 1000;
+  //float dist_this_point = 1000;
+
+  std::vector<pcl::PointIndices>::const_iterator it;
+  std::vector<int>::const_iterator pit;
+  // Vector of cluster pointclouds
+  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
+
+  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
+          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
+          for(pit = it->indices.begin(); pit != it->indices.end(); pit++) 
+          {
+          
+                  cloud_cluster->points.push_back(input_cloud->points[*pit]);
+                  //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
+                  //                                          origin);
+                  //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster);
+          }
+
+         
+      cluster_vec.push_back(cloud_cluster);
+
+    }
+
+    //Ensure at least 6 clusters exist to publish (later clusters may be empty)
+    while (cluster_vec.size() < 6){
+      pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
+      empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
+      cluster_vec.push_back(empty_cluster);
+    }
+    
+
+     // Set initial state
+     KF0.statePre.at<float>(0)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].x;
+     
+     KF0.statePre.at<float>(1)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].y;
+     KF0.statePre.at<float>(2)=0;// initial v_x
+     KF0.statePre.at<float>(3)=0;//initial v_y
+
+     // Set initial state
+     KF1.statePre.at<float>(0)=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
+     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
+
+     firstFrame=false;
+
+     // Print the initial state of the kalman filter for debugging
+     cout<<"KF0.satePre="<<KF0.statePre.at<float>(0)<<","<<KF0.statePre.at<float>(1)<<"\n";
+     cout<<"KF1.satePre="<<KF1.statePre.at<float>(0)<<","<<KF1.statePre.at<float>(1)<<"\n";
+     cout<<"KF2.satePre="<<KF2.statePre.at<float>(0)<<","<<KF2.statePre.at<float>(1)<<"\n";
+     cout<<"KF3.satePre="<<KF3.statePre.at<float>(0)<<","<<KF3.statePre.at<float>(1)<<"\n";
+     cout<<"KF4.satePre="<<KF4.statePre.at<float>(0)<<","<<KF4.statePre.at<float>(1)<<"\n";
+     cout<<"KF5.satePre="<<KF5.statePre.at<float>(0)<<","<<KF5.statePre.at<float>(1)<<"\n";
+
+     //cin.ignore();// To be able to see the printed initial state of the KalmanFilter
+
+
+
+     /* 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);
+
+    }
+    //cout<<"cluster_vec got some clusters\n";
+
+    //Ensure at least 6 clusters exist to publish (later clusters may be empty)
+    while (cluster_vec.size() < 6){
+      pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
+      empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
+      cluster_vec.push_back(empty_cluster);
+    }
+    std_msgs::Float32MultiArray cc;
+    for(int i=0;i<6;i++)
+    {
+        cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].x);
+        cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].y);
+        cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].z);
+
+    }
+    cout<<"6 clusters initialized\n";
+
+    //cc_pos.publish(cc);// Publish cluster mid-points.
+    KFT(cc);
+    int i=0;
+    bool publishedCluster[6];
+    for(auto it=objID.begin();it!=objID.end();it++)
+        { cout<<"Inside the for loop\n";
+            
+            switch(*it)
+            {
+                cout<<"Inside the switch case\n";
+                case 0: { 
+                            publish_cloud(pub_cluster0,cluster_vec[i]);
+                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
+                            i++;
+                            break;
+
+                        }
+                case 1: {
+                            publish_cloud(pub_cluster1,cluster_vec[i]);
+                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
+                            i++;
+                            break;
+
+                        }
+                case 2: {
+                            publish_cloud(pub_cluster2,cluster_vec[i]);
+                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
+                            i++;
+                            break;
+
+                        }
+                case 3: {
+                            publish_cloud(pub_cluster3,cluster_vec[i]);
+                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
+                            i++;
+                            break;
+
+                        }
+                case 4: {
+                            publish_cloud(pub_cluster4,cluster_vec[i]);
+                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
+                            i++;
+                            break;
+
+                        }
+
+                case 5: {
+                            publish_cloud(pub_cluster5,cluster_vec[i]);
+                            publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
+                            i++;
+                            break;
+
+                        }
+                default: break;
+            }
+    
+        }
+
+} 
+
+}   
+
+
+  
+
+int main(int argc, char** argv)
+{
+    // ROS init
+    ros::init (argc,argv,"KFTracker");
+    ros::NodeHandle nh;
+
+   
+    // Publishers to publish the state of the objects (pos and vel)
+    //objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
+
+
+
+cout<<"About to setup callback\n";
+
+// Create a ROS subscriber for the input point cloud
+  ros::Subscriber sub = nh.subscribe ("filtered_cloud", 1, cloud_cb);
+  // Create a ROS publisher for the output point cloud
+  pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_0", 1);
+  pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_1", 1);
+  pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_2", 1);
+  pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_3", 1);
+  pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_4", 1);
+  pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_5", 1);
+      // Subscribe to the clustered pointclouds
+    //ros::Subscriber c1=nh.subscribe("ccs",100,KFT); 
+    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();
+
+
+}