Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
visualization_thread.cpp
1 
2 /***************************************************************************
3  * visualization_thread.cpp - Visualization via rviz
4  *
5  * Created: Fri Nov 11 00:20:45 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "visualization_thread.h"
23 #include "cluster_colors.h"
24 
25 #include <core/threading/mutex_locker.h>
26 #include <utils/math/angle.h>
27 
28 #include <ros/ros.h>
29 #include <visualization_msgs/MarkerArray.h>
30 #ifdef USE_POSEPUB
31 # include <geometry_msgs/PointStamped.h>
32 #endif
33 #include <Eigen/Geometry>
34 
35 extern "C"
36 {
37 #ifdef HAVE_QHULL_2011
38 # include "libqhull/libqhull.h"
39 # include "libqhull/mem.h"
40 # include "libqhull/qset.h"
41 # include "libqhull/geom.h"
42 # include "libqhull/merge.h"
43 # include "libqhull/poly.h"
44 # include "libqhull/io.h"
45 # include "libqhull/stat.h"
46 #else
47 # include "qhull/qhull.h"
48 # include "qhull/mem.h"
49 # include "qhull/qset.h"
50 # include "qhull/geom.h"
51 # include "qhull/merge.h"
52 # include "qhull/poly.h"
53 # include "qhull/io.h"
54 # include "qhull/stat.h"
55 #endif
56 }
57 
58 #define CFG_PREFIX "/perception/tabletop-objects/"
59 #define CFG_PREFIX_VIS "/perception/tabletop-objects/visualization/"
60 
61 using namespace fawkes;
62 
63 /** @class TabletopVisualizationThread "visualization_thread.h"
64  * Send Marker messages to rviz.
65  * This class takes input from the table top object detection thread and
66  * publishes according marker messages for visualization in rviz.
67  * @author Tim Niemueller
68  */
69 
70 /** Constructor. */
72  : fawkes::Thread("TabletopVisualizationThread", Thread::OPMODE_WAITFORWAKEUP)
73 {
75 }
76 
77 
78 void
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) {} // ignored, use default
88  if (cfg_show_frustrum_) {
89  cfg_horizontal_va_ = deg2rad(config->get_float(CFG_PREFIX"horizontal_viewing_angle"));
90  cfg_vertical_va_ = deg2rad(config->get_float(CFG_PREFIX"vertical_viewing_angle"));
91  }
92  cfg_duration_ = 120;
93  try {
94  cfg_duration_ = config->get_uint(CFG_PREFIX_VIS"display_duration");
95  } catch (Exception &e) {} // ignored, use default
96 
97  try {
98  cfg_show_cvxhull_vertices_ = config->get_bool(CFG_PREFIX_VIS"show_convex_hull_vertices");
99  } catch (Exception &e) {} // ignored, use default
100  try {
101  cfg_show_cvxhull_line_highlighting_ = config->get_bool(CFG_PREFIX_VIS"show_convex_hull_line_highlighting");
102  } catch (Exception &e) {} // ignored, use default
103  try {
104  cfg_show_cvxhull_vertex_ids_ = config->get_bool(CFG_PREFIX_VIS"show_convex_hull_vertex_ids");
105  } catch (Exception &e) {} // ignored, use default
106 
107  vispub_ = new ros::Publisher();
108  *vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
109 #ifdef USE_POSEPUB
110  posepub_ = new ros::Publisher();
111  *posepub_ = rosnode->advertise<geometry_msgs::PointStamped>("table_point", 10);
112 #endif
113  last_id_num_ = 0;
114 }
115 
116 void
118 {
119  visualization_msgs::MarkerArray m;
120 
121  for (size_t i = 0; i < last_id_num_; ++i) {
122  visualization_msgs::Marker delop;
123  delop.header.frame_id = frame_id_;
124  delop.header.stamp = ros::Time::now();
125  delop.ns = "tabletop";
126  delop.id = i;
127  delop.action = visualization_msgs::Marker::DELETE;
128  m.markers.push_back(delop);
129  }
130  vispub_->publish(m);
131 
132 
133  vispub_->shutdown();
134  delete vispub_;
135 #ifdef USE_POSEPUB
136  posepub_->shutdown();
137  delete posepub_;
138 #endif
139 }
140 
141 
142 void
144 {
145  MutexLocker lock(&mutex_);
146  visualization_msgs::MarkerArray m;
147 
148  unsigned int idnum = 0;
149 
150  for (size_t i = 0; i < centroids_.size(); ++i) {
151  try {
153  centroid(tf::Point(centroids_[i][0], centroids_[i][1], centroids_[i][2]),
154  fawkes::Time(0, 0), frame_id_);
155  tf::Stamped<tf::Point> baserel_centroid;
156  tf_listener->transform_point("/base_link", centroid, baserel_centroid);
157 
158  char *tmp;
159  if (asprintf(&tmp, "TObj %zu", i) != -1) {
160  // Copy to get memory freed on exception
161  std::string id = tmp;
162  free(tmp);
163 
164  visualization_msgs::Marker text;
165  text.header.frame_id = "/base_link";
166  text.header.stamp = ros::Time::now();
167  text.ns = "tabletop";
168  text.id = idnum++;
169  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
170  text.action = visualization_msgs::Marker::ADD;
171  text.pose.position.x = baserel_centroid[0];
172  text.pose.position.y = baserel_centroid[1];
173  text.pose.position.z = baserel_centroid[2] + 0.13;
174  text.pose.orientation.w = 1.;
175  text.scale.z = 0.05; // 5cm high
176  text.color.r = text.color.g = text.color.b = 1.0f;
177  text.color.a = 1.0;
178  text.lifetime = ros::Duration(cfg_duration_, 0);
179  text.text = id;
180  m.markers.push_back(text);
181  }
182 
183  visualization_msgs::Marker sphere;
184  sphere.header.frame_id = "/base_link";
185  sphere.header.stamp = ros::Time::now();
186  sphere.ns = "tabletop";
187  sphere.id = idnum++;
188  sphere.type = visualization_msgs::Marker::CYLINDER;
189  sphere.action = visualization_msgs::Marker::ADD;
190  sphere.pose.position.x = baserel_centroid[0];
191  sphere.pose.position.y = baserel_centroid[1];
192  sphere.pose.position.z = baserel_centroid[2];
193  sphere.pose.orientation.w = 1.;
194  sphere.scale.x = sphere.scale.y = 0.08;
195  sphere.scale.z = 0.09;
196  sphere.color.r = (float)cluster_colors[i][0] / 255.f;
197  sphere.color.g = (float)cluster_colors[i][1] / 255.f;
198  sphere.color.b = (float)cluster_colors[i][2] / 255.f;
199  sphere.color.a = 1.0;
200  sphere.lifetime = ros::Duration(cfg_duration_, 0);
201  m.markers.push_back(sphere);
202  } catch (tf::TransformException &e) {} // ignored
203  }
204 
205  Eigen::Vector4f normal_end = (table_centroid_ + (normal_ * -0.15));
206 
207  visualization_msgs::Marker normal;
208  normal.header.frame_id = frame_id_;
209  normal.header.stamp = ros::Time::now();
210  normal.ns = "tabletop";
211  normal.id = idnum++;
212  normal.type = visualization_msgs::Marker::ARROW;
213  normal.action = visualization_msgs::Marker::ADD;
214  normal.points.resize(2);
215  normal.points[0].x = table_centroid_[0];
216  normal.points[0].y = table_centroid_[1];
217  normal.points[0].z = table_centroid_[2];
218  normal.points[1].x = normal_end[0];
219  normal.points[1].y = normal_end[1];
220  normal.points[1].z = normal_end[2];
221  normal.scale.x = 0.02;
222  normal.scale.y = 0.04;
223  normal.color.r = 0.4;
224  normal.color.g = normal.color.b = 0.f;
225  normal.color.a = 1.0;
226  normal.lifetime = ros::Duration(cfg_duration_, 0);
227  m.markers.push_back(normal);
228 
229  if (cfg_show_cvxhull_line_highlighting_) {
230  // "Good" lines are highlighted
231  visualization_msgs::Marker hull_lines;
232  hull_lines.header.frame_id = frame_id_;
233  hull_lines.header.stamp = ros::Time::now();
234  hull_lines.ns = "tabletop";
235  hull_lines.id = idnum++;
236  hull_lines.type = visualization_msgs::Marker::LINE_LIST;
237  hull_lines.action = visualization_msgs::Marker::ADD;
238  hull_lines.points.resize(good_table_hull_edges_.size());
239  hull_lines.colors.resize(good_table_hull_edges_.size());
240  for (size_t i = 0; i < good_table_hull_edges_.size(); ++i) {
241  hull_lines.points[i].x = good_table_hull_edges_[i][0];
242  hull_lines.points[i].y = good_table_hull_edges_[i][1];
243  hull_lines.points[i].z = good_table_hull_edges_[i][2];
244  hull_lines.colors[i].r = 0.;
245  hull_lines.colors[i].b = 0.;
246  hull_lines.colors[i].a = 0.4;
247  if (good_table_hull_edges_[i][3] > 0.) {
248  hull_lines.colors[i].g = 1.0;
249  } else {
250  hull_lines.colors[i].g = 0.5;
251  }
252  }
253  hull_lines.color.a = 1.0;
254  hull_lines.scale.x = 0.01;
255  hull_lines.lifetime = ros::Duration(cfg_duration_, 0);
256  m.markers.push_back(hull_lines);
257  } else {
258  // Table surrounding polygon
259  visualization_msgs::Marker hull;
260  hull.header.frame_id = frame_id_;
261  hull.header.stamp = ros::Time::now();
262  hull.ns = "tabletop";
263  hull.id = idnum++;
264  hull.type = visualization_msgs::Marker::LINE_STRIP;
265  hull.action = visualization_msgs::Marker::ADD;
266  hull.points.resize(table_hull_vertices_.size() + 1);
267  for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
268  hull.points[i].x = table_hull_vertices_[i][0];
269  hull.points[i].y = table_hull_vertices_[i][1];
270  hull.points[i].z = table_hull_vertices_[i][2];
271  }
272  hull.points[table_hull_vertices_.size()].x = table_hull_vertices_[0][0];
273  hull.points[table_hull_vertices_.size()].y = table_hull_vertices_[0][1];
274  hull.points[table_hull_vertices_.size()].z = table_hull_vertices_[0][2];
275  hull.scale.x = 0.005;
276  hull.color.r = 0.4;
277  hull.color.g = hull.color.b = 0.f;
278  hull.color.a = 0.2;
279  hull.lifetime = ros::Duration(cfg_duration_, 0);
280  m.markers.push_back(hull);
281  }
282 
283  if (cfg_show_cvxhull_vertices_) {
284  visualization_msgs::Marker hull_points;
285  hull_points.header.frame_id = frame_id_;
286  hull_points.header.stamp = ros::Time::now();
287  hull_points.ns = "tabletop";
288  hull_points.id = idnum++;
289  hull_points.type = visualization_msgs::Marker::SPHERE_LIST;
290  hull_points.action = visualization_msgs::Marker::ADD;
291  hull_points.points.resize(table_hull_vertices_.size());
292  for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
293  hull_points.points[i].x = table_hull_vertices_[i][0];
294  hull_points.points[i].y = table_hull_vertices_[i][1];
295  hull_points.points[i].z = table_hull_vertices_[i][2];
296  }
297  hull_points.scale.x = 0.01;
298  hull_points.scale.y = 0.01;
299  hull_points.scale.z = 0.01;
300  hull_points.color.r = 0.8;
301  hull_points.color.g = hull_points.color.b = 0.f;
302  hull_points.color.a = 1.0;
303  hull_points.lifetime = ros::Duration(cfg_duration_, 0);
304  m.markers.push_back(hull_points);
305  }
306 
307  // hull texts
308  if (cfg_show_cvxhull_vertex_ids_) {
309  for (size_t i = 0; i < table_hull_vertices_.size(); ++i) {
310 
311  char *tmp;
312  if (asprintf(&tmp, "Cvx_%zu", i) != -1) {
313  // Copy to get memory freed on exception
314  std::string id = tmp;
315  free(tmp);
316 
317  visualization_msgs::Marker text;
318  text.header.frame_id = frame_id_;
319  text.header.stamp = ros::Time::now();
320  text.ns = "tabletop";
321  text.id = idnum++;
322  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
323  text.action = visualization_msgs::Marker::ADD;
324  text.pose.position.x = table_hull_vertices_[i][0];
325  text.pose.position.y = table_hull_vertices_[i][1];
326  text.pose.position.z = table_hull_vertices_[i][2] + 0.1;
327  text.pose.orientation.w = 1.;
328  text.scale.z = 0.03;
329  text.color.r = text.color.g = text.color.b = 1.0f;
330  text.color.a = 1.0;
331  text.lifetime = ros::Duration(cfg_duration_, 0);
332  text.text = id;
333  m.markers.push_back(text);
334  }
335  }
336  }
337 
338  // Table model surrounding polygon
339  if (! table_model_vertices_.empty()) {
340  visualization_msgs::Marker hull_model;
341  hull_model.header.frame_id = frame_id_;
342  hull_model.header.stamp = ros::Time::now();
343  hull_model.ns = "tabletop";
344  hull_model.id = idnum++;
345  hull_model.type = visualization_msgs::Marker::LINE_STRIP;
346  hull_model.action = visualization_msgs::Marker::ADD;
347  hull_model.points.resize(table_model_vertices_.size() + 1);
348  for (size_t i = 0; i < table_model_vertices_.size(); ++i) {
349  hull_model.points[i].x = table_model_vertices_[i][0];
350  hull_model.points[i].y = table_model_vertices_[i][1];
351  hull_model.points[i].z = table_model_vertices_[i][2];
352  }
353  hull_model.points[table_model_vertices_.size()].x = table_model_vertices_[0][0];
354  hull_model.points[table_model_vertices_.size()].y = table_model_vertices_[0][1];
355  hull_model.points[table_model_vertices_.size()].z = table_model_vertices_[0][2];
356  hull_model.scale.x = 0.0075;
357  hull_model.color.r = 0.5;
358  hull_model.color.g = hull_model.color.b = 0.f;
359  hull_model.color.a = 1.0;
360  hull_model.lifetime = ros::Duration(cfg_duration_, 0);
361  m.markers.push_back(hull_model);
362  }
363 
364  //triangulate_hull();
365 
366  if (table_model_vertices_.size() == 4) {
367  visualization_msgs::Marker plane;
368  plane.header.frame_id = frame_id_;
369  plane.header.stamp = ros::Time::now();
370  plane.ns = "tabletop";
371  plane.id = idnum++;
372  plane.type = visualization_msgs::Marker::TRIANGLE_LIST;
373  plane.action = visualization_msgs::Marker::ADD;
374  plane.points.resize(6);
375  for (unsigned int i = 0; i < 3; ++i) {
376  plane.points[i].x = table_model_vertices_[i][0];
377  plane.points[i].y = table_model_vertices_[i][1];
378  plane.points[i].z = table_model_vertices_[i][2];
379  }
380  for (unsigned int i = 2; i < 5; ++i) {
381  plane.points[i+1].x = table_model_vertices_[i % 4][0];
382  plane.points[i+1].y = table_model_vertices_[i % 4][1];
383  plane.points[i+1].z = table_model_vertices_[i % 4][2];
384  }
385  plane.pose.orientation.w = 1.;
386  plane.scale.x = 1.0;
387  plane.scale.y = 1.0;
388  plane.scale.z = 1.0;
389  plane.color.r = ((float)table_color[0] / 255.f) * 0.8;
390  plane.color.g = ((float)table_color[1] / 255.f) * 0.8;
391  plane.color.b = ((float)table_color[2] / 255.f) * 0.8;
392  plane.color.a = 1.0;
393  plane.lifetime = ros::Duration(cfg_duration_, 0);
394  m.markers.push_back(plane);
395  }
396 
397  if (cfg_show_frustrum_) {
398  // Frustrum
399  visualization_msgs::Marker frustrum;
400  frustrum.header.frame_id = frame_id_;
401  frustrum.header.stamp = ros::Time::now();
402  frustrum.ns = "tabletop";
403  frustrum.id = idnum++;
404  frustrum.type = visualization_msgs::Marker::LINE_LIST;
405  frustrum.action = visualization_msgs::Marker::ADD;
406  frustrum.points.resize(8);
407  frustrum.points[0].x = frustrum.points[2].x = frustrum.points[4].x = frustrum.points[6].x = 0.;
408  frustrum.points[0].y = frustrum.points[2].y = frustrum.points[4].y = frustrum.points[6].y = 0.;
409  frustrum.points[0].z = frustrum.points[2].z = frustrum.points[4].z = frustrum.points[6].z = 0.;
410 
411  const float half_hva = cfg_horizontal_va_ * 0.5;
412  const float half_vva = cfg_vertical_va_ * 0.5;
413 
414  Eigen::Matrix3f upper_right_m;
415  upper_right_m =
416  Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ())
417  * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY());
418  Eigen::Vector3f upper_right = upper_right_m * Eigen::Vector3f(4,0,0);
419 
420  Eigen::Matrix3f upper_left_m;
421  upper_left_m =
422  Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ())
423  * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY());
424  Eigen::Vector3f upper_left = upper_left_m * Eigen::Vector3f(4,0,0);
425 
426  Eigen::Matrix3f lower_right_m;
427  lower_right_m =
428  Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ())
429  * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY());
430  Eigen::Vector3f lower_right = lower_right_m * Eigen::Vector3f(2,0,0);
431 
432  Eigen::Matrix3f lower_left_m;
433  lower_left_m =
434  Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ())
435  * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY());
436  Eigen::Vector3f lower_left = lower_left_m * Eigen::Vector3f(2,0,0);
437 
438  frustrum.points[1].x = upper_right[0];
439  frustrum.points[1].y = upper_right[1];
440  frustrum.points[1].z = upper_right[2];
441 
442  frustrum.points[3].x = lower_right[0];
443  frustrum.points[3].y = lower_right[1];
444  frustrum.points[3].z = lower_right[2];
445 
446  frustrum.points[5].x = lower_left[0];
447  frustrum.points[5].y = lower_left[1];
448  frustrum.points[5].z = lower_left[2];
449 
450  frustrum.points[7].x = upper_left[0];
451  frustrum.points[7].y = upper_left[1];
452  frustrum.points[7].z = upper_left[2];
453 
454  frustrum.scale.x = 0.005;
455  frustrum.color.r = 1.0;
456  frustrum.color.g = frustrum.color.b = 0.f;
457  frustrum.color.a = 1.0;
458  frustrum.lifetime = ros::Duration(cfg_duration_, 0);
459  m.markers.push_back(frustrum);
460 
461 
462  visualization_msgs::Marker frustrum_triangles;
463  frustrum_triangles.header.frame_id = frame_id_;
464  frustrum_triangles.header.stamp = ros::Time::now();
465  frustrum_triangles.ns = "tabletop";
466  frustrum_triangles.id = idnum++;
467  frustrum_triangles.type = visualization_msgs::Marker::TRIANGLE_LIST;
468  frustrum_triangles.action = visualization_msgs::Marker::ADD;
469  frustrum_triangles.points.resize(9);
470  frustrum_triangles.points[0].x =
471  frustrum_triangles.points[3].x = frustrum_triangles.points[6].x = 0.;
472  frustrum_triangles.points[0].y =
473  frustrum_triangles.points[3].y = frustrum_triangles.points[3].y = 0.;
474  frustrum_triangles.points[0].z
475  = frustrum_triangles.points[3].z = frustrum_triangles.points[3].z = 0.;
476 
477  frustrum_triangles.points[1].x = upper_right[0];
478  frustrum_triangles.points[1].y = upper_right[1];
479  frustrum_triangles.points[1].z = upper_right[2];
480 
481  frustrum_triangles.points[2].x = lower_right[0];
482  frustrum_triangles.points[2].y = lower_right[1];
483  frustrum_triangles.points[2].z = lower_right[2];
484 
485  frustrum_triangles.points[4].x = lower_left[0];
486  frustrum_triangles.points[4].y = lower_left[1];
487  frustrum_triangles.points[4].z = lower_left[2];
488 
489  frustrum_triangles.points[5].x = upper_left[0];
490  frustrum_triangles.points[5].y = upper_left[1];
491  frustrum_triangles.points[5].z = upper_left[2];
492 
493  frustrum_triangles.points[7].x = lower_left[0];
494  frustrum_triangles.points[7].y = lower_left[1];
495  frustrum_triangles.points[7].z = lower_left[2];
496 
497  frustrum_triangles.points[8].x = lower_right[0];
498  frustrum_triangles.points[8].y = lower_right[1];
499  frustrum_triangles.points[8].z = lower_right[2];
500 
501  frustrum_triangles.scale.x = 1;
502  frustrum_triangles.scale.y = 1;
503  frustrum_triangles.scale.z = 1;
504  frustrum_triangles.color.r = 1.0;
505  frustrum_triangles.color.g = frustrum_triangles.color.b = 0.f;
506  frustrum_triangles.color.a = 0.23;
507  frustrum_triangles.lifetime = ros::Duration(cfg_duration_, 0);
508  m.markers.push_back(frustrum_triangles);
509  } // end show frustrum
510 
511  for (size_t i = idnum; i < last_id_num_; ++i) {
512  visualization_msgs::Marker delop;
513  delop.header.frame_id = frame_id_;
514  delop.header.stamp = ros::Time::now();
515  delop.ns = "tabletop";
516  delop.id = i;
517  delop.action = visualization_msgs::Marker::DELETE;
518  m.markers.push_back(delop);
519  }
520  last_id_num_ = idnum;
521 
522  vispub_->publish(m);
523 
524 #ifdef USE_POSEPUB
525  geometry_msgs::PointStamped p;
526  p.header.frame_id = frame_id_;
527  p.header.stamp = ros::Time::now();
528  p.point.x = table_centroid_[0];
529  p.point.y = table_centroid_[1];
530  p.point.z = table_centroid_[2];
531  posepub_->publish(p);
532 #endif
533 }
534 
535 
536 /** Visualize the given data.
537  * @param frame_id coordinate frame ID
538  * @param table_centroid table centroid
539  * @param normal table normal vector
540  * @param table_hull_vertices vertices of the table convex hull
541  * @param table_model_vertices vertices belonging to the table model
542  * @param good_table_hull_edges good chosen edges
543  * @param centroids object centroids
544  */
545 void
546 TabletopVisualizationThread::visualize(const std::string &frame_id,
547  Eigen::Vector4f &table_centroid,
548  Eigen::Vector4f &normal,
549  V_Vector4f &table_hull_vertices,
550  V_Vector4f &table_model_vertices,
551  V_Vector4f &good_table_hull_edges,
552  V_Vector4f &centroids) throw()
553 {
554  MutexLocker lock(&mutex_);
555  frame_id_ = frame_id;
556  table_centroid_ = table_centroid;
557  normal_ = normal;
558  table_hull_vertices_ = table_hull_vertices;
559  table_model_vertices_ = table_model_vertices;
560  good_table_hull_edges_ = good_table_hull_edges;
561  centroids_ = centroids;
562  wakeup();
563 }
564 
565 
566 void
567 TabletopVisualizationThread::triangulate_hull()
568 {
569  if (table_model_vertices_.empty()) {
570  table_triangle_vertices_.clear();
571  return;
572  }
573 
574 
575  // Don't need to, resizing and overwriting them all later
576  //table_triangle_vertices_.clear();
577 
578  // True if qhull should free points in qh_freeqhull() or reallocation
579  boolT ismalloc = True;
580  qh DELAUNAY = True;
581 
582  int dim = 3;
583  char *flags = const_cast<char *>("qhull Qt Pp");;
584  FILE *outfile = NULL;
585  FILE *errfile = stderr;
586 
587  // Array of coordinates for each point
588  coordT *points = (coordT *)calloc(table_model_vertices_.size() * dim, sizeof(coordT));
589 
590  for (size_t i = 0; i < table_model_vertices_.size(); ++i)
591  {
592  points[i * dim + 0] = (coordT)table_model_vertices_[i][0];
593  points[i * dim + 1] = (coordT)table_model_vertices_[i][1];
594  points[i * dim + 2] = (coordT)table_model_vertices_[i][2];
595  }
596 
597  // Compute convex hull
598  int exitcode = qh_new_qhull(dim, table_model_vertices_.size(), points,
599  ismalloc, flags, outfile, errfile);
600 
601  if (exitcode != 0) {
602  // error, return empty vector
603  // Deallocates memory (also the points)
604  qh_freeqhull(!qh_ALL);
605  int curlong, totlong;
606  qh_memfreeshort (&curlong, &totlong);
607  return;
608  }
609 
610  qh_triangulate();
611 
612  int num_facets = qh num_facets;
613 
614  table_triangle_vertices_.resize(num_facets * dim);
615  facetT *facet;
616  size_t i = 0;
617  FORALLfacets
618  {
619  vertexT *vertex;
620  int vertex_n, vertex_i;
621  FOREACHvertex_i_(facet->vertices)
622  {
623  table_triangle_vertices_[i + vertex_i][0] = vertex->point[0];
624  table_triangle_vertices_[i + vertex_i][1] = vertex->point[1];
625  table_triangle_vertices_[i + vertex_i][2] = vertex->point[2];
626  }
627 
628  i += dim;
629  }
630 
631  // Deallocates memory (also the points)
632  qh_freeqhull(!qh_ALL);
633  int curlong, totlong;
634  qh_memfreeshort(&curlong, &totlong);
635 }