22 #include "visualization_thread.h" 23 #include "cluster_colors.h" 25 #include <core/threading/mutex_locker.h> 26 #include <utils/math/angle.h> 29 #include <visualization_msgs/MarkerArray.h> 31 # include <geometry_msgs/PointStamped.h> 33 #include <Eigen/Geometry> 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" 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" 57 #define CFG_PREFIX "/perception/tabletop-objects/" 58 #define CFG_PREFIX_VIS "/perception/tabletop-objects/visualization/" 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;
85 cfg_show_frustrum_ =
config->
get_bool(CFG_PREFIX_VIS
"show_frustrum");
87 if (cfg_show_frustrum_) {
93 cfg_duration_ =
config->
get_uint(CFG_PREFIX_VIS
"display_duration");
97 cfg_show_cvxhull_vertices_ =
config->
get_bool(CFG_PREFIX_VIS
"show_convex_hull_vertices");
100 cfg_show_cvxhull_line_highlighting_ =
config->
get_bool(CFG_PREFIX_VIS
"show_convex_hull_line_highlighting");
103 cfg_show_cvxhull_vertex_ids_ =
config->
get_bool(CFG_PREFIX_VIS
"show_convex_hull_vertex_ids");
106 cfg_cylinder_fitting_ =
config->
get_bool(CFG_PREFIX
"enable_cylinder_fitting");
111 vispub_ =
new ros::Publisher();
112 *vispub_ =
rosnode->advertise<visualization_msgs::MarkerArray>(
"visualization_marker_array", 100);
114 posepub_ =
new ros::Publisher();
115 *posepub_ =
rosnode->advertise<geometry_msgs::PointStamped>(
"table_point", 10);
123 visualization_msgs::MarkerArray m;
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";
131 delop.action = visualization_msgs::Marker::DELETE;
132 m.markers.push_back(delop);
140 posepub_->shutdown();
150 visualization_msgs::MarkerArray m;
152 unsigned int idnum = 0;
153 for (M_Vector4f::iterator it = centroids_.begin(); it != centroids_.end(); it++) {
163 centroid(tf::Point(it->second[0], it->second[1], it->second[2]),
169 if (asprintf(&tmp,
"TObj %u", it->first) != -1) {
171 std::string
id = tmp;
174 visualization_msgs::Marker text;
175 text.header.frame_id = cfg_base_frame_;
176 text.header.stamp = ros::Time::now();
177 text.ns =
"tabletop";
179 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
180 text.action = visualization_msgs::Marker::ADD;
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.;
189 text.color.r = text.color.g = text.color.b = 1.0f;
191 text.lifetime = ros::Duration(cfg_duration_, 0);
193 m.markers.push_back(text);
196 visualization_msgs::Marker sphere;
197 sphere.header.frame_id = cfg_base_frame_;
198 sphere.header.stamp = ros::Time::now();
199 sphere.ns =
"tabletop";
201 sphere.type = visualization_msgs::Marker::CYLINDER;
202 sphere.action = visualization_msgs::Marker::ADD;
204 if (cfg_cylinder_fitting_) {
210 sphere.scale.x = sphere.scale.y = 2 * cylinder_params_[it->first][0];
211 sphere.scale.z = cylinder_params_[it->first][1];
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;
218 sphere.color.r = 0.0;
219 sphere.color.g = 1.0;
220 sphere.color.b = 0.0;
227 sphere.color.a = 1.0;
234 sphere.pose.position.x = centroid[0];
235 sphere.pose.position.y = centroid[1];
236 sphere.pose.position.z = centroid[2];
238 tf::Quaternion table_quat(tf::Vector3(0, 1, 0), cylinder_params_[2][0]);
245 sphere.pose.orientation.w = 1.;
247 sphere.lifetime = ros::Duration(cfg_duration_, 0);
248 m.markers.push_back(sphere);
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);
268 Eigen::Vector4f normal_end = (table_centroid_ + (normal_ * -0.15));
270 visualization_msgs::Marker normal;
272 normal.header.stamp = ros::Time::now();
273 normal.ns =
"tabletop";
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);
292 if (cfg_show_cvxhull_line_highlighting_) {
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;
313 hull_lines.colors[i].g = 0.5;
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);
322 visualization_msgs::Marker hull;
323 hull.header.frame_id = frame_id_;
324 hull.header.stamp = ros::Time::now();
325 hull.ns =
"tabletop";
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];
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;
340 hull.color.g = hull.color.b = 0.f;
342 hull.lifetime = ros::Duration(cfg_duration_, 0);
343 m.markers.push_back(hull);
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];
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);
371 if (cfg_show_cvxhull_vertex_ids_) {
372 for (
size_t i = 0; i < table_hull_vertices_.size(); ++i) {
375 if (asprintf(&tmp,
"Cvx_%zu", i) != -1) {
377 std::string
id = tmp;
380 visualization_msgs::Marker text;
381 text.header.frame_id = frame_id_;
382 text.header.stamp = ros::Time::now();
383 text.ns =
"tabletop";
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.;
392 text.color.r = text.color.g = text.color.b = 1.0f;
394 text.lifetime = ros::Duration(cfg_duration_, 0);
396 m.markers.push_back(text);
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";
408 hull.type = visualization_msgs::Marker::LINE_STRIP;
409 hull.action = visualization_msgs::Marker::ADD;
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];
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];
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];
432 hull.scale.x = 0.0075;
434 hull.color.g = hull.color.b = 0.f;
436 hull.lifetime = ros::Duration(cfg_duration_, 0);
437 m.markers.push_back(hull);
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";
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];
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];
461 plane.pose.orientation.w = 1.;
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;
469 plane.lifetime = ros::Duration(cfg_duration_, 0);
470 m.markers.push_back(plane);
473 if (cfg_show_frustrum_ && !table_model_vertices_.empty()) {
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.;
487 const float half_hva = cfg_horizontal_va_ * 0.5;
488 const float half_vva = cfg_vertical_va_ * 0.5;
490 Eigen::Matrix3f 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);
496 Eigen::Matrix3f 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);
502 Eigen::Matrix3f 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);
508 Eigen::Matrix3f 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);
514 frustrum.points[1].x = upper_right[0];
515 frustrum.points[1].y = upper_right[1];
516 frustrum.points[1].z = upper_right[2];
518 frustrum.points[3].x = lower_right[0];
519 frustrum.points[3].y = lower_right[1];
520 frustrum.points[3].z = lower_right[2];
522 frustrum.points[5].x = lower_left[0];
523 frustrum.points[5].y = lower_left[1];
524 frustrum.points[5].z = lower_left[2];
526 frustrum.points[7].x = upper_left[0];
527 frustrum.points[7].y = upper_left[1];
528 frustrum.points[7].z = upper_left[2];
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);
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.;
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];
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];
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];
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];
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];
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];
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);
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";
593 delop.action = visualization_msgs::Marker::DELETE;
594 m.markers.push_back(delop);
596 last_id_num_ = idnum;
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);
627 Eigen::Vector4f &table_centroid, Eigen::Vector4f &normal,
630 M_Vector4f &cylinder_params, std::map<unsigned int, double> &obj_confidence,
631 std::map<unsigned int, signed int>& best_obj_guess)
throw ()
634 frame_id_ = frame_id;
635 table_centroid_ = table_centroid;
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;
649 TabletopVisualizationThread::triangulate_hull()
651 if (table_model_vertices_.empty()) {
652 table_triangle_vertices_.clear();
661 boolT ismalloc = True;
665 char *flags =
const_cast<char *
>(
"qhull Qt Pp");;
666 FILE *outfile = NULL;
667 FILE *errfile = stderr;
670 coordT *points = (coordT *)calloc(table_model_vertices_.size() * dim,
sizeof(coordT));
672 for (
size_t i = 0; i < table_model_vertices_.size(); ++i)
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];
680 int exitcode = qh_new_qhull(dim, table_model_vertices_.size(), points,
681 ismalloc, flags, outfile, errfile);
686 qh_freeqhull(!qh_ALL);
687 int curlong, totlong;
688 qh_memfreeshort (&curlong, &totlong);
694 int num_facets = qh num_facets;
696 table_triangle_vertices_.resize(num_facets * dim);
702 int vertex_n, vertex_i;
703 FOREACHvertex_i_(facet->vertices)
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];
714 qh_freeqhull(!qh_ALL);
715 int curlong, totlong;
716 qh_memfreeshort(&curlong, &totlong);
std::string frame_id
The frame_id associated this data.
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.
std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > V_Vector4f
Aligned vector of vectors/points.
A class for handling time.
Thread class encapsulation of pthreads.
void wakeup()
Wake up thread.
virtual void init()
Initialize the thread.
Base class for exceptions in Fawkes.
TabletopVisualizationThread()
Constructor.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Wrapper class to add time stamp and frame ID to base types.
virtual void finalize()
Finalize the thread.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
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.
Configuration * config
This is the Configuration member used to access the configuration.
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 ¢roids, M_Vector4f &cylinder_params, std::map< unsigned int, double > &obj_confidence, std::map< unsigned int, signed int > &best_obj_guess)
Visualize the given data.