cluster.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2020, Robobrain.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /* Author: Konstantinos Konstantinidis */
31 
32 #include "cluster.hpp"
33 
34 
35 static inline double normalize_angle_positive(double angle){
36  //Normalizes the angle to be 0 to 2*M_PI.
37  //It takes and returns radians.
38  return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
39 }
40 static inline double normalize_angle(double angle){
41  //Normalizes the angle to be -M_PI circle to +M_PI circle
42  //It takes and returns radians.
43  double a = normalize_angle_positive(angle);
44  if (a > M_PI)
45  a -= 2.0 *M_PI;
46  return a;
47 }
48 
49 static inline double shortest_angular_distance(double from, double to){
50  //Given 2 angles, this returns the shortest angular difference.
51  //The inputs and outputs are radians.
52  //The result would always be -pi <= result <= pi.
53  //Adding the result to "from" results to "to".
54  return normalize_angle(to-from);
55 }
56 
57 Cluster::Cluster(unsigned long int id, const pointList& new_points, const double& dt, const std::string& world_frame, const tf::Transform& ego_pose){
58 
59  this->id = id;
60  this->r = rand() / double(RAND_MAX);
61  this->g = rand() / double(RAND_MAX);
62  this->b = rand() / double(RAND_MAX);
63  a = 1.0;
64  age = 1;
65  frame_name = world_frame;
66 
67  new_cluster = new_points;
68 
69  ego_coordinates.first = ego_pose.getOrigin().getX();
70  ego_coordinates.second= ego_pose.getOrigin().getY();
71 
72 
73  calcMean(new_points);
74  previous_mean_values = mean_values;
75  rectangleFitting(new_points);
76 
77  LshapeTracker l_shape_tracker_ukf(closest_corner_point.first, closest_corner_point.second, L1, L2, normalize_angle(thetaL1), dt);
78  this->Lshape = l_shape_tracker_ukf;
79  Lshape.BoxModel(cx, cy, cvx, cvy, th, psi, comega, L1_box, L2_box, length_box, width_box);
80 
81  populateTrackingMsgs(dt);
82 }
83 
84 void Cluster::update(const pointList& new_points, const double dt, const tf::Transform& ego_pose) {
85 
86  ego_coordinates.first = ego_pose.getOrigin().getX();
87  ego_coordinates.second= ego_pose.getOrigin().getY();
88 
89  age++;
90  previous_mean_values = mean_values;
91  new_cluster = new_points;
92 
93  calcMean(new_points);
94  rectangleFitting(new_points);
95 
96  Lshape.update(thetaL1, closest_corner_point.first, closest_corner_point.second, L1, L2, dt, new_points.size());
97 
98  Lshape.BoxModel(cx, cy, cvx, cvy, th, psi, comega, L1_box, L2_box, length_box, width_box);
99 
100  populateTrackingMsgs(dt);
101 
102 }
103 
104 void Cluster::populateTrackingMsgs(const double& dt){
105  // This function populates the datmo/Tracks msgs.
106 
107  msg_track_box_kf.id = this->id;
108  msg_track_box_kf.odom.header.stamp = ros::Time::now();
109  msg_track_box_kf.odom.header.frame_id = frame_name;
110  msg_track_box_kf.odom.pose.pose.position.x = cx;
111  msg_track_box_kf.odom.pose.pose.position.y = cy;
112  msg_track_box_kf.odom.twist.twist.linear.x = cvx;
113  msg_track_box_kf.odom.twist.twist.linear.y = cvy;
114  msg_track_box_kf.length = length_box;
115  msg_track_box_kf.width = width_box;
116 
117  quaternion.setRPY(0, 0, psi);
118  msg_track_box_kf.odom.pose.pose.orientation = tf2::toMsg(quaternion);
119  msg_track_box_kf.odom.twist.twist.angular.z = comega;
120 
121 }
122 
123 void Cluster::rectangleFitting(const pointList& new_cluster){
124  //This function is based on ¨Efficient L-Shape Fitting for
125  //Vehicle Detection Using Laser Scanners¨
126 
127  unsigned int n = new_cluster.size();
128  VectorXd e1(2),e2(2);
129  MatrixXd X(n, 2);
130  for (unsigned int i = 0; i < n; ++i) {
131  X(i,0) = new_cluster[i].first;
132  X(i,1) = new_cluster[i].second;
133  }
134  VectorXd C1(n),C2(n);
135  double q;
136  unsigned int i =0;
137  th = 0.0;
138  //TODO make d configurable through Rviz
139  unsigned int d = 25;
140  ArrayX2d Q(d,2);
141  float step = (3.14/2)/d;
142  //#pragma omp parallel for
143  for (i = 0; i < d; ++i) {
144  e1 << cos(th), sin(th);
145  e2 <<-sin(th), cos(th);
146  C1 = X * e1;
147  C2 = X * e2;
148 
149  //q = areaCriterion(C1,C2);
150  q = closenessCriterion(C1,C2,0.001);
151  Q(i,0) = th;
152  Q(i,1) = q;
153 
154  th = th + step;
155  }
156 
157  ArrayX2d::Index max_index;
158  Q.col(1).maxCoeff(&max_index);//find Q with maximum value
159  th = Q(max_index,0);
160  e1 << cos(th), sin(th);
161  e2 <<-sin(th), cos(th);
162  C1 = X * e1;
163  C2 = X * e2;
164  // The coefficients of the four lines are calculated
165  double a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4;
166  a1 = cos(th);
167  b1 = sin(th);
168  c1 = C1.minCoeff();
169  a2 = -sin(th);
170  b2 = cos(th);
171  c2 = C2.minCoeff();
172  a3 = cos(th);
173  b3 = sin(th);
174  c3 = C1.maxCoeff();
175  a4 = -sin(th);
176  b4 = cos(th);
177  c4 = C2.maxCoeff();
178 
179  std::vector<Point> corners;
180  corners.push_back(lineIntersection(a2, b2, c2, a3, b3, c3));
181  corners.push_back(lineIntersection(a1, b1, c1, a2, b2, c2));
182  corners.push_back(lineIntersection(a1, b1, c1, a4, b4, c4));
183  corners.push_back(lineIntersection(a4, b4, c4, a3, b3, c3));
184  corner_list = corners;
185 
186  //Find the corner point that is closest to the ego vehicle
187  double min_distance = pow(pow(corners[0].first - ego_coordinates.first,2.0)+pow(corners[0].second - ego_coordinates.second,2.0),0.5);
188  unsigned int idx = 0;
189  closest_corner_point = corners[0];
190  double distance;
191  for (unsigned int i = 1; i < 4; ++i) {
192  distance = pow(pow(corners[i].first - ego_coordinates.first,2.0)+pow(corners[i].second - ego_coordinates.second,2.0),0.5);
193  if (distance<min_distance) {
194  min_distance = distance;
195  closest_corner_point = corners[i];
196  idx = i;
197  }
198  }
199 
200  //Populate the l1l2 pointlist
201  std::vector<Point> l1l2_list;
202  if (idx==3) {
203  l1l2_list.push_back(corners[0]);
204  }
205  else{
206  l1l2_list.push_back(corners[idx+1]);
207  }
208  l1l2_list.push_back(corners[idx]);
209  if (idx==0) {
210  l1l2_list.push_back(corners[3]);
211  }
212  else{
213  l1l2_list.push_back(corners[idx-1]);
214  }
215  l1l2 = l1l2_list;
216 
217 
218  double dx = l1l2_list[1].first - l1l2_list[0].first;
219  double dy = l1l2_list[1].second- l1l2_list[0].second;
220  L1 = pow(pow(dx,2.0)+pow(dy,2.0),0.5);
221  dx = l1l2_list[1].first - l1l2_list[2].first;
222  dy = l1l2_list[1].second- l1l2_list[2].second;
223  L2 = pow(pow(dx,2.0)+pow(dy,2.0),0.5);
224 
225  thetaL1 = atan2((l1l2[0].second - l1l2[1].second),(l1l2[0].first - l1l2[1].first));
226 
227  thetaL2 = atan2((l1l2[2].second - l1l2[1].second),(l1l2[2].first - l1l2[1].first));
228 
229 }
230 visualization_msgs::Marker Cluster::getBoundingBoxVisualisationMessage() {
231 
232  visualization_msgs::Marker bb_msg;
233  bb_msg.header.stamp = ros::Time::now();
234  bb_msg.header.frame_id = frame_name;
235  bb_msg.ns = "bounding_boxes";
236  bb_msg.action = visualization_msgs::Marker::ADD;
237  bb_msg.pose.orientation.w = 1.0;
238  bb_msg.type = visualization_msgs::Marker::LINE_STRIP;
239  bb_msg.id = this->id;
240  //bb_msg.scale.x = 0.3; //line width
241  bb_msg.scale.x = 0.008; //line width
242  bb_msg.color.g = this->g;
243  bb_msg.color.b = this->b;
244  bb_msg.color.r = this->r;
245  bb_msg.color.a = 1.0;
246 
247  geometry_msgs::Point p;
248  for (unsigned int i = 0; i < 4; ++i) {
249  p.x = corner_list[i].first;
250  p.y = corner_list[i].second;
251  bb_msg.points.push_back(p);
252  }
253  p.x = corner_list[0].first;
254  p.y = corner_list[0].second;
255  bb_msg.points.push_back(p);
256 
257  return bb_msg;
258 }
259 visualization_msgs::Marker Cluster::getBoxModelKFVisualisationMessage() {
260 
261  visualization_msgs::Marker bb_msg;
262 
263  bb_msg.header.stamp = ros::Time::now();
264  bb_msg.header.frame_id = frame_name;
265  bb_msg.ns = "box_models_kf";
266  bb_msg.action = visualization_msgs::Marker::ADD;
267  bb_msg.pose.orientation.w = 1.0;
268  bb_msg.type = visualization_msgs::Marker::LINE_STRIP;
269  bb_msg.id = this->id;
270  bb_msg.scale.x = 0.02; //line width
271  bb_msg.color.g = g;
272  bb_msg.color.b = b;
273  bb_msg.color.r = r;
274  bb_msg.color.a = a;
275 
276  geometry_msgs::Point p;
277  double x = L1_box/2;
278  double y = L2_box/2;
279  p.x = cx + x*cos(th) - y*sin(th);
280  p.y = cy + x*sin(th) + y*cos(th);
281  bb_msg.points.push_back(p);
282  x = + L1_box/2;
283  y = - L2_box/2;
284  p.x = cx + x*cos(th) - y*sin(th);
285  p.y = cy + x*sin(th) + y*cos(th);
286  bb_msg.points.push_back(p);
287  x = - L1_box/2;
288  y = - L2_box/2;
289  p.x = cx + x*cos(th) - y*sin(th);
290  p.y = cy + x*sin(th) + y*cos(th);
291  bb_msg.points.push_back(p);
292  x = - L1_box/2;
293  y = + L2_box/2;
294  p.x = cx + x*cos(th) - y*sin(th);
295  p.y = cy + x*sin(th) + y*cos(th);
296  bb_msg.points.push_back(p);
297  x = + L1_box/2;
298  y = + L2_box/2;
299  p.x = cx + x*cos(th) - y*sin(th);
300  p.y = cy + x*sin(th) + y*cos(th);
301  bb_msg.points.push_back(p);
302 
303  return bb_msg;
304 
305 }
306 
307 visualization_msgs::Marker Cluster::getLShapeVisualisationMessage() {
308 
309  visualization_msgs::Marker l1l2_msg;
310 
311  l1l2_msg.header.stamp = ros::Time::now();
312  l1l2_msg.header.frame_id = frame_name;
313  l1l2_msg.ns = "L-Shapes";
314  l1l2_msg.action = visualization_msgs::Marker::ADD;
315  l1l2_msg.pose.orientation.w = 1.0;
316  l1l2_msg.type = visualization_msgs::Marker::LINE_STRIP;
317  l1l2_msg.id = this->id;
318  //l1l2_msg.scale.x = 0.3; //line width
319  l1l2_msg.scale.x = 0.1; //line width
320  l1l2_msg.color.r = 1;
321  l1l2_msg.color.g = 0;
322  l1l2_msg.scale.x = 0.1; //line width
323  l1l2_msg.color.b = 0;
324  l1l2_msg.color.a = 1.0;
325 
326  double theta_degrees = thetaL1 * (180.0/3.141592653589793238463);
327  if (theta_degrees > 360){
328  l1l2_msg.color.r = 1.0;
329  l1l2_msg.color.g = 0;
330  l1l2_msg.color.b = 0;
331  }
332 
333  geometry_msgs::Point p;
334  for (unsigned int i = 0; i < 3; ++i) {
335  p.x = l1l2[i].first;
336  p.y = l1l2[i].second;
337  l1l2_msg.points.push_back(p);
338  }
339 
340  return l1l2_msg;
341 
342 }
343 Point Cluster::lineIntersection(double& a1, double& b1, double& c1, double& a2, double& b2, double& c2){
344  // Function that returns the intersection point of two lines given their equations on
345  // the form: a1x + b1x = c1, a2x + b2x = c2
346  // There is no check for the determinant being zero because of the way the lines are
347  // calculated, it is not possible for this to happen.
348  // source: geeksforgeeks point of intesection of two lines
349  double determinant = a1*b2 - a2*b1;
350  Point intersection_point;
351  intersection_point.first = (b2*c1 - b1*c2)/determinant;
352  intersection_point.second = (a1*c2 - a2*c1)/determinant;
353 
354  return intersection_point;
355 }
356 double Cluster::areaCriterion(const VectorXd& C1, const VectorXd& C2){
357 
358  double a;
359  a = -(C1.maxCoeff()-C1.minCoeff())*(C2.maxCoeff()-C2.minCoeff());
360 
361  return a;
362 
363 }
364 double Cluster::closenessCriterion(const VectorXd& C1, const VectorXd& C2, const double& d0){
365  //Algorithm 4 of "Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners"
366 
367  double c1_max, c1_min, c2_max, c2_min;
368  c1_max = C1.maxCoeff();
369  c1_min = C1.minCoeff();
370  c2_max = C2.maxCoeff();
371  c2_min = C2.minCoeff();
372  VectorXd C1_max = c1_max - C1.array();
373  VectorXd C1_min = C1.array() - c1_min;
374  VectorXd D1, D2;
375  if(C1_max.squaredNorm() < C1_min.squaredNorm()){
376  D1 = C1_max;
377  }
378  else{
379  D1 = C1_min;
380  }
381  VectorXd C2_max = c2_max - C2.array();
382  VectorXd C2_min = C2.array() - c2_min;
383  if(C2_max.squaredNorm() < C2_min.squaredNorm()){
384  D2 = C2_max;
385  }
386  else{
387  D2 = C2_min;
388  }
389 
390  double d, min;
391  double b =0 ;
392  for (int i = 0; i < D1.size(); ++i) {
393  min = std::min(D1(i),D2(i));
394  d = std::max(min,d0);
395  b = b + 1/d;
396  }
397 
398  return b;
399 }
400 visualization_msgs::Marker Cluster::getThetaBoxVisualisationMessage() {
401 
402  visualization_msgs::Marker arrow_marker;
403  arrow_marker.type = visualization_msgs::Marker::ARROW;
404  arrow_marker.header.stamp = ros::Time::now();
405  arrow_marker.ns = "thetaBox";
406  arrow_marker.action = visualization_msgs::Marker::ADD;
407  arrow_marker.color.a = 0.5;
408  arrow_marker.color.g = this->g;
409  arrow_marker.color.b = this->b;
410  arrow_marker.color.r = this->r;
411  arrow_marker.id = this->id;
412  arrow_marker.pose.position.x = cx;
413  arrow_marker.pose.position.y = cy;
414  arrow_marker.pose.position.z = 0;
415  arrow_marker.scale.x = length_box;
416  arrow_marker.scale.y = width_box;
417  arrow_marker.scale.z = 0.01;
418 
419  arrow_marker.header.frame_id = frame_name;
420 
421  //quaternion.setRPY(0,0,orientation);
422  quaternion.setRPY(0,0,psi);
423  arrow_marker.pose.orientation = tf2::toMsg(quaternion);
424 
425  return arrow_marker;
426 }
427 visualization_msgs::Marker Cluster::getThetaL1VisualisationMessage() {
428 
429  visualization_msgs::Marker arrow_marker;
430  arrow_marker.header.frame_id = frame_name;
431  arrow_marker.type = visualization_msgs::Marker::ARROW;
432  arrow_marker.header.stamp = ros::Time::now();
433  arrow_marker.ns = "thetaL1";
434  arrow_marker.action = visualization_msgs::Marker::ADD;
435  arrow_marker.color.a = 1.0;
436  arrow_marker.color.r = 1;
437  arrow_marker.color.g = 0;
438  arrow_marker.color.b = 0;
439  arrow_marker.id = this->id;
440 
441  tf2::Quaternion quat_theta;
442  quat_theta.setRPY(0,0,thetaL1);
443  arrow_marker.pose.orientation = tf2::toMsg(quat_theta);
444  arrow_marker.pose.position.x = closest_corner_point.first;
445  arrow_marker.pose.position.y = closest_corner_point.second;
446  arrow_marker.pose.position.z = 0;
447  arrow_marker.scale.x = L1_box;
448  //arrow_marker.scale.x = 0.1;
449  arrow_marker.scale.y = 0.04;
450  arrow_marker.scale.z = 0.001;
451 
452  return arrow_marker;
453 }
454 visualization_msgs::Marker Cluster::getThetaL2VisualisationMessage() {
455 
456  visualization_msgs::Marker arrow_marker;
457  arrow_marker.type = visualization_msgs::Marker::ARROW;
458  //arrow_marker.header.frame_id = frame_name;
459  arrow_marker.header.stamp = ros::Time::now();
460  arrow_marker.ns = "thetaL2";
461  arrow_marker.action = visualization_msgs::Marker::ADD;
462  arrow_marker.color.a = 1.0;
463  arrow_marker.color.g = 1;
464  arrow_marker.color.g = 0;
465  arrow_marker.color.b = 0;
466  arrow_marker.id = this->id;
467 
468  arrow_marker.header.frame_id = frame_name;
469  tf2::Quaternion quat_theta;
470  quat_theta.setRPY(0,0,thetaL2);
471  //quat_theta.normalize();
472  arrow_marker.pose.orientation = tf2::toMsg(quat_theta);
473  arrow_marker.pose.position.x = closest_corner_point.first;
474  arrow_marker.pose.position.y = closest_corner_point.second;
475  arrow_marker.scale.x = L2_box;
476  arrow_marker.scale.y = 0.04;
477  arrow_marker.scale.z = 0.01;
478  return arrow_marker;
479 }
480 
481 visualization_msgs::Marker Cluster::getArrowVisualisationMessage() {
482 
483  visualization_msgs::Marker arrow_marker;
484  arrow_marker.type = visualization_msgs::Marker::ARROW;
485  //arrow_marker.header.frame_id = frame_name;
486  arrow_marker.header.frame_id = frame_name;
487  arrow_marker.header.stamp = ros::Time::now();
488  arrow_marker.ns = "velocities";
489  arrow_marker.action = visualization_msgs::Marker::ADD;
490  arrow_marker.color.a = 1.0;
491  arrow_marker.color.g = g;
492  arrow_marker.color.b = b;
493  arrow_marker.color.r = r;
494  arrow_marker.id = this->id;
495  arrow_marker.scale.x = 0.05; //Shaft diameter of the arrow
496  arrow_marker.scale.y = 0.1; //Head diameter of the arrow
497 
498  geometry_msgs::Point p;
499  p.x = cx;
500  p.y = cy;
501  p.z = 0;
502  arrow_marker.points.push_back(p);
503 
504  p.x = cx + cvx *1;
505  p.y = cy + cvy *1;
506  p.z = 0;
507  arrow_marker.points.push_back(p);
508  return arrow_marker;
509 }
510  visualization_msgs::Marker Cluster::getClosestCornerPointVisualisationMessage() {
511 
512  visualization_msgs::Marker corner_msg;
513  corner_msg.type = visualization_msgs::Marker::POINTS;
514  corner_msg.header.frame_id = frame_name;
515  corner_msg.header.stamp = ros::Time::now();
516  corner_msg.ns = "closest_corner";
517  corner_msg.action = visualization_msgs::Marker::ADD;
518  corner_msg.pose.orientation.w = 1.0;
519  //corner_msg.scale.x = 0.3;
520  //corner_msg.scale.y = 0.3;
521  corner_msg.scale.x = 0.08;
522  corner_msg.scale.y = 0.08;
523  corner_msg.color.a = 1.0;
524  corner_msg.color.g = 0.0;
525  corner_msg.color.b = 0.0;
526  corner_msg.color.r = 0.0;
527  corner_msg.id = this->id;
528 
529  geometry_msgs::Point p;
530  p.x = closest_corner_point.first;
531  p.y = closest_corner_point.second;
532  p.z = 0;
533  corner_msg.points.push_back(p);
534 
535  return corner_msg;
536 }
537  visualization_msgs::Marker Cluster::getBoundingBoxCenterVisualisationMessage() {
538 
539  visualization_msgs::Marker boxcenter_marker;
540  boxcenter_marker.type = visualization_msgs::Marker::POINTS;
541  boxcenter_marker.header.frame_id = frame_name;
542  boxcenter_marker.header.stamp = ros::Time::now();
543  boxcenter_marker.ns = "bounding_box_center";
544  boxcenter_marker.action = visualization_msgs::Marker::ADD;
545  boxcenter_marker.pose.orientation.w = 1.0;
546  boxcenter_marker.scale.x = 0.1;
547  boxcenter_marker.scale.y = 0.1;
548  boxcenter_marker.color.a = 1.0;
549  boxcenter_marker.color.r = 1;
550  boxcenter_marker.color.g = 1;
551  boxcenter_marker.color.b = 0;
552  boxcenter_marker.id = this->id;
553 
554  geometry_msgs::Point p;
555  p.x = cx;
556  p.y = cy;
557  boxcenter_marker.points.push_back(p);
558 
559  return boxcenter_marker;
560 }
561 visualization_msgs::Marker Cluster::getClusterVisualisationMessage() {
562 
563  visualization_msgs::Marker cluster_vmsg;
564  cluster_vmsg.header.frame_id = frame_name;
565  cluster_vmsg.header.stamp = ros::Time::now();
566  cluster_vmsg.ns = "clusters";
567  cluster_vmsg.action = visualization_msgs::Marker::ADD;
568  cluster_vmsg.pose.orientation.w = 1.0;
569  cluster_vmsg.type = visualization_msgs::Marker::POINTS;
570  cluster_vmsg.scale.x = 0.02;
571  cluster_vmsg.scale.y = 0.02;
572  cluster_vmsg.id = this->id;
573 
574  cluster_vmsg.color.g = this->g;
575  cluster_vmsg.color.b = this->b;
576  cluster_vmsg.color.r = this->r;
577  cluster_vmsg.color.a = 1.0;
578 
579 
580  geometry_msgs::Point p;
581 
582  for(unsigned int j=0; j<new_cluster.size(); ++j){
583  p.x = new_cluster[j].first;
584  p.y = new_cluster[j].second;
585  p.z = 0;
586  cluster_vmsg.points.push_back(p);
587  }
588 
589  return cluster_vmsg;
590 }
591 
592 visualization_msgs::Marker Cluster::getBoxSolidVisualisationMessage() {
593 
594 
595  visualization_msgs::Marker marker;
596  marker.header.frame_id = frame_name;
597  marker.header.stamp = ros::Time::now();
598  marker.ns = "boxes";
599  marker.id = this->id;
600  marker.type = visualization_msgs::Marker::CUBE;
601  marker.action = visualization_msgs::Marker::ADD;
602 
603  marker.pose.position.x = cx;
604  marker.pose.position.y = cy;
605  marker.pose.position.z = 0;
606 
607  quaternion.setRPY(0, 0, psi);
608  marker.pose.orientation = tf2::toMsg(quaternion);
609 
610  marker.scale.x = length_box;
611  marker.scale.y = width_box;
612  marker.scale.z = 0.01;
613 
614  marker.color.r = this->r;
615  marker.color.g = this->g;
616  marker.color.b = this->b;
617  marker.color.a = 1.0;
618 
619  return marker;
620 }
621 
622 visualization_msgs::Marker Cluster::getLineVisualisationMessage() {
623 
624  visualization_msgs::Marker line_msg;
625 
626  line_msg.header.stamp = ros::Time::now();
627  line_msg.header.frame_id = frame_name;
628  line_msg.ns = "lines";
629  line_msg.action = visualization_msgs::Marker::ADD;
630  line_msg.pose.orientation.w = 1.0;
631  line_msg.type = visualization_msgs::Marker::LINE_STRIP;
632  line_msg.id = this->id;
633  line_msg.scale.x = 0.1; //line width
634  line_msg.color.g = this->g;
635  line_msg.color.b = this->b;
636  line_msg.color.r = this->r;
637  line_msg.color.a = 1.0;
638 
639 
640  //Feed the cluster into the Iterative End-Point Fit Function
641  //and the l_shape_extractor and then save them into the l_shapes vector
642  // Line and L-Shape Extraction
643 
644  std::vector<Point> pointListOut;
645  Cluster::ramerDouglasPeucker(new_cluster, 0.1, pointListOut);
646  geometry_msgs::Point p;
647  for(unsigned int k =0 ;k<pointListOut.size();++k){
648  p.x = pointListOut[k].first;
649  p.y = pointListOut[k].second;
650  p.z = 0;
651 
652  line_msg.points.push_back(p);
653  }
654  double l;
655  if(pointListOut.size()==1){
656  for(unsigned int k=0; k<pointListOut.size()-1;++k){
657  l = sqrt(pow(pointListOut[k+1].first - pointListOut[k].first,2) + pow(pointListOut[k+1].second - pointListOut[k].second,2));
658  }
659  }
660  return line_msg;
661 
662 }
663 void Cluster::calcMean(const pointList& c){
664 
665  double sum_x = 0, sum_y = 0;
666 
667  for(unsigned int i = 0; i<c.size(); ++i){
668 
669  sum_x = sum_x + c[i].first;
670  sum_y = sum_y + c[i].second;
671  }
672 
673  this->mean_values.first = sum_x / c.size();
674  this->mean_values.second= sum_y / c.size();
675 }
676 double Cluster::perpendicularDistance(const Point &pt, const Point &lineStart, const Point &lineEnd){
677  //2D implementation of the Ramer-Douglas-Peucker algorithm
678  //By Tim Sheerman-Chase, 2016
679  //Released under CC0
680  //https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
681  double dx = lineEnd.first - lineStart.first;
682  double dy = lineEnd.second - lineStart.second;
683 
684  //Normalise
685  double mag = pow(pow(dx,2.0)+pow(dy,2.0),0.5);
686  if(mag > 0.0)
687  {
688  dx /= mag; dy /= mag;
689  }
690 
691  double pvx = pt.first - lineStart.first;
692  double pvy = pt.second - lineStart.second;
693 
694  //Get dot product (project pv onto normalized direction)
695  double pvdot = dx * pvx + dy * pvy;
696 
697  //Scale line direction vector
698  double dsx = pvdot * dx;
699  double dsy = pvdot * dy;
700 
701  //Subtract this from pv
702  double ax = pvx - dsx;
703  double ay = pvy - dsy;
704 
705  return pow(pow(ax,2.0)+pow(ay,2.0),0.5);
706 }
707 void Cluster::ramerDouglasPeucker(const std::vector<Point> &pointList, double epsilon, std::vector<Point> &out){
708  //2D implementation of the Ramer-Douglas-Peucker algorithm
709  //By Tim Sheerman-Chase, 2016
710  //Released under CC0
711  //https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
712 
713  // Find the point with the maximum distance from line between start and end
714  double dmax = 0.0;
715  size_t index = 0;
716  size_t end = pointList.size()-1;
717  for(size_t i = 1; i < end; i++)
718  {
719  double d = perpendicularDistance(pointList[i], pointList[0], pointList[end]);
720  if (d > dmax)
721  {
722  index = i;
723  dmax = d;
724  }
725  }
726 
727  // If max distance is greater than epsilon, recursively simplify
728  if(dmax > epsilon)
729  {
730  // Recursive call
731  std::vector<Point> recResults1;
732  std::vector<Point> recResults2;
733  std::vector<Point> firstLine(pointList.begin(), pointList.begin()+index+1);
734  std::vector<Point> lastLine(pointList.begin()+index, pointList.end());
735  ramerDouglasPeucker(firstLine, epsilon, recResults1);
736  ramerDouglasPeucker(lastLine, epsilon, recResults2);
737 
738  // Build the result list
739  out.assign(recResults1.begin(), recResults1.end()-1);
740  out.insert(out.end(), recResults2.begin(), recResults2.end());
741  if(out.size()<2)
742  throw std::runtime_error("Problem assembling output");
743  }
744  else
745  {
746  //Just return start and end points
747  out.clear();
748  out.push_back(pointList[0]);
749  out.push_back(pointList[end]);
750  }
751 }
d
d
Cluster::new_cluster
pointList new_cluster
Definition: cluster.hpp:91
Cluster::msg_track_box_kf
datmo::Track msg_track_box_kf
Definition: cluster.hpp:57
Cluster::getArrowVisualisationMessage
visualization_msgs::Marker getArrowVisualisationMessage()
Definition: cluster.cpp:481
Cluster::corner_list
std::vector< Point > corner_list
Definition: cluster.hpp:92
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
Cluster::l1l2
std::vector< Point > l1l2
Definition: cluster.hpp:94
Cluster::getClosestCornerPointVisualisationMessage
visualization_msgs::Marker getClosestCornerPointVisualisationMessage()
Definition: cluster.cpp:510
normalize_angle
static double normalize_angle(double angle)
Definition: cluster.cpp:40
Cluster::closest_corner_point
Point closest_corner_point
Definition: cluster.hpp:100
Cluster::Lshape
LshapeTracker Lshape
Definition: cluster.hpp:80
Cluster::cvy
double cvy
Definition: cluster.hpp:86
Cluster::update
void update(const pointList &, const double dt, const tf::Transform &ego_pose)
Definition: cluster.cpp:84
Cluster::g
float g
Definition: cluster.hpp:61
Cluster::getBoundingBoxVisualisationMessage
visualization_msgs::Marker getBoundingBoxVisualisationMessage()
Definition: cluster.cpp:230
Cluster::width_box
double width_box
Definition: cluster.hpp:86
Point
std::pair< double, double > Point
Definition: cluster.hpp:45
Cluster::lineIntersection
Point lineIntersection(double &, double &, double &, double &, double &, double &)
Definition: cluster.cpp:343
Point
tf::Vector3 Point
shortest_angular_distance
static double shortest_angular_distance(double from, double to)
Definition: cluster.cpp:49
Cluster::cx
double cx
Definition: cluster.hpp:86
Cluster::cvx
double cvx
Definition: cluster.hpp:86
Cluster::populateTrackingMsgs
void populateTrackingMsgs(const double &dt)
Definition: cluster.cpp:104
Cluster::cy
double cy
Definition: cluster.hpp:86
Cluster::th
double th
Definition: cluster.hpp:86
Cluster::areaCriterion
double areaCriterion(const VectorXd &, const VectorXd &)
Definition: cluster.cpp:356
Cluster::b
float b
Definition: cluster.hpp:61
Cluster::L1_box
double L1_box
Definition: cluster.hpp:86
Cluster::age
unsigned long int age
Definition: cluster.hpp:60
Cluster::length_box
double length_box
Definition: cluster.hpp:86
Cluster::r
float r
Definition: cluster.hpp:61
Cluster::frame_name
std::string frame_name
Definition: cluster.hpp:54
pointList
std::vector< Point > pointList
Definition: cluster.hpp:46
Cluster::L2
double L2
Definition: cluster.hpp:85
Cluster::Cluster
Cluster(unsigned long int id, const pointList &, const double &, const std::string &, const tf::Transform &ego_pose)
Definition: cluster.cpp:57
Cluster::getThetaL2VisualisationMessage
visualization_msgs::Marker getThetaL2VisualisationMessage()
Definition: cluster.cpp:454
Cluster::getThetaL1VisualisationMessage
visualization_msgs::Marker getThetaL1VisualisationMessage()
Definition: cluster.cpp:427
Cluster::getClusterVisualisationMessage
visualization_msgs::Marker getClusterVisualisationMessage()
Definition: cluster.cpp:561
Cluster::id
unsigned long int id
Definition: cluster.hpp:59
Cluster::getLineVisualisationMessage
visualization_msgs::Marker getLineVisualisationMessage()
Definition: cluster.cpp:622
Cluster::getThetaBoxVisualisationMessage
visualization_msgs::Marker getThetaBoxVisualisationMessage()
Definition: cluster.cpp:400
min
double min(double a, double b)
Cluster::thetaL1
double thetaL1
Definition: cluster.hpp:85
Cluster::getBoxModelKFVisualisationMessage
visualization_msgs::Marker getBoxModelKFVisualisationMessage()
Definition: cluster.cpp:259
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
normalize_angle_positive
static double normalize_angle_positive(double angle)
Definition: cluster.cpp:35
Cluster::quaternion
tf2::Quaternion quaternion
Definition: cluster.hpp:93
Cluster::getBoundingBoxCenterVisualisationMessage
visualization_msgs::Marker getBoundingBoxCenterVisualisationMessage()
Definition: cluster.cpp:537
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
Cluster::thetaL2
double thetaL2
Definition: cluster.hpp:85
tf2::toMsg
B toMsg(const A &a)
pow
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Cluster::ramerDouglasPeucker
void ramerDouglasPeucker(const std::vector< Point > &, double, std::vector< Point > &)
Definition: cluster.cpp:707
step
unsigned int step
Cluster::comega
double comega
Definition: cluster.hpp:86
Cluster::psi
double psi
Definition: cluster.hpp:86
Cluster::ego_coordinates
Point ego_coordinates
Definition: cluster.hpp:55
LshapeTracker::BoxModel
void BoxModel(double &x, double &y, double &vx, double &vy, double &theta, double &psi, double &omega, double &L1, double &L2, double &length, double &width)
Definition: l_shape_tracker.cpp:260
Cluster::mean_values
std::pair< double, double > mean_values
Definition: cluster.hpp:97
Cluster::getBoxSolidVisualisationMessage
visualization_msgs::Marker getBoxSolidVisualisationMessage()
Definition: cluster.cpp:592
Cluster::perpendicularDistance
double perpendicularDistance(const Point &, const Point &, const Point &)
Definition: cluster.cpp:676
Cluster::previous_mean_values
std::pair< double, double > previous_mean_values
Definition: cluster.hpp:98
LshapeTracker::update
void update(const double &thetaL1, const double &x_corner, const double &y_corner, const double &L1, const double &L2, const double &dt, const int cluster_size)
Definition: l_shape_tracker.cpp:147
Cluster::a
float a
Definition: cluster.hpp:61
Cluster::rectangleFitting
void rectangleFitting(const pointList &)
Definition: cluster.cpp:123
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
ros::Time::now
static Time now()
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Cluster::calcMean
void calcMean(const pointList &)
Definition: cluster.cpp:663
Cluster::L2_box
double L2_box
Definition: cluster.hpp:86
Cluster::closenessCriterion
double closenessCriterion(const VectorXd &, const VectorXd &, const double &)
Definition: cluster.cpp:364
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Cluster::L1
double L1
Definition: cluster.hpp:85
Cluster::getLShapeVisualisationMessage
visualization_msgs::Marker getLShapeVisualisationMessage()
Definition: cluster.cpp:307

datmo
Author(s): Kostas Konstantinidis
autogenerated on Tue May 2 2023 02:58:06