Fawkes API  Fawkes Development Version
visualization_thread.cpp
1 
2 /***************************************************************************
3  * visualization_thread.cpp - Visualization via rviz
4  *
5  * Created: Fri Nov 11 00:20:45 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "visualization_thread.h"
23 
24 #include "cluster_colors.h"
25 
26 #include <core/threading/mutex_locker.h>
27 #include <ros/ros.h>
28 #include <utils/math/angle.h>
29 #include <visualization_msgs/MarkerArray.h>
30 #ifdef USE_POSEPUB
31 # include <geometry_msgs/PointStamped.h>
32 #endif
33 #include <Eigen/Geometry>
34 
35 extern "C" {
36 #ifdef HAVE_QHULL_2011
37 # include "libqhull/geom.h"
38 # include "libqhull/io.h"
39 # include "libqhull/libqhull.h"
40 # include "libqhull/mem.h"
41 # include "libqhull/merge.h"
42 # include "libqhull/poly.h"
43 # include "libqhull/qset.h"
44 # include "libqhull/stat.h"
45 #else
46 # include "qhull/geom.h"
47 # include "qhull/io.h"
48 # include "qhull/mem.h"
49 # include "qhull/merge.h"
50 # include "qhull/poly.h"
51 # include "qhull/qhull.h"
52 # include "qhull/qset.h"
53 # include "qhull/stat.h"
54 #endif
55 }
56 
57 #include <cassert>
58 
59 #define CFG_PREFIX "/perception/tabletop-objects/"
60 #define CFG_PREFIX_VIS "/perception/tabletop-objects/visualization/"
61 
62 using namespace fawkes;
63 
64 /** @class TabletopVisualizationThread "visualization_thread.h"
65  * Send Marker messages to rviz.
66  * This class takes input from the table top object detection thread and
67  * publishes according marker messages for visualization in rviz.
68  * @author Tim Niemueller
69  */
70 
71 /** Constructor. */
73 : fawkes::Thread("TabletopVisualizationThread", Thread::OPMODE_WAITFORWAKEUP)
74 {
76 }
77 
78 void
80 {
81  cfg_show_frustrum_ = false;
82  cfg_show_cvxhull_vertices_ = true;
83  cfg_show_cvxhull_line_highlighting_ = true;
84  cfg_show_cvxhull_vertex_ids_ = true;
85  try {
86  cfg_show_frustrum_ = config->get_bool(CFG_PREFIX_VIS "show_frustrum");
87  } catch (Exception &e) {
88  } // ignored, use default
89  if (cfg_show_frustrum_) {
90  cfg_horizontal_va_ = deg2rad(config->get_float(CFG_PREFIX "horizontal_viewing_angle"));
91  cfg_vertical_va_ = deg2rad(config->get_float(CFG_PREFIX "vertical_viewing_angle"));
92  }
93  cfg_duration_ = 120;
94  try {
95  cfg_duration_ = config->get_uint(CFG_PREFIX_VIS "display_duration");
96  } catch (Exception &e) {
97  } // ignored, use default
98 
99  try {
100  cfg_show_cvxhull_vertices_ = config->get_bool(CFG_PREFIX_VIS "show_convex_hull_vertices");
101  } catch (Exception &e) {
102  } // ignored, use default
103  try {
104  cfg_show_cvxhull_line_highlighting_ =
105  config->get_bool(CFG_PREFIX_VIS "show_convex_hull_line_highlighting");
106  } catch (Exception &e) {
107  } // ignored, use default
108  try {
109  cfg_show_cvxhull_vertex_ids_ = config->get_bool(CFG_PREFIX_VIS "show_convex_hull_vertex_ids");
110  } catch (Exception &e) {
111  } // ignored, use default
112  try {
113  cfg_cylinder_fitting_ = config->get_bool(CFG_PREFIX "enable_cylinder_fitting");
114  } catch (Exception &e) {
115  } // ignored, use default
116 
117  cfg_base_frame_ = config->get_string("/frames/base");
118 
119  vispub_ = new ros::Publisher();
120  *vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
121 #ifdef USE_POSEPUB
122  posepub_ = new ros::Publisher();
123  *posepub_ = rosnode->advertise<geometry_msgs::PointStamped>("table_point", 10);
124 #endif
125  last_id_num_ = 0;
126 }
127 
128 void
130 {
131  visualization_msgs::MarkerArray m;
132 
133  for (size_t i = 0; i < last_id_num_; ++i) {
134  visualization_msgs::Marker delop;
135  delop.header.frame_id = frame_id_;
136  delop.header.stamp = ros::Time::now();
137  delop.ns = "tabletop";
138  delop.id = i;
139  delop.action = visualization_msgs::Marker::DELETE;
140  m.markers.push_back(delop);
141  }
142  vispub_->publish(m);
143 
144  vispub_->shutdown();
145  delete vispub_;
146 #ifdef USE_POSEPUB
147  posepub_->shutdown();
148  delete posepub_;
149 #endif
150 }
151 
152 void
154 {
155  MutexLocker lock(&mutex_);
156  visualization_msgs::MarkerArray m;
157 
158  unsigned int idnum = 0;
159  for (M_Vector4f::iterator it = centroids_.begin(); it != centroids_.end(); ++it) {
160  try {
161  /*
162  tf::Stamped<tf::Point> centroid(tf::Point(centroids_[i][0], centroids_[i][1], centroids_[i][2]), fawkes::Time(0, 0), frame_id_);
163  tf::Stamped<tf::Point> baserel_centroid;
164  tf_listener->transform_point(cfg_base_frame_, centroid, baserel_centroid);
165  */
166 
167  tf::Stamped<tf::Point> centroid(tf::Point(it->second[0], it->second[1], it->second[2]),
168  fawkes::Time(0, 0),
169  cfg_base_frame_);
170  tf::Stamped<tf::Point> camrel_centroid;
171  tf_listener->transform_point(frame_id_, centroid, camrel_centroid);
172 
173  char *tmp;
174  if (asprintf(&tmp, "TObj %u", it->first) != -1) {
175  // Copy to get memory freed on exception
176  std::string id = tmp;
177  free(tmp);
178 
179  visualization_msgs::Marker text;
180  text.header.frame_id = cfg_base_frame_;
181  text.header.stamp = ros::Time::now();
182  text.ns = "tabletop";
183  text.id = idnum++;
184  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
185  text.action = visualization_msgs::Marker::ADD;
186  /* text.pose.position.x = baserel_centroid[0];
187  text.pose.position.y = baserel_centroid[1];
188  text.pose.position.z = baserel_centroid[2] + 0.17;*/
189  text.pose.position.x = centroid[0];
190  text.pose.position.y = centroid[1];
191  text.pose.position.z = centroid[2] + 0.17;
192  text.pose.orientation.w = 1.;
193  text.scale.z = 0.05; // 5cm high
194  text.color.r = text.color.g = text.color.b = 1.0f;
195  text.color.a = 1.0;
196  text.lifetime = ros::Duration(cfg_duration_, 0);
197  text.text = id;
198  m.markers.push_back(text);
199  }
200 
201  visualization_msgs::Marker sphere;
202  sphere.header.frame_id = cfg_base_frame_;
203  sphere.header.stamp = ros::Time::now();
204  sphere.ns = "tabletop";
205  sphere.id = idnum++;
206  sphere.type = visualization_msgs::Marker::CYLINDER;
207  sphere.action = visualization_msgs::Marker::ADD;
208 
209  if (cfg_cylinder_fitting_) {
210  /*
211  sphere.scale.x = sphere.scale.y = 0.08;
212  sphere.scale.z = 0.09;
213  */
214  sphere.scale.x = sphere.scale.y = 2 * cylinder_params_[it->first][0];
215  sphere.scale.z = cylinder_params_[it->first][1];
216  //if (obj_confidence_[it->first] >= 0.5)
217  if (best_obj_guess_[it->first] < 0) {
218  sphere.color.r = 1.0;
219  sphere.color.g = 0.0;
220  sphere.color.b = 0.0;
221  } else {
222  sphere.color.r = 0.0;
223  sphere.color.g = 1.0;
224  sphere.color.b = 0.0;
225  }
226  /*
227  sphere.color.r = (float)cluster_colors[it->first % MAX_CENTROIDS][0] / 255.f;
228  sphere.color.g = (float)cluster_colors[it->first % MAX_CENTROIDS][1] / 255.f;
229  sphere.color.b = (float)cluster_colors[it->first % MAX_CENTROIDS][2] / 255.f;
230  */
231  sphere.color.a = 1.0;
232 
233  /*
234  sphere.pose.position.x = baserel_centroid[0];
235  sphere.pose.position.y = baserel_centroid[1];
236  sphere.pose.position.z = baserel_centroid[2];
237  */
238  sphere.pose.position.x = centroid[0];
239  sphere.pose.position.y = centroid[1];
240  sphere.pose.position.z = centroid[2];
241  //////////////
242  tf::Quaternion table_quat(tf::Vector3(0, 1, 0), cylinder_params_[2][0]);
243  /*
244  sphere.pose.orientation.x = table_quat.getX();
245  sphere.pose.orientation.y = table_quat.getY();
246  sphere.pose.orientation.z = table_quat.getZ();
247  sphere.pose.orientation.w = table_quat.getW();
248  */
249  sphere.pose.orientation.w = 1.;
250  //////////////
251  sphere.lifetime = ros::Duration(cfg_duration_, 0);
252  m.markers.push_back(sphere);
253  } else {
254  sphere.pose.position.x = centroid[0];
255  sphere.pose.position.y = centroid[1];
256  sphere.pose.position.z = centroid[2];
257  sphere.pose.orientation.w = 1.;
258  sphere.scale.x = sphere.scale.y = 0.08;
259  sphere.scale.z = 0.09;
260  sphere.color.r = (float)cluster_colors[it->first % MAX_CENTROIDS][0] / 255.f;
261  sphere.color.g = (float)cluster_colors[it->first % MAX_CENTROIDS][1] / 255.f;
262  sphere.color.b = (float)cluster_colors[it->first % MAX_CENTROIDS][2] / 255.f;
263  sphere.color.a = 1.0;
264  sphere.lifetime = ros::Duration(cfg_duration_, 0);
265  m.markers.push_back(sphere);
266  }
267  } catch (Exception &e) {
268  } // ignored
269  }
270 
271  Eigen::Vector4f normal_end = (table_centroid_ + (normal_ * -0.15));
272 
273  visualization_msgs::Marker normal;
274  normal.header.frame_id = frame_id_;
275  normal.header.stamp = ros::Time::now();
276  normal.ns = "tabletop";
277  normal.id = idnum++;
278  normal.type = visualization_msgs::Marker::ARROW;
279  normal.action = visualization_msgs::Marker::ADD;
280  normal.points.resize(2);
281  normal.points[0].x = table_centroid_[0];
282  normal.points[0].y = table_centroid_[1];
283  normal.points[0].z = table_centroid_[2];
284  normal.points[1].x = normal_end[0];
285  normal.points[1].y = normal_end[1];
286  normal.points[1].z = normal_end[2];
287  normal.scale.x = 0.02;
288  normal.scale.y = 0.04;
289  normal.color.r = 0.4;
290  normal.color.g = normal.color.b = 0.f;
291  normal.color.a = 1.0;
292  normal.lifetime = ros::Duration(cfg_duration_, 0);
293  m.markers.push_back(normal);
294 
295  if (cfg_show_cvxhull_line_highlighting_) {
296  // "Good" lines are highlighted
297  visualization_msgs::Marker hull_lines;
298  hull_lines.header.frame_id = frame_id_;
299  hull_lines.header.stamp = ros::Time::now();
300  hull_lines.ns = "tabletop";
301  hull_lines.id = idnum++;
302  hull_lines.type = visualization_msgs::Marker::LINE_LIST;
303  hull_lines.action = visualization_msgs::Marker::ADD;
304  hull_lines.points.resize(good_table_hull_edges_.size());
305  hull_lines.colors.resize(good_table_hull_edges_.size());
306  for (size_t i = 0; i < good_table_hull_edges_.size(); ++i) {
307  hull_lines.points[i].x = good_table_hull_edges_[i][0];
308  hull_lines.points[i].y = good_table_hull_edges_[i][1];
309  hull_lines.points[i].z = good_table_hull_edges_[i][2];
310  hull_lines.colors[i].r = 0.;
311  hull_lines.colors[i].b = 0.;
312  hull_lines.colors[i].a = 0.4;
313  if (good_table_hull_edges_[i][3] > 0.) {
314  hull_lines.colors[i].g = 1.0;
315  } else {
316  hull_lines.colors[i].g = 0.5;
317  }
318  }
319  hull_lines.color.a = 1.0;
320  hull_lines.scale.x = 0.01;
321  hull_lines.lifetime = ros::Duration(cfg_duration_, 0);
322  m.markers.push_back(hull_lines);
323  } else {
324  // Table surrounding polygon
325  visualization_msgs::Marker hull;
326  hull.header.frame_id = frame_id_;
327  hull.header.stamp = ros::Time::now();
328  hull.ns = "tabletop";
329  hull.id = idnum++;
330  hull.type = visualization_msgs::Marker::LINE_STRIP;
331  hull.action = visualization_msgs::Marker::ADD;
332  hull.points.resize(table_hull_vertices_.size() + 1);
333  for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
334  hull.points[i].x = table_hull_vertices_[i][0];
335  hull.points[i].y = table_hull_vertices_[i][1];
336  hull.points[i].z = table_hull_vertices_[i][2];
337  }
338  hull.points[table_hull_vertices_.size()].x = table_hull_vertices_[0][0];
339  hull.points[table_hull_vertices_.size()].y = table_hull_vertices_[0][1];
340  hull.points[table_hull_vertices_.size()].z = table_hull_vertices_[0][2];
341  hull.scale.x = 0.005;
342  hull.color.r = 0.4;
343  hull.color.g = hull.color.b = 0.f;
344  hull.color.a = 0.2;
345  hull.lifetime = ros::Duration(cfg_duration_, 0);
346  m.markers.push_back(hull);
347  }
348 
349  if (cfg_show_cvxhull_vertices_) {
350  visualization_msgs::Marker hull_points;
351  hull_points.header.frame_id = frame_id_;
352  hull_points.header.stamp = ros::Time::now();
353  hull_points.ns = "tabletop";
354  hull_points.id = idnum++;
355  hull_points.type = visualization_msgs::Marker::SPHERE_LIST;
356  hull_points.action = visualization_msgs::Marker::ADD;
357  hull_points.points.resize(table_hull_vertices_.size());
358  for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
359  hull_points.points[i].x = table_hull_vertices_[i][0];
360  hull_points.points[i].y = table_hull_vertices_[i][1];
361  hull_points.points[i].z = table_hull_vertices_[i][2];
362  }
363  hull_points.scale.x = 0.01;
364  hull_points.scale.y = 0.01;
365  hull_points.scale.z = 0.01;
366  hull_points.color.r = 0.8;
367  hull_points.color.g = hull_points.color.b = 0.f;
368  hull_points.color.a = 1.0;
369  hull_points.lifetime = ros::Duration(cfg_duration_, 0);
370  m.markers.push_back(hull_points);
371  }
372 
373  // hull texts
374  if (cfg_show_cvxhull_vertex_ids_) {
375  for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
376  char *tmp;
377  if (asprintf(&tmp, "Cvx_%zu", i) != -1) {
378  // Copy to get memory freed on exception
379  std::string id = tmp;
380  free(tmp);
381 
382  visualization_msgs::Marker text;
383  text.header.frame_id = frame_id_;
384  text.header.stamp = ros::Time::now();
385  text.ns = "tabletop";
386  text.id = idnum++;
387  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
388  text.action = visualization_msgs::Marker::ADD;
389  text.pose.position.x = table_hull_vertices_[i][0];
390  text.pose.position.y = table_hull_vertices_[i][1];
391  text.pose.position.z = table_hull_vertices_[i][2] + 0.1;
392  text.pose.orientation.w = 1.;
393  text.scale.z = 0.03;
394  text.color.r = text.color.g = text.color.b = 1.0f;
395  text.color.a = 1.0;
396  text.lifetime = ros::Duration(cfg_duration_, 0);
397  text.text = id;
398  m.markers.push_back(text);
399  }
400  }
401  }
402 
403  // Table model surrounding polygon
404  if (!(table_model_vertices_.empty() && table_hull_vertices_.empty())) {
405  visualization_msgs::Marker hull;
406  hull.header.frame_id = frame_id_;
407  hull.header.stamp = ros::Time::now();
408  hull.ns = "tabletop";
409  hull.id = idnum++;
410  hull.type = visualization_msgs::Marker::LINE_STRIP;
411  hull.action = visualization_msgs::Marker::ADD;
412 
413  if (!table_model_vertices_.empty()) {
414  hull.points.resize(table_model_vertices_.size() + 1);
415  for (size_t i = 0; i < table_model_vertices_.size(); ++i) {
416  hull.points[i].x = table_model_vertices_[i][0];
417  hull.points[i].y = table_model_vertices_[i][1];
418  hull.points[i].z = table_model_vertices_[i][2];
419  }
420  hull.points[table_model_vertices_.size()].x = table_model_vertices_[0][0];
421  hull.points[table_model_vertices_.size()].y = table_model_vertices_[0][1];
422  hull.points[table_model_vertices_.size()].z = table_model_vertices_[0][2];
423  } else if (!table_hull_vertices_.empty()) {
424  hull.points.resize(table_hull_vertices_.size() + 1);
425  for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
426  hull.points[i].x = table_hull_vertices_[i][0];
427  hull.points[i].y = table_hull_vertices_[i][1];
428  hull.points[i].z = table_hull_vertices_[i][2];
429  }
430  hull.points[table_hull_vertices_.size()].x = table_hull_vertices_[0][0];
431  hull.points[table_hull_vertices_.size()].y = table_hull_vertices_[0][1];
432  hull.points[table_hull_vertices_.size()].z = table_hull_vertices_[0][2];
433  }
434  hull.scale.x = 0.0075;
435  hull.color.r = 0.5;
436  hull.color.g = hull.color.b = 0.f;
437  hull.color.a = 1.0;
438  hull.lifetime = ros::Duration(cfg_duration_, 0);
439  m.markers.push_back(hull);
440  }
441 
442  //triangulate_hull();
443 
444  if (table_model_vertices_.size() == 4) {
445  visualization_msgs::Marker plane;
446  plane.header.frame_id = frame_id_;
447  plane.header.stamp = ros::Time::now();
448  plane.ns = "tabletop";
449  plane.id = idnum++;
450  plane.type = visualization_msgs::Marker::TRIANGLE_LIST;
451  plane.action = visualization_msgs::Marker::ADD;
452  plane.points.resize(6);
453  for (unsigned int i = 0; i < 3; ++i) {
454  plane.points[i].x = table_model_vertices_[i][0];
455  plane.points[i].y = table_model_vertices_[i][1];
456  plane.points[i].z = table_model_vertices_[i][2];
457  }
458  for (unsigned int i = 2; i < 5; ++i) {
459  plane.points[i + 1].x = table_model_vertices_[i % 4][0];
460  plane.points[i + 1].y = table_model_vertices_[i % 4][1];
461  plane.points[i + 1].z = table_model_vertices_[i % 4][2];
462  }
463  plane.pose.orientation.w = 1.;
464  plane.scale.x = 1.0;
465  plane.scale.y = 1.0;
466  plane.scale.z = 1.0;
467  plane.color.r = ((float)table_color[0] / 255.f) * 0.8;
468  plane.color.g = ((float)table_color[1] / 255.f) * 0.8;
469  plane.color.b = ((float)table_color[2] / 255.f) * 0.8;
470  plane.color.a = 1.0;
471  plane.lifetime = ros::Duration(cfg_duration_, 0);
472  m.markers.push_back(plane);
473  }
474 
475  if (cfg_show_frustrum_ && !table_model_vertices_.empty()) {
476  // Frustrum
477  visualization_msgs::Marker frustrum;
478  frustrum.header.frame_id = frame_id_;
479  frustrum.header.stamp = ros::Time::now();
480  frustrum.ns = "tabletop";
481  frustrum.id = idnum++;
482  frustrum.type = visualization_msgs::Marker::LINE_LIST;
483  frustrum.action = visualization_msgs::Marker::ADD;
484  frustrum.points.resize(8);
485  frustrum.points[0].x = frustrum.points[2].x = frustrum.points[4].x = frustrum.points[6].x = 0.;
486  frustrum.points[0].y = frustrum.points[2].y = frustrum.points[4].y = frustrum.points[6].y = 0.;
487  frustrum.points[0].z = frustrum.points[2].z = frustrum.points[4].z = frustrum.points[6].z = 0.;
488 
489  const float half_hva = cfg_horizontal_va_ * 0.5;
490  const float half_vva = cfg_vertical_va_ * 0.5;
491 
492  Eigen::Matrix3f upper_right_m;
493  upper_right_m = Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ())
494  * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY());
495  Eigen::Vector3f upper_right = upper_right_m * Eigen::Vector3f(4, 0, 0);
496 
497  Eigen::Matrix3f upper_left_m;
498  upper_left_m = Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ())
499  * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY());
500  Eigen::Vector3f upper_left = upper_left_m * Eigen::Vector3f(4, 0, 0);
501 
502  Eigen::Matrix3f lower_right_m;
503  lower_right_m = Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ())
504  * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY());
505  Eigen::Vector3f lower_right = lower_right_m * Eigen::Vector3f(2, 0, 0);
506 
507  Eigen::Matrix3f lower_left_m;
508  lower_left_m = Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ())
509  * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY());
510  Eigen::Vector3f lower_left = lower_left_m * Eigen::Vector3f(2, 0, 0);
511 
512  frustrum.points[1].x = upper_right[0];
513  frustrum.points[1].y = upper_right[1];
514  frustrum.points[1].z = upper_right[2];
515 
516  frustrum.points[3].x = lower_right[0];
517  frustrum.points[3].y = lower_right[1];
518  frustrum.points[3].z = lower_right[2];
519 
520  frustrum.points[5].x = lower_left[0];
521  frustrum.points[5].y = lower_left[1];
522  frustrum.points[5].z = lower_left[2];
523 
524  frustrum.points[7].x = upper_left[0];
525  frustrum.points[7].y = upper_left[1];
526  frustrum.points[7].z = upper_left[2];
527 
528  frustrum.scale.x = 0.005;
529  frustrum.color.r = 1.0;
530  frustrum.color.g = frustrum.color.b = 0.f;
531  frustrum.color.a = 1.0;
532  frustrum.lifetime = ros::Duration(cfg_duration_, 0);
533  m.markers.push_back(frustrum);
534 
535  visualization_msgs::Marker frustrum_triangles;
536  frustrum_triangles.header.frame_id = frame_id_;
537  frustrum_triangles.header.stamp = ros::Time::now();
538  frustrum_triangles.ns = "tabletop";
539  frustrum_triangles.id = idnum++;
540  frustrum_triangles.type = visualization_msgs::Marker::TRIANGLE_LIST;
541  frustrum_triangles.action = visualization_msgs::Marker::ADD;
542  frustrum_triangles.points.resize(9);
543  frustrum_triangles.points[0].x = frustrum_triangles.points[3].x =
544  frustrum_triangles.points[6].x = 0.;
545  frustrum_triangles.points[0].y = frustrum_triangles.points[3].y =
546  frustrum_triangles.points[3].y = 0.;
547  frustrum_triangles.points[0].z = frustrum_triangles.points[3].z =
548  frustrum_triangles.points[3].z = 0.;
549 
550  frustrum_triangles.points[1].x = upper_right[0];
551  frustrum_triangles.points[1].y = upper_right[1];
552  frustrum_triangles.points[1].z = upper_right[2];
553 
554  frustrum_triangles.points[2].x = lower_right[0];
555  frustrum_triangles.points[2].y = lower_right[1];
556  frustrum_triangles.points[2].z = lower_right[2];
557 
558  frustrum_triangles.points[4].x = lower_left[0];
559  frustrum_triangles.points[4].y = lower_left[1];
560  frustrum_triangles.points[4].z = lower_left[2];
561 
562  frustrum_triangles.points[5].x = upper_left[0];
563  frustrum_triangles.points[5].y = upper_left[1];
564  frustrum_triangles.points[5].z = upper_left[2];
565 
566  frustrum_triangles.points[7].x = lower_left[0];
567  frustrum_triangles.points[7].y = lower_left[1];
568  frustrum_triangles.points[7].z = lower_left[2];
569 
570  frustrum_triangles.points[8].x = lower_right[0];
571  frustrum_triangles.points[8].y = lower_right[1];
572  frustrum_triangles.points[8].z = lower_right[2];
573 
574  frustrum_triangles.scale.x = 1;
575  frustrum_triangles.scale.y = 1;
576  frustrum_triangles.scale.z = 1;
577  frustrum_triangles.color.r = 1.0;
578  frustrum_triangles.color.g = frustrum_triangles.color.b = 0.f;
579  frustrum_triangles.color.a = 0.23;
580  frustrum_triangles.lifetime = ros::Duration(cfg_duration_, 0);
581  m.markers.push_back(frustrum_triangles);
582  } // end show frustrum
583 
584  for (size_t i = idnum; i < last_id_num_; ++i) {
585  visualization_msgs::Marker delop;
586  delop.header.frame_id = frame_id_;
587  delop.header.stamp = ros::Time::now();
588  delop.ns = "tabletop";
589  delop.id = i;
590  delop.action = visualization_msgs::Marker::DELETE;
591  m.markers.push_back(delop);
592  }
593  last_id_num_ = idnum;
594 
595  vispub_->publish(m);
596 
597 #ifdef USE_POSEPUB
598  geometry_msgs::PointStamped p;
599  p.header.frame_id = frame_id_;
600  p.header.stamp = ros::Time::now();
601  p.point.x = table_centroid_[0];
602  p.point.y = table_centroid_[1];
603  p.point.z = table_centroid_[2];
604  posepub_->publish(p);
605 #endif
606 }
607 
608 /**
609  * Visualize the given data.
610  * @param frame_id reference frame ID
611  * @param table_centroid centroid of table
612  * @param normal normal vector of table
613  * @param table_hull_vertices points of the table hull
614  * @param table_model_vertices points of the fitted table model
615  * @param good_table_hull_edges "good" egdes in table hull, i.e. edges that have
616  * been considered for determining the table orientation
617  * @param centroids object cluster centroids
618  * @param cylinder_params The result of the cylinder fitting of the objects
619  * @param obj_confidence The fitting confidences
620  * @param best_obj_guess The best guesses of the objects
621  */
622 void
623 TabletopVisualizationThread::visualize(const std::string & frame_id,
624  Eigen::Vector4f & table_centroid,
625  Eigen::Vector4f & normal,
626  V_Vector4f & table_hull_vertices,
627  V_Vector4f & table_model_vertices,
628  V_Vector4f & good_table_hull_edges,
629  M_Vector4f & centroids,
630  M_Vector4f & cylinder_params,
631  std::map<unsigned int, double> & obj_confidence,
632  std::map<unsigned int, signed int> &best_obj_guess) throw()
633 {
634  MutexLocker lock(&mutex_);
635  frame_id_ = frame_id;
636  table_centroid_ = table_centroid;
637  normal_ = normal;
638  table_hull_vertices_ = table_hull_vertices;
639  table_model_vertices_ = table_model_vertices;
640  good_table_hull_edges_ = good_table_hull_edges;
641  centroids_ = centroids;
642  cylinder_params_ = cylinder_params;
643  obj_confidence_ = obj_confidence;
644  best_obj_guess_ = best_obj_guess;
645  wakeup();
646 }
647 
648 void
649 TabletopVisualizationThread::triangulate_hull()
650 {
651  if (table_model_vertices_.empty()) {
652  table_triangle_vertices_.clear();
653  return;
654  }
655 
656  // Don't need to, resizing and overwriting them all later
657  //table_triangle_vertices_.clear();
658 
659  // True if qhull should free points in qh_freeqhull() or reallocation
660  boolT ismalloc = True;
661  qh DELAUNAY = True;
662 
663  int dim = 3;
664  char *flags = const_cast<char *>("qhull Qt Pp");
665  ;
666  FILE *outfile = NULL;
667  FILE *errfile = stderr;
668 
669  // Array of coordinates for each point
670  coordT *points = (coordT *)calloc(table_model_vertices_.size() * dim, sizeof(coordT));
671 
672  for (size_t i = 0; i < table_model_vertices_.size(); ++i) {
673  points[i * dim + 0] = (coordT)table_model_vertices_[i][0];
674  points[i * dim + 1] = (coordT)table_model_vertices_[i][1];
675  points[i * dim + 2] = (coordT)table_model_vertices_[i][2];
676  }
677 
678  // Compute convex hull
679  int exitcode =
680  qh_new_qhull(dim, table_model_vertices_.size(), points, ismalloc, flags, outfile, errfile);
681 
682  if (exitcode != 0) {
683  // error, return empty vector
684  // Deallocates memory (also the points)
685  qh_freeqhull(!qh_ALL);
686  int curlong, totlong;
687  qh_memfreeshort(&curlong, &totlong);
688  return;
689  }
690 
691  qh_triangulate();
692 
693  int q_num_facets = qh num_facets;
694 
695  table_triangle_vertices_.resize(q_num_facets * dim);
696  facetT *facet = NULL;
697  int i = 0;
698  FORALLfacets
699  {
700  assert(facet);
701  vertexT *vertex = NULL;
702  int vertex_n = 0, vertex_i = 0;
703  FOREACHvertex_i_(facet->vertices)
704  {
705  assert(vertex);
706  assert(i + vertex_i < vertex_n);
707  table_triangle_vertices_[i + vertex_i][0] = vertex->point[0];
708  table_triangle_vertices_[i + vertex_i][1] = vertex->point[1];
709  table_triangle_vertices_[i + vertex_i][2] = vertex->point[2];
710  }
711 
712  i += dim;
713  }
714 
715  // Deallocates memory (also the points)
716  qh_freeqhull(!qh_ALL);
717  int curlong, totlong;
718  qh_memfreeshort(&curlong, &totlong);
719 }
std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > V_Vector4f
Aligned vector of vectors/points.
std::map< unsigned int, Eigen::Vector4f, std::less< unsigned int >, Eigen::aligned_allocator< std::pair< const unsigned int, Eigen::Vector4f > > > M_Vector4f
aligned map of vectors.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void visualize(const std::string &frame_id, Eigen::Vector4f &table_centroid, Eigen::Vector4f &normal, V_Vector4f &table_hull_vertices, V_Vector4f &table_model_vertices, V_Vector4f &good_table_hull_edges, M_Vector4f &centroids, M_Vector4f &cylinder_params, std::map< unsigned int, double > &obj_confidence, std::map< unsigned int, signed int > &best_obj_guess)
Visualize the given data.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Mutex locking helper.
Definition: mutex_locker.h:34
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:729
A class for handling time.
Definition: time.h:93
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
std::string frame_id
The frame_id associated this data.
Definition: types.h:133
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36