├── CMakeLists.txt
├── config
    └── param.yaml
├── include
    └── hungarian.hpp
├── launch
    └── pointcloud2_cluster_tracking.launch
├── package.xml
└── src
    ├── hungarian.cpp
    └── tracking.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(pointcloud2_cluster_tracking)
 3 | 
 4 | add_compile_options(-std=c++14)
 5 | 
 6 | find_package(catkin REQUIRED COMPONENTS
 7 |   roboskel_msgs
 8 |   sensor_msgs
 9 |   std_msgs
10 |   pcl_ros
11 |   roscpp
12 |   tf
13 | )
14 | 
15 | find_package(PCL 1.8.1 REQUIRED)
16 | include_directories(${PCL_INCLUDE_DIRS})
17 | link_directories(${PCL_LIBRARY_DIRS})
18 | add_definitions(${PCL_DEFINITIONS})
19 | 
20 | catkin_package()
21 | include_directories(
22 |     include
23 |     ${catkin_INCLUDE_DIRS}
24 | )
25 | 
26 | add_executable(pointcloud2_cluster_tracking src/tracking.cpp src/hungarian.cpp)
27 | target_link_libraries(pointcloud2_cluster_tracking ${catkin_LIBRARIES} ${PCL_LIBRARIES})
28 | 


--------------------------------------------------------------------------------
/config/param.yaml:
--------------------------------------------------------------------------------
 1 | input_topic: pointcloud2_clustering/clusters
 2 | out_topic: pointcloud2_cluster_tracking/clusters
 3 | marker_topic: visualization_marker
 4 | marker_frame_id: "/base_link"
 5 | 
 6 | # method: 2 #1 if you want to delete overlap from clusters and 2 if you do not want to delete the overlap from clusters
 7 | 
 8 | size: 2
 9 | overlap: 0.2
10 | marker_flag: true # (bool)marker_flag=true : markers for centroids activated
11 | maxHungDist: 700 
12 | trackTheUntracked: true
13 | minMotionDist: 1000.0
14 | max_dist: 0.31 # when two clusters are closer from max_dist (meters)


--------------------------------------------------------------------------------
/include/hungarian.hpp:
--------------------------------------------------------------------------------
 1 | /********************************************************************
 2 |  ********************************************************************
 3 |  **
 4 |  ** libhungarian by Cyrill Stachniss, 2004
 5 |  ** http://www2.informatik.uni-freiburg.de/~stachnis/misc.html
 6 |  **
 7 |  ** Modified and adapted from C to C++ by Justin Buchanan
 8 |  **
 9 |  ** Solving the Minimum Assignment Problem using the
10 |  ** Hungarian Method.
11 |  **
12 |  ** ** This file may be freely copied and distributed! **
13 |  **
14 |  ** Parts of the used code was originally provided by the
15 |  ** "Stanford GraphGase", but I made changes to this code.
16 |  ** As asked by  the copyright node of the "Stanford GraphGase",
17 |  ** I hereby proclaim that this file are *NOT* part of the
18 |  ** "Stanford GraphGase" distrubition!
19 |  **
20 |  ** This file is distributed in the hope that it will be useful,
21 |  ** but WITHOUT ANY WARRANTY; without even the implied
22 |  ** warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
23 |  ** PURPOSE.
24 |  **
25 |  ** https://github.com/RoboJackets/hungarian
26 |  **
27 |  ********************************************************************
28 |  ********************************************************************/
29 | 
30 | #pragma once
31 | 
32 | #include <vector>
33 | 
34 | namespace Hungarian {
35 | 
36 | typedef enum {
37 |   MODE_MINIMIZE_COST,
38 |   MODE_MAXIMIZE_UTIL,
39 | } MODE;
40 | 
41 | typedef enum {
42 |   NOT_ASSIGNED,
43 |   ASSIGNED,
44 | } ASSIGN;
45 | 
46 | using Matrix = std::vector<std::vector<int>>;
47 | 
48 | struct Result {
49 |   // True if the algorithm completed and found a solution.
50 |   bool success = false;
51 |   // The solution
52 |   Matrix assignment;
53 |   // A normalized form of the input cost matrix.
54 |   Matrix cost;
55 |   // The costs incurred by the assignment
56 |   int totalCost = 0;
57 | };
58 | 
59 | /**
60 |  * Runs the hungarian algorithm on the input cost matrix and returns a result
61 |  * containing the normalized (square) cost matrix and a solution if one was
62 |  * found.
63 |  */
64 | Result Solve(const Matrix &input, MODE mode);
65 | 
66 | void PrintMatrix(const Matrix &m);
67 | 
68 | };  // namespace Hungarian


--------------------------------------------------------------------------------
/launch/pointcloud2_cluster_tracking.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8" standalone="no" ?>
 2 | <launch>
 3 | 	<arg name="method" default="2"/>
 4 | 	<!-- Use method 1 if you want to delete overlap from clusters and 2 if you do not want to delete the overlap from clusters -->
 5 | 
 6 |     <node pkg="pointcloud2_cluster_tracking" type="pointcloud2_cluster_tracking" respawn="false" name="pointcloud2_cluster_tracking" output="screen">
 7 |         <rosparam file="$(find pointcloud2_cluster_tracking)/config/param.yaml" command="load" />
 8 |         <rosparam param="method" subst_value="True">$(arg method)</rosparam>
 9 |     </node>
10 | </launch>


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>pointcloud2_cluster_tracking</name>
 4 |   <version>1.0.0</version>
 5 |   <description>The pointcloud2_cluster_tracking package</description>
 6 | 
 7 |   <author email="cathrine.m.oik@gmail.com">Katerina Oikonomou</author>
 8 | 
 9 |   <maintainer email="cathrine.m.oik@gmail.com">Katerina Oikonomou</maintainer>
10 |   <maintainer email="gstavrinos@iit.demokritos.gr">George Stavrinos</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 |   <buildtool_depend>catkin</buildtool_depend>
19 |   <depend>roboskel_msgs</depend>
20 |   <depend>sensor_msgs</depend>
21 |   <depend>std_msgs</depend>
22 |   <depend>pcl_ros</depend>
23 |   <depend>roscpp</depend>
24 |   <depend>tf</depend>
25 | 
26 |   <export>
27 |   </export>
28 | </package>
29 | 


