22 #include "visualization_thread.h" 24 #include "cluster_colors.h" 26 #include <core/threading/mutex_locker.h> 28 #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/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" 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" 59 #define CFG_PREFIX "/perception/tabletop-objects/" 60 #define CFG_PREFIX_VIS "/perception/tabletop-objects/visualization/" 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;
86 cfg_show_frustrum_ =
config->
get_bool(CFG_PREFIX_VIS
"show_frustrum");
89 if (cfg_show_frustrum_) {
95 cfg_duration_ =
config->
get_uint(CFG_PREFIX_VIS
"display_duration");
100 cfg_show_cvxhull_vertices_ =
config->
get_bool(CFG_PREFIX_VIS
"show_convex_hull_vertices");
104 cfg_show_cvxhull_line_highlighting_ =
105 config->
get_bool(CFG_PREFIX_VIS
"show_convex_hull_line_highlighting");
109 cfg_show_cvxhull_vertex_ids_ =
config->
get_bool(CFG_PREFIX_VIS
"show_convex_hull_vertex_ids");
113 cfg_cylinder_fitting_ =
config->
get_bool(CFG_PREFIX
"enable_cylinder_fitting");
119 vispub_ =
new ros::Publisher();
120 *vispub_ =
rosnode->advertise<visualization_msgs::MarkerArray>(
"visualization_marker_array", 100);
122 posepub_ =
new ros::Publisher();
123 *posepub_ =
rosnode->advertise<geometry_msgs::PointStamped>(
"table_point", 10);
131 visualization_msgs::MarkerArray m;
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";
139 delop.action = visualization_msgs::Marker::DELETE;
140 m.markers.push_back(delop);
147 posepub_->shutdown();
156 visualization_msgs::MarkerArray m;
158 unsigned int idnum = 0;
159 for (M_Vector4f::iterator it = centroids_.begin(); it != centroids_.end(); ++it) {
174 if (asprintf(&tmp,
"TObj %u", it->first) != -1) {
176 std::string
id = tmp;
179 visualization_msgs::Marker text;
180 text.header.frame_id = cfg_base_frame_;
181 text.header.stamp = ros::Time::now();
182 text.ns =
"tabletop";
184 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
185 text.action = visualization_msgs::Marker::ADD;
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.;
194 text.color.r = text.color.g = text.color.b = 1.0f;
196 text.lifetime = ros::Duration(cfg_duration_, 0);
198 m.markers.push_back(text);
201 visualization_msgs::Marker sphere;
202 sphere.header.frame_id = cfg_base_frame_;
203 sphere.header.stamp = ros::Time::now();
204 sphere.ns =
"tabletop";
206 sphere.type = visualization_msgs::Marker::CYLINDER;
207 sphere.action = visualization_msgs::Marker::ADD;
209 if (cfg_cylinder_fitting_) {
214 sphere.scale.x = sphere.scale.y = 2 * cylinder_params_[it->first][0];
215 sphere.scale.z = cylinder_params_[it->first][1];
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;
222 sphere.color.r = 0.0;
223 sphere.color.g = 1.0;
224 sphere.color.b = 0.0;
231 sphere.color.a = 1.0;
238 sphere.pose.position.x = centroid[0];
239 sphere.pose.position.y = centroid[1];
240 sphere.pose.position.z = centroid[2];
242 tf::Quaternion table_quat(tf::Vector3(0, 1, 0), cylinder_params_[2][0]);
249 sphere.pose.orientation.w = 1.;
251 sphere.lifetime = ros::Duration(cfg_duration_, 0);
252 m.markers.push_back(sphere);
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);
271 Eigen::Vector4f normal_end = (table_centroid_ + (normal_ * -0.15));
273 visualization_msgs::Marker normal;
275 normal.header.stamp = ros::Time::now();
276 normal.ns =
"tabletop";
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);
295 if (cfg_show_cvxhull_line_highlighting_) {
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;
316 hull_lines.colors[i].g = 0.5;
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);
325 visualization_msgs::Marker hull;
326 hull.header.frame_id = frame_id_;
327 hull.header.stamp = ros::Time::now();
328 hull.ns =
"tabletop";
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];
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;
343 hull.color.g = hull.color.b = 0.f;
345 hull.lifetime = ros::Duration(cfg_duration_, 0);
346 m.markers.push_back(hull);
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];
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);
374 if (cfg_show_cvxhull_vertex_ids_) {
375 for (
size_t i = 0; i < table_hull_vertices_.size(); ++i) {
377 if (asprintf(&tmp,
"Cvx_%zu", i) != -1) {
379 std::string
id = tmp;
382 visualization_msgs::Marker text;
383 text.header.frame_id = frame_id_;
384 text.header.stamp = ros::Time::now();
385 text.ns =
"tabletop";
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.;
394 text.color.r = text.color.g = text.color.b = 1.0f;
396 text.lifetime = ros::Duration(cfg_duration_, 0);
398 m.markers.push_back(text);
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";
410 hull.type = visualization_msgs::Marker::LINE_STRIP;
411 hull.action = visualization_msgs::Marker::ADD;
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];
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];
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];
434 hull.scale.x = 0.0075;
436 hull.color.g = hull.color.b = 0.f;
438 hull.lifetime = ros::Duration(cfg_duration_, 0);
439 m.markers.push_back(hull);
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";
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];
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];
463 plane.pose.orientation.w = 1.;
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;
471 plane.lifetime = ros::Duration(cfg_duration_, 0);
472 m.markers.push_back(plane);
475 if (cfg_show_frustrum_ && !table_model_vertices_.empty()) {
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.;
489 const float half_hva = cfg_horizontal_va_ * 0.5;
490 const float half_vva = cfg_vertical_va_ * 0.5;
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);
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);
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);
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);
512 frustrum.points[1].x = upper_right[0];
513 frustrum.points[1].y = upper_right[1];
514 frustrum.points[1].z = upper_right[2];
516 frustrum.points[3].x = lower_right[0];
517 frustrum.points[3].y = lower_right[1];
518 frustrum.points[3].z = lower_right[2];
520 frustrum.points[5].x = lower_left[0];
521 frustrum.points[5].y = lower_left[1];
522 frustrum.points[5].z = lower_left[2];
524 frustrum.points[7].x = upper_left[0];
525 frustrum.points[7].y = upper_left[1];
526 frustrum.points[7].z = upper_left[2];
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);
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.;
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];
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];
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];
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];
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];
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];
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);
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";
590 delop.action = visualization_msgs::Marker::DELETE;
591 m.markers.push_back(delop);
593 last_id_num_ = idnum;
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);
624 Eigen::Vector4f & table_centroid,
625 Eigen::Vector4f & normal,
631 std::map<unsigned int, double> & obj_confidence,
632 std::map<unsigned int, signed int> &best_obj_guess)
throw()
635 frame_id_ = frame_id;
636 table_centroid_ = table_centroid;
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;
649 TabletopVisualizationThread::triangulate_hull()
651 if (table_model_vertices_.empty()) {
652 table_triangle_vertices_.clear();
660 boolT ismalloc = True;
664 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) {
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];
680 qh_new_qhull(dim, table_model_vertices_.size(), points, ismalloc, flags, outfile, errfile);
685 qh_freeqhull(!qh_ALL);
686 int curlong, totlong;
687 qh_memfreeshort(&curlong, &totlong);
693 int q_num_facets = qh num_facets;
695 table_triangle_vertices_.resize(q_num_facets * dim);
696 facetT *facet = NULL;
701 vertexT *vertex = NULL;
702 int vertex_n = 0, vertex_i = 0;
703 FOREACHvertex_i_(facet->vertices)
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];
716 qh_freeqhull(!qh_ALL);
717 int curlong, totlong;
718 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.
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.