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
35extern "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
62using 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
78void
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
128void
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
152void
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 */
622void
623TabletopVisualizationThread::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) noexcept
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
648void
649TabletopVisualizationThread::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) noexcept
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