Fawkes API  Fawkes Development Version
visualization_thread.cpp
00001 
00002 /***************************************************************************
00003  *  visualization_thread.cpp - Visualization via rviz
00004  *
00005  *  Created: Fri Nov 11 00:20:45 2011
00006  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 #include "visualization_thread.h"
00023 #include "cluster_colors.h"
00024 
00025 #include <core/threading/mutex_locker.h>
00026 #include <utils/math/angle.h>
00027 
00028 #include <ros/ros.h>
00029 #include <visualization_msgs/MarkerArray.h>
00030 #ifdef USE_POSEPUB
00031 #  include <geometry_msgs/PointStamped.h>
00032 #endif
00033 #include <Eigen/Geometry>
00034 
00035 extern "C"
00036 {
00037 #ifdef HAVE_QHULL_2011
00038 #  include "libqhull/libqhull.h"
00039 #  include "libqhull/mem.h"
00040 #  include "libqhull/qset.h"
00041 #  include "libqhull/geom.h"
00042 #  include "libqhull/merge.h"
00043 #  include "libqhull/poly.h"
00044 #  include "libqhull/io.h"
00045 #  include "libqhull/stat.h"
00046 #else
00047 #  include "qhull/qhull.h"
00048 #  include "qhull/mem.h"
00049 #  include "qhull/qset.h"
00050 #  include "qhull/geom.h"
00051 #  include "qhull/merge.h"
00052 #  include "qhull/poly.h"
00053 #  include "qhull/io.h"
00054 #  include "qhull/stat.h"
00055 #endif
00056 }
00057 
00058 #define CFG_PREFIX "/perception/tabletop-objects/"
00059 #define CFG_PREFIX_VIS "/perception/tabletop-objects/visualization/"
00060 
00061 using namespace fawkes;
00062 
00063 /** @class TabletopVisualizationThread "visualization_thread.h"
00064  * Send Marker messages to rviz.
00065  * This class takes input from the table top object detection thread and
00066  * publishes according marker messages for visualization in rviz.
00067  * @author Tim Niemueller
00068  */
00069 
00070 /** Constructor. */
00071 TabletopVisualizationThread::TabletopVisualizationThread()
00072   : fawkes::Thread("TabletopVisualizationThread", Thread::OPMODE_WAITFORWAKEUP)
00073 {
00074   set_coalesce_wakeups(true);
00075 }
00076 
00077 
00078 void
00079 TabletopVisualizationThread::init()
00080 {
00081   cfg_show_frustrum_ = false;
00082   cfg_show_cvxhull_vertices_ = true;
00083   cfg_show_cvxhull_line_highlighting_ = true;
00084   cfg_show_cvxhull_vertex_ids_ = true;
00085   try {
00086     cfg_show_frustrum_ = config->get_bool(CFG_PREFIX_VIS"show_frustrum");
00087   } catch (Exception &e) {} // ignored, use default
00088   if (cfg_show_frustrum_) {
00089     cfg_horizontal_va_ = deg2rad(config->get_float(CFG_PREFIX"horizontal_viewing_angle"));
00090     cfg_vertical_va_   = deg2rad(config->get_float(CFG_PREFIX"vertical_viewing_angle"));
00091   }
00092   cfg_duration_ = 120;
00093   try {
00094     cfg_duration_ = config->get_uint(CFG_PREFIX_VIS"display_duration");
00095   } catch (Exception &e) {} // ignored, use default
00096 
00097   try {
00098     cfg_show_cvxhull_vertices_ = config->get_bool(CFG_PREFIX_VIS"show_convex_hull_vertices");
00099   } catch (Exception &e) {} // ignored, use default
00100   try {
00101     cfg_show_cvxhull_line_highlighting_ = config->get_bool(CFG_PREFIX_VIS"show_convex_hull_line_highlighting");
00102   } catch (Exception &e) {} // ignored, use default
00103   try {
00104     cfg_show_cvxhull_vertex_ids_ = config->get_bool(CFG_PREFIX_VIS"show_convex_hull_vertex_ids");
00105   } catch (Exception &e) {} // ignored, use default
00106 
00107   vispub_ = new ros::Publisher();
00108   *vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
00109 #ifdef USE_POSEPUB
00110   posepub_ = new ros::Publisher();
00111   *posepub_ = rosnode->advertise<geometry_msgs::PointStamped>("table_point", 10);
00112 #endif
00113   last_id_num_ = 0;
00114 }
00115 
00116 void
00117 TabletopVisualizationThread::finalize()
00118 {
00119   visualization_msgs::MarkerArray m;
00120 
00121   for (size_t i = 0; i < last_id_num_; ++i) {
00122     visualization_msgs::Marker delop;
00123     delop.header.frame_id = frame_id_;
00124     delop.header.stamp = ros::Time::now();
00125     delop.ns = "tabletop";
00126     delop.id = i;
00127     delop.action = visualization_msgs::Marker::DELETE;
00128     m.markers.push_back(delop);
00129   }
00130   vispub_->publish(m);
00131 
00132 
00133   vispub_->shutdown();
00134   delete vispub_;
00135 #ifdef USE_POSEPUB
00136   posepub_->shutdown();
00137   delete posepub_;
00138 #endif
00139 }
00140 
00141 
00142 void
00143 TabletopVisualizationThread::loop()
00144 {
00145   MutexLocker lock(&mutex_);
00146   visualization_msgs::MarkerArray m;
00147 
00148   unsigned int idnum = 0;
00149 
00150   for (size_t i = 0; i < centroids_.size(); ++i) {
00151     try {
00152       tf::Stamped<tf::Point>
00153         centroid(tf::Point(centroids_[i][0], centroids_[i][1], centroids_[i][2]),
00154                  fawkes::Time(0, 0), frame_id_);
00155       tf::Stamped<tf::Point> baserel_centroid;
00156       tf_listener->transform_point("/base_link", centroid, baserel_centroid);
00157 
00158       char *tmp;
00159       if (asprintf(&tmp, "TObj %zu", i) != -1) {
00160         // Copy to get memory freed on exception
00161         std::string id = tmp;
00162         free(tmp);
00163 
00164         visualization_msgs::Marker text;
00165         text.header.frame_id = "/base_link";
00166         text.header.stamp = ros::Time::now();
00167         text.ns = "tabletop";
00168         text.id = idnum++;
00169         text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00170         text.action = visualization_msgs::Marker::ADD;
00171         text.pose.position.x = baserel_centroid[0];
00172         text.pose.position.y = baserel_centroid[1];
00173         text.pose.position.z = baserel_centroid[2] + 0.13;
00174         text.pose.orientation.w = 1.;
00175         text.scale.z = 0.05; // 5cm high
00176         text.color.r = text.color.g = text.color.b = 1.0f;
00177         text.color.a = 1.0;
00178         text.lifetime = ros::Duration(cfg_duration_, 0);
00179         text.text = id;
00180         m.markers.push_back(text);
00181       }
00182 
00183       visualization_msgs::Marker sphere;
00184       sphere.header.frame_id = "/base_link";
00185       sphere.header.stamp = ros::Time::now();
00186       sphere.ns = "tabletop";
00187       sphere.id = idnum++;
00188       sphere.type = visualization_msgs::Marker::CYLINDER;
00189       sphere.action = visualization_msgs::Marker::ADD;
00190       sphere.pose.position.x = baserel_centroid[0];
00191       sphere.pose.position.y = baserel_centroid[1];
00192       sphere.pose.position.z = baserel_centroid[2];
00193       sphere.pose.orientation.w = 1.;
00194       sphere.scale.x = sphere.scale.y = 0.08;
00195       sphere.scale.z = 0.09;
00196       sphere.color.r = (float)cluster_colors[i][0] / 255.f;
00197       sphere.color.g = (float)cluster_colors[i][1] / 255.f;
00198       sphere.color.b = (float)cluster_colors[i][2] / 255.f;
00199       sphere.color.a = 1.0;
00200       sphere.lifetime = ros::Duration(cfg_duration_, 0);
00201       m.markers.push_back(sphere);
00202     } catch (tf::TransformException &e) {} // ignored
00203   }
00204 
00205   Eigen::Vector4f normal_end = (table_centroid_ + (normal_ * -0.15));
00206 
00207   visualization_msgs::Marker normal;
00208   normal.header.frame_id = frame_id_;
00209   normal.header.stamp = ros::Time::now();
00210   normal.ns = "tabletop";
00211   normal.id = idnum++;
00212   normal.type = visualization_msgs::Marker::ARROW;
00213   normal.action = visualization_msgs::Marker::ADD;
00214   normal.points.resize(2);
00215   normal.points[0].x = table_centroid_[0];
00216   normal.points[0].y = table_centroid_[1];
00217   normal.points[0].z = table_centroid_[2];
00218   normal.points[1].x = normal_end[0];
00219   normal.points[1].y = normal_end[1];
00220   normal.points[1].z = normal_end[2];
00221   normal.scale.x = 0.02;
00222   normal.scale.y = 0.04;
00223   normal.color.r = 0.4;
00224   normal.color.g = normal.color.b = 0.f;
00225   normal.color.a = 1.0;
00226   normal.lifetime = ros::Duration(cfg_duration_, 0);
00227   m.markers.push_back(normal);
00228 
00229   if (cfg_show_cvxhull_line_highlighting_) {
00230     // "Good" lines are highlighted
00231     visualization_msgs::Marker hull_lines;
00232     hull_lines.header.frame_id = frame_id_;
00233     hull_lines.header.stamp = ros::Time::now();
00234     hull_lines.ns = "tabletop";
00235     hull_lines.id = idnum++;
00236     hull_lines.type = visualization_msgs::Marker::LINE_LIST;
00237     hull_lines.action = visualization_msgs::Marker::ADD;
00238     hull_lines.points.resize(good_table_hull_edges_.size());
00239     hull_lines.colors.resize(good_table_hull_edges_.size());
00240     for (size_t i = 0; i < good_table_hull_edges_.size(); ++i) {
00241       hull_lines.points[i].x = good_table_hull_edges_[i][0];
00242       hull_lines.points[i].y = good_table_hull_edges_[i][1];
00243       hull_lines.points[i].z = good_table_hull_edges_[i][2];
00244       hull_lines.colors[i].r = 0.;
00245       hull_lines.colors[i].b = 0.;
00246       hull_lines.colors[i].a = 0.4;
00247       if (good_table_hull_edges_[i][3] > 0.) {
00248         hull_lines.colors[i].g = 1.0;
00249       } else {
00250         hull_lines.colors[i].g = 0.5;
00251       }
00252     }
00253     hull_lines.color.a = 1.0;
00254     hull_lines.scale.x = 0.01;
00255     hull_lines.lifetime = ros::Duration(cfg_duration_, 0);
00256     m.markers.push_back(hull_lines);
00257   } else {
00258     // Table surrounding polygon
00259     visualization_msgs::Marker hull;
00260     hull.header.frame_id = frame_id_;
00261     hull.header.stamp = ros::Time::now();
00262     hull.ns = "tabletop";
00263     hull.id = idnum++;
00264     hull.type = visualization_msgs::Marker::LINE_STRIP;
00265     hull.action = visualization_msgs::Marker::ADD;
00266     hull.points.resize(table_hull_vertices_.size() + 1);
00267     for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
00268       hull.points[i].x = table_hull_vertices_[i][0];
00269       hull.points[i].y = table_hull_vertices_[i][1];
00270       hull.points[i].z = table_hull_vertices_[i][2];
00271     }
00272     hull.points[table_hull_vertices_.size()].x = table_hull_vertices_[0][0];
00273     hull.points[table_hull_vertices_.size()].y = table_hull_vertices_[0][1];
00274     hull.points[table_hull_vertices_.size()].z = table_hull_vertices_[0][2];
00275     hull.scale.x = 0.005;
00276     hull.color.r = 0.4;
00277     hull.color.g = hull.color.b = 0.f;
00278     hull.color.a = 0.2;
00279     hull.lifetime = ros::Duration(cfg_duration_, 0);
00280     m.markers.push_back(hull);
00281   }
00282 
00283   if (cfg_show_cvxhull_vertices_) {
00284     visualization_msgs::Marker hull_points;
00285     hull_points.header.frame_id = frame_id_;
00286     hull_points.header.stamp = ros::Time::now();
00287     hull_points.ns = "tabletop";
00288     hull_points.id = idnum++;
00289     hull_points.type = visualization_msgs::Marker::SPHERE_LIST;
00290     hull_points.action = visualization_msgs::Marker::ADD;
00291     hull_points.points.resize(table_hull_vertices_.size());
00292     for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
00293       hull_points.points[i].x = table_hull_vertices_[i][0];
00294       hull_points.points[i].y = table_hull_vertices_[i][1];
00295       hull_points.points[i].z = table_hull_vertices_[i][2];
00296     }
00297     hull_points.scale.x = 0.01;
00298     hull_points.scale.y = 0.01;
00299     hull_points.scale.z = 0.01;
00300     hull_points.color.r = 0.8;
00301     hull_points.color.g = hull_points.color.b = 0.f;
00302     hull_points.color.a = 1.0;
00303     hull_points.lifetime = ros::Duration(cfg_duration_, 0);
00304     m.markers.push_back(hull_points);
00305   }
00306 
00307   // hull texts
00308   if (cfg_show_cvxhull_vertex_ids_) {
00309     for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
00310 
00311       char *tmp;
00312       if (asprintf(&tmp, "Cvx_%zu", i) != -1) {
00313         // Copy to get memory freed on exception
00314         std::string id = tmp;
00315         free(tmp);
00316 
00317         visualization_msgs::Marker text;
00318         text.header.frame_id = frame_id_;
00319         text.header.stamp = ros::Time::now();
00320         text.ns = "tabletop";
00321         text.id = idnum++;
00322         text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00323         text.action = visualization_msgs::Marker::ADD;
00324         text.pose.position.x = table_hull_vertices_[i][0];
00325         text.pose.position.y = table_hull_vertices_[i][1];
00326         text.pose.position.z = table_hull_vertices_[i][2] + 0.1;
00327         text.pose.orientation.w = 1.;
00328         text.scale.z = 0.03;
00329         text.color.r = text.color.g = text.color.b = 1.0f;
00330         text.color.a = 1.0;
00331         text.lifetime = ros::Duration(cfg_duration_, 0);
00332         text.text = id;
00333         m.markers.push_back(text);
00334       }
00335     }
00336   }
00337 
00338   // Table model surrounding polygon
00339   if (! table_model_vertices_.empty()) {
00340     visualization_msgs::Marker hull_model;
00341     hull_model.header.frame_id = frame_id_;
00342     hull_model.header.stamp = ros::Time::now();
00343     hull_model.ns = "tabletop";
00344     hull_model.id = idnum++;
00345     hull_model.type = visualization_msgs::Marker::LINE_STRIP;
00346     hull_model.action = visualization_msgs::Marker::ADD;
00347     hull_model.points.resize(table_model_vertices_.size() + 1);
00348     for (size_t i = 0; i < table_model_vertices_.size(); ++i) {
00349       hull_model.points[i].x = table_model_vertices_[i][0];
00350       hull_model.points[i].y = table_model_vertices_[i][1];
00351       hull_model.points[i].z = table_model_vertices_[i][2];
00352     }
00353     hull_model.points[table_model_vertices_.size()].x = table_model_vertices_[0][0];
00354     hull_model.points[table_model_vertices_.size()].y = table_model_vertices_[0][1];
00355     hull_model.points[table_model_vertices_.size()].z = table_model_vertices_[0][2];
00356     hull_model.scale.x = 0.0075;
00357     hull_model.color.r = 0.5;
00358     hull_model.color.g = hull_model.color.b = 0.f;
00359     hull_model.color.a = 1.0;
00360     hull_model.lifetime = ros::Duration(cfg_duration_, 0);
00361     m.markers.push_back(hull_model);
00362   }
00363 
00364   //triangulate_hull();
00365 
00366   if (table_model_vertices_.size() == 4) {
00367     visualization_msgs::Marker plane;
00368     plane.header.frame_id = frame_id_;
00369     plane.header.stamp = ros::Time::now();
00370     plane.ns = "tabletop";
00371     plane.id = idnum++;
00372     plane.type = visualization_msgs::Marker::TRIANGLE_LIST;
00373     plane.action = visualization_msgs::Marker::ADD;
00374     plane.points.resize(6);
00375     for (unsigned int i = 0; i < 3; ++i) {
00376       plane.points[i].x = table_model_vertices_[i][0];
00377       plane.points[i].y = table_model_vertices_[i][1];
00378       plane.points[i].z = table_model_vertices_[i][2];
00379     }
00380     for (unsigned int i = 2; i < 5; ++i) {
00381       plane.points[i+1].x = table_model_vertices_[i % 4][0];
00382       plane.points[i+1].y = table_model_vertices_[i % 4][1];
00383       plane.points[i+1].z = table_model_vertices_[i % 4][2];
00384     }
00385     plane.pose.orientation.w = 1.;
00386     plane.scale.x = 1.0;
00387     plane.scale.y = 1.0;
00388     plane.scale.z = 1.0;
00389     plane.color.r = ((float)table_color[0] / 255.f) * 0.8;
00390     plane.color.g = ((float)table_color[1] / 255.f) * 0.8;
00391     plane.color.b = ((float)table_color[2] / 255.f) * 0.8;
00392     plane.color.a = 1.0;
00393     plane.lifetime = ros::Duration(cfg_duration_, 0);
00394     m.markers.push_back(plane);
00395   }
00396 
00397   if (cfg_show_frustrum_) {
00398     // Frustrum
00399     visualization_msgs::Marker frustrum;
00400     frustrum.header.frame_id = frame_id_;
00401     frustrum.header.stamp = ros::Time::now();
00402     frustrum.ns = "tabletop";
00403     frustrum.id = idnum++;
00404     frustrum.type = visualization_msgs::Marker::LINE_LIST;
00405     frustrum.action = visualization_msgs::Marker::ADD;
00406     frustrum.points.resize(8);
00407     frustrum.points[0].x = frustrum.points[2].x = frustrum.points[4].x = frustrum.points[6].x = 0.;
00408     frustrum.points[0].y = frustrum.points[2].y = frustrum.points[4].y = frustrum.points[6].y = 0.;
00409     frustrum.points[0].z = frustrum.points[2].z = frustrum.points[4].z = frustrum.points[6].z = 0.;
00410 
00411     const float half_hva = cfg_horizontal_va_ * 0.5;
00412     const float half_vva = cfg_vertical_va_ * 0.5;
00413 
00414     Eigen::Matrix3f upper_right_m;
00415     upper_right_m =
00416       Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ())
00417       * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY());
00418     Eigen::Vector3f upper_right = upper_right_m * Eigen::Vector3f(4,0,0);
00419 
00420     Eigen::Matrix3f upper_left_m;
00421     upper_left_m =
00422       Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ())
00423       * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY());
00424     Eigen::Vector3f upper_left = upper_left_m * Eigen::Vector3f(4,0,0);
00425 
00426     Eigen::Matrix3f lower_right_m;
00427     lower_right_m =
00428       Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ())
00429       * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY());
00430     Eigen::Vector3f lower_right = lower_right_m * Eigen::Vector3f(2,0,0);
00431 
00432     Eigen::Matrix3f lower_left_m;
00433     lower_left_m =
00434       Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ())
00435       * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY());
00436     Eigen::Vector3f lower_left = lower_left_m * Eigen::Vector3f(2,0,0);
00437 
00438     frustrum.points[1].x = upper_right[0];
00439     frustrum.points[1].y = upper_right[1];
00440     frustrum.points[1].z = upper_right[2];
00441 
00442     frustrum.points[3].x = lower_right[0];
00443     frustrum.points[3].y = lower_right[1];
00444     frustrum.points[3].z = lower_right[2];
00445 
00446     frustrum.points[5].x = lower_left[0];
00447     frustrum.points[5].y = lower_left[1];
00448     frustrum.points[5].z = lower_left[2];
00449 
00450     frustrum.points[7].x = upper_left[0];
00451     frustrum.points[7].y = upper_left[1];
00452     frustrum.points[7].z = upper_left[2];
00453 
00454     frustrum.scale.x = 0.005;
00455     frustrum.color.r = 1.0;
00456     frustrum.color.g = frustrum.color.b = 0.f;
00457     frustrum.color.a = 1.0;
00458     frustrum.lifetime = ros::Duration(cfg_duration_, 0);
00459     m.markers.push_back(frustrum);
00460 
00461 
00462     visualization_msgs::Marker frustrum_triangles;
00463     frustrum_triangles.header.frame_id = frame_id_;
00464     frustrum_triangles.header.stamp = ros::Time::now();
00465     frustrum_triangles.ns = "tabletop";
00466     frustrum_triangles.id = idnum++;
00467     frustrum_triangles.type = visualization_msgs::Marker::TRIANGLE_LIST;
00468     frustrum_triangles.action = visualization_msgs::Marker::ADD;
00469     frustrum_triangles.points.resize(9);
00470     frustrum_triangles.points[0].x =
00471       frustrum_triangles.points[3].x = frustrum_triangles.points[6].x = 0.;
00472     frustrum_triangles.points[0].y =
00473       frustrum_triangles.points[3].y = frustrum_triangles.points[3].y = 0.;
00474     frustrum_triangles.points[0].z
00475       = frustrum_triangles.points[3].z = frustrum_triangles.points[3].z = 0.;
00476 
00477     frustrum_triangles.points[1].x = upper_right[0];
00478     frustrum_triangles.points[1].y = upper_right[1];
00479     frustrum_triangles.points[1].z = upper_right[2];
00480 
00481     frustrum_triangles.points[2].x = lower_right[0];
00482     frustrum_triangles.points[2].y = lower_right[1];
00483     frustrum_triangles.points[2].z = lower_right[2];
00484 
00485     frustrum_triangles.points[4].x = lower_left[0];
00486     frustrum_triangles.points[4].y = lower_left[1];
00487     frustrum_triangles.points[4].z = lower_left[2];
00488 
00489     frustrum_triangles.points[5].x = upper_left[0];
00490     frustrum_triangles.points[5].y = upper_left[1];
00491     frustrum_triangles.points[5].z = upper_left[2];
00492 
00493     frustrum_triangles.points[7].x = lower_left[0];
00494     frustrum_triangles.points[7].y = lower_left[1];
00495     frustrum_triangles.points[7].z = lower_left[2];
00496 
00497     frustrum_triangles.points[8].x = lower_right[0];
00498     frustrum_triangles.points[8].y = lower_right[1];
00499     frustrum_triangles.points[8].z = lower_right[2];
00500 
00501     frustrum_triangles.scale.x = 1;
00502     frustrum_triangles.scale.y = 1;
00503     frustrum_triangles.scale.z = 1;
00504     frustrum_triangles.color.r = 1.0;
00505     frustrum_triangles.color.g = frustrum_triangles.color.b = 0.f;
00506     frustrum_triangles.color.a = 0.23;
00507     frustrum_triangles.lifetime = ros::Duration(cfg_duration_, 0);
00508     m.markers.push_back(frustrum_triangles);
00509   } // end show frustrum
00510 
00511   for (size_t i = idnum; i < last_id_num_; ++i) {
00512     visualization_msgs::Marker delop;
00513     delop.header.frame_id = frame_id_;
00514     delop.header.stamp = ros::Time::now();
00515     delop.ns = "tabletop";
00516     delop.id = i;
00517     delop.action = visualization_msgs::Marker::DELETE;
00518     m.markers.push_back(delop);
00519   }
00520   last_id_num_ = idnum;
00521 
00522   vispub_->publish(m);
00523 
00524 #ifdef USE_POSEPUB
00525   geometry_msgs::PointStamped p;
00526   p.header.frame_id = frame_id_;
00527   p.header.stamp = ros::Time::now();
00528   p.point.x = table_centroid_[0];
00529   p.point.y = table_centroid_[1];
00530   p.point.z = table_centroid_[2];
00531   posepub_->publish(p);
00532 #endif
00533 }
00534 
00535 
00536 /** Visualize the given data.
00537  * @param frame_id coordinate frame ID
00538  * @param table_centroid table centroid
00539  * @param normal table normal vector
00540  * @param table_hull_vertices vertices of the table convex hull
00541  * @param table_model_vertices vertices belonging to the table model
00542  * @param good_table_hull_edges good chosen edges
00543  * @param centroids object centroids
00544  */
00545 void
00546 TabletopVisualizationThread::visualize(const std::string &frame_id,
00547                                        Eigen::Vector4f &table_centroid,
00548                                        Eigen::Vector4f &normal,
00549                                        V_Vector4f &table_hull_vertices,
00550                                        V_Vector4f &table_model_vertices,
00551                                        V_Vector4f &good_table_hull_edges,
00552                                        V_Vector4f &centroids) throw()
00553 {
00554   MutexLocker lock(&mutex_);
00555   frame_id_ = frame_id;
00556   table_centroid_ = table_centroid;
00557   normal_ = normal;
00558   table_hull_vertices_ = table_hull_vertices;
00559   table_model_vertices_ = table_model_vertices;
00560   good_table_hull_edges_ = good_table_hull_edges;
00561   centroids_ = centroids;
00562   wakeup();
00563 }
00564 
00565 
00566 void
00567 TabletopVisualizationThread::triangulate_hull()
00568 {
00569   if (table_model_vertices_.empty()) {
00570     table_triangle_vertices_.clear();
00571     return;
00572   }
00573     
00574 
00575   // Don't need to, resizing and overwriting them all later
00576   //table_triangle_vertices_.clear();
00577 
00578   // True if qhull should free points in qh_freeqhull() or reallocation
00579   boolT ismalloc = True;
00580   qh DELAUNAY = True;
00581 
00582   int dim = 3;
00583   char *flags = const_cast<char *>("qhull Qt Pp");;
00584   FILE *outfile = NULL;
00585   FILE *errfile = stderr;
00586 
00587   // Array of coordinates for each point
00588   coordT *points = (coordT *)calloc(table_model_vertices_.size() * dim, sizeof(coordT));
00589 
00590   for (size_t i = 0; i < table_model_vertices_.size(); ++i)
00591   {
00592     points[i * dim + 0] = (coordT)table_model_vertices_[i][0];
00593     points[i * dim + 1] = (coordT)table_model_vertices_[i][1];
00594     points[i * dim + 2] = (coordT)table_model_vertices_[i][2];
00595   }
00596 
00597   // Compute convex hull
00598   int exitcode = qh_new_qhull(dim, table_model_vertices_.size(), points,
00599                               ismalloc, flags, outfile, errfile);
00600 
00601   if (exitcode != 0) {
00602     // error, return empty vector
00603     // Deallocates memory (also the points)
00604     qh_freeqhull(!qh_ALL);
00605     int curlong, totlong;
00606     qh_memfreeshort (&curlong, &totlong);
00607     return;
00608   }
00609 
00610   qh_triangulate();
00611 
00612   int num_facets = qh num_facets;
00613 
00614   table_triangle_vertices_.resize(num_facets * dim);
00615   facetT *facet;
00616   size_t i = 0;
00617   FORALLfacets
00618   {
00619     vertexT *vertex;
00620     int vertex_n, vertex_i;
00621     FOREACHvertex_i_(facet->vertices)
00622     {
00623       table_triangle_vertices_[i + vertex_i][0] = vertex->point[0];
00624       table_triangle_vertices_[i + vertex_i][1] = vertex->point[1];
00625       table_triangle_vertices_[i + vertex_i][2] = vertex->point[2];
00626     }
00627 
00628     i += dim;
00629   }
00630 
00631   // Deallocates memory (also the points)
00632   qh_freeqhull(!qh_ALL);
00633   int curlong, totlong;
00634   qh_memfreeshort(&curlong, &totlong);
00635 }