--------------------------------------------------------------------------------
/src/hungarian.cpp:
--------------------------------------------------------------------------------
  1 | /********************************************************************
  2 |  ********************************************************************
  3 |  **
  4 |  ** libhungarian by Cyrill Stachniss, 2004
  5 |  ** http://www2.informatik.uni-freiburg.de/~stachnis/misc.html
  6 |  **
  7 |  ** Modified and adapted from C to C++ by Justin Buchanan
  8 |  **
  9 |  ** Solving the Minimum Assignment Problem using the
 10 |  ** Hungarian Method.
 11 |  **
 12 |  ** ** This file may be freely copied and distributed! **
 13 |  **
 14 |  ** Parts of the used code was originally provided by the
 15 |  ** "Stanford GraphGase", but I made changes to this code.
 16 |  ** As asked by  the copyright node of the "Stanford GraphGase",
 17 |  ** I hereby proclaim that this file are *NOT* part of the
 18 |  ** "Stanford GraphGase" distrubition!
 19 |  **
 20 |  ** This file is distributed in the hope that it will be useful,
 21 |  ** but WITHOUT ANY WARRANTY; without even the implied
 22 |  ** warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 23 |  ** PURPOSE.
 24 |  **
 25 |  ** https://github.com/RoboJackets/hungarian
 26 |  **
 27 |  ********************************************************************
 28 |  ********************************************************************/
 29 | 
 30 | #include "hungarian.hpp"
 31 | 
 32 | #include <algorithm>
 33 | #include <cstdio>
 34 | #include <iostream>
 35 | #include <limits>
 36 | 
 37 | namespace Hungarian {
 38 | 
 39 | namespace {
 40 | 
 41 | const bool verbose = false;
 42 | 
 43 | Matrix normalizeInput(const Matrix &input, MODE mode) {
 44 |   const int org_rows = input.size(), org_cols = input[0].size();
 45 | 
 46 |   // is the number of cols not equal to number of rows?
 47 |   // if yes, expand with 0-cols / 0-cols
 48 |   const int mrank = std::max(org_rows, org_cols);
 49 | 
 50 |   Matrix output;
 51 |   output.resize(mrank, std::vector<int>(mrank, 0));
 52 | 
 53 |   int max_cost = 0;
 54 |   for (int i = 0; i < org_rows; i++) {
 55 |     for (int j = 0; j < org_cols; j++) {
 56 |       output[i][j] = input[i][j];
 57 |       max_cost = std::max(max_cost, output[i][j]);
 58 |     }
 59 |   }
 60 | 
 61 |   if (mode == MODE_MAXIMIZE_UTIL) {
 62 |     for (int i = 0; i < org_rows; i++) {
 63 |       for (int j = 0; j < org_cols; j++) {
 64 |         output[i][j] = max_cost - output[i][j];
 65 |       }
 66 |     }
 67 |   }
 68 | 
 69 |   return output;
 70 | }
 71 | 
 72 | }  // namespace
 73 | 
 74 | Result Solve(const Matrix &input, MODE mode) {
 75 |   Result result;
 76 |   result.success = true;
 77 |   result.cost = normalizeInput(input, mode);
 78 | 
 79 |   const int INF = std::numeric_limits<int>::max();
 80 | 
 81 |   // mrank == rows == cols. it's a square matrix.
 82 |   const int mrank = result.cost.size();
 83 | 
 84 |   result.assignment.resize(mrank, std::vector<int>(mrank, NOT_ASSIGNED));
 85 | 
 86 |   int j, k, l, s, t, q, unmatched;
 87 | 
 88 |   int cost = 0;
 89 | 
 90 |   std::vector<int> col_mate = std::vector<int>(mrank, 0);
 91 |   std::vector<int> unchosen_row = std::vector<int>(mrank, 0);
 92 |   std::vector<int> row_dec = std::vector<int>(mrank, 0);
 93 |   std::vector<int> slack_row = std::vector<int>(mrank, 0);
 94 | 
 95 |   std::vector<int> row_mate = std::vector<int>(mrank, 0);
 96 |   std::vector<int> parent_row = std::vector<int>(mrank, 0);
 97 |   std::vector<int> col_inc = std::vector<int>(mrank, 0);
 98 |   std::vector<int> slack = std::vector<int>(mrank, 0);
 99 | 
100 |   for (int i = 0; i < mrank; ++i) {
101 |     for (int j = 0; j < mrank; ++j) {
102 |       result.assignment[i][j] = NOT_ASSIGNED;
103 |     }
104 |   }
105 | 
106 |   // Begin subtract column minima in order to start with lots of zeroes 12
107 |   if (verbose) fprintf(stderr, "Using heuristic\n");
108 |   for (l = 0; l < mrank; l++) {
109 |     s = result.cost[0][l];
110 |     for (k = 1; k < mrank; k++) {
111 |       if (result.cost[k][l] < s) {
112 |         s = result.cost[k][l];
113 |       }
114 |     }
115 |     cost += s;
116 |     if (s != 0) {
117 |       for (k = 0; k < mrank; k++) {
118 |         result.cost[k][l] -= s;
119 |       }
120 |     }
121 |   }
122 |   // End subtract column minima in order to start with lots of zeroes 12
123 | 
124 |   // Begin initial state 16
125 |   t = 0;
126 |   for (l = 0; l < mrank; l++) {
127 |     row_mate[l] = -1;
128 |     parent_row[l] = -1;
129 |     col_inc[l] = 0;
130 |     slack[l] = INF;
131 |   }
132 |   for (k = 0; k < mrank; k++) {
133 |     s = result.cost[k][0];
134 |     for (l = 1; l < mrank; l++) {
135 |       if (result.cost[k][l] < s) {
136 |         s = result.cost[k][l];
137 |       }
138 |     }
139 |     row_dec[k] = s;
140 |     for (l = 0; l < mrank; l++) {
141 |       if (s == result.cost[k][l] && row_mate[l] < 0) {
142 |         col_mate[k] = l;
143 |         row_mate[l] = k;
144 |         if (verbose) fprintf(stderr, "matching col %d==row %d\n", l, k);
145 |         goto row_done;
146 |       }
147 |     }
148 |     col_mate[k] = -1;
149 |     if (verbose) fprintf(stderr, "node %d: unmatched row %d\n", t, k);
150 |     unchosen_row[t++] = k;
151 |   row_done:;
152 |   }
153 |   // End initial state 16
154 | 
155 |   // Begin Hungarian algorithm 18
156 |   if (t == 0) goto done;
157 |   unmatched = t;
158 |   while (true) {
159 |     if (verbose) fprintf(stderr, "Matched %d rows.\n", mrank - t);
160 |     q = 0;
161 |     while (true) {
162 |       while (q < t) {
163 |         // Begin explore node q of the forest 19
164 |         {
165 |           k = unchosen_row[q];
166 |           s = row_dec[k];
167 |           for (l = 0; l < mrank; l++)
168 |             if (slack[l]) {
169 |               int del;
170 |               del = result.cost[k][l] - s + col_inc[l];
171 |               if (del < slack[l]) {
172 |                 if (del == 0) {
173 |                   if (row_mate[l] < 0) goto breakthru;
174 |                   slack[l] = 0;
175 |                   parent_row[l] = k;
176 |                   if (verbose)
177 |                     fprintf(stderr, "node %d: row %d==col %d--row %d\n", t,
178 |                             row_mate[l], l, k);
179 |                   unchosen_row[t++] = row_mate[l];
180 |                 } else {
181 |                   slack[l] = del;
182 |                   slack_row[l] = k;
183 |                 }
184 |               }
185 |             }
186 |         }
187 |         // End explore node q of the forest 19
188 |         q++;
189 |       }
190 | 
191 |       // Begin introduce a new zero into the matrix 21
192 |       s = INF;
193 |       for (l = 0; l < mrank; l++)
194 |         if (slack[l] && slack[l] < s) s = slack[l];
195 |       for (q = 0; q < t; q++) {
196 |         row_dec[unchosen_row[q]] += s;
197 |       }
198 |       for (l = 0; l < mrank; l++)
199 |         if (slack[l]) {
200 |           slack[l] -= s;
201 |           if (slack[l] == 0) {
202 |             // Begin look at a new zero 22
203 |             k = slack_row[l];
204 |             if (verbose)
205 |               fprintf(stderr,
206 |                       "Decreasing uncovered elements by %d produces zero at "
207 |                       "[%d,%d]\n",
208 |                       s, k, l);
209 |             if (row_mate[l] < 0) {
210 |               for (int j = l + 1; j < mrank; j++) {
211 |                 if (slack[j] == 0) {
212 |                   col_inc[j] += s;
213 |                 }
214 |               }
215 |               goto breakthru;
216 |             } else {
217 |               parent_row[l] = k;
218 |               if (verbose)
219 |                 fprintf(stderr, "node %d: row %d==col %d--row %d\n", t,
220 |                         row_mate[l], l, k);
221 |               unchosen_row[t++] = row_mate[l];
222 |             }
223 |             // End look at a new zero 22
224 |           }
225 |         } else {
226 |           col_inc[l] += s;
227 |         }
228 |       // End introduce a new zero into the matrix 21
229 |     }
230 |   breakthru:
231 |     // Begin update the matching 20
232 |     if (verbose) fprintf(stderr, "Breakthrough at node %d of %d!\n", q, t);
233 |     while (true) {
234 |       int j = col_mate[k];
235 |       col_mate[k] = l;
236 |       row_mate[l] = k;
237 |       if (verbose) fprintf(stderr, "rematching col %d==row %d\n", l, k);
238 |       if (j < 0) break;
239 |       k = parent_row[j];
240 |       l = j;
241 |     }
242 |     // End update the matching 20
243 |     if (--unmatched == 0) goto done;
244 |     // Begin get ready for another stage 17
245 |     t = 0;
246 |     for (l = 0; l < mrank; l++) {
247 |       parent_row[l] = -1;
248 |       slack[l] = INF;
249 |     }
250 |     for (k = 0; k < mrank; k++)
251 |       if (col_mate[k] < 0) {
252 |         if (verbose) fprintf(stderr, "node %d: unmatched row %d\n", t, k);
253 |         unchosen_row[t++] = k;
254 |       }
255 |     // End get ready for another stage 17
256 |   }
257 | done:
258 | 
259 |   // Begin doublecheck the solution 23
260 |   for (k = 0; k < mrank; k++) {
261 |     for (l = 0; l < mrank; l++) {
262 |       if (result.cost[k][l] < row_dec[k] - col_inc[l]) return {};
263 |     }
264 |   }
265 |   for (k = 0; k < mrank; k++) {
266 |     l = col_mate[k];
267 |     if (l < 0 || result.cost[k][l] != row_dec[k] - col_inc[l]) return {};
268 |   }
269 |   k = 0;
270 |   for (l = 0; l < mrank; l++) {
271 |     if (col_inc[l]) {
272 |       k++;
273 |     }
274 |   }
275 |   if (k > mrank) return {};
276 |   // End doublecheck the solution 23
277 |   // End Hungarian algorithm 18
278 | 
279 |   for (int i = 0; i < mrank; ++i) {
280 |     result.assignment[i][col_mate[i]] = ASSIGNED;
281 |     /*TRACE("%d - %d\n", i, col_mate[i]);*/
282 |   }
283 |   for (k = 0; k < mrank; ++k) {
284 |     for (l = 0; l < mrank; ++l) {
285 |       /*TRACE("%d ",result.cost[k][l]-row_dec[k]+col_inc[l]);*/
286 |       result.cost[k][l] = result.cost[k][l] - row_dec[k] + col_inc[l];
287 |     }
288 |     /*TRACE("\n");*/
289 |   }
290 |   for (int i = 0; i < mrank; i++) {
291 |     cost += row_dec[i];
292 |   }
293 |   for (int i = 0; i < mrank; i++) {
294 |     cost -= col_inc[i];
295 |   }
296 |   if (verbose) fprintf(stderr, "Cost is %d\n", cost);
297 | 
298 |   result.totalCost = cost;
299 |   return result;
300 | }
301 | 
302 | void PrintMatrix(const Matrix &m) {
303 |   const int rows = m.size(), cols = m[0].size();
304 |   fprintf(stderr, "\n");
305 |   for (int i = 0; i < rows; i++) {
306 |     fprintf(stderr, " [");
307 |     for (int j = 0; j < cols; j++) {
308 |       fprintf(stderr, "%5d ", m[i][j]);
309 |     }
310 |     fprintf(stderr, "]\n");
311 |   }
312 |   fprintf(stderr, "\n");
313 | }
314 | 
315 | }  // namespace Hungarian
316 | 


