22 #include "tabletop_objects_thread.h"
24 #include "cluster_colors.h"
25 #ifdef HAVE_VISUAL_DEBUGGING
26 # include "visualization_thread_base.h"
29 #include <pcl_utils/comparisons.h>
30 #include <pcl_utils/utils.h>
31 #include <utils/math/angle.h>
32 #include <utils/time/wait.h>
33 #ifdef USE_TIMETRACKER
34 # include <utils/time/tracker.h>
36 #include <interfaces/Position3DInterface.h>
37 #include <interfaces/SwitchInterface.h>
38 #include <pcl/ModelCoefficients.h>
39 #include <pcl/common/centroid.h>
40 #include <pcl/common/distances.h>
41 #include <pcl/common/transforms.h>
42 #include <pcl/features/normal_3d.h>
43 #include <pcl/filters/conditional_removal.h>
44 #include <pcl/filters/extract_indices.h>
45 #include <pcl/filters/passthrough.h>
46 #include <pcl/filters/project_inliers.h>
47 #include <pcl/filters/statistical_outlier_removal.h>
48 #include <pcl/kdtree/kdtree.h>
49 #include <pcl/kdtree/kdtree_flann.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/point_types.h>
52 #include <pcl/registration/distances.h>
53 #include <pcl/sample_consensus/method_types.h>
54 #include <pcl/sample_consensus/model_types.h>
55 #include <pcl/segmentation/extract_clusters.h>
56 #include <pcl/surface/convex_hull.h>
57 #include <utils/hungarian_method/hungarian.h>
58 #include <utils/time/tracker_macros.h>
64 #define CFG_PREFIX "/perception/tabletop-objects/"
75 :
Thread(
"TabletopObjectsThread",
Thread::OPMODE_CONTINUOUS),
78 #ifdef HAVE_VISUAL_DEBUGGING
91 cfg_depth_filter_min_x_ =
config->
get_float(CFG_PREFIX
"depth_filter_min_x");
92 cfg_depth_filter_max_x_ =
config->
get_float(CFG_PREFIX
"depth_filter_max_x");
94 cfg_segm_max_iterations_ =
config->
get_uint(CFG_PREFIX
"table_segmentation_max_iterations");
95 cfg_segm_distance_threshold_ =
97 cfg_segm_inlier_quota_ =
config->
get_float(CFG_PREFIX
"table_segmentation_inlier_quota");
98 cfg_table_min_cluster_quota_ =
config->
get_float(CFG_PREFIX
"table_min_cluster_quota");
99 cfg_table_downsample_leaf_size_ =
config->
get_float(CFG_PREFIX
"table_downsample_leaf_size");
100 cfg_table_cluster_tolerance_ =
config->
get_float(CFG_PREFIX
"table_cluster_tolerance");
101 cfg_max_z_angle_deviation_ =
config->
get_float(CFG_PREFIX
"max_z_angle_deviation");
102 cfg_table_min_height_ =
config->
get_float(CFG_PREFIX
"table_min_height");
103 cfg_table_max_height_ =
config->
get_float(CFG_PREFIX
"table_max_height");
104 cfg_table_model_enable_ =
config->
get_bool(CFG_PREFIX
"table_model_enable");
105 cfg_table_model_length_ =
config->
get_float(CFG_PREFIX
"table_model_length");
106 cfg_table_model_width_ =
config->
get_float(CFG_PREFIX
"table_model_width");
107 cfg_table_model_step_ =
config->
get_float(CFG_PREFIX
"table_model_step");
110 cfg_cluster_tolerance_ =
config->
get_float(CFG_PREFIX
"cluster_tolerance");
111 cfg_cluster_min_size_ =
config->
get_uint(CFG_PREFIX
"cluster_min_size");
112 cfg_cluster_max_size_ =
config->
get_uint(CFG_PREFIX
"cluster_max_size");
116 cfg_centroid_max_age_ =
config->
get_uint(CFG_PREFIX
"centroid_max_age");
117 cfg_centroid_max_distance_ =
config->
get_float(CFG_PREFIX
"centroid_max_distance");
118 cfg_centroid_min_distance_ =
config->
get_float(CFG_PREFIX
"centroid_min_distance");
119 cfg_centroid_max_height_ =
config->
get_float(CFG_PREFIX
"centroid_max_height");
120 cfg_cylinder_fitting_ =
config->
get_bool(CFG_PREFIX
"enable_cylinder_fitting");
121 cfg_track_objects_ =
config->
get_bool(CFG_PREFIX
"enable_object_tracking");
123 cfg_verbose_cylinder_fitting_ =
config->
get_bool(CFG_PREFIX
"verbose_cylinder_fitting");
125 cfg_verbose_cylinder_fitting_ =
false;
130 input_ = pcl_utils::cloudptr_from_refptr(finput_);
134 colored_input_ = pcl_utils::cloudptr_from_refptr(fcoloredinput_);
135 converted_input_.reset(
new Cloud());
136 input_ = converted_input_;
137 converted_input_->header.frame_id = colored_input_->header.frame_id;
138 converted_input_->header.stamp = colored_input_->header.stamp;
140 throw Exception(
"Point cloud '%s' does not exist or not XYZ or XYZ/RGB PCL",
141 cfg_input_pointcloud_.c_str());
145 double rotation[4] = {0., 0., 0., 1.};
146 table_pos_if_ = NULL;
151 table_pos_if_->
write();
158 pos_ifs_.resize(MAX_CENTROIDS, NULL);
159 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
161 if (asprintf(&tmp,
"Tabletop Object %u", i + 1) != -1) {
163 std::string
id = tmp;
174 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
183 fclusters_->header.frame_id = input_->header.frame_id;
184 fclusters_->is_dense =
false;
186 clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_);
191 for (
int i = 0; i < MAX_CENTROIDS; i++) {
193 f_tmp_cloud->header.frame_id = input_->header.frame_id;
194 f_tmp_cloud->is_dense =
false;
196 if (asprintf(&tmp_name,
"obj_cluster_%u", i) != -1) {
201 f_obj_clusters_.push_back(f_tmp_cloud);
202 tmp_cloud = pcl_utils::cloudptr_from_refptr(f_tmp_cloud);
203 obj_clusters_.push_back(tmp_cloud);
210 std::vector<double> init_likelihoods;
211 init_likelihoods.resize(NUM_KNOWN_OBJS_ + 1, 0.0);
216 known_obj_dimensions_.resize(NUM_KNOWN_OBJS_);
219 known_obj_dimensions_[0][0] = 0.07;
220 known_obj_dimensions_[0][1] = 0.07;
221 known_obj_dimensions_[0][2] = 0.104;
223 known_obj_dimensions_[1][0] = 0.088;
224 known_obj_dimensions_[1][1] = 0.088;
225 known_obj_dimensions_[1][2] = 0.155;
227 known_obj_dimensions_[2][0] = 0.106;
228 known_obj_dimensions_[2][1] = 0.106;
229 known_obj_dimensions_[2][2] = 0.277;
233 table_inclination_ = 0.0;
235 ftable_model_ =
new Cloud();
236 table_model_ = pcl_utils::cloudptr_from_refptr(ftable_model_);
237 table_model_->header.frame_id = input_->header.frame_id;
241 fsimplified_polygon_ =
new Cloud();
242 simplified_polygon_ = pcl_utils::cloudptr_from_refptr(fsimplified_polygon_);
243 simplified_polygon_->header.frame_id = input_->header.frame_id;
247 grid_.setFilterFieldName(
"x");
248 grid_.setFilterLimits(cfg_depth_filter_min_x_, cfg_depth_filter_max_x_);
249 grid_.setLeafSize(cfg_voxel_leaf_size_, cfg_voxel_leaf_size_, cfg_voxel_leaf_size_);
251 seg_.setOptimizeCoefficients(
true);
252 seg_.setModelType(pcl::SACMODEL_PLANE);
253 seg_.setMethodType(pcl::SAC_RANSAC);
254 seg_.setMaxIterations(cfg_segm_max_iterations_);
255 seg_.setDistanceThreshold(cfg_segm_distance_threshold_);
263 old_centroids_.clear();
264 for (
unsigned int i = 0; i < MAX_CENTROIDS; i++)
265 free_ids_.push_back(i);
267 #ifdef USE_TIMETRACKER
270 ttc_full_loop_ = tt_->add_class(
"Full Loop");
271 ttc_msgproc_ = tt_->add_class(
"Message Processing");
272 ttc_convert_ = tt_->add_class(
"Input Conversion");
273 ttc_voxelize_ = tt_->add_class(
"Downsampling");
274 ttc_plane_ = tt_->add_class(
"Plane Segmentation");
275 ttc_extract_plane_ = tt_->add_class(
"Plane Extraction");
276 ttc_plane_downsampling_ = tt_->add_class(
"Plane Downsampling");
277 ttc_cluster_plane_ = tt_->add_class(
"Plane Clustering");
278 ttc_convex_hull_ = tt_->add_class(
"Convex Hull");
279 ttc_simplify_polygon_ = tt_->add_class(
"Polygon Simplification");
280 ttc_find_edge_ = tt_->add_class(
"Polygon Edge");
281 ttc_transform_ = tt_->add_class(
"Transform");
282 ttc_transform_model_ = tt_->add_class(
"Model Transformation");
283 ttc_extract_non_plane_ = tt_->add_class(
"Non-Plane Extraction");
284 ttc_polygon_filter_ = tt_->add_class(
"Polygon Filter");
285 ttc_table_to_output_ = tt_->add_class(
"Table to Output");
286 ttc_cluster_objects_ = tt_->add_class(
"Object Clustering");
287 ttc_visualization_ = tt_->add_class(
"Visualization");
288 ttc_hungarian_ = tt_->add_class(
"Hungarian Method (centroids)");
289 ttc_old_centroids_ = tt_->add_class(
"Old Centroid Removal");
290 ttc_obj_extraction_ = tt_->add_class(
"Object Extraction");
299 simplified_polygon_.reset();
307 for (PosIfsVector::iterator it = pos_ifs_.begin(); it != pos_ifs_.end(); ++it) {
314 ftable_model_.reset();
315 fsimplified_polygon_.reset();
317 delete last_pcl_time_;
318 #ifdef USE_TIMETRACKER
323 template <
typename Po
intType>
325 comparePoints2D(
const PointType &p1,
const PointType &p2)
327 double angle1 = atan2(p1.y, p1.x) + M_PI;
328 double angle2 = atan2(p2.y, p2.x) + M_PI;
329 return (angle1 > angle2);
336 TabletopObjectsThread::is_polygon_edge_better(PointType &cb_br_p1p,
337 PointType &cb_br_p2p,
342 Eigen::Vector3f cb_br_p1(cb_br_p1p.x, cb_br_p1p.y, cb_br_p1p.z);
343 Eigen::Vector3f cb_br_p2(cb_br_p2p.x, cb_br_p2p.y, cb_br_p2p.z);
344 Eigen::Vector3f cb_br_p1_p2_center = (cb_br_p1 + cb_br_p2) * 0.5;
346 Eigen::Vector3f br_p1(br_p1p.x, br_p1p.y, br_p1p.z);
347 Eigen::Vector3f br_p2(br_p2p.x, br_p2p.y, br_p2p.z);
348 Eigen::Vector3f br_p1_p2_center = (br_p2 + br_p1) * 0.5;
350 double dist_x = (cb_br_p1_p2_center[0] - br_p1_p2_center[0]);
356 || ((abs(dist_x) <= 0.25) && ((br_p2 - br_p1).norm() < (cb_br_p2 - cb_br_p1).norm())))
365 TIMETRACK_START(ttc_full_loop_);
369 TIMETRACK_START(ttc_msgproc_);
384 TimeWait::wait(250000);
385 TIMETRACK_ABORT(ttc_full_loop_);
389 TIMETRACK_END(ttc_msgproc_);
392 if (colored_input_) {
393 pcl_utils::get_time(colored_input_, pcl_time);
395 pcl_utils::get_time(input_, pcl_time);
397 if (*last_pcl_time_ == pcl_time) {
398 TimeWait::wait(20000);
399 TIMETRACK_ABORT(ttc_full_loop_);
402 *last_pcl_time_ = pcl_time;
404 if (colored_input_) {
405 TIMETRACK_START(ttc_convert_);
406 convert_colored_input();
407 TIMETRACK_END(ttc_convert_);
410 TIMETRACK_START(ttc_voxelize_);
412 CloudPtr temp_cloud(
new Cloud);
413 CloudPtr temp_cloud2(
new Cloud);
414 pcl::ExtractIndices<PointType> extract_;
415 CloudPtr cloud_hull_;
416 CloudPtr model_cloud_hull_;
417 CloudPtr cloud_proj_;
418 CloudPtr cloud_filt_;
419 CloudPtr cloud_above_;
420 CloudPtr cloud_objs_;
421 pcl::search::KdTree<PointType> kdtree_;
423 grid_.setInputCloud(input_);
424 grid_.filter(*temp_cloud);
426 if (temp_cloud->points.size() <= 10) {
431 TimeWait::wait(50000);
435 TIMETRACK_INTER(ttc_voxelize_, ttc_plane_)
437 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
438 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
439 Eigen::Vector4f baserel_table_centroid(0, 0, 0, 0);
448 bool happy_with_plane =
false;
449 while (!happy_with_plane) {
450 happy_with_plane =
true;
452 if (temp_cloud->points.size() <= 10) {
454 "[L %u] no more points for plane detection, skipping loop",
456 set_position(table_pos_if_,
false);
457 TIMETRACK_ABORT(ttc_plane_);
458 TIMETRACK_ABORT(ttc_full_loop_);
459 TimeWait::wait(50000);
463 seg_.setInputCloud(temp_cloud);
464 seg_.segment(*inliers, *coeff);
467 if ((
double)inliers->indices.size()
468 < (cfg_segm_inlier_quota_ * (double)temp_cloud->points.size())) {
471 "[L %u] no table in scene, skipping loop (%zu inliers, required %f, voxelized size %zu)",
473 inliers->indices.size(),
474 (cfg_segm_inlier_quota_ * temp_cloud->points.size()),
475 temp_cloud->points.size());
476 set_position(table_pos_if_,
false);
477 TIMETRACK_ABORT(ttc_plane_);
478 TIMETRACK_ABORT(ttc_full_loop_);
479 TimeWait::wait(50000);
490 input_->header.frame_id);
494 tf::Vector3 z_axis(0, 0, copysign(1.0, baserel_normal.z()));
495 table_inclination_ = z_axis.angle(baserel_normal);
496 if (fabs(z_axis.angle(baserel_normal)) > cfg_max_z_angle_deviation_) {
497 happy_with_plane =
false;
499 "[L %u] table normal (%f,%f,%f) Z angle deviation |%f| > %f, excluding",
504 z_axis.angle(baserel_normal),
505 cfg_max_z_angle_deviation_);
510 happy_with_plane =
false;
513 if (happy_with_plane) {
519 pcl::compute3DCentroid(*temp_cloud, *inliers, table_centroid);
524 input_->header.frame_id);
527 baserel_table_centroid[0] = baserel_centroid.x();
528 baserel_table_centroid[1] = baserel_centroid.y();
529 baserel_table_centroid[2] = baserel_centroid.z();
531 if ((baserel_centroid.z() < cfg_table_min_height_)
532 || (baserel_centroid.z() > cfg_table_max_height_)) {
533 happy_with_plane =
false;
535 "[L %u] table height %f not in range [%f, %f]",
537 baserel_centroid.z(),
538 cfg_table_min_height_,
539 cfg_table_max_height_);
547 if (!happy_with_plane) {
550 extract_.setNegative(
true);
551 extract_.setInputCloud(temp_cloud);
552 extract_.setIndices(inliers);
553 extract_.filter(extracted);
554 *temp_cloud = extracted;
562 TIMETRACK_INTER(ttc_plane_, ttc_extract_plane_)
564 extract_.setNegative(
false);
565 extract_.setInputCloud(temp_cloud);
566 extract_.setIndices(inliers);
567 extract_.filter(*temp_cloud2);
570 pcl::ProjectInliers<PointType> proj;
571 proj.setModelType(pcl::SACMODEL_PLANE);
572 proj.setInputCloud(temp_cloud2);
573 proj.setModelCoefficients(coeff);
574 cloud_proj_.reset(
new Cloud());
575 proj.filter(*cloud_proj_);
577 TIMETRACK_INTER(ttc_extract_plane_, ttc_plane_downsampling_);
589 CloudPtr cloud_table_voxelized(
new Cloud());
590 pcl::VoxelGrid<PointType> table_grid;
591 table_grid.setLeafSize(cfg_table_downsample_leaf_size_,
592 cfg_table_downsample_leaf_size_,
593 cfg_table_downsample_leaf_size_);
594 table_grid.setInputCloud(cloud_proj_);
595 table_grid.filter(*cloud_table_voxelized);
597 TIMETRACK_INTER(ttc_plane_downsampling_, ttc_cluster_plane_);
600 pcl::search::KdTree<PointType>::Ptr kdtree_table(
new pcl::search::KdTree<PointType>());
601 kdtree_table->setInputCloud(cloud_table_voxelized);
603 std::vector<pcl::PointIndices> table_cluster_indices;
604 pcl::EuclideanClusterExtraction<PointType> table_ec;
605 table_ec.setClusterTolerance(cfg_table_cluster_tolerance_);
606 table_ec.setMinClusterSize(cfg_table_min_cluster_quota_ * cloud_table_voxelized->points.size());
607 table_ec.setMaxClusterSize(cloud_table_voxelized->points.size());
608 table_ec.setSearchMethod(kdtree_table);
609 table_ec.setInputCloud(cloud_table_voxelized);
610 table_ec.extract(table_cluster_indices);
612 if (!table_cluster_indices.empty()) {
614 CloudPtr cloud_table_extracted(
new Cloud());
615 pcl::PointIndices::ConstPtr table_cluster_indices_ptr(
616 new pcl::PointIndices(table_cluster_indices[0]));
617 pcl::ExtractIndices<PointType> table_cluster_extract;
618 table_cluster_extract.setNegative(
false);
619 table_cluster_extract.setInputCloud(cloud_table_voxelized);
620 table_cluster_extract.setIndices(table_cluster_indices_ptr);
621 table_cluster_extract.filter(*cloud_table_extracted);
622 *cloud_proj_ = *cloud_table_extracted;
625 pcl::compute3DCentroid(*cloud_proj_, table_centroid);
630 "[L %u] table plane clustering did not generate any clusters",
634 TIMETRACK_INTER(ttc_cluster_plane_, ttc_convex_hull_)
637 pcl::ConvexHull<PointType> hr;
638 #ifdef PCL_VERSION_COMPARE
639 # if PCL_VERSION_COMPARE(>=, 1, 5, 0)
645 hr.setInputCloud(cloud_proj_);
646 cloud_hull_.reset(
new Cloud());
647 hr.reconstruct(*cloud_hull_);
649 if (cloud_hull_->points.empty()) {
650 logger->
log_warn(
name(),
"[L %u] convex hull of table empty, skipping loop", loop_count_);
651 TIMETRACK_ABORT(ttc_convex_hull_);
652 TIMETRACK_ABORT(ttc_full_loop_);
653 set_position(table_pos_if_,
false);
657 TIMETRACK_INTER(ttc_convex_hull_, ttc_simplify_polygon_)
659 CloudPtr simplified_polygon = simplify_polygon(cloud_hull_, 0.02);
660 *simplified_polygon_ = *simplified_polygon;
663 *cloud_hull_ = *simplified_polygon;
665 TIMETRACK_INTER(ttc_simplify_polygon_, ttc_find_edge_)
667 #ifdef HAVE_VISUAL_DEBUGGING
669 good_hull_edges.resize(cloud_hull_->points.size() * 2);
679 tf::Quaternion q = t.getRotation();
680 Eigen::Affine3f affine_cloud =
681 Eigen::Translation3f(t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z())
682 * Eigen::Quaternionf(q.w(), q.x(), q.y(), q.z());
685 CloudPtr baserel_polygon_cloud(
new Cloud());
686 pcl::transformPointCloud(*cloud_hull_, *baserel_polygon_cloud, affine_cloud);
690 Eigen::Vector3f left_frustrum_normal =
691 Eigen::AngleAxisf(cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
692 * (-1. * Eigen::Vector3f::UnitY());
694 Eigen::Vector3f right_frustrum_normal =
695 Eigen::AngleAxisf(-cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
696 * Eigen::Vector3f::UnitY();
698 Eigen::Vector3f lower_frustrum_normal =
699 Eigen::AngleAxisf(cfg_vertical_va_ * 0.5, Eigen::Vector3f::UnitY())
700 * Eigen::Vector3f::UnitZ();
704 #ifdef HAVE_VISUAL_DEBUGGING
705 size_t geidx1 = std::numeric_limits<size_t>::max();
706 size_t geidx2 = std::numeric_limits<size_t>::max();
709 size_t lf_pidx1, lf_pidx2;
710 pidx1 = pidx2 = lf_pidx1 = lf_pidx2 = std::numeric_limits<size_t>::max();
721 const size_t psize = cloud_hull_->points.size();
722 #ifdef HAVE_VISUAL_DEBUGGING
723 size_t good_edge_points = 0;
725 for (
size_t i = 0; i < psize; ++i) {
727 PointType &p1p = cloud_hull_->points[i];
728 PointType &p2p = cloud_hull_->points[(i + 1) % psize];
730 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
731 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
733 PointType &br_p1p = baserel_polygon_cloud->points[i];
734 PointType &br_p2p = baserel_polygon_cloud->points[(i + 1) % psize];
737 if (!(((left_frustrum_normal.dot(p1) < 0.03) && (left_frustrum_normal.dot(p2) < 0.03))
738 || ((right_frustrum_normal.dot(p1) < 0.03)
739 && (right_frustrum_normal.dot(p2) < 0.03)))) {
743 if ((lower_frustrum_normal.dot(p1) < 0.01) && (lower_frustrum_normal.dot(p2) < 0.01)) {
746 if ((lf_pidx1 == std::numeric_limits<size_t>::max())
747 || is_polygon_edge_better(br_p1p,
749 baserel_polygon_cloud->points[lf_pidx1],
750 baserel_polygon_cloud->points[lf_pidx2])) {
754 lf_pidx2 = (i + 1) % psize;
760 #ifdef HAVE_VISUAL_DEBUGGING
762 for (
unsigned int j = 0; j < 3; ++j)
763 good_hull_edges[good_edge_points][j] = p1[j];
764 good_hull_edges[good_edge_points][3] = 0.;
766 for (
unsigned int j = 0; j < 3; ++j)
767 good_hull_edges[good_edge_points][j] = p2[j];
768 good_hull_edges[good_edge_points][3] = 0.;
772 if (pidx1 != std::numeric_limits<size_t>::max()) {
774 PointType &cb_br_p1p = baserel_polygon_cloud->points[pidx1];
775 PointType &cb_br_p2p = baserel_polygon_cloud->points[pidx2];
777 if (!is_polygon_edge_better(cb_br_p1p, cb_br_p2p, br_p1p, br_p2p)) {
793 pidx2 = (i + 1) % psize;
794 #ifdef HAVE_VISUAL_DEBUGGING
795 geidx1 = good_edge_points - 2;
796 geidx2 = good_edge_points - 1;
809 if (lf_pidx1 != std::numeric_limits<size_t>::max()) {
810 PointType &lp1p = baserel_polygon_cloud->points[lf_pidx1];
811 PointType &lp2p = baserel_polygon_cloud->points[lf_pidx2];
813 Eigen::Vector4f lp1(lp1p.x, lp1p.y, lp1p.z, 0.);
814 Eigen::Vector4f lp2(lp2p.x, lp2p.y, lp2p.z, 0.);
817 if (pidx1 == std::numeric_limits<size_t>::max()) {
821 #ifdef HAVE_VISUAL_DEBUGGING
822 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
823 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
824 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
825 geidx1 = good_edge_points++;
827 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
828 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
829 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
830 geidx2 = good_edge_points++;
834 PointType &p1p = baserel_polygon_cloud->points[pidx1];
835 PointType &p2p = baserel_polygon_cloud->points[pidx2];
837 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0.);
838 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0.);
842 (p1[0] > baserel_table_centroid[0]) || (p2[0] > baserel_table_centroid[0])) {
848 #ifdef HAVE_VISUAL_DEBUGGING
849 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
850 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
851 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
852 geidx1 = good_edge_points++;
854 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
855 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
856 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
857 geidx2 = good_edge_points++;
865 #ifdef HAVE_VISUAL_DEBUGGING
866 if (good_edge_points > 0) {
867 good_hull_edges[geidx1][3] = 1.0;
868 good_hull_edges[geidx2][3] = 1.0;
870 good_hull_edges.resize(good_edge_points);
873 TIMETRACK_END(ttc_find_edge_);
875 model_cloud_hull_.reset(
new Cloud());
876 if (cfg_table_model_enable_ && (pidx1 != std::numeric_limits<size_t>::max())
877 && (pidx2 != std::numeric_limits<size_t>::max())) {
878 TIMETRACK_START(ttc_transform_);
882 PointType &p1p = cloud_hull_->points[pidx1];
883 PointType &p2p = cloud_hull_->points[pidx2];
885 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
886 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
889 Eigen::Vector3f model_normal = Eigen::Vector3f::UnitZ();
890 Eigen::Vector3f normal(coeff->values[0], coeff->values[1], coeff->values[2]);
893 Eigen::Vector3f table_centroid_3f =
894 Eigen::Vector3f(table_centroid[0], table_centroid[1], table_centroid[2]);
897 Eigen::Vector3f p1_p2 = p2 - p1;
898 Eigen::Vector3f p1_p2_center = (p2 + p1) * 0.5;
900 Eigen::Vector3f p1_p2_normal_cross = p1_p2.cross(normal);
901 p1_p2_normal_cross.normalize();
905 double nD = -p1_p2_normal_cross.dot(p1_p2_center);
906 double p1_p2_centroid_dist = p1_p2_normal_cross.dot(table_centroid_3f) + nD;
907 if (p1_p2_centroid_dist < 0) {
909 p1_p2_normal_cross *= -1;
912 Eigen::Vector3f table_center =
913 p1_p2_center + p1_p2_normal_cross * (cfg_table_model_width_ * 0.5);
915 for (
unsigned int i = 0; i < 3; ++i)
916 table_centroid[i] = table_center[i];
917 table_centroid[3] = 0.;
920 std::vector<Eigen::Vector3f> tpoints(4);
921 tpoints[0] = p1_p2_center + p1_p2 * (cfg_table_model_length_ * 0.5);
922 tpoints[1] = tpoints[0] + p1_p2_normal_cross * cfg_table_model_width_;
923 tpoints[3] = p1_p2_center - p1_p2 * (cfg_table_model_length_ * 0.5);
924 tpoints[2] = tpoints[3] + p1_p2_normal_cross * cfg_table_model_width_;
926 model_cloud_hull_->points.resize(4);
927 model_cloud_hull_->height = 1;
928 model_cloud_hull_->width = 4;
929 model_cloud_hull_->is_dense =
true;
930 for (
int i = 0; i < 4; ++i) {
931 model_cloud_hull_->points[i].x = tpoints[i][0];
932 model_cloud_hull_->points[i].y = tpoints[i][1];
933 model_cloud_hull_->points[i].z = tpoints[i][2];
940 Eigen::Vector3f rotaxis = model_normal.cross(normal);
942 double angle = acos(normal.dot(model_normal));
945 Eigen::Affine3f affine =
946 Eigen::Translation3f(table_centroid.x(), table_centroid.y(), table_centroid.z())
947 * Eigen::AngleAxisf(angle, rotaxis);
949 Eigen::Vector3f model_p1(-cfg_table_model_width_ * 0.5, cfg_table_model_length_ * 0.5, 0.),
950 model_p2(-cfg_table_model_width_ * 0.5, -cfg_table_model_length_ * 0.5, 0.);
951 model_p1 = affine * model_p1;
952 model_p2 = affine * model_p2;
955 Eigen::Vector3f model_p1_p2 = model_p2 - model_p1;
956 model_p1_p2.normalize();
958 Eigen::Vector3f model_rotaxis = model_p1_p2.cross(p1_p2);
959 model_rotaxis.normalize();
960 double angle_p1_p2 = acos(model_p1_p2.dot(p1_p2));
966 affine = Eigen::Translation3f(table_centroid.x(), table_centroid.y(), table_centroid.z())
967 * Eigen::AngleAxisf(angle_p1_p2, model_rotaxis) * Eigen::AngleAxisf(angle, rotaxis);
970 Eigen::Quaternionf qt;
971 qt = Eigen::AngleAxisf(angle_p1_p2, model_rotaxis) * Eigen::AngleAxisf(angle, rotaxis);
974 set_position(table_pos_if_,
true, table_centroid, qt);
976 TIMETRACK_INTER(ttc_transform_, ttc_transform_model_)
979 CloudPtr table_model = generate_table_model(cfg_table_model_length_,
980 cfg_table_model_width_,
981 cfg_table_model_step_);
982 pcl::transformPointCloud(*table_model, *table_model_, affine.matrix());
985 table_model_->header.frame_id = input_->header.frame_id;
987 TIMETRACK_END(ttc_transform_model_);
991 set_position(table_pos_if_,
false);
994 TIMETRACK_ABORT(ttc_find_edge_);
997 TIMETRACK_START(ttc_extract_non_plane_);
999 cloud_filt_.reset(
new Cloud());
1000 extract_.setNegative(
true);
1001 extract_.filter(*cloud_filt_);
1003 TIMETRACK_INTER(ttc_extract_non_plane_, ttc_polygon_filter_);
1011 bool viewpoint_above =
true;
1017 viewpoint_above = (baserel_viewpoint.z() > table_centroid[2]);
1019 logger->
log_warn(
name(),
"[L %u] could not transform viewpoint to base link", loop_count_);
1034 pcl::ComparisonOps::CompareOp op =
1035 viewpoint_above ? (coeff->values[3] > 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT)
1036 : (coeff->values[3] < 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT);
1039 pcl::ConditionAnd<PointType>::Ptr above_cond(
new pcl::ConditionAnd<PointType>());
1040 above_cond->addComparison(above_comp);
1041 pcl::ConditionalRemoval<PointType> above_condrem;
1042 above_condrem.setCondition(above_cond);
1043 above_condrem.setInputCloud(cloud_filt_);
1044 cloud_above_.reset(
new Cloud());
1045 above_condrem.filter(*cloud_above_);
1049 if (cloud_filt_->points.size() < cfg_cluster_min_size_) {
1051 TIMETRACK_ABORT(ttc_polygon_filter_);
1052 TIMETRACK_ABORT(ttc_full_loop_);
1057 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
1059 pcl::ConditionAnd<PointType>::Ptr polygon_cond(
new pcl::ConditionAnd<PointType>());
1063 (model_cloud_hull_ && !model_cloud_hull_->points.empty()) ? *model_cloud_hull_
1065 polygon_cond->addComparison(inpoly_comp);
1068 pcl::ConditionalRemoval<PointType> condrem;
1069 condrem.setCondition(polygon_cond);
1070 condrem.setInputCloud(cloud_above_);
1072 cloud_objs_.reset(
new Cloud());
1073 condrem.filter(*cloud_objs_);
1082 TIMETRACK_INTER(ttc_polygon_filter_, ttc_table_to_output_);
1084 std::vector<int> indices(cloud_proj_->points.size());
1085 for (uint i = 0; i < indices.size(); i++)
1087 ColorCloudPtr tmp_clusters = colorize_cluster(cloud_proj_, indices, table_color);
1088 tmp_clusters->height = 1;
1089 tmp_clusters->is_dense =
false;
1090 tmp_clusters->width = cloud_proj_->points.size();
1092 TIMETRACK_INTER(ttc_table_to_output_, ttc_cluster_objects_);
1094 unsigned int object_count = 0;
1095 if (cloud_objs_->points.size() > 0) {
1098 pcl::StatisticalOutlierRemoval<PointType> sor;
1099 sor.setInputCloud(cloud_objs_);
1101 sor.setStddevMulThresh(0.2);
1102 sor.filter(*cloud_objs_);
1105 std::vector<pcl::PointCloud<ColorPointType>::Ptr> tmp_obj_clusters(MAX_CENTROIDS);
1106 object_count = cluster_objects(cloud_objs_, tmp_clusters, tmp_obj_clusters);
1107 if (object_count == 0) {
1111 TIMETRACK_INTER(ttc_hungarian_, ttc_old_centroids_)
1114 for (OldCentroidVector::iterator it = old_centroids_.begin(); it != old_centroids_.end(); ++it) {
1115 it->increment_age();
1118 delete_old_centroids(old_centroids_, cfg_centroid_max_age_);
1120 delete_near_centroids(centroids_, old_centroids_, cfg_centroid_min_distance_);
1122 TIMETRACK_END(ttc_old_centroids_);
1125 for (
unsigned int i = 0; i < pos_ifs_.size(); i++) {
1126 if (!centroids_.count(i)) {
1127 set_position(pos_ifs_[i],
false);
1132 for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); ++it) {
1133 set_position(pos_ifs_[it->first],
true, it->second);
1136 TIMETRACK_INTER(ttc_cluster_objects_, ttc_visualization_)
1138 *clusters_ = *tmp_clusters;
1139 fclusters_->header.frame_id = input_->header.frame_id;
1140 pcl_utils::copy_time(input_, fclusters_);
1141 pcl_utils::copy_time(input_, ftable_model_);
1142 pcl_utils::copy_time(input_, fsimplified_polygon_);
1144 for (
unsigned int i = 0; i < f_obj_clusters_.size(); i++) {
1145 if (centroids_.count(i)) {
1146 *obj_clusters_[i] = *tmp_obj_clusters[i];
1148 obj_clusters_[i]->clear();
1153 pcl_utils::copy_time(input_, f_obj_clusters_[i]);
1156 #ifdef HAVE_VISUAL_DEBUGGING
1158 Eigen::Vector4f normal;
1159 normal[0] = coeff->values[0];
1160 normal[1] = coeff->values[1];
1161 normal[2] = coeff->values[2];
1165 hull_vertices.resize(cloud_hull_->points.size());
1166 for (
unsigned int i = 0; i < cloud_hull_->points.size(); ++i) {
1167 hull_vertices[i][0] = cloud_hull_->points[i].x;
1168 hull_vertices[i][1] = cloud_hull_->points[i].y;
1169 hull_vertices[i][2] = cloud_hull_->points[i].z;
1170 hull_vertices[i][3] = 0.;
1174 if (model_cloud_hull_ && !model_cloud_hull_->points.empty()) {
1175 model_vertices.resize(model_cloud_hull_->points.size());
1176 for (
unsigned int i = 0; i < model_cloud_hull_->points.size(); ++i) {
1177 model_vertices[i][0] = model_cloud_hull_->points[i].x;
1178 model_vertices[i][1] = model_cloud_hull_->points[i].y;
1179 model_vertices[i][2] = model_cloud_hull_->points[i].z;
1180 model_vertices[i][3] = 0.;
1184 visthread_->visualize(input_->header.frame_id,
1192 obj_shape_confidence_,
1197 TIMETRACK_END(ttc_visualization_);
1198 TIMETRACK_END(ttc_full_loop_);
1200 #ifdef USE_TIMETRACKER
1201 if (++tt_loopcount_ >= 5) {
1203 tt_->print_to_stdout();
1208 std::vector<pcl::PointIndices>
1209 TabletopObjectsThread::extract_object_clusters(CloudConstPtr input)
1211 TIMETRACK_START(ttc_obj_extraction_);
1212 std::vector<pcl::PointIndices> cluster_indices;
1213 if (input->empty()) {
1214 TIMETRACK_ABORT(ttc_obj_extraction_);
1215 return cluster_indices;
1219 pcl::search::KdTree<PointType>::Ptr kdtree_cl(
new pcl::search::KdTree<PointType>());
1220 kdtree_cl->setInputCloud(input);
1222 pcl::EuclideanClusterExtraction<PointType> ec;
1223 ec.setClusterTolerance(cfg_cluster_tolerance_);
1224 ec.setMinClusterSize(cfg_cluster_min_size_);
1225 ec.setMaxClusterSize(cfg_cluster_max_size_);
1226 ec.setSearchMethod(kdtree_cl);
1227 ec.setInputCloud(input);
1228 ec.extract(cluster_indices);
1231 TIMETRACK_END(ttc_obj_extraction_);
1232 return cluster_indices;
1236 TabletopObjectsThread::next_id()
1238 if (free_ids_.empty()) {
1242 int id = free_ids_.front();
1243 free_ids_.pop_front();
1248 TabletopObjectsThread::cluster_objects(CloudConstPtr input_cloud,
1249 ColorCloudPtr tmp_clusters,
1250 std::vector<ColorCloudPtr> &tmp_obj_clusters)
1252 unsigned int object_count = 0;
1253 std::vector<pcl::PointIndices> cluster_indices = extract_object_clusters(input_cloud);
1254 std::vector<pcl::PointIndices>::const_iterator it;
1255 unsigned int num_points = 0;
1256 for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
1257 num_points += it->indices.size();
1259 CentroidMap tmp_centroids;
1261 if (num_points > 0) {
1262 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids(
1265 unsigned int centroid_i = 0;
1267 std::vector<double> init_likelihoods;
1268 init_likelihoods.resize(NUM_KNOWN_OBJS_ + 1, 0.0);
1269 for (uint i = 0; i < MAX_CENTROIDS; i++)
1270 obj_likelihoods_[i] = init_likelihoods;
1272 for (it = cluster_indices.begin(); it != cluster_indices.end() && centroid_i < MAX_CENTROIDS;
1273 ++it, ++centroid_i) {
1275 "********************Processing obj_%u********************",
1282 ColorCloudPtr single_cluster =
1283 colorize_cluster(input_cloud, it->indices, cluster_colors[centroid_i]);
1284 single_cluster->header.frame_id = input_cloud->header.frame_id;
1285 single_cluster->width = it->indices.size();
1286 single_cluster->height = 1;
1288 ColorCloudPtr obj_in_base_frame(
new ColorCloud());
1289 obj_in_base_frame->header.frame_id = cfg_base_frame_;
1290 obj_in_base_frame->width = it->indices.size();
1291 obj_in_base_frame->height = 1;
1292 obj_in_base_frame->points.resize(it->indices.size());
1297 pcl_utils::transform_pointcloud(cfg_base_frame_,
1302 pcl::compute3DCentroid(*obj_in_base_frame, new_centroids[centroid_i]);
1304 if (cfg_cylinder_fitting_) {
1305 new_centroids[centroid_i] =
1306 fit_cylinder(obj_in_base_frame, new_centroids[centroid_i], centroid_i);
1309 object_count = centroid_i;
1310 new_centroids.resize(object_count);
1314 CentroidMap cylinder_params(cylinder_params_);
1315 std::map<uint, signed int> best_obj_guess(best_obj_guess_);
1316 std::map<uint, double> obj_shape_confidence(obj_shape_confidence_);
1317 std::map<uint, std::vector<double>> obj_likelihoods(obj_likelihoods_);
1318 cylinder_params_.clear();
1319 best_obj_guess_.clear();
1320 obj_shape_confidence_.clear();
1321 obj_likelihoods_.clear();
1323 std::map<uint, int> assigned_ids;
1324 if (cfg_track_objects_) {
1325 assigned_ids = track_objects(new_centroids);
1327 for (
unsigned int i = 0; i < new_centroids.size(); i++) {
1328 assigned_ids[i] = i;
1333 for (uint i = 0; i < new_centroids.size(); i++) {
1336 assigned_id = assigned_ids.at(i);
1337 }
catch (
const std::out_of_range &e) {
1342 if (assigned_id == -1)
1344 tmp_centroids[assigned_id] = new_centroids[i];
1345 cylinder_params_[assigned_id] = cylinder_params[i];
1346 obj_shape_confidence_[assigned_id] = obj_shape_confidence[i];
1347 best_obj_guess_[assigned_id] = best_obj_guess[i];
1348 obj_likelihoods_[assigned_id] = obj_likelihoods[i];
1349 ColorCloudPtr colorized_cluster =
1350 colorize_cluster(input_cloud,
1351 cluster_indices[i].indices,
1352 cluster_colors[assigned_id % MAX_CENTROIDS]);
1353 *tmp_clusters += *colorized_cluster;
1354 tmp_obj_clusters[assigned_id] = colorized_cluster;
1358 remove_high_centroids(table_centroid, tmp_centroids);
1360 if (object_count > 0)
1365 for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); ++it) {
1366 old_centroids_.push_back(
OldCentroid(it->first, it->second));
1369 centroids_ = tmp_centroids;
1370 return object_count;
1373 TabletopObjectsThread::ColorCloudPtr
1374 TabletopObjectsThread::colorize_cluster(CloudConstPtr input_cloud,
1375 const std::vector<int> &cluster,
1376 const uint8_t color[])
1378 ColorCloudPtr result(
new ColorCloud());
1379 result->resize(cluster.size());
1380 result->header.frame_id = input_cloud->header.frame_id;
1382 for (std::vector<int>::const_iterator it = cluster.begin(); it != cluster.end(); ++it, ++i) {
1383 ColorPointType & p1 = result->points.at(i);
1384 const PointType &p2 = input_cloud->points.at(*it);
1396 TabletopObjectsThread::compute_bounding_box_scores(
1397 Eigen::Vector3f & cluster_dim,
1398 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> &scores)
1400 scores.resize(NUM_KNOWN_OBJS_);
1402 for (
int i = 0; i < NUM_KNOWN_OBJS_; i++) {
1403 scores[i][0] = compute_similarity(cluster_dim[0], known_obj_dimensions_[i][0]);
1404 scores[i][1] = compute_similarity(cluster_dim[1], known_obj_dimensions_[i][1]);
1405 scores[i][2] = compute_similarity(cluster_dim[2], known_obj_dimensions_[i][2]);
1411 TabletopObjectsThread::compute_similarity(
double d1,
double d2)
1413 return exp(-50.0 * ((d1 - d2) * (d1 - d2)));
1419 const Eigen::Vector4f & centroid,
1420 const Eigen::Quaternionf & attitude)
1425 tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()),
1426 tf::Vector3(centroid[0], centroid[1], centroid[2])),
1428 input_->header.frame_id);
1430 iface->
set_frame(cfg_result_frame_.c_str());
1437 if (visibility_history >= 0) {
1442 tf::Vector3 & origin = baserel_pose.getOrigin();
1443 tf::Quaternion quat = baserel_pose.getRotation();
1444 double translation[3] = {origin.x(), origin.y(), origin.z()};
1445 double rotation[4] = {quat.x(), quat.y(), quat.z(), quat.w()};
1450 if (visibility_history <= 0) {
1454 double translation[3] = {0, 0, 0};
1455 double rotation[4] = {0, 0, 0, 1};
1463 TabletopObjectsThread::CloudPtr
1464 TabletopObjectsThread::generate_table_model(
const float length,
1466 const float thickness,
1468 const float max_error)
1470 CloudPtr c(
new Cloud());
1472 const float length_2 = fabs(length) * 0.5;
1473 const float width_2 = fabs(width) * 0.5;
1474 const float thickness_2 = fabs(thickness) * 0.5;
1477 const unsigned int l_base_num = std::max(2u, (
unsigned int)floor(length / step));
1478 const unsigned int num_w =
1480 + ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1481 const unsigned int w_base_num = std::max(2u, (
unsigned int)floor(width / step));
1482 const unsigned int num_h =
1484 + ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1485 const unsigned int t_base_num = std::max(2u, (
unsigned int)floor(thickness / step));
1486 const unsigned int num_t =
1488 + ((thickness < t_base_num * step) ? 0 : ((thickness - t_base_num * step) > max_error ? 2 : 1));
1495 c->width = num_t * num_w * num_h;
1497 c->points.resize((
size_t)num_t * (
size_t)num_w * (
size_t)num_h);
1499 unsigned int idx = 0;
1500 for (
unsigned int t = 0; t < num_t; ++t) {
1501 for (
unsigned int w = 0; w < num_w; ++w) {
1502 for (
unsigned int h = 0; h < num_h; ++h) {
1503 PointType &p = c->points[idx++];
1505 p.x = h * step - width_2;
1506 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error)
1509 p.y = w * step - length_2;
1510 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error)
1513 p.z = t * step - thickness_2;
1514 if ((t == num_t - 1) && fabs(p.z - thickness_2) > max_error)
1523 TabletopObjectsThread::CloudPtr
1524 TabletopObjectsThread::generate_table_model(
const float length,
1527 const float max_error)
1529 CloudPtr c(
new Cloud());
1531 const float length_2 = fabs(length) * 0.5;
1532 const float width_2 = fabs(width) * 0.5;
1535 const unsigned int l_base_num = std::max(2u, (
unsigned int)floor(length / step));
1536 const unsigned int num_w =
1538 + ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1539 const unsigned int w_base_num = std::max(2u, (
unsigned int)floor(width / step));
1540 const unsigned int num_h =
1542 + ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1548 c->width = num_w * num_h;
1550 c->points.resize((
size_t)num_w * (
size_t)num_h);
1552 unsigned int idx = 0;
1553 for (
unsigned int w = 0; w < num_w; ++w) {
1554 for (
unsigned int h = 0; h < num_h; ++h) {
1555 PointType &p = c->points[idx++];
1557 p.x = h * step - width_2;
1558 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error)
1561 p.y = w * step - length_2;
1562 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error)
1572 TabletopObjectsThread::CloudPtr
1573 TabletopObjectsThread::simplify_polygon(CloudPtr polygon,
float dist_threshold)
1575 const float sqr_dist_threshold = dist_threshold * dist_threshold;
1576 CloudPtr result(
new Cloud());
1577 const size_t psize = polygon->points.size();
1578 result->points.resize(psize);
1579 size_t res_points = 0;
1581 for (
size_t i = 1; i <= psize; ++i) {
1582 PointType &p1p = polygon->points[i - i_dist];
1585 if (result->points.empty()) {
1591 PointType &p2p = polygon->points[i % psize];
1592 PointType &p3p = (i == psize) ? result->points[0] : polygon->points[(i + 1) % psize];
1594 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0);
1595 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0);
1596 Eigen::Vector4f p3(p3p.x, p3p.y, p3p.z, 0);
1598 Eigen::Vector4f line_dir = p3 - p1;
1600 double sqr_dist = pcl::sqrPointToLineDistance(p2, p1, line_dir);
1601 if (sqr_dist < sqr_dist_threshold) {
1605 result->points[res_points++] = p2p;
1609 result->header.frame_id = polygon->header.frame_id;
1610 result->header.stamp = polygon->header.stamp;
1611 result->width = res_points;
1613 result->is_dense =
false;
1614 result->points.resize(res_points);
1620 TabletopObjectsThread::convert_colored_input()
1622 converted_input_->header.seq = colored_input_->header.seq;
1623 converted_input_->header.frame_id = colored_input_->header.frame_id;
1624 converted_input_->header.stamp = colored_input_->header.stamp;
1625 converted_input_->width = colored_input_->width;
1626 converted_input_->height = colored_input_->height;
1627 converted_input_->is_dense = colored_input_->is_dense;
1629 const size_t size = colored_input_->points.size();
1630 converted_input_->points.resize(size);
1631 for (
size_t i = 0; i < size; ++i) {
1632 const ColorPointType &in = colored_input_->points[i];
1633 PointType & out = converted_input_->points[i];
1642 TabletopObjectsThread::delete_old_centroids(OldCentroidVector centroids,
unsigned int age)
1644 centroids.erase(std::remove_if(centroids.begin(),
1647 if (centroid.get_age() > age) {
1648 free_ids_.push_back(centroid.get_id());
1657 TabletopObjectsThread::delete_near_centroids(CentroidMap reference,
1658 OldCentroidVector centroids,
1661 centroids.erase(std::remove_if(centroids.begin(),
1664 for (CentroidMap::const_iterator it = reference.begin();
1665 it != reference.end();
1667 if (pcl::distances::l2(it->second, old.get_centroid())
1669 free_ids_.push_back(old.get_id());
1679 TabletopObjectsThread::remove_high_centroids(Eigen::Vector4f table_centroid, CentroidMap centroids)
1686 input_->header.frame_id);
1689 for (CentroidMap::iterator it = centroids.begin(); it != centroids.end();) {
1696 float d = sp_baserel_centroid.z() - sp_baserel_table.z();
1697 if (d > cfg_centroid_max_height_) {
1699 free_ids_.push_back(it->first);
1700 centroids.erase(it++);
1714 TabletopObjectsThread::fit_cylinder(ColorCloudConstPtr obj_in_base_frame,
1715 Eigen::Vector4f
const ¢roid,
1716 uint
const & centroid_i)
1718 Eigen::Vector4f new_centroid(centroid);
1719 ColorPointType pnt_min, pnt_max;
1720 Eigen::Vector3f obj_dim;
1721 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> obj_size_scores;
1722 pcl::getMinMax3D(*obj_in_base_frame, pnt_min, pnt_max);
1723 obj_dim[0] = fabs(pnt_max.x - pnt_min.x);
1724 obj_dim[1] = fabs(pnt_max.y - pnt_min.y);
1725 obj_dim[2] = fabs(pnt_max.z - pnt_min.z);
1726 compute_bounding_box_scores(obj_dim, obj_size_scores);
1729 name(),
"Computed object dimensions: %f %f %f", obj_dim[0], obj_dim[1], obj_dim[2]);
1731 for (
int os = 0; os < NUM_KNOWN_OBJS_; os++) {
1733 "** Cup %i: %f in x, %f in y, %f in z.",
1735 obj_size_scores[os][0],
1736 obj_size_scores[os][1],
1737 obj_size_scores[os][2]);
1738 obj_likelihoods_[centroid_i][os] =
1739 (double)obj_size_scores[os][0] * obj_size_scores[os][1] * obj_size_scores[os][2];
1743 pcl::NormalEstimation<ColorPointType, pcl::Normal> ne;
1744 pcl::SACSegmentationFromNormals<ColorPointType, pcl::Normal> seg;
1745 pcl::ExtractIndices<ColorPointType> extract;
1746 pcl::ExtractIndices<pcl::Normal> extract_normals;
1748 pcl::search::KdTree<ColorPointType>::Ptr tree_cyl(
new pcl::search::KdTree<ColorPointType>());
1749 pcl::ModelCoefficients::Ptr coefficients_cylinder(
new pcl::ModelCoefficients);
1750 pcl::PointIndices::Ptr inliers_cylinder(
new pcl::PointIndices);
1753 ne.setSearchMethod(tree_cyl);
1754 ne.setInputCloud(obj_in_base_frame);
1756 ne.compute(*obj_normals);
1760 seg.setOptimizeCoefficients(
true);
1761 seg.setModelType(pcl::SACMODEL_CYLINDER);
1762 seg.setMethodType(pcl::SAC_RANSAC);
1763 seg.setNormalDistanceWeight(0.1);
1764 seg.setMaxIterations(1000);
1765 seg.setDistanceThreshold(0.07);
1766 seg.setRadiusLimits(0, 0.12);
1768 seg.setInputCloud(obj_in_base_frame);
1769 seg.setInputNormals(obj_normals);
1773 seg.segment(*inliers_cylinder, *coefficients_cylinder);
1776 extract.setInputCloud(obj_in_base_frame);
1777 extract.setIndices(inliers_cylinder);
1778 extract.setNegative(
false);
1781 extract.filter(*cloud_cylinder_baserel);
1783 cylinder_params_[centroid_i][0] = 0;
1784 cylinder_params_[centroid_i][1] = 0;
1785 if (cloud_cylinder_baserel->points.empty()) {
1787 obj_shape_confidence_[centroid_i] = 0.0;
1793 obj_shape_confidence_[centroid_i] =
1794 (double)(cloud_cylinder_baserel->points.size()) / (obj_in_base_frame->points.size() * 1.0);
1796 "Cylinder fit confidence = %zu/%zu = %f",
1797 cloud_cylinder_baserel->points.size(),
1798 obj_in_base_frame->points.size(),
1799 obj_shape_confidence_[centroid_i]);
1801 ColorPointType pnt_min;
1802 ColorPointType pnt_max;
1803 pcl::getMinMax3D(*cloud_cylinder_baserel, pnt_min, pnt_max);
1804 if (cfg_verbose_cylinder_fitting_) {
1806 "Cylinder height according to cylinder inliers: %f",
1807 pnt_max.z - pnt_min.z);
1810 "Cylinder radius according to cylinder fitting: %f",
1811 (*coefficients_cylinder).values[6]);
1812 logger->
log_debug(
name(),
"Cylinder radius according to bounding box y: %f", obj_dim[1] / 2);
1816 cylinder_params_[centroid_i][0] = obj_dim[1] / 2;
1819 cylinder_params_[centroid_i][1] = obj_dim[2];
1824 new_centroid[0] = pnt_min.x + 0.5 * (pnt_max.x - pnt_min.x);
1825 new_centroid[1] = pnt_min.y + 0.5 * (pnt_max.y - pnt_min.y);
1826 new_centroid[2] = pnt_min.z + 0.5 * (pnt_max.z - pnt_min.z);
1829 signed int detected_obj_id = -1;
1830 double best_confidence = 0.0;
1831 if (cfg_verbose_cylinder_fitting_) {
1834 for (
int os = 0; os < NUM_KNOWN_OBJS_; os++) {
1835 if (cfg_verbose_cylinder_fitting_) {
1838 obj_likelihoods_[centroid_i][os] =
1839 (0.6 * obj_likelihoods_[centroid_i][os]) + (0.4 * obj_shape_confidence_[centroid_i]);
1842 if (obj_likelihoods_[centroid_i][os] > best_confidence) {
1843 best_confidence = obj_likelihoods_[centroid_i][os];
1844 detected_obj_id = os;
1847 if (cfg_verbose_cylinder_fitting_) {
1850 if (best_confidence > 0.6) {
1851 best_obj_guess_[centroid_i] = detected_obj_id;
1853 if (cfg_verbose_cylinder_fitting_) {
1855 "MATCH FOUND!! -------------------------> Cup number %i",
1859 best_obj_guess_[centroid_i] = -1;
1860 if (cfg_verbose_cylinder_fitting_) {
1864 if (cfg_verbose_cylinder_fitting_) {
1868 return new_centroid;
1879 std::map<unsigned int, int>
1880 TabletopObjectsThread::track_objects(
1881 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids)
1883 std::map<uint, int> final_assignment;
1886 for (
unsigned int i = 0; i < new_centroids.size(); i++) {
1887 final_assignment[i] = next_id();
1889 return final_assignment;
1891 TIMETRACK_START(ttc_hungarian_);
1892 hungarian_problem_t hp;
1894 std::vector<unsigned int> obj_ids(centroids_.size());
1898 hp.num_rows = new_centroids.size();
1899 hp.num_cols = centroids_.size();
1900 hp.cost = (
int **)calloc(hp.num_rows,
sizeof(
int *));
1901 for (
int i = 0; i < hp.num_rows; i++)
1902 hp.cost[i] = (
int *)calloc(hp.num_cols,
sizeof(
int));
1903 for (
int row = 0; row < hp.num_rows; row++) {
1904 unsigned int col = 0;
1905 for (CentroidMap::iterator col_it = centroids_.begin(); col_it != centroids_.end();
1907 double distance = pcl::distances::l2(new_centroids[row], col_it->second);
1908 hp.cost[row][col] = (int)(distance * 1000);
1909 obj_ids[col] = col_it->first;
1913 solver.
init(hp.cost, hp.num_rows, hp.num_cols, HUNGARIAN_MODE_MINIMIZE_COST);
1916 int assignment_size;
1919 for (
int row = 0; row < assignment_size; row++) {
1920 if (row >= hp.num_rows) {
1921 id = obj_ids.at(assignment[row]);
1922 old_centroids_.push_back(
OldCentroid(
id, centroids_.at(
id)));
1924 }
else if (assignment[row] >= hp.num_cols) {
1925 bool assigned =
false;
1927 for (OldCentroidVector::iterator it = old_centroids_.begin(); it != old_centroids_.end();
1929 if (pcl::distances::l2(new_centroids[row], it->get_centroid())
1930 <= cfg_centroid_max_distance_) {
1932 old_centroids_.erase(it);
1942 id = obj_ids[assignment[row]];
1946 if (pcl::distances::l2(centroids_[
id], new_centroids[row]) > cfg_centroid_max_distance_) {
1948 old_centroids_.push_back(
OldCentroid(
id, centroids_[
id]));
1952 final_assignment[row] = id;
1954 return final_assignment;
1958 #ifdef HAVE_VISUAL_DEBUGGING
1962 visthread_ = visthread;