22 #include "tabletop_objects_thread.h"
23 #include "cluster_colors.h"
24 #ifdef HAVE_VISUAL_DEBUGGING
25 # include "visualization_thread_base.h"
28 #include <pcl_utils/utils.h>
29 #include <pcl_utils/comparisons.h>
30 #include <utils/time/wait.h>
31 #include <utils/math/angle.h>
32 #ifdef USE_TIMETRACKER
33 # include <utils/time/tracker.h>
36 #include <pcl/point_cloud.h>
37 #include <pcl/point_types.h>
38 #include <pcl/sample_consensus/method_types.h>
39 #include <pcl/sample_consensus/model_types.h>
40 #include <pcl/segmentation/extract_clusters.h>
41 #include <pcl/filters/extract_indices.h>
42 #include <pcl/surface/convex_hull.h>
43 #include <pcl/kdtree/kdtree.h>
44 #include <pcl/kdtree/kdtree_flann.h>
45 #include <pcl/filters/project_inliers.h>
46 #include <pcl/filters/conditional_removal.h>
47 #include <pcl/common/centroid.h>
48 #include <pcl/common/transforms.h>
49 #include <pcl/common/distances.h>
51 #include <interfaces/Position3DInterface.h>
52 #include <interfaces/SwitchInterface.h>
57 #define CFG_PREFIX "/perception/tabletop-objects/"
67 #ifdef USE_TIMETRACKER
68 #define TIMETRACK_START(c) \
71 #define TIMETRACK_INTER(c1, c2) \
75 #define TIMETRACK_END(c) \
78 #define TIMETRACK_ABORT(c) \
83 #define TIMETRACK_START(c)
84 #define TIMETRACK_INTER(c1, c2)
85 #define TIMETRACK_END(c)
86 #define TIMETRACK_ABORT(c)
91 :
Thread(
"TabletopObjectsThread",
Thread::OPMODE_CONTINUOUS),
94 #ifdef HAVE_VISUAL_DEBUGGING
109 cfg_depth_filter_min_x_ =
config->
get_float(CFG_PREFIX
"depth_filter_min_x");
110 cfg_depth_filter_max_x_ =
config->
get_float(CFG_PREFIX
"depth_filter_max_x");
112 cfg_segm_max_iterations_ =
114 cfg_segm_distance_threshold_ =
116 cfg_segm_inlier_quota_ =
118 cfg_max_z_angle_deviation_ =
config->
get_float(CFG_PREFIX
"max_z_angle_deviation");
119 cfg_table_min_height_ =
config->
get_float(CFG_PREFIX
"table_min_height");
120 cfg_table_max_height_ =
config->
get_float(CFG_PREFIX
"table_max_height");
121 cfg_table_model_length_ =
config->
get_float(CFG_PREFIX
"table_model_length");
122 cfg_table_model_width_ =
config->
get_float(CFG_PREFIX
"table_model_width");
123 cfg_table_model_step_ =
config->
get_float(CFG_PREFIX
"table_model_step");
126 cfg_cluster_tolerance_ =
config->
get_float(CFG_PREFIX
"cluster_tolerance");
127 cfg_cluster_min_size_ =
config->
get_uint(CFG_PREFIX
"cluster_min_size");
128 cfg_cluster_max_size_ =
config->
get_uint(CFG_PREFIX
"cluster_max_size");
132 input_ = pcl_utils::cloudptr_from_refptr(finput_);
135 double rotation[4] = {0., 0., 0., 1.};
136 table_pos_if_ = NULL;
139 table_pos_if_->
write();
147 pos_ifs_.resize(MAX_CENTROIDS, NULL);
148 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
150 if (asprintf(&tmp,
"Tabletop Object %u", i + 1) != -1) {
152 std::string
id = tmp;
164 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
174 fclusters_->header.frame_id = finput_->header.frame_id;
175 fclusters_->is_dense =
false;
177 clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_);
179 ftable_model_ =
new Cloud();
180 table_model_ = pcl_utils::cloudptr_from_refptr(ftable_model_);
181 table_model_->header.frame_id = finput_->header.frame_id;
185 fsimplified_polygon_ =
new Cloud();
186 simplified_polygon_ = pcl_utils::cloudptr_from_refptr(fsimplified_polygon_);
187 simplified_polygon_->header.frame_id = finput_->header.frame_id;
191 grid_.setFilterFieldName(
"x");
192 grid_.setFilterLimits(cfg_depth_filter_min_x_, cfg_depth_filter_max_x_);
193 grid_.setLeafSize(cfg_voxel_leaf_size_, cfg_voxel_leaf_size_, cfg_voxel_leaf_size_);
195 seg_.setOptimizeCoefficients(
true);
196 seg_.setModelType(pcl::SACMODEL_PLANE);
197 seg_.setMethodType(pcl::SAC_RANSAC);
198 seg_.setMaxIterations(cfg_segm_max_iterations_);
199 seg_.setDistanceThreshold(cfg_segm_distance_threshold_);
203 #ifdef USE_TIMETRACKER
206 ttc_full_loop_ = tt_->add_class(
"Full Loop");
207 ttc_msgproc_ = tt_->add_class(
"Message Processing");
208 ttc_voxelize_ = tt_->add_class(
"Downsampling");
209 ttc_plane_ = tt_->add_class(
"Plane Segmentation");
210 ttc_extract_plane_ = tt_->add_class(
"Plane Extraction");
211 ttc_plane_downsampling_ = tt_->add_class(
"Plane Downsampling");
212 ttc_cluster_plane_ = tt_->add_class(
"Plane Clustering");
213 ttc_convex_hull_ = tt_->add_class(
"Convex Hull");
214 ttc_simplify_polygon_ = tt_->add_class(
"Polygon Simplification");
215 ttc_find_edge_ = tt_->add_class(
"Polygon Edge");
216 ttc_transform_ = tt_->add_class(
"Transform");
217 ttc_transform_model_ = tt_->add_class(
"Model Transformation");
218 ttc_extract_non_plane_ = tt_->add_class(
"Non-Plane Extraction");
219 ttc_polygon_filter_ = tt_->add_class(
"Polygon Filter");
220 ttc_table_to_output_ = tt_->add_class(
"Table to Output");
221 ttc_cluster_objects_ = tt_->add_class(
"Object Clustering");
222 ttc_visualization_ = tt_->add_class(
"Visualization");
232 simplified_polygon_.reset();
240 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
247 ftable_model_.reset();
248 fsimplified_polygon_.reset();
251 template <
typename Po
intType>
253 comparePoints2D(
const PointType &p1,
const PointType &p2)
255 double angle1 = atan2(p1.y, p1.x) + M_PI;
256 double angle2 = atan2(p2.y, p2.x) + M_PI;
257 return (angle1 > angle2);
265 TabletopObjectsThread::is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p,
266 PointType &br_p1p, PointType &br_p2p)
269 Eigen::Vector3f cb_br_p1(cb_br_p1p.x, cb_br_p1p.y, cb_br_p1p.z);
270 Eigen::Vector3f cb_br_p2(cb_br_p2p.x, cb_br_p2p.y, cb_br_p2p.z);
271 Eigen::Vector3f cb_br_p1_p2_center = (cb_br_p1 + cb_br_p2) * 0.5;
273 Eigen::Vector3f br_p1(br_p1p.x, br_p1p.y, br_p1p.z);
274 Eigen::Vector3f br_p2(br_p2p.x, br_p2p.y, br_p2p.z);
275 Eigen::Vector3f br_p1_p2_center = (br_p2 + br_p1) * 0.5;
277 double dist_x = (cb_br_p1_p2_center[0] - br_p1_p2_center[0]);
282 if ( (dist_x < -0.25) ||
283 ((abs(dist_x) <= 0.25) && ((br_p2 - br_p1).norm() < (cb_br_p2 - cb_br_p1).norm())) )
293 TIMETRACK_START(ttc_full_loop_);
297 TIMETRACK_START(ttc_msgproc_);
316 TimeWait::wait(250000);
320 TIMETRACK_INTER(ttc_msgproc_, ttc_voxelize_)
322 CloudPtr temp_cloud(
new Cloud);
323 CloudPtr temp_cloud2(
new Cloud);
324 pcl::ExtractIndices<PointType> extract_;
325 CloudPtr cloud_hull_;
326 CloudPtr model_cloud_hull_;
327 CloudPtr cloud_proj_;
328 CloudPtr cloud_filt_;
329 CloudPtr cloud_above_;
330 CloudPtr cloud_objs_;
331 pcl::search::KdTree<PointType> kdtree_;
333 grid_.setInputCloud(input_);
334 grid_.filter(*temp_cloud);
336 if (temp_cloud->points.size() <= 10) {
341 TimeWait::wait(50000);
345 TIMETRACK_INTER(ttc_voxelize_, ttc_plane_)
347 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
348 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
349 Eigen::Vector4f table_centroid, baserel_table_centroid(0,0,0,0);
358 bool happy_with_plane =
false;
359 while (! happy_with_plane) {
360 happy_with_plane =
true;
362 if (temp_cloud->points.size() <= 10) {
363 logger->
log_warn(
name(),
"[L %u] no more points for plane detection, skipping loop", loop_count_);
364 set_position(table_pos_if_,
false);
365 TIMETRACK_ABORT(ttc_plane_);
366 TIMETRACK_ABORT(ttc_full_loop_);
367 TimeWait::wait(50000);
371 seg_.setInputCloud(temp_cloud);
372 seg_.segment(*inliers, *coeff);
375 if ((
double)inliers->indices.size() < (cfg_segm_inlier_quota_ * (double)temp_cloud->points.size())) {
376 logger->
log_warn(
name(),
"[L %u] no table in scene, skipping loop (%zu inliers, required %f, voxelized size %zu)",
377 loop_count_, inliers->indices.size(),
378 (cfg_segm_inlier_quota_ * temp_cloud->points.size()), temp_cloud->points.size());
379 set_position(table_pos_if_,
false);
380 TIMETRACK_ABORT(ttc_plane_);
381 TIMETRACK_ABORT(ttc_full_loop_);
382 TimeWait::wait(50000);
390 table_normal(tf::Vector3(coeff->values[0], coeff->values[1], coeff->values[2]),
395 tf::Vector3 z_axis(0, 0, copysign(1.0, baserel_normal.z()));
397 if (fabs(z_axis.angle(baserel_normal)) > cfg_max_z_angle_deviation_ ) {
398 happy_with_plane =
false;
399 logger->
log_warn(
name(),
"[L %u] table normal (%f,%f,%f) Z angle deviation |%f| > %f, excluding",
400 loop_count_, baserel_normal.x(), baserel_normal.y(), baserel_normal.z(),
401 z_axis.angle(baserel_normal), cfg_max_z_angle_deviation_);
408 if (happy_with_plane) {
414 pcl::compute3DCentroid(*temp_cloud, *inliers, table_centroid);
416 centroid(tf::Point(table_centroid[0], table_centroid[1], table_centroid[2]),
420 baserel_table_centroid[0] = baserel_centroid.x();
421 baserel_table_centroid[1] = baserel_centroid.y();
422 baserel_table_centroid[2] = baserel_centroid.z();
424 if ((baserel_centroid.z() < cfg_table_min_height_) ||
425 (baserel_centroid.z() > cfg_table_max_height_))
427 happy_with_plane =
false;
429 loop_count_, baserel_centroid.z(),
430 cfg_table_min_height_, cfg_table_max_height_);
439 if (! happy_with_plane) {
442 extract_.setNegative(
true);
443 extract_.setInputCloud(temp_cloud);
444 extract_.setIndices(inliers);
445 extract_.filter(extracted);
446 *temp_cloud = extracted;
454 TIMETRACK_INTER(ttc_plane_, ttc_extract_plane_)
456 extract_.setNegative(
false);
457 extract_.setInputCloud(temp_cloud);
458 extract_.setIndices(inliers);
459 extract_.filter(*temp_cloud2);
463 pcl::ProjectInliers<PointType> proj;
464 proj.setModelType(pcl::SACMODEL_PLANE);
465 proj.setInputCloud(temp_cloud2);
466 proj.setModelCoefficients(coeff);
467 cloud_proj_.reset(
new Cloud());
468 proj.filter(*cloud_proj_);
470 TIMETRACK_INTER(ttc_extract_plane_, ttc_plane_downsampling_);
482 CloudPtr cloud_table_voxelized(
new Cloud());
483 pcl::VoxelGrid<PointType> table_grid;
484 table_grid.setLeafSize(0.04, 0.04, 0.04);
485 table_grid.setInputCloud(cloud_proj_);
486 table_grid.filter(*cloud_table_voxelized);
488 TIMETRACK_INTER(ttc_plane_downsampling_, ttc_cluster_plane_);
491 pcl::search::KdTree<PointType>::Ptr
492 kdtree_table(
new pcl::search::KdTree<PointType>());
493 kdtree_table->setInputCloud(cloud_table_voxelized);
495 std::vector<pcl::PointIndices> table_cluster_indices;
496 pcl::EuclideanClusterExtraction<PointType> table_ec;
497 table_ec.setClusterTolerance(0.044);
498 table_ec.setMinClusterSize(0.8 * cloud_table_voxelized->points.size());
499 table_ec.setMaxClusterSize(cloud_table_voxelized->points.size());
500 table_ec.setSearchMethod(kdtree_table);
501 table_ec.setInputCloud(cloud_table_voxelized);
502 table_ec.extract(table_cluster_indices);
504 if (! table_cluster_indices.empty()) {
506 CloudPtr cloud_table_extracted(
new Cloud());
507 pcl::PointIndices::ConstPtr table_cluster_indices_ptr(
new pcl::PointIndices(table_cluster_indices[0]));
508 pcl::ExtractIndices<PointType> table_cluster_extract;
509 table_cluster_extract.setNegative(
false);
510 table_cluster_extract.setInputCloud(cloud_table_voxelized);
511 table_cluster_extract.setIndices(table_cluster_indices_ptr);
512 table_cluster_extract.filter(*cloud_table_extracted);
513 *cloud_proj_ = *cloud_table_extracted;
516 logger->
log_info(
name(),
"[L %u] table plane clustering did not generate any clusters", loop_count_);
519 TIMETRACK_INTER(ttc_cluster_plane_, ttc_convex_hull_)
523 pcl::ConvexHull<PointType> hr;
525 hr.setInputCloud(cloud_proj_);
526 cloud_hull_.reset(
new Cloud());
527 hr.reconstruct(*cloud_hull_);
530 if (cloud_hull_->points.empty()) {
531 logger->
log_warn(
name(),
"[L %u] convex hull of table empty, skipping loop", loop_count_);
532 TIMETRACK_ABORT(ttc_convex_hull_);
533 TIMETRACK_ABORT(ttc_full_loop_);
534 set_position(table_pos_if_,
false);
538 TIMETRACK_INTER(ttc_convex_hull_, ttc_simplify_polygon_)
540 CloudPtr simplified_polygon = simplify_polygon(cloud_hull_, 0.02);
541 *simplified_polygon_ = *simplified_polygon;
544 *cloud_hull_ = *simplified_polygon;
546 TIMETRACK_INTER(ttc_simplify_polygon_, ttc_find_edge_)
548 #ifdef HAVE_VISUAL_DEBUGGING
550 good_hull_edges.resize(cloud_hull_->points.size() * 2);
562 tf::Quaternion q = t.getRotation();
563 Eigen::Affine3f affine_cloud =
564 Eigen::Translation3f(t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z())
565 * Eigen::Quaternionf(q.w(), q.x(), q.y(), q.z());
568 CloudPtr baserel_polygon_cloud(
new Cloud());
569 pcl::transformPointCloud(*cloud_hull_, *baserel_polygon_cloud, affine_cloud);
573 Eigen::Vector3f left_frustrum_normal =
574 Eigen::AngleAxisf( cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
575 * (-1. * Eigen::Vector3f::UnitY());
577 Eigen::Vector3f right_frustrum_normal =
578 Eigen::AngleAxisf(- cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
579 * Eigen::Vector3f::UnitY();
581 Eigen::Vector3f lower_frustrum_normal =
582 Eigen::AngleAxisf(cfg_vertical_va_ * 0.5, Eigen::Vector3f::UnitY())
583 * Eigen::Vector3f::UnitZ();
587 #ifdef HAVE_VISUAL_DEBUGGING
588 size_t geidx1 = std::numeric_limits<size_t>::max();
589 size_t geidx2 = std::numeric_limits<size_t>::max();
592 size_t lf_pidx1, lf_pidx2;
593 pidx1 = pidx2 = lf_pidx1 = lf_pidx2 = std::numeric_limits<size_t>::max();
604 const size_t psize = cloud_hull_->points.size();
605 #ifdef HAVE_VISUAL_DEBUGGING
606 size_t good_edge_points = 0;
608 for (
size_t i = 0; i < psize; ++i) {
610 PointType &p1p = cloud_hull_->points[i ];
611 PointType &p2p = cloud_hull_->points[(i+1) % psize];
613 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
614 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
616 PointType &br_p1p = baserel_polygon_cloud->points[i ];
617 PointType &br_p2p = baserel_polygon_cloud->points[(i + 1) % psize];
620 if ( ! (((left_frustrum_normal.dot(p1) < 0.03) &&
621 (left_frustrum_normal.dot(p2) < 0.03)) ||
622 ((right_frustrum_normal.dot(p1) < 0.03) &&
623 (right_frustrum_normal.dot(p2) < 0.03)) ) )
628 if ((lower_frustrum_normal.dot(p1) < 0.01) &&
629 (lower_frustrum_normal.dot(p2) < 0.01)) {
632 if ( (lf_pidx1 == std::numeric_limits<size_t>::max()) ||
633 is_polygon_edge_better(br_p1p, br_p2p,
634 baserel_polygon_cloud->points[lf_pidx1], baserel_polygon_cloud->points[lf_pidx2]))
639 lf_pidx2 = (i + 1) % psize;
645 #ifdef HAVE_VISUAL_DEBUGGING
647 for (
unsigned int j = 0; j < 3; ++j)
648 good_hull_edges[good_edge_points][j] = p1[j];
649 good_hull_edges[good_edge_points][3] = 0.;
651 for (
unsigned int j = 0; j < 3; ++j)
652 good_hull_edges[good_edge_points][j] = p2[j];
653 good_hull_edges[good_edge_points][3] = 0.;
657 if (pidx1 != std::numeric_limits<size_t>::max()) {
659 PointType &cb_br_p1p = baserel_polygon_cloud->points[pidx1];
660 PointType &cb_br_p2p = baserel_polygon_cloud->points[pidx2];
662 if (! is_polygon_edge_better(cb_br_p1p, cb_br_p2p, br_p1p, br_p2p))
679 pidx2 = (i + 1) % psize;
680 #ifdef HAVE_VISUAL_DEBUGGING
681 geidx1 = good_edge_points - 2;
682 geidx2 = good_edge_points - 1;
695 if (lf_pidx1 != std::numeric_limits<size_t>::max()) {
696 PointType &lp1p = baserel_polygon_cloud->points[lf_pidx1];
697 PointType &lp2p = baserel_polygon_cloud->points[lf_pidx2];
699 Eigen::Vector4f lp1(lp1p.x, lp1p.y, lp1p.z, 0.);
700 Eigen::Vector4f lp2(lp2p.x, lp2p.y, lp2p.z, 0.);
703 if (pidx1 == std::numeric_limits<size_t>::max()) {
707 #ifdef HAVE_VISUAL_DEBUGGING
708 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
709 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
710 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
711 geidx1 = good_edge_points++;
713 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
714 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
715 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
716 geidx2 = good_edge_points++;
721 PointType &p1p = baserel_polygon_cloud->points[pidx1];
722 PointType &p2p = baserel_polygon_cloud->points[pidx2];
724 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0.);
725 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0.);
729 (p1[0] > baserel_table_centroid[0]) || (p2[0] > baserel_table_centroid[0]))
736 #ifdef HAVE_VISUAL_DEBUGGING
737 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
738 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
739 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
740 geidx1 = good_edge_points++;
742 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
743 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
744 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
745 geidx2 = good_edge_points++;
753 #ifdef HAVE_VISUAL_DEBUGGING
754 if (good_edge_points > 0) {
755 good_hull_edges[geidx1][3] = 1.0;
756 good_hull_edges[geidx2][3] = 1.0;
758 good_hull_edges.resize(good_edge_points);
761 TIMETRACK_INTER(ttc_find_edge_, ttc_transform_)
765 PointType &p1p = cloud_hull_->points[pidx1];
766 PointType &p2p = cloud_hull_->points[pidx2];
768 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
769 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
772 Eigen::Vector3f model_normal = Eigen::Vector3f::UnitZ();
773 Eigen::Vector3f normal(coeff->values[0], coeff->values[1], coeff->values[2]);
776 Eigen::Vector3f table_centroid_3f =
777 Eigen::Vector3f(table_centroid[0], table_centroid[1], table_centroid[2]);
780 Eigen::Vector3f p1_p2 = p2 - p1;
781 Eigen::Vector3f p1_p2_center = (p2 + p1) * 0.5;
783 Eigen::Vector3f p1_p2_normal_cross = p1_p2.cross(normal);
784 p1_p2_normal_cross.normalize();
788 double nD = - p1_p2_normal_cross.dot(p1_p2_center);
789 double p1_p2_centroid_dist = p1_p2_normal_cross.dot(table_centroid_3f) + nD;
790 if (p1_p2_centroid_dist < 0) {
792 p1_p2_normal_cross *= -1;
795 Eigen::Vector3f table_center =
796 p1_p2_center + p1_p2_normal_cross * (cfg_table_model_width_ * 0.5);
798 for (
unsigned int i = 0; i < 3; ++i) table_centroid[i] = table_center[i];
799 table_centroid[3] = 0.;
802 std::vector<Eigen::Vector3f> tpoints(4);
803 tpoints[0] = p1_p2_center + p1_p2 * (cfg_table_model_length_ * 0.5);
804 tpoints[1] = tpoints[0] + p1_p2_normal_cross * cfg_table_model_width_;
805 tpoints[3] = p1_p2_center - p1_p2 * (cfg_table_model_length_ * 0.5);
806 tpoints[2] = tpoints[3] + p1_p2_normal_cross * cfg_table_model_width_;
808 model_cloud_hull_.reset(
new Cloud());
809 model_cloud_hull_->points.resize(4);
810 model_cloud_hull_->height = 1;
811 model_cloud_hull_->width = 4;
812 model_cloud_hull_->is_dense =
true;
813 for (
int i = 0; i < 4; ++i) {
814 model_cloud_hull_->points[i].x = tpoints[i][0];
815 model_cloud_hull_->points[i].y = tpoints[i][1];
816 model_cloud_hull_->points[i].z = tpoints[i][2];
823 Eigen::Vector3f rotaxis = model_normal.cross(normal);
825 double angle = acos(normal.dot(model_normal));
828 Eigen::Affine3f affine =
829 Eigen::Translation3f(table_centroid.x(), table_centroid.y(),
831 * Eigen::AngleAxisf(angle, rotaxis);
834 model_p1(-cfg_table_model_width_ * 0.5, cfg_table_model_length_ * 0.5, 0.),
835 model_p2(-cfg_table_model_width_ * 0.5, -cfg_table_model_length_ * 0.5, 0.);
836 model_p1 = affine * model_p1;
837 model_p2 = affine * model_p2;
840 Eigen::Vector3f model_p1_p2 = model_p2 - model_p1;
841 model_p1_p2.normalize();
843 Eigen::Vector3f model_rotaxis = model_p1_p2.cross(p1_p2);
844 model_rotaxis.normalize();
845 double angle_p1_p2 = acos(model_p1_p2.dot(p1_p2));
852 Eigen::Translation3f(table_centroid.x(), table_centroid.y(),
854 * Eigen::AngleAxisf(angle_p1_p2, model_rotaxis)
855 * Eigen::AngleAxisf(angle, rotaxis);
859 Eigen::Quaternionf qt;
860 qt = Eigen::AngleAxisf(angle_p1_p2, model_rotaxis)
861 * Eigen::AngleAxisf(angle, rotaxis);
864 set_position(table_pos_if_,
true, table_centroid, qt);
866 TIMETRACK_INTER(ttc_transform_, ttc_transform_model_)
869 CloudPtr table_model = generate_table_model(cfg_table_model_length_, cfg_table_model_width_, cfg_table_model_step_);
870 pcl::transformPointCloud(*table_model, *table_model_, affine.matrix());
873 table_model_->header.frame_id = finput_->header.frame_id;
875 TIMETRACK_END(ttc_transform_model_);
878 set_position(table_pos_if_,
false);
881 TIMETRACK_ABORT(ttc_find_edge_);
884 TIMETRACK_START(ttc_extract_non_plane_);
886 cloud_filt_.reset(
new Cloud());
887 extract_.setNegative(
true);
888 extract_.filter(*cloud_filt_);
890 TIMETRACK_INTER(ttc_extract_non_plane_, ttc_polygon_filter_);
904 pcl::ComparisonOps::CompareOp op =
905 coeff->values[3] > 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT;
908 pcl::ConditionAnd<PointType>::Ptr
909 above_cond(
new pcl::ConditionAnd<PointType>());
910 above_cond->addComparison(above_comp);
911 pcl::ConditionalRemoval<PointType> above_condrem(above_cond);
912 above_condrem.setInputCloud(cloud_filt_);
914 cloud_above_.reset(
new Cloud());
915 above_condrem.filter(*cloud_above_);
919 if (cloud_filt_->points.size() < cfg_cluster_min_size_) {
921 TIMETRACK_ABORT(ttc_polygon_filter_);
922 TIMETRACK_ABORT(ttc_full_loop_);
927 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
929 pcl::ConditionAnd<PointType>::Ptr
930 polygon_cond(
new pcl::ConditionAnd<PointType>());
934 (model_cloud_hull_ && ! model_cloud_hull_->points.empty()) ? *model_cloud_hull_ : *cloud_hull_));
935 polygon_cond->addComparison(inpoly_comp);
938 pcl::ConditionalRemoval<PointType> condrem(polygon_cond);
939 condrem.setInputCloud(cloud_above_);
941 cloud_objs_.reset(
new Cloud());
942 condrem.filter(*cloud_objs_);
951 TIMETRACK_INTER(ttc_polygon_filter_, ttc_table_to_output_)
954 tmp_clusters->header.frame_id = clusters_->header.frame_id;
955 std::vector<int> &indices = inliers->indices;
956 tmp_clusters->height = 1;
960 tmp_clusters->width = indices.size();
961 tmp_clusters->points.resize(indices.size());
962 for (
size_t i = 0; i < indices.size(); ++i) {
963 PointType &p1 = temp_cloud2->points[i];
965 ColorPointType &p2 = tmp_clusters->points[i];
969 p2.r = table_color[0];
970 p2.g = table_color[1];
971 p2.b = table_color[2];
974 TIMETRACK_INTER(ttc_table_to_output_, ttc_cluster_objects_)
976 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
977 centroids.resize(MAX_CENTROIDS);
978 unsigned int centroid_i = 0;
980 if (cloud_objs_->points.size() > 0) {
982 pcl::search::KdTree<PointType>::Ptr
983 kdtree_cl(
new pcl::search::KdTree<PointType>());
984 kdtree_cl->setInputCloud(cloud_objs_);
986 std::vector<pcl::PointIndices> cluster_indices;
987 pcl::EuclideanClusterExtraction<PointType> ec;
988 ec.setClusterTolerance(cfg_cluster_tolerance_);
989 ec.setMinClusterSize(cfg_cluster_min_size_);
990 ec.setMaxClusterSize(cfg_cluster_max_size_);
991 ec.setSearchMethod(kdtree_cl);
992 ec.setInputCloud(cloud_objs_);
993 ec.extract(cluster_indices);
997 ColorCloudPtr colored_clusters(
new ColorCloud());
998 colored_clusters->header.frame_id = clusters_->header.frame_id;
999 std::vector<pcl::PointIndices>::const_iterator it;
1000 unsigned int cci = 0;
1002 unsigned int num_points = 0;
1003 for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
1004 num_points += it->indices.size();
1006 if (num_points > 0) {
1007 colored_clusters->points.resize(num_points);
1008 for (it = cluster_indices.begin();
1009 it != cluster_indices.end() && centroid_i < MAX_CENTROIDS;
1012 pcl::compute3DCentroid(*cloud_objs_, it->indices, centroids[centroid_i]);
1014 std::vector<int>::const_iterator pit;
1015 for (pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
1016 ColorPointType &p1 = colored_clusters->points[cci++];
1017 PointType &p2 = cloud_objs_->points[*pit];
1021 p1.r = cluster_colors[centroid_i][0];
1022 p1.g = cluster_colors[centroid_i][1];;
1023 p1.b = cluster_colors[centroid_i][2];;
1027 *tmp_clusters += *colored_clusters;
1035 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
1036 set_position(pos_ifs_[i], i < centroid_i, centroids[i]);
1038 centroids.resize(centroid_i);
1040 TIMETRACK_INTER(ttc_cluster_objects_, ttc_visualization_)
1042 *clusters_ = *tmp_clusters;
1043 pcl_utils::copy_time(finput_, fclusters_);
1044 pcl_utils::copy_time(finput_, ftable_model_);
1045 pcl_utils::copy_time(finput_, fsimplified_polygon_);
1047 #ifdef HAVE_VISUAL_DEBUGGING
1049 Eigen::Vector4f normal;
1050 normal[0] = coeff->values[0];
1051 normal[1] = coeff->values[1];
1052 normal[2] = coeff->values[2];
1056 hull_vertices.resize(cloud_hull_->points.size());
1057 for (
unsigned int i = 0; i < cloud_hull_->points.size(); ++i) {
1058 hull_vertices[i][0] = cloud_hull_->points[i].x;
1059 hull_vertices[i][1] = cloud_hull_->points[i].y;
1060 hull_vertices[i][2] = cloud_hull_->points[i].z;
1061 hull_vertices[i][3] = 0.;
1065 if (model_cloud_hull_ && ! model_cloud_hull_->points.empty()) {
1066 model_vertices.resize(model_cloud_hull_->points.size());
1067 for (
unsigned int i = 0; i < model_cloud_hull_->points.size(); ++i) {
1068 model_vertices[i][0] = model_cloud_hull_->points[i].x;
1069 model_vertices[i][1] = model_cloud_hull_->points[i].y;
1070 model_vertices[i][2] = model_cloud_hull_->points[i].z;
1071 model_vertices[i][3] = 0.;
1075 visthread_->visualize(input_->header.frame_id,
1076 table_centroid, normal, hull_vertices, model_vertices,
1077 good_hull_edges, centroids);
1081 TIMETRACK_END(ttc_visualization_);
1082 TIMETRACK_END(ttc_full_loop_);
1084 #ifdef USE_TIMETRACKER
1085 if (++tt_loopcount_ >= 5) {
1087 tt_->print_to_stdout();
1095 bool is_visible,
const Eigen::Vector4f ¢roid,
1096 const Eigen::Quaternionf &attitude)
1101 spose(tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()),
1102 tf::Vector3(centroid[0], centroid[1], centroid[2])),
1105 iface->
set_frame(cfg_result_frame_.c_str());
1112 if (visibility_history >= 0) {
1117 tf::Vector3 &origin = baserel_pose.getOrigin();
1118 tf::Quaternion quat = baserel_pose.getRotation();
1119 double translation[3] = { origin.x(), origin.y(), origin.z() };
1120 double rotation[4] = { quat.x(), quat.y(), quat.z(), quat.w() };
1125 if (visibility_history <= 0) {
1129 double translation[3] = { 0, 0, 0 };
1130 double rotation[4] = { 0, 0, 0, 1 };
1139 TabletopObjectsThread::CloudPtr
1140 TabletopObjectsThread::generate_table_model(
const float length,
const float width,
1141 const float thickness,
const float step,
1142 const float max_error)
1144 CloudPtr c(
new Cloud());
1146 const float length_2 = fabs(length) * 0.5;
1147 const float width_2 = fabs(width) * 0.5;
1148 const float thickness_2 = fabs(thickness) * 0.5;
1151 const unsigned int l_base_num = std::max(2u, (
unsigned int)floor(length / step));
1152 const unsigned int num_w = l_base_num +
1153 ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1154 const unsigned int w_base_num = std::max(2u, (
unsigned int)floor(width / step));
1155 const unsigned int num_h = w_base_num +
1156 ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1157 const unsigned int t_base_num = std::max(2u, (
unsigned int)floor(thickness / step));
1158 const unsigned int num_t = t_base_num +
1159 ((thickness < t_base_num * step) ? 0 : ((thickness - t_base_num * step) > max_error ? 2 : 1));
1166 c->width = num_t * num_w * num_h;
1168 c->points.resize(num_t * num_w * num_h);
1170 unsigned int idx = 0;
1171 for (
unsigned int t = 0; t < num_t; ++t) {
1172 for (
unsigned int w = 0; w < num_w; ++w) {
1173 for (
unsigned int h = 0; h < num_h; ++h) {
1174 PointType &p = c->points[idx++];
1176 p.x = h * step - width_2;
1177 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error) p.x = width_2;
1179 p.y = w * step - length_2;
1180 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error) p.y = length_2;
1182 p.z = t * step - thickness_2;
1183 if ((t == num_t - 1) && fabs(p.z - thickness_2) > max_error) p.z = thickness_2;
1191 TabletopObjectsThread::CloudPtr
1192 TabletopObjectsThread::generate_table_model(
const float length,
const float width,
1193 const float step,
const float max_error)
1195 CloudPtr c(
new Cloud());
1197 const float length_2 = fabs(length) * 0.5;
1198 const float width_2 = fabs(width) * 0.5;
1201 const unsigned int l_base_num = std::max(2u, (
unsigned int)floor(length / step));
1202 const unsigned int num_w = l_base_num +
1203 ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1204 const unsigned int w_base_num = std::max(2u, (
unsigned int)floor(width / step));
1205 const unsigned int num_h = w_base_num +
1206 ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1212 c->width = num_w * num_h;
1214 c->points.resize(num_w * num_h);
1216 unsigned int idx = 0;
1217 for (
unsigned int w = 0; w < num_w; ++w) {
1218 for (
unsigned int h = 0; h < num_h; ++h) {
1219 PointType &p = c->points[idx++];
1221 p.x = h * step - width_2;
1222 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error) p.x = width_2;
1224 p.y = w * step - length_2;
1225 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error) p.y = length_2;
1235 TabletopObjectsThread::CloudPtr
1236 TabletopObjectsThread::simplify_polygon(CloudPtr polygon,
float dist_threshold)
1238 const float sqr_dist_threshold = dist_threshold * dist_threshold;
1239 CloudPtr result(
new Cloud());
1240 const size_t psize = polygon->points.size();
1241 result->points.resize(psize);
1242 size_t res_points = 0;
1244 for (
size_t i = 1; i <= psize; ++i) {
1245 PointType &p1p = polygon->points[i - i_dist ];
1248 if (result->points.empty()) {
1254 PointType &p2p = polygon->points[i % psize];
1255 PointType &p3p = (i == psize) ? result->points[0] : polygon->points[(i + 1) % psize];
1257 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0);
1258 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0);
1259 Eigen::Vector4f p3(p3p.x, p3p.y, p3p.z, 0);
1261 Eigen::Vector4f line_dir = p3 - p1;
1263 double sqr_dist = pcl::sqrPointToLineDistance(p2, p1, line_dir);
1264 if (sqr_dist < sqr_dist_threshold) {
1268 result->points[res_points++] = p2p;
1272 result->header.frame_id = polygon->header.frame_id;
1273 result->header.stamp = polygon->header.stamp;
1274 result->width = res_points;
1276 result->is_dense =
false;
1277 result->points.resize(res_points);
1283 #ifdef HAVE_VISUAL_DEBUGGING
1287 visthread_ = visthread;
void set_frame(const char *new_frame)
Set frame value.
bool is_enabled() const
Get enabled value.
Compare points' distance to a plane.
bool msgq_empty()
Check if queue is empty.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void remove_pointcloud(const char *id)
Remove the point cloud.
Fawkes library namespace.
std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > V_Vector4f
Aligned vector of vectors/points.
A class for handling time.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Check if point is inside or outside a given polygon.
void write()
Write from local copy into BlackBoard memory.
Base class for virtualization thread.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
virtual ~TabletopObjectsThread()
Destructor.
Clock * clock
By means of this member access to the clock is given.
void reset()
Reset pointer.
void msgq_pop()
Erase first message from queue.
boost::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Position3DInterface Fawkes BlackBoard Interface.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
void set_enabled(const bool new_enabled)
Set enabled value.
TabletopObjectsThread()
Constructor.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
Wrapper class to add time stamp and frame ID to base types.
int32_t visibility_history() const
Get visibility_history value.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
virtual void init()
Initialize the thread.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
virtual void loop()
Code to execute in the thread.
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.
boost::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.