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