main_naive.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <string.h>
3 #include <fstream>
4 #include <algorithm>
5 #include <iterator>
8 #include <opencv2/core/core.hpp>
9 #include <opencv2/highgui/highgui.hpp>
10 #include <opencv2/imgproc/imgproc.hpp>
11 #include <opencv2/video/video.hpp>
12 #include "opencv2/video/tracking.hpp"
13 #include <ros/ros.h>
14 #include <pcl/io/pcd_io.h>
15 #include <pcl/point_types.h>
16 #include "pcl_ros/point_cloud.h"
17 #include <geometry_msgs/Point.h>
18 #include <std_msgs/Float32MultiArray.h>
19 #include <std_msgs/Int32MultiArray.h>
20 
21 #include <sensor_msgs/PointCloud2.h>
23 #include <pcl/point_cloud.h>
24 #include <pcl/point_types.h>
25 #include <pcl/common/geometry.h>
26 #include <pcl/filters/extract_indices.h>
27 #include <pcl/filters/voxel_grid.h>
28 #include <pcl/features/normal_3d.h>
29 #include <pcl/kdtree/kdtree.h>
30 #include <pcl/sample_consensus/method_types.h>
31 #include <pcl/sample_consensus/model_types.h>
32 #include <pcl/segmentation/sac_segmentation.h>
33 #include <pcl/segmentation/extract_clusters.h>
34 
35 using namespace std;
36 using namespace cv;
37 
38 
40 
41  // KF init
42  int stateDim=4;// [x,y,v_x,v_y]//,w,h]
43  int measDim=2;// [z_x,z_y,z_w,z_h]
44  int ctrlDim=0;
45  cv::KalmanFilter KF0(stateDim,measDim,ctrlDim,CV_32F);
46  cv::KalmanFilter KF1(stateDim,measDim,ctrlDim,CV_32F);
47  cv::KalmanFilter KF2(stateDim,measDim,ctrlDim,CV_32F);
48  cv::KalmanFilter KF3(stateDim,measDim,ctrlDim,CV_32F);
49  cv::KalmanFilter KF4(stateDim,measDim,ctrlDim,CV_32F);
50  cv::KalmanFilter KF5(stateDim,measDim,ctrlDim,CV_32F);
51 
58 
59  std::vector<geometry_msgs::Point> prevClusterCenters;
60 
61 
62  cv::Mat state(stateDim,1,CV_32F);
63  cv::Mat_<float> measurement(2,1);
64 
65  std::vector<int> objID;// Output of the data association using KF
66  // measurement.setTo(Scalar(0));
67 
68 bool firstFrame=true;
69 
70 // calculate euclidean distance of two points
71  double euclidean_distance(geometry_msgs::Point& p1, geometry_msgs::Point& p2)
72  {
73  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));
74  }
75 /*
76 //Count unique object IDs. just to make sure same ID has not been assigned to two KF_Trackers.
77 int countIDs(vector<int> v)
78 {
79  transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = distance(v.end(), v.begin())
80  sort(v.begin(), v.end()); // Average case O(n log n), worst case O(n^2) (usually implemented as quicksort.
81  // To guarantee worst case O(n log n) replace with make_heap, then sort_heap.
82 
83  // Unique will take a sorted range, and move things around to get duplicated
84  // items to the back and returns an iterator to the end of the unique section of the range
85  auto unique_end = unique(v.begin(), v.end()); // Again n comparisons
86  return distance(unique_end, v.begin()); // Constant time for random access iterators (like vector's)
87 }
88 */
89 
90 /*
91 
92 objID: vector containing the IDs of the clusters that should be associated with each KF_Tracker
93 objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
94 */
95 void KFT(const std_msgs::Float32MultiArray ccs)
96 {
97 
98 
99 
100  // First predict, to update the internal statePre variable
101 
102  std::vector<cv::Mat> pred{KF0.predict(),KF1.predict(),KF2.predict(),KF3.predict(),KF4.predict(),KF5.predict()};
103  //cout<<"Pred successfull\n";
104 
105  //cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
106  // cout<<"Prediction 1 ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
107 
108  // Get measurements
109  // Extract the position of the clusters forom the multiArray. To check if the data
110  // coming in, check the .z (every third) coordinate and that will be 0.0
111  std::vector<geometry_msgs::Point> clusterCenters;//clusterCenters
112 
113  int i=0;
114  for (std::vector<float>::const_iterator it=ccs.data.begin();it!=ccs.data.end();it+=3)
115  {
116  geometry_msgs::Point pt;
117  pt.x=*it;
118  pt.y=*(it+1);
119  pt.z=*(it+2);
120 
121  clusterCenters.push_back(pt);
122 
123  }
124 
125  // cout<<"CLusterCenters Obtained"<<"\n";
126  std::vector<geometry_msgs::Point> KFpredictions;
127  i=0;
128  for (auto it=pred.begin();it!=pred.end();it++)
129  {
130  geometry_msgs::Point pt;
131  pt.x=(*it).at<float>(0);
132  pt.y=(*it).at<float>(1);
133  pt.z=(*it).at<float>(2);
134 
135  KFpredictions.push_back(pt);
136 
137  }
138  // cout<<"Got predictions"<<"\n";
139 
140  /* Original Version using Kalman filter prediction
141 
142  // Find the cluster that is more probable to be belonging to a given KF.
143  objID.clear();//Clear the objID vector
144  for(int filterN=0;filterN<6;filterN++)
145  {
146  std::vector<float> distVec;
147  for(int n=0;n<6;n++)
148  distVec.push_back(euclidean_distance(KFpredictions[filterN],clusterCenters[n]));
149 
150  // cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n";
151  objID.push_back(std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())));
152  // cout<<"MinD for filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
153 
154  }
155 
156  // cout<<"Got object IDs"<<"\n";
157  //countIDs(objID);// for verif/corner cases
158  Original version using kalman filter prediction
159  */
160  //display objIDs
161  /* DEBUG
162  cout<<"objID= ";
163  for(auto it=objID.begin();it!=objID.end();it++)
164  cout<<*it<<" ,";
165  cout<<"\n";
166  */
167 
168 /* Naive version without using kalman filter */
169 objID.clear();//Clear the objID vector
170  for(int filterN=0;filterN<6;filterN++)
171  {
172  std::vector<float> distVec;
173  for(int n=0;n<6;n++)
174  distVec.push_back(euclidean_distance(prevClusterCenters[n],clusterCenters[n]));
175 
176  // cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n";
177  objID.push_back(std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())));
178  // cout<<"MinD for filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
179 
180  }
181  /* Naive version without kalman filter */
182  //prevClusterCenters.clear();
183  // Set the current associated clusters to the prevClusterCenters
184  for (int i=0;i<6;i++)
185  {
186  prevClusterCenters[objID.at(i)]=clusterCenters.at(i);
187 
188  }
189 
190  /* Naive version without kalman filter */
191 
192 /* Naive version without using kalman filter */
193 
194 
195  std_msgs::Int32MultiArray obj_id;
196  for(auto it=objID.begin();it!=objID.end();it++)
197  obj_id.data.push_back(*it);
198  objID_pub.publish(obj_id);
199  // convert clusterCenters from geometry_msgs::Point to floats
200  std::vector<std::vector<float> > cc;
201  for (int i=0;i<clusterCenters.size();i++)
202  {
203  vector<float> pt;
204  pt.push_back(clusterCenters[i].x);
205  pt.push_back(clusterCenters[i].y);
206  pt.push_back(clusterCenters[i].z);
207 
208  cc.push_back(pt);
209  }
210  //cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
211  float meas0[3]={cc[0].at(0),cc[0].at(1)};
212  float meas1[3]={cc[1].at(0),cc[1].at(1)};
213  float meas2[3]={cc[2].at(0),cc[2].at(1)};
214  float meas3[3]={cc[3].at(0),cc[3].at(1)};
215  float meas4[3]={cc[4].at(0),cc[4].at(1)};
216  float meas5[3]={cc[5].at(0),cc[5].at(1)};
217 
218 
219 
220  // The update phase
221  cv::Mat meas0Mat=cv::Mat(2,1,CV_32F,meas0);
222  cv::Mat meas1Mat=cv::Mat(2,1,CV_32F,meas1);
223  cv::Mat meas2Mat=cv::Mat(2,1,CV_32F,meas2);
224  cv::Mat meas3Mat=cv::Mat(2,1,CV_32F,meas3);
225  cv::Mat meas4Mat=cv::Mat(2,1,CV_32F,meas4);
226  cv::Mat meas5Mat=cv::Mat(2,1,CV_32F,meas5);
227 
228 //cout<<"meas0Mat"<<meas0Mat<<"\n";
229 
230  Mat estimated0 = KF0.correct(meas0Mat);
231  Mat estimated1 = KF0.correct(meas1Mat);
232  Mat estimated2 = KF0.correct(meas2Mat);
233  Mat estimated3 = KF0.correct(meas3Mat);
234  Mat estimated4 = KF0.correct(meas4Mat);
235  Mat estimated5 = KF0.correct(meas5Mat);
236 
237  // Publish the point clouds belonging to each clusters
238 
239 
240  // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
241  // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
242 //cout<<"DONE KF_TRACKER\n";
243 
244 }
245 void publish_cloud(ros::Publisher& pub, pcl::PointCloud<pcl::PointXYZ>::Ptr cluster){
246  sensor_msgs::PointCloud2::Ptr clustermsg (new sensor_msgs::PointCloud2);
247  pcl::toROSMsg (*cluster , *clustermsg);
248  clustermsg->header.frame_id = "/map";
249  clustermsg->header.stamp = ros::Time::now();
250  pub.publish (*clustermsg);
251 
252 }
253 
254 
255 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
256 
257 {
258  cout<<"IF firstFrame="<<firstFrame<<"\n";
259  // If this is the first frame, initialize kalman filters for the clustered objects
260 if (firstFrame)
261 {
262  // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
263  // Could be made generic by creating a Kalman Filter only when a new object is detected
264  KF0.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
265  KF1.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
266  KF2.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
267  KF3.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
268  KF4.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
269  KF5.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
270 
271  cv::setIdentity(KF0.measurementMatrix);
272  cv::setIdentity(KF1.measurementMatrix);
273  cv::setIdentity(KF2.measurementMatrix);
274  cv::setIdentity(KF3.measurementMatrix);
275  cv::setIdentity(KF4.measurementMatrix);
276  cv::setIdentity(KF5.measurementMatrix);
277  // Process Noise Covariance Matrix Q
278  // [ Ex 0 0 0 0 0 ]
279  // [ 0 Ey 0 0 0 0 ]
280  // [ 0 0 Ev_x 0 0 0 ]
281  // [ 0 0 0 1 Ev_y 0 ]
284  setIdentity(KF0.processNoiseCov, Scalar::all(1e-4));
285  setIdentity(KF1.processNoiseCov, Scalar::all(1e-4));
286  setIdentity(KF2.processNoiseCov, Scalar::all(1e-4));
287  setIdentity(KF3.processNoiseCov, Scalar::all(1e-4));
288  setIdentity(KF4.processNoiseCov, Scalar::all(1e-4));
289  setIdentity(KF5.processNoiseCov, Scalar::all(1e-4));
290  // Meas noise cov matrix R
291  cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(1e-1));
292  cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(1e-1));
293  cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(1e-1));
294  cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(1e-1));
295  cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(1e-1));
296  cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1));
297 
298 // Process the point cloud
299  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
300  pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
301  /* Creating the KdTree from input point cloud*/
302  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
303 
304  pcl::fromROSMsg (*input, *input_cloud);
305 
306  tree->setInputCloud (input_cloud);
307 
308  /* Here we are creating a vector of PointIndices, which contains the actual index
309  * information in a vector<int>. The indices of each detected cluster are saved here.
310  * Cluster_indices is a vector containing one instance of PointIndices for each detected
311  * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
312  */
313  std::vector<pcl::PointIndices> cluster_indices;
314  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
315  ec.setClusterTolerance (0.08);
316  ec.setMinClusterSize (10);
317  ec.setMaxClusterSize (600);
318  ec.setSearchMethod (tree);
319  ec.setInputCloud (input_cloud);
320  /* Extract the clusters out of pc and save indices in cluster_indices.*/
321  ec.extract (cluster_indices);
322 
323  /* To separate each cluster out of the vector<PointIndices> we have to
324  * iterate through cluster_indices, create a new PointCloud for each
325  * entry and write all points of the current cluster in the PointCloud.
326  */
327  //pcl::PointXYZ origin (0,0,0);
328  //float mindist_this_cluster = 1000;
329  //float dist_this_point = 1000;
330 
331  std::vector<pcl::PointIndices>::const_iterator it;
332  std::vector<int>::const_iterator pit;
333  // Vector of cluster pointclouds
334  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
335 
336  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
337  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
338  for(pit = it->indices.begin(); pit != it->indices.end(); pit++)
339  {
340 
341  cloud_cluster->points.push_back(input_cloud->points[*pit]);
342  //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
343  // origin);
344  //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster);
345  }
346 
347 
348  cluster_vec.push_back(cloud_cluster);
349 
350  }
351 
352  //Ensure at least 6 clusters exist to publish (later clusters may be empty)
353  while (cluster_vec.size() < 6){
354  pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
355  empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
356  cluster_vec.push_back(empty_cluster);
357  }
358 
359 
360  // Set initial state
361  KF0.statePre.at<float>(0)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].x;
362 
363  KF0.statePre.at<float>(1)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].y;
364  KF0.statePre.at<float>(2)=0;// initial v_x
365  KF0.statePre.at<float>(3)=0;//initial v_y
366 
367  // Set initial state
368  KF1.statePre.at<float>(0)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].x;
369  KF1.statePre.at<float>(1)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].y;
370  KF1.statePre.at<float>(2)=0;// initial v_x
371  KF1.statePre.at<float>(3)=0;//initial v_y
372 
373  // Set initial state
374  KF2.statePre.at<float>(0)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].x;
375  KF2.statePre.at<float>(1)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].y;
376  KF2.statePre.at<float>(2)=0;// initial v_x
377  KF2.statePre.at<float>(3)=0;//initial v_y
378 
379 
380  // Set initial state
381  KF3.statePre.at<float>(0)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].x;
382  KF3.statePre.at<float>(1)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].y;
383  KF3.statePre.at<float>(2)=0;// initial v_x
384  KF3.statePre.at<float>(3)=0;//initial v_y
385 
386  // Set initial state
387  KF4.statePre.at<float>(0)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].x;
388  KF4.statePre.at<float>(1)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].y;
389  KF4.statePre.at<float>(2)=0;// initial v_x
390  KF4.statePre.at<float>(3)=0;//initial v_y
391 
392  // Set initial state
393  KF5.statePre.at<float>(0)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].x;
394  KF5.statePre.at<float>(1)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].y;
395  KF5.statePre.at<float>(2)=0;// initial v_x
396  KF5.statePre.at<float>(3)=0;//initial v_y
397 
398  firstFrame=false;
399 
400  // Print the initial state of the kalman filter for debugging
401  cout<<"KF0.satePre="<<KF0.statePre.at<float>(0)<<","<<KF0.statePre.at<float>(1)<<"\n";
402  cout<<"KF1.satePre="<<KF1.statePre.at<float>(0)<<","<<KF1.statePre.at<float>(1)<<"\n";
403  cout<<"KF2.satePre="<<KF2.statePre.at<float>(0)<<","<<KF2.statePre.at<float>(1)<<"\n";
404  cout<<"KF3.satePre="<<KF3.statePre.at<float>(0)<<","<<KF3.statePre.at<float>(1)<<"\n";
405  cout<<"KF4.satePre="<<KF4.statePre.at<float>(0)<<","<<KF4.statePre.at<float>(1)<<"\n";
406  cout<<"KF5.satePre="<<KF5.statePre.at<float>(0)<<","<<KF5.statePre.at<float>(1)<<"\n";
407 
408  //cin.ignore();// To be able to see the printed initial state of the KalmanFilter
409 
410 
411 
412  /* Naive version without kalman filter */
413  for (int i=0;i<6;i++)
414  {
415  geometry_msgs::Point pt;
416  pt.x=cluster_vec[i]->points[cluster_vec[i]->points.size()/2].x;
417  pt.y=cluster_vec[i]->points[cluster_vec[i]->points.size()/2].y;
418  prevClusterCenters.push_back(pt);
419  }
420 
421  /* Naive version without kalman filter */
422 }
423 
424 
425 else
426 {
427  cout<<"ELSE firstFrame="<<firstFrame<<"\n";
428  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
429  pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
430  /* Creating the KdTree from input point cloud*/
431  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
432 
433  pcl::fromROSMsg (*input, *input_cloud);
434 
435  tree->setInputCloud (input_cloud);
436 
437  /* Here we are creating a vector of PointIndices, which contains the actual index
438  * information in a vector<int>. The indices of each detected cluster are saved here.
439  * Cluster_indices is a vector containing one instance of PointIndices for each detected
440  * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
441  */
442  std::vector<pcl::PointIndices> cluster_indices;
443  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
444  ec.setClusterTolerance (0.08);
445  ec.setMinClusterSize (10);
446  ec.setMaxClusterSize (600);
447  ec.setSearchMethod (tree);
448  ec.setInputCloud (input_cloud);
449 cout<<"PCL init successfull\n";
450  /* Extract the clusters out of pc and save indices in cluster_indices.*/
451  ec.extract (cluster_indices);
452 cout<<"PCL extract successfull\n";
453  /* To separate each cluster out of the vector<PointIndices> we have to
454  * iterate through cluster_indices, create a new PointCloud for each
455  * entry and write all points of the current cluster in the PointCloud.
456  */
457  //pcl::PointXYZ origin (0,0,0);
458  //float mindist_this_cluster = 1000;
459  //float dist_this_point = 1000;
460 
461  std::vector<pcl::PointIndices>::const_iterator it;
462  std::vector<int>::const_iterator pit;
463  // Vector of cluster pointclouds
464  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cluster_vec;
465 
466  for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
467  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
468  for(pit = it->indices.begin(); pit != it->indices.end(); pit++)
469  {
470 
471  cloud_cluster->points.push_back(input_cloud->points[*pit]);
472  //dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
473  // origin);
474  //mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster);
475  }
476 
477 
478  cluster_vec.push_back(cloud_cluster);
479 
480  }
481  //cout<<"cluster_vec got some clusters\n";
482 
483  //Ensure at least 6 clusters exist to publish (later clusters may be empty)
484  while (cluster_vec.size() < 6){
485  pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>);
486  empty_cluster->points.push_back(pcl::PointXYZ(0,0,0));
487  cluster_vec.push_back(empty_cluster);
488  }
489  std_msgs::Float32MultiArray cc;
490  for(int i=0;i<6;i++)
491  {
492  cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].x);
493  cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].y);
494  cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].z);
495 
496  }
497  cout<<"6 clusters initialized\n";
498 
499  //cc_pos.publish(cc);// Publish cluster mid-points.
500  KFT(cc);
501  int i=0;
502  bool publishedCluster[6];
503  for(auto it=objID.begin();it!=objID.end();it++)
504  { cout<<"Inside the for loop\n";
505 
506  switch(*it)
507  {
508  cout<<"Inside the switch case\n";
509  case 0: {
510  publish_cloud(pub_cluster0,cluster_vec[i]);
511  publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
512  i++;
513  break;
514 
515  }
516  case 1: {
517  publish_cloud(pub_cluster1,cluster_vec[i]);
518  publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
519  i++;
520  break;
521 
522  }
523  case 2: {
524  publish_cloud(pub_cluster2,cluster_vec[i]);
525  publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
526  i++;
527  break;
528 
529  }
530  case 3: {
531  publish_cloud(pub_cluster3,cluster_vec[i]);
532  publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
533  i++;
534  break;
535 
536  }
537  case 4: {
538  publish_cloud(pub_cluster4,cluster_vec[i]);
539  publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
540  i++;
541  break;
542 
543  }
544 
545  case 5: {
546  publish_cloud(pub_cluster5,cluster_vec[i]);
547  publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID
548  i++;
549  break;
550 
551  }
552  default: break;
553  }
554 
555  }
556 
557 }
558 
559 }
560 
561 
562 
563 
564 int main(int argc, char** argv)
565 {
566  // ROS init
567  ros::init (argc,argv,"KFTracker");
568  ros::NodeHandle nh;
569 
570 
571  // Publishers to publish the state of the objects (pos and vel)
572  //objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
573 
574 
575 
576 cout<<"About to setup callback\n";
577 
578 // Create a ROS subscriber for the input point cloud
579  ros::Subscriber sub = nh.subscribe ("filtered_cloud", 1, cloud_cb);
580  // Create a ROS publisher for the output point cloud
581  pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_0", 1);
582  pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_1", 1);
583  pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_2", 1);
584  pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_3", 1);
585  pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_4", 1);
586  pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2> ("cluster_5", 1);
587  // Subscribe to the clustered pointclouds
588  //ros::Subscriber c1=nh.subscribe("ccs",100,KFT);
589  objID_pub = nh.advertise<std_msgs::Int32MultiArray>("obj_id", 1);
590 /* Point cloud clustering
591 */
592 
593  //cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
594 
595 
596 /* Point cloud clustering
597 */
598 
599 
600  ros::spin();
601 
602 
603 }
void setIdentity(geometry_msgs::Transform &tx)
cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F)
cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int main(int argc, char **argv)
Definition: main_naive.cpp:564
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::vector< int > objID
Definition: main_naive.cpp:65
ros::Publisher pub_cluster2
Definition: main_naive.cpp:54
void KFT(const std_msgs::Float32MultiArray ccs)
Definition: main_naive.cpp:95
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROSCPP_DECL void spin(Spinner &spinner)
cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F)
cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F)
ros::Publisher objID_pub
Definition: main_naive.cpp:39
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
Definition: main_naive.cpp:255
cv::Mat_< float > measurement(2, 1)
TFSIMD_FORCE_INLINE const tfScalar & x() const
cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
cv::Mat state(stateDim, 1, CV_32F)
TFSIMD_FORCE_INLINE const tfScalar & z() const
std::vector< geometry_msgs::Point > prevClusterCenters
Definition: main_naive.cpp:59
int stateDim
Definition: main_naive.cpp:42
int ctrlDim
Definition: main_naive.cpp:44
ros::Publisher pub_cluster4
Definition: main_naive.cpp:56
void publish_cloud(ros::Publisher &pub, pcl::PointCloud< pcl::PointXYZ >::Ptr cluster)
Definition: main_naive.cpp:245
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
static Time now()
ros::Publisher pub_cluster1
Definition: main_naive.cpp:53
cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F)
ros::Publisher pub_cluster5
Definition: main_naive.cpp:57
double euclidean_distance(geometry_msgs::Point &p1, geometry_msgs::Point &p2)
Definition: main_naive.cpp:71
ros::Publisher pub_cluster3
Definition: main_naive.cpp:55
int measDim
Definition: main_naive.cpp:43
ros::Publisher pub_cluster0
Definition: main_naive.cpp:52
bool firstFrame
Definition: main_naive.cpp:68


multi_object_tracking_lidar
Author(s): Praveen Palanisamy
autogenerated on Sat Jul 18 2020 03:29:17