--------------------------------------------------------------------------------
/src/tracking.cpp:
--------------------------------------------------------------------------------
  1 | #include <cmath>
  2 | #include <ros/ros.h>
  3 | #include "hungarian.hpp"
  4 | #include <pcl/io/pcd_io.h>
  5 | #include <pcl/point_types.h>
  6 | #include <pcl/common/centroid.h>
  7 | #include <pcl_ros/point_cloud.h>
  8 | #include <tf/transform_listener.h>
  9 | #include <sensor_msgs/PointCloud.h>
 10 | #include <pcl/impl/point_types.hpp>
 11 | #include <sensor_msgs/PointCloud2.h>
 12 | #include <sensor_msgs/ChannelFloat32.h>
 13 | #include <pcl/filters/extract_indices.h>
 14 | #include <sensor_msgs/point_cloud_conversion.h>
 15 | #include <roboskel_msgs/PointCloud2_Segments.h>
 16 | #include <visualization_msgs/MarkerArray.h>
 17 | #include <visualization_msgs/Marker.h>
 18 | #include <algorithm>    // std::find
 19 | #include <vector>       // std::vector
 20 | 
 21 | 
 22 | bool first_time = true;
 23 | bool marker_flag, trackTheUntracked;
 24 | int maxHungDist;
 25 | double minMotionDist, max_dist;
 26 | 
 27 | visualization_msgs::Marker marker_sphere, marker_line;
 28 | 
 29 | std::vector<int> ids, idss, clusterInMotion;
 30 | std::vector<bool> prob_extinction,trackedOrnotIds;;
 31 | std::vector<Eigen::Vector4f> centroids;
 32 | 
 33 | 
 34 | 
 35 | std::vector<float> red = {0, 0, 1, 1, 1, 102.0/255, 102.0/255, 204.0/255, 0, 1};
 36 | std::vector<float> green = {0, 1.0, 0, 1, 1, 102.0/255, 102.0/255, 0, 1, 152.0/255};
 37 | std::vector<float> blue = {1.0, 0, 0, 0, 1, 152.0/255, 52.0/255, 152.0/255, 1, 52.0/255};
 38 | 
 39 | 
 40 | std::pair<double,double> minmaxz (sensor_msgs::PointCloud2 clust){  //return max and min z of cluster
 41 | 
 42 | 
 43 |     pcl::PCLPointCloud2 p2;
 44 |     pcl_conversions::toPCL ( clust , p2 ); //from sensor_msgs::pointcloud2 to pcl::pointcloud2
 45 | 
 46 |     pcl::PointCloud<pcl::PointXYZ> p3;
 47 |     pcl::fromPCLPointCloud2 ( p2 , p3 );       //from pcl::pointcloud2 to pcl::pointcloud
 48 | 
 49 |     double max_z = p3.points[0].z;
 50 |     double min_z = p3.points[0].z;
 51 |     for (int i=1; i < p3.points.size(); i++){   //find max and min z of cluster 
 52 |         if(p3.points[i].z > max_z){
 53 |             max_z = p3.points[i].z;
 54 |         }
 55 |         if(p3.points[i].z < min_z){
 56 |             min_z = p3.points[i].z;
 57 |         }
 58 |     }
 59 | 
 60 |     return std::make_pair( max_z , min_z );
 61 | }
 62 | 
 63 | pcl::PointCloud<pcl::PointXYZ> saveAllPoints(sensor_msgs::PointCloud2 clust){  //save all points of a cluster to pointcloud form
 64 | 
 65 | 
 66 |     pcl::PCLPointCloud2 pc2;
 67 |     pcl_conversions::toPCL ( clust , pc2 );   //from sensor_msgs::pointcloud2 to pcl::pointcloud2
 68 | 
 69 |     pcl::PointCloud<pcl::PointXYZ> pc;
 70 |     pcl::fromPCLPointCloud2 ( pc2 , pc );               //from pcl::pointcloud2 to pcl::pointcloud
 71 | 
 72 |     return pc;
 73 | }
 74 | 
 75 | pcl::PointCloud<pcl::PointXYZ> saveAllZValuePoints(sensor_msgs::PointCloud2 clust, double zvalue){  //save all points whose z is equal to zvalue
 76 | 
 77 |     pcl::PointCloud<pcl::PointXYZ> pcz3;
 78 |     pcl::PointCloud<pcl::PointXYZ> pc;
 79 |     pc=saveAllPoints(clust);
 80 | 
 81 |     for(int i=0; i < pc.points.size(); i++){        //add points with zvalue to a new pointcloud
 82 |         if(pc.points[i].z == zvalue){
 83 |             pcz3.push_back(pc.points[i]);
 84 |         }
 85 |     }
 86 | 
 87 |     return pcz3;
 88 | }
 89 | 
 90 | pcl::PointCloud<pcl::PointXYZ> saveAllZPointsFrom(sensor_msgs::PointCloud2 clust, double zFrom){   // save all points whose z is grater than or equal to zFrom
 91 | 
 92 |     pcl::PointCloud<pcl::PointXYZ> pcz3;
 93 |     pcl::PointCloud<pcl::PointXYZ> pc;
 94 |     pc=saveAllPoints(clust); 
 95 | 
 96 |     for(int i=0; i < pc.points.size(); i++){        //add points with max z to a new pointcloud
 97 |         if(pc.points[i].z >= zFrom  ){
 98 |             pcz3.push_back(pc.points[i]);
 99 |         }
100 |     }
101 | 
102 |     return pcz3;
103 | }
104 | 
105 | pcl::PointCloud<pcl::PointXYZ> saveAllZPointsUntil(sensor_msgs::PointCloud2 clust, double zUntil){   // save all points whose z is less than or equal to zUntil
106 | 
107 |     pcl::PointCloud<pcl::PointXYZ> pcz3;
108 |     pcl::PointCloud<pcl::PointXYZ> pc;
109 |     pc=saveAllPoints(clust);
110 | 
111 |     for(int i=0; i < pc.points.size(); i++){        //add points with max z to a new pointcloud
112 |         if(pc.points[i].z <= zUntil  ){
113 |             pcz3.push_back(pc.points[i]);
114 |         }
115 |     }
116 | 
117 |     return pcz3;
118 | }
119 | 
120 | 
121 | void trackUntrackedClusters(std::vector<Eigen::Vector4f> untracked_msg, roboskel_msgs::PointCloud2_Segments& msg, std::vector<Eigen::Vector4f> msg_centroid_vec, size_t size_new  ){ //try to track the untracked clusters with untracked centroids 
122 | 
123 |     std::vector<Eigen::Vector4f> untracked_centr;
124 | 
125 |     for (int i=0; i < ids.size(); i++) {
126 |         if (trackedOrnotIds[i] == false)
127 |             untracked_centr.push_back(centroids[i]);
128 |     }
129 | 
130 |     int centrs_size = untracked_centr.size();
131 |     int msg_size = untracked_msg.size();
132 | 
133 |     if (centrs_size != 0 && msg_size != 0 ) {
134 | 
135 |         std::vector<std::vector<int> > newdists(centrs_size, std::vector<int>(msg_size , 10000));// TODO currently, 10000 is the maximum (2d) int distance with a 10 meter laser scanner. Initial value represents a point connected to bottom.
136 | 
137 |         for(unsigned i=0; i < centrs_size; i++){
138 |             for(unsigned j=0; j < msg_size; j++){
139 |                 newdists[i][j] = 1000 * sqrt(pow(untracked_centr[i][0]-untracked_msg[j][0], 2) + pow(untracked_centr[i][1]-untracked_msg[j][1], 2));                 
140 |             }
141 |         }
142 | 
143 |         Hungarian::Result p = Hungarian::Solve(newdists, Hungarian::MODE_MINIMIZE_COST);
144 | 
145 |         // Hungarian::PrintMatrix(p.assignment);
146 | 
147 |         newdists = p.assignment;
148 | 
149 |         double distt;
150 |         for(unsigned j=0; j < msg_size; j++){
151 |             for(unsigned i=0; i < centrs_size; i++){
152 | 
153 |                 if (newdists[i][j] == 1){
154 | 
155 |                     distt = 1000 * sqrt(pow(untracked_centr[i][0]-untracked_msg[j][0], 2) + pow(untracked_centr[i][1]-untracked_msg[j][1], 2));
156 | 
157 |                     if (distt<maxHungDist) {
158 | 
159 |                         int temp_pos = -1;
160 | 
161 |                         for(int k=0; k < centroids.size(); k++) {
162 |                             if(untracked_centr[i] == centroids[k] && trackedOrnotIds[k] == false ){
163 | 
164 |                                 temp_pos = k;
165 |                                 trackedOrnotIds[k] = true;
166 |                                 break;
167 |                             }
168 |                         }          
169 |                         if(temp_pos != -1) {
170 | 
171 |                             for(int k=0; k < size_new; k++) {
172 |                                 if(untracked_msg[j] == msg_centroid_vec[k] && msg.cluster_id[k] == -1 ){
173 | 
174 |                                     msg.cluster_id[k] = ids[temp_pos];
175 |                                     centroids[temp_pos]=msg_centroid_vec[k];
176 |                                     break;
177 |                                 }
178 |                             }
179 |                         }
180 |                     }
181 |                     break; 
182 |                 }           
183 |             }
184 |         }
185 |     } 
186 | }
187 | 
188 | 
189 | void checkClustersDistance(roboskel_msgs::PointCloud2_Segments base_msg, size_t size_old, roboskel_msgs::PointCloud2_Segments& msg ){ // check if the distance between two clusters is less than max_dist in base_msg. if it is true check if one of two clusters missing from the msg
190 | 
191 |     std::vector<pcl::PointCloud<pcl::PointXYZ>> pcz(size_old); 
192 | 
193 |     double max_z;
194 | 
195 |     for(int l=0; l<clusterInMotion.size(); l++){
196 |         for(unsigned j=0; j < base_msg.clusters.size(); j++){
197 |             if(base_msg.cluster_id[j] == clusterInMotion[l]){
198 | 
199 |                 std::pair<double,double> temp_maxz = minmaxz(base_msg.clusters[j]);
200 |                 max_z = temp_maxz.first;
201 | 
202 |                 pcz[j]=saveAllZPointsFrom(base_msg.clusters[j], (max_z + base_msg.middle_z)/2);
203 | 
204 |                 for(unsigned i=0; i < base_msg.clusters.size(); i++){
205 |                     if(i!=j){
206 | 
207 |                         temp_maxz = minmaxz(base_msg.clusters[i]);
208 |                         max_z = temp_maxz.first;
209 | 
210 |                         pcz[i]=saveAllZPointsFrom(base_msg.clusters[i], (max_z + base_msg.middle_z)/2);
211 | 
212 |                         bool dist_less_04 = false;
213 | 
214 |                         for (int k=0; k < pcz[j].points.size(); k++){      //for every point in the cluster, find min y and max y  
215 |                             for (int n=0; n < pcz[i].points.size(); n++){
216 | 
217 |                                 double dist = sqrt(pow(pcz[j].points[k].x-pcz[i].points[n].x, 2) + pow(pcz[j].points[k].y-pcz[i].points[n].y, 2) + pow(pcz[j].points[k].z-pcz[i].points[n].z, 2));
218 | 
219 |                                 if(dist <= max_dist) {
220 | 
221 |                                     bool id1=false;
222 |                                     bool id2 = false;
223 |                                     int pos1 = -1;
224 |                                         
225 | 
226 |                                     for(int m=0; m < msg.cluster_id.size(); m++) {
227 |                                         if(msg.cluster_id[m] == base_msg.cluster_id[j]) id1 = true;
228 |                                         if(msg.cluster_id[m] == base_msg.cluster_id[i]) id2 = true;
229 | 
230 |                                         if(id1==true && id2==true) break;
231 |                                     }
232 | 
233 |                                     if(id1==true && id2==false){
234 |                                         idss.push_back(base_msg.cluster_id[j]);
235 |                                         idss.push_back(base_msg.cluster_id[i]);
236 |                                         std::cout << "Clusters too close cluster_id = " << base_msg.cluster_id[j] << "cluster_id = " << base_msg.cluster_id[i] << std::endl;
237 |                                     }
238 |                                     else if(id1==false && id2==true){
239 |                                         idss.push_back(base_msg.cluster_id[i]);
240 |                                         idss.push_back(base_msg.cluster_id[j]);
241 |                                         std::cout << "Clusters too close cluster_id = " << base_msg.cluster_id[i] << "cluster_id = " << base_msg.cluster_id[j] << std::endl;
242 |                                     }
243 | 
244 |                                     dist_less_04 = true;
245 |                                     break;
246 |                                 }
247 |                             }
248 | 
249 |                             if(dist_less_04 == true) break;
250 |                         }
251 |                     }
252 |                 }
253 |             }
254 |         }
255 |     }
256 | }
257 | 
258 | 
259 | bool checkforsameXYpoints(pcl::PointCloud<pcl::PointXYZ> pcz_max, pcl::PointCloud<pcl::PointXYZ> pcz_min){ // check if a maxz point is enclosed to minz points
260 | 
261 |    
262 |     double xmin,xmax, ymin, ymax;
263 | 
264 |     xmin= pcz_min.points[0].x;
265 |     xmax= pcz_min.points[0].x;
266 |     ymin= pcz_min.points[0].y;
267 |     ymax= pcz_min.points[0].y;
268 | 
269 |     for(int i=1; i < pcz_min.points.size(); i++){        
270 |         if(pcz_min.points[i].x < xmin){
271 |             xmin= pcz_min.points[i].x;
272 |         }
273 |         if(pcz_min.points[i].x > xmax){
274 |             xmax= pcz_min.points[i].x;
275 |         }
276 |         if(pcz_min.points[i].y < ymin){
277 |             ymin= pcz_min.points[i].y;
278 |         }
279 |         if(pcz_min.points[i].y > ymax){
280 |             ymax= pcz_min.points[i].y;
281 |         }
282 |     }
283 | 
284 |     bool same=false;
285 | 
286 |     for(int k=0; k < pcz_max.points.size(); k++){        
287 |         if(pcz_max.points[k].x >= xmin && pcz_max.points[k].x <= xmax && pcz_max.points[k].y >= ymin && pcz_max.points[k].y <= ymax){
288 |             same=true;
289 |             break;
290 |         }
291 |     }
292 |     return same;
293 | }
294 | 
295 | void checkIfClusterMove(roboskel_msgs::PointCloud2_Segments msg, size_t size_new){  //check if a cluster moves if it does, then store it
296 | 
297 | 
298 |     for(unsigned j=0; j < size_new; j++){
299 | 
300 |         bool already_exist=false;
301 |         for(int i=0; i<clusterInMotion.size(); i++){
302 |             if(clusterInMotion[i] == msg.cluster_id[j]){
303 |                 already_exist=true;
304 |                 break;
305 |             }
306 |         }
307 |         if(already_exist==false){ 
308 | 
309 |             bool find_id=false;
310 | 
311 |             for(int i=0; i<idss.size(); i+=2){
312 |                 if (idss[i]== msg.cluster_id[j]){
313 |                     find_id=true;
314 |                     break;
315 |                 }
316 |             }
317 |             if(find_id==false){
318 | 
319 |                 pcl::PointCloud<pcl::PointXYZ> pczmax;
320 |                 pcl::PointCloud<pcl::PointXYZ> pczmin;
321 | 
322 |                 double max_z, min_z;
323 |                 std::pair<double,double> z_minmax;
324 | 
325 |                 z_minmax = minmaxz(msg.clusters[j]);
326 |                 max_z = z_minmax.first;
327 |                 min_z = z_minmax.second;
328 | 
329 |                 pczmax=saveAllZValuePoints(msg.clusters[j], max_z);
330 |                 pczmin=saveAllZValuePoints(msg.clusters[j], min_z);
331 | 
332 |                 Eigen::Vector4f max_centroid;
333 |                 pcl::compute3DCentroid ( pczmax , max_centroid);
334 | 
335 |                 Eigen::Vector4f min_centroid;
336 |                 pcl::compute3DCentroid ( pczmin , min_centroid);
337 | 
338 |                 double disttt;
339 | 
340 |                 disttt = 1000 * sqrt(pow(max_centroid[0]-min_centroid[0], 2) + pow(max_centroid[1]-min_centroid[1], 2));
341 |                 if(disttt > minMotionDist){
342 |                     //                                double check                    //
343 |                     pczmax=saveAllZPointsFrom(msg.clusters[j], (max_z + msg.middle_z)/2);
344 |                     pczmin=saveAllZPointsUntil(msg.clusters[j],  (min_z + msg.middle_z)/2);
345 |                     bool samepoints = true;
346 |                     if(pczmax.size()!=0 and pczmin.size()!=0 ){
347 |                         samepoints=checkforsameXYpoints(pczmax, pczmin);
348 |                     }
349 |                    if(samepoints==false){
350 | 
351 |                         clusterInMotion.push_back(msg.cluster_id[j]);
352 |                         prob_extinction.push_back(false);
353 |                         
354 |                         std::cout << "cluster in motion id = " << msg.cluster_id[j] << "dist = " << disttt << std::endl;    
355 |                     }
356 |                 }
357 |             }
358 |         }
359 |     }
360 | }
361 | 
362 | 
363 | class Centroid_tracking{
364 | public:
365 | 
366 |     int max_id ;
367 | 
368 |     roboskel_msgs::PointCloud2_Segments base_msg;
369 | 
370 |     Centroid_tracking ( roboskel_msgs::PointCloud2_Segments& base_msg, int max_id ) 
371 |     {
372 |         this->base_msg = base_msg ;
373 |         this->max_id = max_id ;
374 |     }
375 | 
376 |     void track ( roboskel_msgs::PointCloud2_Segments& msg ) {
377 | 
378 |         std::cout << "!!" << std::endl;
379 | 
380 |         std::vector<Eigen::Vector4f> msg_centroid_vec;
381 |         std::vector<Eigen::Vector4f> base_centroid_vec;
382 | 
383 |         trackedOrnotIds.clear();
384 |         std::vector<Eigen::Vector4f> untracked_msg;
385 | 
386 |         double dist, z_maxnew;
387 |         std::pair<double,double> z_minmax;
388 |         bool find_id;
389 | 
390 |         //first frame 
391 |         for (int i=0; i < base_msg.clusters.size(); i++)
392 |         {
393 |             pcl::PointXYZ centroidpoint ;
394 |             pcl::PCLPointCloud2 pc2;
395 |             pcl_conversions::toPCL ( base_msg.clusters[i] , pc2 );
396 | 
397 |             pcl::PointCloud<pcl::PointXYZ> cloud2;
398 |             pcl::fromPCLPointCloud2 ( pc2 , cloud2 );
399 | 
400 |             Eigen::Vector4f base_centroid;
401 |             pcl::compute3DCentroid ( cloud2 , base_centroid);
402 |             base_centroid_vec.push_back( base_centroid );
403 | 
404 |             if(first_time==true && trackTheUntracked == true){
405 |                 centroids.push_back(base_centroid);
406 |                 ids.push_back(i);
407 |             }
408 |         }
409 |         first_time = false;
410 |         //second frame
411 |         for (int i=0; i < msg.clusters.size(); i++)
412 |         {
413 |             pcl::PointXYZ centroidpoint ;
414 |             pcl::PCLPointCloud2 pc2;
415 |             pcl_conversions::toPCL ( msg.clusters[i] , pc2 );
416 | 
417 |             pcl::PointCloud<pcl::PointXYZ> cloud2;
418 |             pcl::fromPCLPointCloud2 ( pc2 , cloud2 );
419 |             Eigen::Vector4f base_centroid;
420 |             pcl::compute3DCentroid ( cloud2 , base_centroid);
421 | 
422 |             msg_centroid_vec.push_back( base_centroid );
423 |             msg.cluster_id[i] = -1;
424 |         }
425 | 
426 |         if(trackTheUntracked == true){
427 |             for (int i=0; i < ids.size(); i++) 
428 |                 trackedOrnotIds.push_back(false);
429 |         }
430 | 
431 |         size_t size_old = base_centroid_vec.size();
432 |         size_t size_new = msg_centroid_vec.size();
433 |         std::vector<std::vector<int> > dists(size_old, std::vector<int>(size_new , 10000));// TODO currently, 10000 is the maximum (2d) int distance with a 10 meter laser scanner. Initial value represents a point connected to bottom.
434 | 
435 |         for(unsigned i=0; i < size_old; i++){
436 |             for(unsigned j=0; j < size_new; j++){
437 |                 dists[i][j] = 1000 * sqrt(pow(base_centroid_vec[i][0]-msg_centroid_vec[j][0], 2) + pow(base_centroid_vec[i][1]-msg_centroid_vec[j][1], 2));                 
438 |             }
439 |         }
440 | 
441 | 
442 |         Hungarian::Result r = Hungarian::Solve(dists, Hungarian::MODE_MINIMIZE_COST);
443 | 
444 |         //Hungarian::PrintMatrix(r.assignment);
445 | 
446 |         dists = r.assignment;
447 | 
448 |         if(marker_flag==true) {
449 | 
450 |             marker_line.points.clear();        
451 |             marker_sphere.points.clear();
452 |             marker_sphere.colors.clear();
453 |             marker_line.colors.clear();
454 |         }
455 | 
456 |         for(unsigned j=0; j < size_new; j++){
457 |             for(unsigned i=0; i < size_old; i++){
458 | 
459 |                 if (dists[i][j] == 1){
460 | 
461 |                     dist = 1000 * sqrt(pow(base_centroid_vec[i][0]-msg_centroid_vec[j][0], 2) + pow(base_centroid_vec[i][1]-msg_centroid_vec[j][1], 2) );
462 | 
463 |                     if (dist<maxHungDist) {
464 |                         msg.cluster_id[j] = base_msg.cluster_id[i];
465 | 
466 |                         if(trackTheUntracked == true){
467 | 
468 |                             for (unsigned m=0; m<ids.size(); m++){
469 |                                 if(ids[m] == msg.cluster_id[j]){
470 |                                     centroids[m]=msg_centroid_vec[j];
471 |                                     trackedOrnotIds[m] = true;
472 |                                     break;
473 |                                 }
474 |                             }
475 |                         }
476 | 
477 |                         if(marker_flag==true) {
478 | 
479 |                             uint mod = msg.cluster_id[j] % 10;
480 |                             std_msgs::ColorRGBA c;
481 |                                 
482 |                             c.r = red[mod];
483 |                             c.g = green[mod];
484 |                             c.b=blue[mod];
485 |                             c.a=0.7;
486 | 
487 |                             geometry_msgs::Point p;
488 |                                
489 |                             p.x = base_centroid_vec[i][0];
490 |                             p.y = base_centroid_vec[i][1];
491 |                             p.z = base_centroid_vec[i][2];
492 |                                                        
493 |                             marker_line.points.push_back(p);
494 |                             marker_sphere.points.push_back(p);
495 | 
496 |                             geometry_msgs::Point pp;                    
497 | 
498 |                             pp.x = msg_centroid_vec[j][0];
499 |                             pp.y = msg_centroid_vec[j][1];
500 |                             pp.z = msg_centroid_vec[j][2];
501 |                                
502 |                             marker_line.points.push_back(pp);
503 |                             marker_sphere.points.push_back(pp);
504 | 
505 |                             marker_line.colors.push_back(c);
506 |                             marker_line.colors.push_back(c);
507 |                             marker_sphere.colors.push_back(c);
508 |                             c.a=0.3;
509 |                             marker_sphere.colors.push_back(c);
510 |                         }
511 |                     }
512 |                     break;
513 |                 }
514 |             }
515 |             if(trackTheUntracked == true && msg.cluster_id[j] == -1) untracked_msg.push_back(msg_centroid_vec[j]);
516 |         }
517 | 
518 |         //------------------------------------------------------------------------------------//
519 |  
520 |         //--------------------------try to track the untracked clusters----------------------------//
521 | 
522 |         if(trackTheUntracked == true) trackUntrackedClusters(untracked_msg, msg, msg_centroid_vec, size_new );
523 | 
524 |         //----------------------------same cluster ---------------------------------------// check whether there is a new cluster over an existing one or not 
525 | 
526 |         for(int i=0; i<size_new; i++){
527 |             if(msg.cluster_id[i] == -1){
528 | 
529 |                 pcl::PointCloud<pcl::PointXYZ> pc_untracked;
530 |                 pc_untracked=saveAllPoints(msg.clusters[i]);
531 | 
532 |                 for (int j = 0; j < size_new; j++){
533 |                     if( msg.cluster_id[j] != -1){
534 | 
535 |                         pcl::PointCloud<pcl::PointXYZ> pc_existCluster;
536 |                         pc_existCluster=saveAllPoints(msg.clusters[j]);
537 | 
538 |                        bool same_cluster = checkforsameXYpoints(pc_existCluster, pc_untracked);
539 |                        if(same_cluster==true){
540 | 
541 |                             msg.cluster_id[i] = msg.cluster_id[j];
542 | 
543 |                             if(trackTheUntracked == true){
544 |                                 for(int n=0; n<ids.size(); n++){
545 |                                     if(ids[n] == msg.cluster_id[i]){
546 |                                         centroids[n] = msg_centroid_vec[i];
547 |                                         break;
548 |                                     }
549 |                                 }
550 |                             }
551 |                             std::cout << "Find same cluster id = " << msg.cluster_id[j] << std::endl;
552 |                             break;    
553 |                         }
554 |                     }
555 |                 }
556 |             }
557 |         }
558 | 
559 |         //------------------------------------------------------------------------------------//
560 | 
561 |        
562 |         //-----------------------distance <= max_dist------------------------------//
563 | 
564 |         checkClustersDistance(base_msg, size_old, msg);
565 | 
566 | 
567 |         //----------------------------------probability for extiction------------------------------------------------------------------// check clusters which is in motion if their maxz is less than 1.5*(regular)middlez  
568 |         
569 |          
570 |         for(int k=0; k<clusterInMotion.size(); k++ ) {
571 |             for(int j=0; j < size_new; j++) {
572 |                 if(clusterInMotion[k] == msg.cluster_id[j]){
573 |                     z_minmax = minmaxz(msg.clusters[j]);
574 |                     z_maxnew = z_minmax.first;
575 |                     if(z_maxnew< 3*msg.middle_z/2) prob_extinction[k]=true;
576 |                     else prob_extinction[k]=false;
577 |                     break;    
578 |                 }
579 |             }     
580 |         }
581 | 
582 |         //-------------------------------------check for new cluster------------------------------------------------------------//
583 | 
584 |         for(int j=0; j < size_new; j++) {
585 |             if(msg.cluster_id[j] == -1){
586 |                 std::cout << "untracked!!!!!!! " << std::endl;
587 | 
588 |                 for(int k=0; k<clusterInMotion.size(); k++ ) {  
589 |                     find_id=false;
590 |                     for(int i=0; i < size_new; i++) {
591 |                         if(clusterInMotion[k] == msg.cluster_id[i]){
592 |                             find_id=true;
593 |                             if(prob_extinction[k] == true) find_id=false;
594 | 
595 |                             break;
596 |                         }
597 |                     }
598 |                     if(find_id==false){    // image check needed!!
599 |                         msg.cluster_id[j] = clusterInMotion[k];
600 |                         prob_extinction[k]=false;
601 |                         if(trackTheUntracked == true){
602 |                             for(int n=0; n<ids.size(); n++){
603 |                                 if(ids[n] == clusterInMotion[k]){
604 |                                     centroids[n] = msg_centroid_vec[j];
605 |                                     break;
606 |                                 }
607 |                             }
608 |                         }
609 |                         break;
610 |                     }
611 |                 }
612 |                 if(msg.cluster_id[j] == -1){
613 |                     std::cout << "New Cluster!!!! " << std::endl;
614 |                     msg.cluster_id[j] = ++max_id;
615 |                     if(trackTheUntracked == true){
616 |                         centroids.push_back(msg_centroid_vec[j]);
617 |                         ids.push_back(msg.cluster_id[j]);
618 |                     }
619 |                 }
620 |             }
621 |         }
622 | 
623 |         //---------------------------------------------------------------------------------------------//
624 | 
625 |         for(int i=1; i<idss.size(); i+=2){
626 |             for(int j=0; j<size_new; j++){
627 |                 if(idss[i]==msg.cluster_id[j]){
628 |                     idss.erase(idss.begin()+i-1, idss.begin()+i);
629 |                     break;
630 |                 }
631 |             }
632 |         }
633 | 
634 |         //------------------------------check if cluster is in movement---------------------------------//
635 | 
636 |         checkIfClusterMove(msg, size_new);
637 |         
638 |         if(clusterInMotion.size()==1) {
639 |             msg.idForTracking=clusterInMotion[0];
640 |         }
641 | 
642 |        //---------------------------------------------------------------------------------------------//
643 |     }
644 | };
645 | 
646 | 
647 | 
648 | ros::Publisher pub;
649 | ros::Subscriber sub;
650 | ros::Publisher marker_pub;
651 | 
652 | int size , max_id ,method;
653 | double overlap, offset ;
654 | 
655 | 
656 | std::vector<roboskel_msgs::PointCloud2_Segments> v_;
657 | std::vector<roboskel_msgs::PointCloud2_Segments> new_v(2);
658 | 
659 | visualization_msgs::MarkerArray marker;
660 | std::string marker_frame_id;
661 | 
662 | 
663 | std::pair<double,double> overlap_range (const roboskel_msgs::PointCloud2_Segments& cls){
664 | 
665 |     double z_max = 0 ;
666 |     double height_after ,height_before ;
667 |     double z_min = std::numeric_limits<double>::max();
668 | 
669 |     std::vector<roboskel_msgs::PointCloud2_Segments> vec;
670 | 
671 |     vec.push_back(cls);
672 | 
673 |     if (vec.size() > size){
674 |         vec.erase(vec.begin());
675 |     }
676 | 
677 |     for (unsigned i=0; i < vec.size(); i++)
678 |     {
679 |         double offset;
680 | 
681 |         if ( i > 0 ){
682 | 
683 |             offset = ( 1.0 - overlap ) * (double)( ros::Duration( vec[i].first_stamp - vec[0].first_stamp ).toSec()) * (double)( cls.factor );
684 | 
685 |             for (unsigned j=0; j < vec[i].clusters.size(); j++)
686 |             {
687 |                 sensor_msgs::PointCloud cloud;
688 |                 sensor_msgs::convertPointCloud2ToPointCloud( vec[i].clusters[j] , cloud );
689 | 
690 |                 for (unsigned k=0; k < cloud.points.size(); k++)
691 |                 {
692 |                     cloud.points[k].z += offset ;
693 |                     if (cloud.points[k].z < z_min){
694 |                         z_min = cloud.points[k].z ;
695 |                     }
696 |                 }
697 |             }
698 |         }
699 |         else {
700 | 
701 |             offset = 0.0;
702 | 
703 |             for (unsigned j=0; j < vec[0].clusters.size(); j++){
704 |                 sensor_msgs::PointCloud cloud;
705 |                 sensor_msgs::convertPointCloud2ToPointCloud( vec[0].clusters[j] , cloud );
706 | 
707 |                 for ( unsigned l=0; l < cloud.points.size(); l++){
708 |                     if ( cloud.points[l].z > z_max ){
709 |                         z_max = cloud.points[l].z;
710 |                     }
711 |                 }
712 |             }
713 |         }
714 | 
715 |     }
716 | 
717 |     return std::make_pair( z_max , z_min );
718 | 
719 | }
720 | 
721 | roboskel_msgs::PointCloud2_Segments clusters_in_overlap (const roboskel_msgs::PointCloud2_Segments& cl , double z_overlap_min , double z_overlap_max ){
722 | 
723 |     sensor_msgs::PointCloud2 pc1;
724 |     roboskel_msgs::PointCloud2_Segments output;
725 | 
726 |     for ( unsigned i=0; i < cl.clusters.size(); i++){
727 | 
728 |         sensor_msgs::PointCloud cloud;
729 |         sensor_msgs::convertPointCloud2ToPointCloud( cl.clusters[i] , cloud );
730 | 
731 |         pcl::PCLPointCloud2 pc2 ;
732 |         sensor_msgs::PointCloud2 cl2 ;
733 | 
734 |         sensor_msgs::convertPointCloudToPointCloud2( cloud , cl2 );
735 |         pcl_conversions::toPCL ( cl2 , pc2 );
736 | 
737 |         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
738 |         pcl::fromPCLPointCloud2 ( pc2 , *cloud2 );
739 |         pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
740 |         pcl::ExtractIndices<pcl::PointXYZ> extract ;
741 | 
742 |         for (int i=0; i < (*cloud2).size(); i++){
743 |             pcl::PointXYZ pt(cloud2->points[i].x , cloud2->points[i].y , cloud2->points[i].z);
744 |             if (pt.z <= z_overlap_max and pt.z >= z_overlap_min){
745 |                 inliers->indices.push_back(i);
746 |             }
747 |         }
748 |         extract.setInputCloud(cloud2);
749 |         extract.setIndices(inliers);
750 |         extract.setNegative(true);
751 |         extract.filter(*cloud2);
752 |         pcl::PCLPointCloud2 pcl_cloud;
753 |         pcl::toPCLPointCloud2(*cloud2, pcl_cloud);
754 |         pcl_conversions::fromPCL(pcl_cloud, pc1);
755 |     
756 |         output.clusters.push_back(pc1);
757 |     }
758 | 
759 |     return output;
760 | }
761 | 
762 | 
763 | 
764 | void callback (const roboskel_msgs::PointCloud2_Segments& msg ){
765 | 
766 | 
767 |     if(marker_flag==1) {
768 | 
769 |         marker_sphere.ns = "shapeOfsphere";
770 |         marker_sphere.id = 0;
771 |         marker_sphere.type = 7;
772 |         marker_sphere.action = visualization_msgs::Marker::ADD;
773 |         marker_sphere.pose.orientation.w = 1.0;
774 |         marker_sphere.color.a = 1.0;
775 | 
776 |         marker_sphere.scale.x = 0.3;
777 |         marker_sphere.scale.y = 0.3;
778 |         marker_sphere.scale.z = 0.3;
779 | 
780 |         marker_line.ns = "shapeOfline";
781 |         marker_line.id = 1;    
782 |         marker_line.type = 5;
783 |         marker_line.action = visualization_msgs::Marker::ADD;
784 |         marker_line.pose.orientation.w = 1.0;
785 |         marker_line.color.a = 1.0;
786 | 
787 |         marker_line.scale.x = 0.1;
788 |         marker_line.scale.y = 0.1;
789 |         marker_line.scale.z = 0.1;
790 | 
791 | 
792 |         marker_sphere.header.frame_id = marker_frame_id;
793 |         marker_sphere.header.stamp = msg.header.stamp;
794 |         marker_sphere.lifetime = ros::Duration();
795 | 
796 |         marker_line.header.frame_id = marker_frame_id;
797 |         marker_line.header.stamp = msg.header.stamp;
798 |         marker_line.lifetime = ros::Duration();
799 | 
800 |         marker.markers.push_back(marker_sphere);
801 |         marker.markers.push_back(marker_line);
802 |     }
803 | 
804 | 
805 | 
806 |     double overlap_height_min , overlap_height_max ;
807 | 
808 |     sensor_msgs::PointCloud cloud;
809 |     roboskel_msgs::PointCloud2_Segments c_;
810 | 
811 |     if ( method == 1 ){
812 |         std::pair<double,double> z_height = overlap_range(msg);
813 |         overlap_height_min = z_height.first;
814 |         overlap_height_max = z_height.second;
815 |     }
816 | 
817 |     v_.push_back(msg);
818 | 
819 |     Centroid_tracking* t;
820 | 
821 |     if (v_.size() > size){
822 |         v_.erase(v_.begin());
823 |     }
824 | 
825 |     if(v_.size()>=size) {
826 | 
827 |         if (method == 1 ){
828 |             new_v[0] = v_[0];
829 |             new_v[1] = clusters_in_overlap(v_[1] , overlap_height_min , overlap_height_max);
830 |         
831 |             for (int i=0; i < v_[1].clusters.size(); i++) {
832 |                 new_v[1].cluster_id.push_back(i);
833 |             }
834 |         }
835 |         else if ( method == 2) {
836 |             for (int i=0; i < v_[1].clusters.size(); i++) {
837 |                 v_[1].cluster_id.push_back(i);
838 |             }
839 |         }
840 |         
841 |         for (unsigned i=0; i < v_[0].cluster_id.size(); i++){
842 |             if (v_[0].cluster_id[i] > max_id){
843 |                 max_id = v_[0].cluster_id[i];
844 |             }
845 |         }
846 |         
847 |         if (method == 1){
848 |             t = new Centroid_tracking( new_v[0] , max_id );
849 |         }
850 |         else if (method == 2 ){
851 |             t = new Centroid_tracking( v_[0] , max_id );
852 |         }
853 |     }
854 |     else {
855 |         t = NULL;
856 | 
857 |         for (unsigned i=0; i < v_[0].clusters.size(); i++){
858 |             v_[0].cluster_id.push_back(i);
859 |         }
860 | 
861 |     }
862 | 
863 |     c_.idForTracking = msg.idForTracking;
864 | 
865 |     if ( t != NULL ){
866 |         if ( method == 1 ){
867 |             t-> track( new_v[1] );
868 | 
869 |             for (unsigned i=0; i < new_v[1].cluster_id.size(); i++){
870 |                 v_[1].cluster_id.push_back(new_v[1].cluster_id[i]);
871 |             }
872 |             c_.idForTracking = new_v[1].idForTracking;
873 |         }
874 |         else if ( method == 2){
875 |             t-> track( v_[1] );
876 |             c_.idForTracking = v_[1].idForTracking;
877 |         }
878 |     }
879 | 
880 |     for (unsigned i=0; i < v_.size(); i++)
881 |     {
882 |         for (int k=0; k < v_[i].cluster_id.size(); k++){
883 |             c_.clusters.push_back(v_[i].clusters[k]);
884 |             c_.cluster_id.push_back(v_[i].cluster_id[k]);
885 |         }
886 |     }
887 | 
888 |     c_.header.stamp = ros::Time::now();
889 |     c_.header.frame_id = msg.header.frame_id;
890 |     c_.factor = msg.factor ;
891 |     c_.overlap = msg.overlap ;
892 |     c_.num_scans = msg.num_scans ;
893 |     c_.first_stamp = msg.first_stamp ;
894 |     c_.angle_min = msg.angle_min;
895 |     c_.angle_max = msg.angle_max;
896 |     c_.angle_increment = msg.angle_increment;
897 |     c_.range_min = msg.range_min;
898 |     c_.range_max = msg.range_max;
899 |     c_.scan_time = msg.scan_time;
900 |     c_.rec_time = msg.rec_time;
901 |     c_.middle_z = msg.middle_z;
902 |     
903 |     pub.publish(c_);
904 | 
905 |     if(marker_flag==true && t != NULL) {
906 | 
907 |         marker_pub.publish(marker);
908 | 
909 |         marker.markers.clear();
910 |     }
911 | }
912 | 
913 | 
914 | int main(int argc, char** argv){
915 | 
916 |     ros::init(argc, argv, "pointcloud2_cluster_tracking");
917 |     ros::NodeHandle n_;
918 | 
919 |     std::string out_topic;
920 |     std::string input_topic;
921 |     std::string marker_topic;
922 | 
923 | 
924 |     n_.param("pointcloud2_cluster_tracking/size", size , 2);
925 |     n_.param("pointcloud2_cluster_tracking/method", method , 1);
926 |     n_.param("pointcloud2_cluster_tracking/overlap", overlap , 0.2);
927 |     n_.param("pointcloud2_cluster_tracking/marker_flag", marker_flag , false);
928 |     n_.param("pointcloud2_cluster_tracking/maxHungDist", maxHungDist , 1000);
929 |     n_.param("pointcloud2_cluster_tracking/trackTheUntracked", trackTheUntracked , false);
930 |     n_.param("pointcloud2_cluster_tracking/minMotionDist", minMotionDist , 15.0);
931 |     n_.param("pointcloud2_cluster_tracking/max_dist", max_dist , 0.0);
932 | 
933 |     n_.param("pointcloud2_cluster_tracking/out_topic", out_topic , std::string("/pointcloud2_cluster_tracking/clusters"));
934 |     n_.param("pointcloud2_cluster_tracking/input_topic", input_topic , std::string("pointcloud2_clustering/clusters"));
935 | 
936 |     if(marker_flag==true) {
937 | 
938 |         n_.param("pointcloud2_cluster_tracking/marker_topic", marker_topic , std::string("visualization_marker"));
939 |         n_.param("pointcloud2_cluster_tracking/marker_frame_id", marker_frame_id , std::string("/base_link"));
940 |         marker_pub = n_.advertise<visualization_msgs::MarkerArray>(marker_topic, 1);
941 |     }
942 | 
943 |     sub = n_.subscribe( input_topic, 1 , callback);
944 |     pub = n_.advertise<roboskel_msgs::PointCloud2_Segments>( out_topic, 1);
945 |     
946 |     ros::spin();
947 | }
948 | 


--------------------------------------------------------------------------------