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/common.h>
41 #include <pcl/common/distances.h>
42 #include <pcl/common/transforms.h>
43 #include <pcl/features/normal_3d.h>
44 #include <pcl/filters/conditional_removal.h>
45 #include <pcl/filters/extract_indices.h>
46 #include <pcl/filters/passthrough.h>
47 #include <pcl/filters/project_inliers.h>
48 #include <pcl/filters/statistical_outlier_removal.h>
49 #include <pcl/kdtree/kdtree.h>
50 #include <pcl/kdtree/kdtree_flann.h>
51 #include <pcl/point_cloud.h>
52 #include <pcl/point_types.h>
53 #include <pcl/registration/distances.h>
54 #include <pcl/sample_consensus/method_types.h>
55 #include <pcl/sample_consensus/model_types.h>
56 #include <pcl/segmentation/extract_clusters.h>
57 #include <pcl/surface/convex_hull.h>
58 #include <utils/hungarian_method/hungarian.h>
59 #include <utils/time/tracker_macros.h>
65 #define CFG_PREFIX "/perception/tabletop-objects/"
76 :
Thread(
"TabletopObjectsThread",
Thread::OPMODE_CONTINUOUS),
79 #ifdef HAVE_VISUAL_DEBUGGING
92 cfg_depth_filter_min_x_ =
config->
get_float(CFG_PREFIX
"depth_filter_min_x");
93 cfg_depth_filter_max_x_ =
config->
get_float(CFG_PREFIX
"depth_filter_max_x");
95 cfg_segm_max_iterations_ =
config->
get_uint(CFG_PREFIX
"table_segmentation_max_iterations");
96 cfg_segm_distance_threshold_ =
98 cfg_segm_inlier_quota_ =
config->
get_float(CFG_PREFIX
"table_segmentation_inlier_quota");
99 cfg_table_min_cluster_quota_ =
config->
get_float(CFG_PREFIX
"table_min_cluster_quota");
100 cfg_table_downsample_leaf_size_ =
config->
get_float(CFG_PREFIX
"table_downsample_leaf_size");
101 cfg_table_cluster_tolerance_ =
config->
get_float(CFG_PREFIX
"table_cluster_tolerance");
102 cfg_max_z_angle_deviation_ =
config->
get_float(CFG_PREFIX
"max_z_angle_deviation");
103 cfg_table_min_height_ =
config->
get_float(CFG_PREFIX
"table_min_height");
104 cfg_table_max_height_ =
config->
get_float(CFG_PREFIX
"table_max_height");
105 cfg_table_model_enable_ =
config->
get_bool(CFG_PREFIX
"table_model_enable");
106 cfg_table_model_length_ =
config->
get_float(CFG_PREFIX
"table_model_length");
107 cfg_table_model_width_ =
config->
get_float(CFG_PREFIX
"table_model_width");
108 cfg_table_model_step_ =
config->
get_float(CFG_PREFIX
"table_model_step");
111 cfg_cluster_tolerance_ =
config->
get_float(CFG_PREFIX
"cluster_tolerance");
112 cfg_cluster_min_size_ =
config->
get_uint(CFG_PREFIX
"cluster_min_size");
113 cfg_cluster_max_size_ =
config->
get_uint(CFG_PREFIX
"cluster_max_size");
117 cfg_centroid_max_age_ =
config->
get_uint(CFG_PREFIX
"centroid_max_age");
118 cfg_centroid_max_distance_ =
config->
get_float(CFG_PREFIX
"centroid_max_distance");
119 cfg_centroid_min_distance_ =
config->
get_float(CFG_PREFIX
"centroid_min_distance");
120 cfg_centroid_max_height_ =
config->
get_float(CFG_PREFIX
"centroid_max_height");
121 cfg_cylinder_fitting_ =
config->
get_bool(CFG_PREFIX
"enable_cylinder_fitting");
122 cfg_track_objects_ =
config->
get_bool(CFG_PREFIX
"enable_object_tracking");
124 cfg_verbose_cylinder_fitting_ =
config->
get_bool(CFG_PREFIX
"verbose_cylinder_fitting");
126 cfg_verbose_cylinder_fitting_ =
false;
131 input_ = pcl_utils::cloudptr_from_refptr(finput_);
135 colored_input_ = pcl_utils::cloudptr_from_refptr(fcoloredinput_);
136 converted_input_.reset(
new Cloud());
137 input_ = converted_input_;
138 converted_input_->header.frame_id = colored_input_->header.frame_id;
139 converted_input_->header.stamp = colored_input_->header.stamp;
141 throw Exception(
"Point cloud '%s' does not exist or not XYZ or XYZ/RGB PCL",
142 cfg_input_pointcloud_.c_str());
146 double rotation[4] = {0., 0., 0., 1.};
147 table_pos_if_ = NULL;
152 table_pos_if_->
write();
159 pos_ifs_.resize(MAX_CENTROIDS, NULL);
160 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
162 if (asprintf(&tmp,
"Tabletop Object %u", i + 1) != -1) {
164 std::string
id = tmp;
175 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
184 fclusters_->header.frame_id = input_->header.frame_id;
185 fclusters_->is_dense =
false;
187 clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_);
192 for (
int i = 0; i < MAX_CENTROIDS; i++) {
194 f_tmp_cloud->header.frame_id = input_->header.frame_id;
195 f_tmp_cloud->is_dense =
false;
197 if (asprintf(&tmp_name,
"obj_cluster_%u", i) != -1) {
202 f_obj_clusters_.push_back(f_tmp_cloud);
203 tmp_cloud = pcl_utils::cloudptr_from_refptr(f_tmp_cloud);
204 obj_clusters_.push_back(tmp_cloud);
211 std::vector<double> init_likelihoods;
212 init_likelihoods.resize(NUM_KNOWN_OBJS_ + 1, 0.0);
217 known_obj_dimensions_.resize(NUM_KNOWN_OBJS_);
220 known_obj_dimensions_[0][0] = 0.07;
221 known_obj_dimensions_[0][1] = 0.07;
222 known_obj_dimensions_[0][2] = 0.104;
224 known_obj_dimensions_[1][0] = 0.088;
225 known_obj_dimensions_[1][1] = 0.088;
226 known_obj_dimensions_[1][2] = 0.155;
228 known_obj_dimensions_[2][0] = 0.106;
229 known_obj_dimensions_[2][1] = 0.106;
230 known_obj_dimensions_[2][2] = 0.277;
234 table_inclination_ = 0.0;
236 ftable_model_ =
new Cloud();
237 table_model_ = pcl_utils::cloudptr_from_refptr(ftable_model_);
238 table_model_->header.frame_id = input_->header.frame_id;
242 fsimplified_polygon_ =
new Cloud();
243 simplified_polygon_ = pcl_utils::cloudptr_from_refptr(fsimplified_polygon_);
244 simplified_polygon_->header.frame_id = input_->header.frame_id;
248 grid_.setFilterFieldName(
"x");
249 grid_.setFilterLimits(cfg_depth_filter_min_x_, cfg_depth_filter_max_x_);
250 grid_.setLeafSize(cfg_voxel_leaf_size_, cfg_voxel_leaf_size_, cfg_voxel_leaf_size_);
252 seg_.setOptimizeCoefficients(
true);
253 seg_.setModelType(pcl::SACMODEL_PLANE);
254 seg_.setMethodType(pcl::SAC_RANSAC);
255 seg_.setMaxIterations(cfg_segm_max_iterations_);
256 seg_.setDistanceThreshold(cfg_segm_distance_threshold_);
264 old_centroids_.clear();
265 for (
unsigned int i = 0; i < MAX_CENTROIDS; i++)
266 free_ids_.push_back(i);
268 #ifdef USE_TIMETRACKER
271 ttc_full_loop_ = tt_->add_class(
"Full Loop");
272 ttc_msgproc_ = tt_->add_class(
"Message Processing");
273 ttc_convert_ = tt_->add_class(
"Input Conversion");
274 ttc_voxelize_ = tt_->add_class(
"Downsampling");
275 ttc_plane_ = tt_->add_class(
"Plane Segmentation");
276 ttc_extract_plane_ = tt_->add_class(
"Plane Extraction");
277 ttc_plane_downsampling_ = tt_->add_class(
"Plane Downsampling");
278 ttc_cluster_plane_ = tt_->add_class(
"Plane Clustering");
279 ttc_convex_hull_ = tt_->add_class(
"Convex Hull");
280 ttc_simplify_polygon_ = tt_->add_class(
"Polygon Simplification");
281 ttc_find_edge_ = tt_->add_class(
"Polygon Edge");
282 ttc_transform_ = tt_->add_class(
"Transform");
283 ttc_transform_model_ = tt_->add_class(
"Model Transformation");
284 ttc_extract_non_plane_ = tt_->add_class(
"Non-Plane Extraction");
285 ttc_polygon_filter_ = tt_->add_class(
"Polygon Filter");
286 ttc_table_to_output_ = tt_->add_class(
"Table to Output");
287 ttc_cluster_objects_ = tt_->add_class(
"Object Clustering");
288 ttc_visualization_ = tt_->add_class(
"Visualization");
289 ttc_hungarian_ = tt_->add_class(
"Hungarian Method (centroids)");
290 ttc_old_centroids_ = tt_->add_class(
"Old Centroid Removal");
291 ttc_obj_extraction_ = tt_->add_class(
"Object Extraction");
300 simplified_polygon_.reset();
308 for (PosIfsVector::iterator it = pos_ifs_.begin(); it != pos_ifs_.end(); ++it) {
315 ftable_model_.reset();
316 fsimplified_polygon_.reset();
318 delete last_pcl_time_;
319 #ifdef USE_TIMETRACKER
324 template <
typename Po
intType>
326 comparePoints2D(
const PointType &p1,
const PointType &p2)
328 double angle1 = atan2(p1.y, p1.x) + M_PI;
329 double angle2 = atan2(p2.y, p2.x) + M_PI;
330 return (angle1 > angle2);
337 TabletopObjectsThread::is_polygon_edge_better(PointType &cb_br_p1p,
338 PointType &cb_br_p2p,
343 Eigen::Vector3f cb_br_p1(cb_br_p1p.x, cb_br_p1p.y, cb_br_p1p.z);
344 Eigen::Vector3f cb_br_p2(cb_br_p2p.x, cb_br_p2p.y, cb_br_p2p.z);
345 Eigen::Vector3f cb_br_p1_p2_center = (cb_br_p1 + cb_br_p2) * 0.5;
347 Eigen::Vector3f br_p1(br_p1p.x, br_p1p.y, br_p1p.z);
348 Eigen::Vector3f br_p2(br_p2p.x, br_p2p.y, br_p2p.z);
349 Eigen::Vector3f br_p1_p2_center = (br_p2 + br_p1) * 0.5;
351 double dist_x = (cb_br_p1_p2_center[0] - br_p1_p2_center[0]);
357 || ((abs(dist_x) <= 0.25) && ((br_p2 - br_p1).norm() < (cb_br_p2 - cb_br_p1).norm())))
366 TIMETRACK_START(ttc_full_loop_);
370 TIMETRACK_START(ttc_msgproc_);
385 TimeWait::wait(250000);
386 TIMETRACK_ABORT(ttc_full_loop_);
390 TIMETRACK_END(ttc_msgproc_);
393 if (colored_input_) {
394 pcl_utils::get_time(colored_input_, pcl_time);
396 pcl_utils::get_time(input_, pcl_time);
398 if (*last_pcl_time_ == pcl_time) {
399 TimeWait::wait(20000);
400 TIMETRACK_ABORT(ttc_full_loop_);
403 *last_pcl_time_ = pcl_time;
405 if (colored_input_) {
406 TIMETRACK_START(ttc_convert_);
407 convert_colored_input();
408 TIMETRACK_END(ttc_convert_);
411 TIMETRACK_START(ttc_voxelize_);
413 CloudPtr temp_cloud(
new Cloud);
414 CloudPtr temp_cloud2(
new Cloud);
415 pcl::ExtractIndices<PointType> extract_;
416 CloudPtr cloud_hull_;
417 CloudPtr model_cloud_hull_;
418 CloudPtr cloud_proj_;
419 CloudPtr cloud_filt_;
420 CloudPtr cloud_above_;
421 CloudPtr cloud_objs_;
422 pcl::search::KdTree<PointType> kdtree_;
424 grid_.setInputCloud(input_);
425 grid_.filter(*temp_cloud);
427 if (temp_cloud->points.size() <= 10) {
432 TimeWait::wait(50000);
436 TIMETRACK_INTER(ttc_voxelize_, ttc_plane_)
438 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
439 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
440 Eigen::Vector4f baserel_table_centroid(0, 0, 0, 0);
449 bool happy_with_plane =
false;
450 while (!happy_with_plane) {
451 happy_with_plane =
true;
453 if (temp_cloud->points.size() <= 10) {
455 "[L %u] no more points for plane detection, skipping loop",
457 set_position(table_pos_if_,
false);
458 TIMETRACK_ABORT(ttc_plane_);
459 TIMETRACK_ABORT(ttc_full_loop_);
460 TimeWait::wait(50000);
464 seg_.setInputCloud(temp_cloud);
465 seg_.segment(*inliers, *coeff);
468 if ((
double)inliers->indices.size()
469 < (cfg_segm_inlier_quota_ * (
double)temp_cloud->points.size())) {
472 "[L %u] no table in scene, skipping loop (%zu inliers, required %f, voxelized size %zu)",
474 inliers->indices.size(),
475 (cfg_segm_inlier_quota_ * temp_cloud->points.size()),
476 temp_cloud->points.size());
477 set_position(table_pos_if_,
false);
478 TIMETRACK_ABORT(ttc_plane_);
479 TIMETRACK_ABORT(ttc_full_loop_);
480 TimeWait::wait(50000);
491 input_->header.frame_id);
495 tf::Vector3 z_axis(0, 0, copysign(1.0, baserel_normal.z()));
496 table_inclination_ = z_axis.angle(baserel_normal);
497 if (fabs(z_axis.angle(baserel_normal)) > cfg_max_z_angle_deviation_) {
498 happy_with_plane =
false;
500 "[L %u] table normal (%f,%f,%f) Z angle deviation |%f| > %f, excluding",
505 z_axis.angle(baserel_normal),
506 cfg_max_z_angle_deviation_);
511 happy_with_plane =
false;
514 if (happy_with_plane) {
520 pcl::compute3DCentroid(*temp_cloud, *inliers, table_centroid);
525 input_->header.frame_id);
528 baserel_table_centroid[0] = baserel_centroid.x();
529 baserel_table_centroid[1] = baserel_centroid.y();
530 baserel_table_centroid[2] = baserel_centroid.z();
532 if ((baserel_centroid.z() < cfg_table_min_height_)
533 || (baserel_centroid.z() > cfg_table_max_height_)) {
534 happy_with_plane =
false;
536 "[L %u] table height %f not in range [%f, %f]",
538 baserel_centroid.z(),
539 cfg_table_min_height_,
540 cfg_table_max_height_);
548 if (!happy_with_plane) {
551 extract_.setNegative(
true);
552 extract_.setInputCloud(temp_cloud);
553 extract_.setIndices(inliers);
554 extract_.filter(extracted);
555 *temp_cloud = extracted;
563 TIMETRACK_INTER(ttc_plane_, ttc_extract_plane_)
565 extract_.setNegative(
false);
566 extract_.setInputCloud(temp_cloud);
567 extract_.setIndices(inliers);
568 extract_.filter(*temp_cloud2);
571 pcl::ProjectInliers<PointType> proj;
572 proj.setModelType(pcl::SACMODEL_PLANE);
573 proj.setInputCloud(temp_cloud2);
574 proj.setModelCoefficients(coeff);
575 cloud_proj_.reset(
new Cloud());
576 proj.filter(*cloud_proj_);
578 TIMETRACK_INTER(ttc_extract_plane_, ttc_plane_downsampling_);
590 CloudPtr cloud_table_voxelized(
new Cloud());
591 pcl::VoxelGrid<PointType> table_grid;
592 table_grid.setLeafSize(cfg_table_downsample_leaf_size_,
593 cfg_table_downsample_leaf_size_,
594 cfg_table_downsample_leaf_size_);
595 table_grid.setInputCloud(cloud_proj_);
596 table_grid.filter(*cloud_table_voxelized);
598 TIMETRACK_INTER(ttc_plane_downsampling_, ttc_cluster_plane_);
601 pcl::search::KdTree<PointType>::Ptr kdtree_table(
new pcl::search::KdTree<PointType>());
602 kdtree_table->setInputCloud(cloud_table_voxelized);
604 std::vector<pcl::PointIndices> table_cluster_indices;
605 pcl::EuclideanClusterExtraction<PointType> table_ec;
606 table_ec.setClusterTolerance(cfg_table_cluster_tolerance_);
607 table_ec.setMinClusterSize(cfg_table_min_cluster_quota_ * cloud_table_voxelized->points.size());
608 table_ec.setMaxClusterSize(cloud_table_voxelized->points.size());
609 table_ec.setSearchMethod(kdtree_table);
610 table_ec.setInputCloud(cloud_table_voxelized);
611 table_ec.extract(table_cluster_indices);
613 if (!table_cluster_indices.empty()) {
615 CloudPtr cloud_table_extracted(
new Cloud());
616 pcl::PointIndices::ConstPtr table_cluster_indices_ptr(
617 new pcl::PointIndices(table_cluster_indices[0]));
618 pcl::ExtractIndices<PointType> table_cluster_extract;
619 table_cluster_extract.setNegative(
false);
620 table_cluster_extract.setInputCloud(cloud_table_voxelized);
621 table_cluster_extract.setIndices(table_cluster_indices_ptr);
622 table_cluster_extract.filter(*cloud_table_extracted);
623 *cloud_proj_ = *cloud_table_extracted;
626 pcl::compute3DCentroid(*cloud_proj_, table_centroid);
631 "[L %u] table plane clustering did not generate any clusters",
635 TIMETRACK_INTER(ttc_cluster_plane_, ttc_convex_hull_)
638 pcl::ConvexHull<PointType> hr;
639 #ifdef PCL_VERSION_COMPARE
640 # if PCL_VERSION_COMPARE(>=, 1, 5, 0)
646 hr.setInputCloud(cloud_proj_);
647 cloud_hull_.reset(
new Cloud());
648 hr.reconstruct(*cloud_hull_);
650 if (cloud_hull_->points.empty()) {
651 logger->
log_warn(
name(),
"[L %u] convex hull of table empty, skipping loop", loop_count_);
652 TIMETRACK_ABORT(ttc_convex_hull_);
653 TIMETRACK_ABORT(ttc_full_loop_);
654 set_position(table_pos_if_,
false);
658 TIMETRACK_INTER(ttc_convex_hull_, ttc_simplify_polygon_)
660 CloudPtr simplified_polygon = simplify_polygon(cloud_hull_, 0.02);
661 *simplified_polygon_ = *simplified_polygon;
664 *cloud_hull_ = *simplified_polygon;
666 TIMETRACK_INTER(ttc_simplify_polygon_, ttc_find_edge_)
668 #ifdef HAVE_VISUAL_DEBUGGING
670 good_hull_edges.resize(cloud_hull_->points.size() * 2);
680 tf::Quaternion q = t.getRotation();
681 Eigen::Affine3f affine_cloud =
682 Eigen::Translation3f(t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z())
683 * Eigen::Quaternionf(q.w(), q.x(), q.y(), q.z());
686 CloudPtr baserel_polygon_cloud(
new Cloud());
687 pcl::transformPointCloud(*cloud_hull_, *baserel_polygon_cloud, affine_cloud);
691 Eigen::Vector3f left_frustrum_normal =
692 Eigen::AngleAxisf(cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
693 * (-1. * Eigen::Vector3f::UnitY());
695 Eigen::Vector3f right_frustrum_normal =
696 Eigen::AngleAxisf(-cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
697 * Eigen::Vector3f::UnitY();
699 Eigen::Vector3f lower_frustrum_normal =
700 Eigen::AngleAxisf(cfg_vertical_va_ * 0.5, Eigen::Vector3f::UnitY())
701 * Eigen::Vector3f::UnitZ();
705 #ifdef HAVE_VISUAL_DEBUGGING
706 size_t geidx1 = std::numeric_limits<size_t>::max();
707 size_t geidx2 = std::numeric_limits<size_t>::max();
710 size_t lf_pidx1, lf_pidx2;
711 pidx1 = pidx2 = lf_pidx1 = lf_pidx2 = std::numeric_limits<size_t>::max();
722 const size_t psize = cloud_hull_->points.size();
723 #ifdef HAVE_VISUAL_DEBUGGING
724 size_t good_edge_points = 0;
726 for (
size_t i = 0; i < psize; ++i) {
728 PointType &p1p = cloud_hull_->points[i];
729 PointType &p2p = cloud_hull_->points[(i + 1) % psize];
731 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
732 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
734 PointType &br_p1p = baserel_polygon_cloud->points[i];
735 PointType &br_p2p = baserel_polygon_cloud->points[(i + 1) % psize];
738 if (!(((left_frustrum_normal.dot(p1) < 0.03) && (left_frustrum_normal.dot(p2) < 0.03))
739 || ((right_frustrum_normal.dot(p1) < 0.03)
740 && (right_frustrum_normal.dot(p2) < 0.03)))) {
744 if ((lower_frustrum_normal.dot(p1) < 0.01) && (lower_frustrum_normal.dot(p2) < 0.01)) {
747 if ((lf_pidx1 == std::numeric_limits<size_t>::max())
748 || is_polygon_edge_better(br_p1p,
750 baserel_polygon_cloud->points[lf_pidx1],
751 baserel_polygon_cloud->points[lf_pidx2])) {
755 lf_pidx2 = (i + 1) % psize;
761 #ifdef HAVE_VISUAL_DEBUGGING
763 for (
unsigned int j = 0; j < 3; ++j)
764 good_hull_edges[good_edge_points][j] = p1[j];
765 good_hull_edges[good_edge_points][3] = 0.;
767 for (
unsigned int j = 0; j < 3; ++j)
768 good_hull_edges[good_edge_points][j] = p2[j];
769 good_hull_edges[good_edge_points][3] = 0.;
773 if (pidx1 != std::numeric_limits<size_t>::max()) {
775 PointType &cb_br_p1p = baserel_polygon_cloud->points[pidx1];
776 PointType &cb_br_p2p = baserel_polygon_cloud->points[pidx2];
778 if (!is_polygon_edge_better(cb_br_p1p, cb_br_p2p, br_p1p, br_p2p)) {
794 pidx2 = (i + 1) % psize;
795 #ifdef HAVE_VISUAL_DEBUGGING
796 geidx1 = good_edge_points - 2;
797 geidx2 = good_edge_points - 1;
810 if (lf_pidx1 != std::numeric_limits<size_t>::max()) {
811 PointType &lp1p = baserel_polygon_cloud->points[lf_pidx1];
812 PointType &lp2p = baserel_polygon_cloud->points[lf_pidx2];
814 Eigen::Vector4f lp1(lp1p.x, lp1p.y, lp1p.z, 0.);
815 Eigen::Vector4f lp2(lp2p.x, lp2p.y, lp2p.z, 0.);
818 if (pidx1 == std::numeric_limits<size_t>::max()) {
822 #ifdef HAVE_VISUAL_DEBUGGING
823 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
824 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
825 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
826 geidx1 = good_edge_points++;
828 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
829 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
830 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
831 geidx2 = good_edge_points++;
835 PointType &p1p = baserel_polygon_cloud->points[pidx1];
836 PointType &p2p = baserel_polygon_cloud->points[pidx2];
838 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0.);
839 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0.);
843 (p1[0] > baserel_table_centroid[0]) || (p2[0] > baserel_table_centroid[0])) {
849 #ifdef HAVE_VISUAL_DEBUGGING
850 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
851 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
852 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
853 geidx1 = good_edge_points++;
855 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
856 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
857 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
858 geidx2 = good_edge_points++;
866 #ifdef HAVE_VISUAL_DEBUGGING
867 if (good_edge_points > 0) {
868 good_hull_edges[geidx1][3] = 1.0;
869 good_hull_edges[geidx2][3] = 1.0;
871 good_hull_edges.resize(good_edge_points);
874 TIMETRACK_END(ttc_find_edge_);
876 model_cloud_hull_.reset(
new Cloud());
877 if (cfg_table_model_enable_ && (pidx1 != std::numeric_limits<size_t>::max())
878 && (pidx2 != std::numeric_limits<size_t>::max())) {
879 TIMETRACK_START(ttc_transform_);
883 PointType &p1p = cloud_hull_->points[pidx1];
884 PointType &p2p = cloud_hull_->points[pidx2];
886 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
887 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
890 Eigen::Vector3f model_normal = Eigen::Vector3f::UnitZ();
891 Eigen::Vector3f normal(coeff->values[0], coeff->values[1], coeff->values[2]);
894 Eigen::Vector3f table_centroid_3f =
895 Eigen::Vector3f(table_centroid[0], table_centroid[1], table_centroid[2]);
898 Eigen::Vector3f p1_p2 = p2 - p1;
899 Eigen::Vector3f p1_p2_center = (p2 + p1) * 0.5;
901 Eigen::Vector3f p1_p2_normal_cross = p1_p2.cross(normal);
902 p1_p2_normal_cross.normalize();
906 double nD = -p1_p2_normal_cross.dot(p1_p2_center);
907 double p1_p2_centroid_dist = p1_p2_normal_cross.dot(table_centroid_3f) + nD;
908 if (p1_p2_centroid_dist < 0) {
910 p1_p2_normal_cross *= -1;
913 Eigen::Vector3f table_center =
914 p1_p2_center + p1_p2_normal_cross * (cfg_table_model_width_ * 0.5);
916 for (
unsigned int i = 0; i < 3; ++i)
917 table_centroid[i] = table_center[i];
918 table_centroid[3] = 0.;
921 std::vector<Eigen::Vector3f> tpoints(4);
922 tpoints[0] = p1_p2_center + p1_p2 * (cfg_table_model_length_ * 0.5);
923 tpoints[1] = tpoints[0] + p1_p2_normal_cross * cfg_table_model_width_;
924 tpoints[3] = p1_p2_center - p1_p2 * (cfg_table_model_length_ * 0.5);
925 tpoints[2] = tpoints[3] + p1_p2_normal_cross * cfg_table_model_width_;
927 model_cloud_hull_->points.resize(4);
928 model_cloud_hull_->height = 1;
929 model_cloud_hull_->width = 4;
930 model_cloud_hull_->is_dense =
true;
931 for (
int i = 0; i < 4; ++i) {
932 model_cloud_hull_->points[i].x = tpoints[i][0];
933 model_cloud_hull_->points[i].y = tpoints[i][1];
934 model_cloud_hull_->points[i].z = tpoints[i][2];
941 Eigen::Vector3f rotaxis = model_normal.cross(normal);
943 double angle = acos(normal.dot(model_normal));
946 Eigen::Affine3f affine =
947 Eigen::Translation3f(table_centroid.x(), table_centroid.y(), table_centroid.z())
948 * Eigen::AngleAxisf(angle, rotaxis);
950 Eigen::Vector3f model_p1(-cfg_table_model_width_ * 0.5, cfg_table_model_length_ * 0.5, 0.),
951 model_p2(-cfg_table_model_width_ * 0.5, -cfg_table_model_length_ * 0.5, 0.);
952 model_p1 = affine * model_p1;
953 model_p2 = affine * model_p2;
956 Eigen::Vector3f model_p1_p2 = model_p2 - model_p1;
957 model_p1_p2.normalize();
959 Eigen::Vector3f model_rotaxis = model_p1_p2.cross(p1_p2);
960 model_rotaxis.normalize();
961 double angle_p1_p2 = acos(model_p1_p2.dot(p1_p2));
967 affine = Eigen::Translation3f(table_centroid.x(), table_centroid.y(), table_centroid.z())
968 * Eigen::AngleAxisf(angle_p1_p2, model_rotaxis) * Eigen::AngleAxisf(angle, rotaxis);
971 Eigen::Quaternionf qt;
972 qt = Eigen::AngleAxisf(angle_p1_p2, model_rotaxis) * Eigen::AngleAxisf(angle, rotaxis);
975 set_position(table_pos_if_,
true, table_centroid, qt);
977 TIMETRACK_INTER(ttc_transform_, ttc_transform_model_)
980 CloudPtr table_model = generate_table_model(cfg_table_model_length_,
981 cfg_table_model_width_,
982 cfg_table_model_step_);
983 pcl::transformPointCloud(*table_model, *table_model_, affine.matrix());
986 table_model_->header.frame_id = input_->header.frame_id;
988 TIMETRACK_END(ttc_transform_model_);
992 set_position(table_pos_if_,
false);
995 TIMETRACK_ABORT(ttc_find_edge_);
998 TIMETRACK_START(ttc_extract_non_plane_);
1000 cloud_filt_.reset(
new Cloud());
1001 extract_.setNegative(
true);
1002 extract_.filter(*cloud_filt_);
1004 TIMETRACK_INTER(ttc_extract_non_plane_, ttc_polygon_filter_);
1012 bool viewpoint_above =
true;
1018 viewpoint_above = (baserel_viewpoint.z() > table_centroid[2]);
1020 logger->
log_warn(
name(),
"[L %u] could not transform viewpoint to base link", loop_count_);
1035 pcl::ComparisonOps::CompareOp op =
1036 viewpoint_above ? (coeff->values[3] > 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT)
1037 : (coeff->values[3] < 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT);
1040 pcl::ConditionAnd<PointType>::Ptr above_cond(
new pcl::ConditionAnd<PointType>());
1041 above_cond->addComparison(above_comp);
1042 pcl::ConditionalRemoval<PointType> above_condrem;
1043 above_condrem.setCondition(above_cond);
1044 above_condrem.setInputCloud(cloud_filt_);
1045 cloud_above_.reset(
new Cloud());
1046 above_condrem.filter(*cloud_above_);
1050 if (cloud_filt_->points.size() < cfg_cluster_min_size_) {
1052 TIMETRACK_ABORT(ttc_polygon_filter_);
1053 TIMETRACK_ABORT(ttc_full_loop_);
1058 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
1060 pcl::ConditionAnd<PointType>::Ptr polygon_cond(
new pcl::ConditionAnd<PointType>());
1064 (model_cloud_hull_ && !model_cloud_hull_->points.empty()) ? *model_cloud_hull_
1066 polygon_cond->addComparison(inpoly_comp);
1069 pcl::ConditionalRemoval<PointType> condrem;
1070 condrem.setCondition(polygon_cond);
1071 condrem.setInputCloud(cloud_above_);
1073 cloud_objs_.reset(
new Cloud());
1074 condrem.filter(*cloud_objs_);
1083 TIMETRACK_INTER(ttc_polygon_filter_, ttc_table_to_output_);
1085 std::vector<int> indices(cloud_proj_->points.size());
1086 for (uint i = 0; i < indices.size(); i++)
1088 ColorCloudPtr tmp_clusters = colorize_cluster(cloud_proj_, indices, table_color);
1089 tmp_clusters->height = 1;
1090 tmp_clusters->is_dense =
false;
1091 tmp_clusters->width = cloud_proj_->points.size();
1093 TIMETRACK_INTER(ttc_table_to_output_, ttc_cluster_objects_);
1095 unsigned int object_count = 0;
1096 if (cloud_objs_->points.size() > 0) {
1099 pcl::StatisticalOutlierRemoval<PointType> sor;
1100 sor.setInputCloud(cloud_objs_);
1102 sor.setStddevMulThresh(0.2);
1103 sor.filter(*cloud_objs_);
1106 std::vector<pcl::PointCloud<ColorPointType>::Ptr> tmp_obj_clusters(MAX_CENTROIDS);
1107 object_count = cluster_objects(cloud_objs_, tmp_clusters, tmp_obj_clusters);
1108 if (object_count == 0) {
1112 TIMETRACK_INTER(ttc_hungarian_, ttc_old_centroids_)
1115 for (OldCentroidVector::iterator it = old_centroids_.begin(); it != old_centroids_.end(); ++it) {
1116 it->increment_age();
1119 delete_old_centroids(old_centroids_, cfg_centroid_max_age_);
1121 delete_near_centroids(centroids_, old_centroids_, cfg_centroid_min_distance_);
1123 TIMETRACK_END(ttc_old_centroids_);
1126 for (
unsigned int i = 0; i < pos_ifs_.size(); i++) {
1127 if (!centroids_.count(i)) {
1128 set_position(pos_ifs_[i],
false);
1133 for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); ++it) {
1134 set_position(pos_ifs_[it->first],
true, it->second);
1137 TIMETRACK_INTER(ttc_cluster_objects_, ttc_visualization_)
1139 *clusters_ = *tmp_clusters;
1140 fclusters_->header.frame_id = input_->header.frame_id;
1141 pcl_utils::copy_time(input_, fclusters_);
1142 pcl_utils::copy_time(input_, ftable_model_);
1143 pcl_utils::copy_time(input_, fsimplified_polygon_);
1145 for (
unsigned int i = 0; i < f_obj_clusters_.size(); i++) {
1146 if (centroids_.count(i)) {
1147 *obj_clusters_[i] = *tmp_obj_clusters[i];
1149 obj_clusters_[i]->clear();
1154 pcl_utils::copy_time(input_, f_obj_clusters_[i]);
1157 #ifdef HAVE_VISUAL_DEBUGGING
1159 Eigen::Vector4f normal;
1160 normal[0] = coeff->values[0];
1161 normal[1] = coeff->values[1];
1162 normal[2] = coeff->values[2];
1166 hull_vertices.resize(cloud_hull_->points.size());
1167 for (
unsigned int i = 0; i < cloud_hull_->points.size(); ++i) {
1168 hull_vertices[i][0] = cloud_hull_->points[i].x;
1169 hull_vertices[i][1] = cloud_hull_->points[i].y;
1170 hull_vertices[i][2] = cloud_hull_->points[i].z;
1171 hull_vertices[i][3] = 0.;
1175 if (model_cloud_hull_ && !model_cloud_hull_->points.empty()) {
1176 model_vertices.resize(model_cloud_hull_->points.size());
1177 for (
unsigned int i = 0; i < model_cloud_hull_->points.size(); ++i) {
1178 model_vertices[i][0] = model_cloud_hull_->points[i].x;
1179 model_vertices[i][1] = model_cloud_hull_->points[i].y;
1180 model_vertices[i][2] = model_cloud_hull_->points[i].z;
1181 model_vertices[i][3] = 0.;
1185 visthread_->visualize(input_->header.frame_id,
1193 obj_shape_confidence_,
1198 TIMETRACK_END(ttc_visualization_);
1199 TIMETRACK_END(ttc_full_loop_);
1201 #ifdef USE_TIMETRACKER
1202 if (++tt_loopcount_ >= 5) {
1204 tt_->print_to_stdout();
1209 std::vector<pcl::PointIndices>
1210 TabletopObjectsThread::extract_object_clusters(CloudConstPtr input)
1212 TIMETRACK_START(ttc_obj_extraction_);
1213 std::vector<pcl::PointIndices> cluster_indices;
1214 if (input->empty()) {
1215 TIMETRACK_ABORT(ttc_obj_extraction_);
1216 return cluster_indices;
1220 pcl::search::KdTree<PointType>::Ptr kdtree_cl(
new pcl::search::KdTree<PointType>());
1221 kdtree_cl->setInputCloud(input);
1223 pcl::EuclideanClusterExtraction<PointType> ec;
1224 ec.setClusterTolerance(cfg_cluster_tolerance_);
1225 ec.setMinClusterSize(cfg_cluster_min_size_);
1226 ec.setMaxClusterSize(cfg_cluster_max_size_);
1227 ec.setSearchMethod(kdtree_cl);
1228 ec.setInputCloud(input);
1229 ec.extract(cluster_indices);
1232 TIMETRACK_END(ttc_obj_extraction_);
1233 return cluster_indices;
1237 TabletopObjectsThread::next_id()
1239 if (free_ids_.empty()) {
1243 int id = free_ids_.front();
1244 free_ids_.pop_front();
1249 TabletopObjectsThread::cluster_objects(CloudConstPtr input_cloud,
1250 ColorCloudPtr tmp_clusters,
1251 std::vector<ColorCloudPtr> &tmp_obj_clusters)
1253 unsigned int object_count = 0;
1254 std::vector<pcl::PointIndices> cluster_indices = extract_object_clusters(input_cloud);
1255 std::vector<pcl::PointIndices>::const_iterator it;
1256 unsigned int num_points = 0;
1257 for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
1258 num_points += it->indices.size();
1260 CentroidMap tmp_centroids;
1262 if (num_points > 0) {
1263 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids(
1266 unsigned int centroid_i = 0;
1268 std::vector<double> init_likelihoods;
1269 init_likelihoods.resize(NUM_KNOWN_OBJS_ + 1, 0.0);
1270 for (uint i = 0; i < MAX_CENTROIDS; i++)
1271 obj_likelihoods_[i] = init_likelihoods;
1273 for (it = cluster_indices.begin(); it != cluster_indices.end() && centroid_i < MAX_CENTROIDS;
1274 ++it, ++centroid_i) {
1276 "********************Processing obj_%u********************",
1283 ColorCloudPtr single_cluster =
1284 colorize_cluster(input_cloud, it->indices, cluster_colors[centroid_i]);
1285 single_cluster->header.frame_id = input_cloud->header.frame_id;
1286 single_cluster->width = it->indices.size();
1287 single_cluster->height = 1;
1289 ColorCloudPtr obj_in_base_frame(
new ColorCloud());
1290 obj_in_base_frame->header.frame_id = cfg_base_frame_;
1291 obj_in_base_frame->width = it->indices.size();
1292 obj_in_base_frame->height = 1;
1293 obj_in_base_frame->points.resize(it->indices.size());
1298 pcl_utils::transform_pointcloud(cfg_base_frame_,
1303 pcl::compute3DCentroid(*obj_in_base_frame, new_centroids[centroid_i]);
1305 if (cfg_cylinder_fitting_) {
1306 new_centroids[centroid_i] =
1307 fit_cylinder(obj_in_base_frame, new_centroids[centroid_i], centroid_i);
1310 object_count = centroid_i;
1311 new_centroids.resize(object_count);
1315 CentroidMap cylinder_params(cylinder_params_);
1316 std::map<uint, signed int> best_obj_guess(best_obj_guess_);
1317 std::map<uint, double> obj_shape_confidence(obj_shape_confidence_);
1318 std::map<uint, std::vector<double>> obj_likelihoods(obj_likelihoods_);
1319 cylinder_params_.clear();
1320 best_obj_guess_.clear();
1321 obj_shape_confidence_.clear();
1322 obj_likelihoods_.clear();
1324 std::map<uint, int> assigned_ids;
1325 if (cfg_track_objects_) {
1326 assigned_ids = track_objects(new_centroids);
1328 for (
unsigned int i = 0; i < new_centroids.size(); i++) {
1329 assigned_ids[i] = i;
1334 for (uint i = 0; i < new_centroids.size(); i++) {
1337 assigned_id = assigned_ids.at(i);
1338 }
catch (
const std::out_of_range &e) {
1343 if (assigned_id == -1)
1345 tmp_centroids[assigned_id] = new_centroids[i];
1346 cylinder_params_[assigned_id] = cylinder_params[i];
1347 obj_shape_confidence_[assigned_id] = obj_shape_confidence[i];
1348 best_obj_guess_[assigned_id] = best_obj_guess[i];
1349 obj_likelihoods_[assigned_id] = obj_likelihoods[i];
1350 ColorCloudPtr colorized_cluster =
1351 colorize_cluster(input_cloud,
1352 cluster_indices[i].indices,
1353 cluster_colors[assigned_id % MAX_CENTROIDS]);
1354 *tmp_clusters += *colorized_cluster;
1355 tmp_obj_clusters[assigned_id] = colorized_cluster;
1359 remove_high_centroids(table_centroid, tmp_centroids);
1361 if (object_count > 0)
1366 for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); ++it) {
1367 old_centroids_.push_back(
OldCentroid(it->first, it->second));
1370 centroids_ = tmp_centroids;
1371 return object_count;
1374 TabletopObjectsThread::ColorCloudPtr
1375 TabletopObjectsThread::colorize_cluster(CloudConstPtr input_cloud,
1376 const std::vector<int> &cluster,
1377 const uint8_t color[])
1379 ColorCloudPtr result(
new ColorCloud());
1380 result->resize(cluster.size());
1381 result->header.frame_id = input_cloud->header.frame_id;
1383 for (std::vector<int>::const_iterator it = cluster.begin(); it != cluster.end(); ++it, ++i) {
1384 ColorPointType & p1 = result->points.at(i);
1385 const PointType &p2 = input_cloud->points.at(*it);
1397 TabletopObjectsThread::compute_bounding_box_scores(
1398 Eigen::Vector3f & cluster_dim,
1399 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> &scores)
1401 scores.resize(NUM_KNOWN_OBJS_);
1403 for (
int i = 0; i < NUM_KNOWN_OBJS_; i++) {
1404 scores[i][0] = compute_similarity(cluster_dim[0], known_obj_dimensions_[i][0]);
1405 scores[i][1] = compute_similarity(cluster_dim[1], known_obj_dimensions_[i][1]);
1406 scores[i][2] = compute_similarity(cluster_dim[2], known_obj_dimensions_[i][2]);
1412 TabletopObjectsThread::compute_similarity(
double d1,
double d2)
1414 return exp(-50.0 * ((d1 - d2) * (d1 - d2)));
1420 const Eigen::Vector4f & centroid,
1421 const Eigen::Quaternionf & attitude)
1426 tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()),
1427 tf::Vector3(centroid[0], centroid[1], centroid[2])),
1429 input_->header.frame_id);
1431 iface->
set_frame(cfg_result_frame_.c_str());
1438 if (visibility_history >= 0) {
1443 tf::Vector3 & origin = baserel_pose.getOrigin();
1444 tf::Quaternion quat = baserel_pose.getRotation();
1445 double translation[3] = {origin.x(), origin.y(), origin.z()};
1446 double rotation[4] = {quat.x(), quat.y(), quat.z(), quat.w()};
1451 if (visibility_history <= 0) {
1455 double translation[3] = {0, 0, 0};
1456 double rotation[4] = {0, 0, 0, 1};
1464 TabletopObjectsThread::CloudPtr
1465 TabletopObjectsThread::generate_table_model(
const float length,
1467 const float thickness,
1469 const float max_error)
1471 CloudPtr c(
new Cloud());
1473 const float length_2 = fabs(length) * 0.5;
1474 const float width_2 = fabs(width) * 0.5;
1475 const float thickness_2 = fabs(thickness) * 0.5;
1478 const unsigned int l_base_num = std::max(2u, (
unsigned int)floor(length / step));
1479 const unsigned int num_w =
1481 + ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1482 const unsigned int w_base_num = std::max(2u, (
unsigned int)floor(width / step));
1483 const unsigned int num_h =
1485 + ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1486 const unsigned int t_base_num = std::max(2u, (
unsigned int)floor(thickness / step));
1487 const unsigned int num_t =
1489 + ((thickness < t_base_num * step) ? 0 : ((thickness - t_base_num * step) > max_error ? 2 : 1));
1496 c->width = num_t * num_w * num_h;
1498 c->points.resize((
size_t)num_t * (
size_t)num_w * (
size_t)num_h);
1500 unsigned int idx = 0;
1501 for (
unsigned int t = 0; t < num_t; ++t) {
1502 for (
unsigned int w = 0; w < num_w; ++w) {
1503 for (
unsigned int h = 0; h < num_h; ++h) {
1504 PointType &p = c->points[idx++];
1506 p.x = h * step - width_2;
1507 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error)
1510 p.y = w * step - length_2;
1511 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error)
1514 p.z = t * step - thickness_2;
1515 if ((t == num_t - 1) && fabs(p.z - thickness_2) > max_error)
1524 TabletopObjectsThread::CloudPtr
1525 TabletopObjectsThread::generate_table_model(
const float length,
1528 const float max_error)
1530 CloudPtr c(
new Cloud());
1532 const float length_2 = fabs(length) * 0.5;
1533 const float width_2 = fabs(width) * 0.5;
1536 const unsigned int l_base_num = std::max(2u, (
unsigned int)floor(length / step));
1537 const unsigned int num_w =
1539 + ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1540 const unsigned int w_base_num = std::max(2u, (
unsigned int)floor(width / step));
1541 const unsigned int num_h =
1543 + ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1549 c->width = num_w * num_h;
1551 c->points.resize((
size_t)num_w * (
size_t)num_h);
1553 unsigned int idx = 0;
1554 for (
unsigned int w = 0; w < num_w; ++w) {
1555 for (
unsigned int h = 0; h < num_h; ++h) {
1556 PointType &p = c->points[idx++];
1558 p.x = h * step - width_2;
1559 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error)
1562 p.y = w * step - length_2;
1563 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error)
1573 TabletopObjectsThread::CloudPtr
1574 TabletopObjectsThread::simplify_polygon(CloudPtr polygon,
float dist_threshold)
1576 const float sqr_dist_threshold = dist_threshold * dist_threshold;
1577 CloudPtr result(
new Cloud());
1578 const size_t psize = polygon->points.size();
1579 result->points.resize(psize);
1580 size_t res_points = 0;
1582 for (
size_t i = 1; i <= psize; ++i) {
1583 PointType &p1p = polygon->points[i - i_dist];
1586 if (result->points.empty()) {
1592 PointType &p2p = polygon->points[i % psize];
1593 PointType &p3p = (i == psize) ? result->points[0] : polygon->points[(i + 1) % psize];
1595 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0);
1596 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0);
1597 Eigen::Vector4f p3(p3p.x, p3p.y, p3p.z, 0);
1599 Eigen::Vector4f line_dir = p3 - p1;
1601 double sqr_dist = pcl::sqrPointToLineDistance(p2, p1, line_dir);
1602 if (sqr_dist < sqr_dist_threshold) {
1606 result->points[res_points++] = p2p;
1610 result->header.frame_id = polygon->header.frame_id;
1611 result->header.stamp = polygon->header.stamp;
1612 result->width = res_points;
1614 result->is_dense =
false;
1615 result->points.resize(res_points);
1621 TabletopObjectsThread::convert_colored_input()
1623 converted_input_->header.seq = colored_input_->header.seq;
1624 converted_input_->header.frame_id = colored_input_->header.frame_id;
1625 converted_input_->header.stamp = colored_input_->header.stamp;
1626 converted_input_->width = colored_input_->width;
1627 converted_input_->height = colored_input_->height;
1628 converted_input_->is_dense = colored_input_->is_dense;
1630 const size_t size = colored_input_->points.size();
1631 converted_input_->points.resize(size);
1632 for (
size_t i = 0; i < size; ++i) {
1633 const ColorPointType &in = colored_input_->points[i];
1634 PointType & out = converted_input_->points[i];
1643 TabletopObjectsThread::delete_old_centroids(OldCentroidVector centroids,
unsigned int age)
1645 centroids.erase(std::remove_if(centroids.begin(),
1648 if (centroid.get_age() > age) {
1649 free_ids_.push_back(centroid.get_id());
1658 TabletopObjectsThread::delete_near_centroids(CentroidMap reference,
1659 OldCentroidVector centroids,
1662 centroids.erase(std::remove_if(centroids.begin(),
1665 for (CentroidMap::const_iterator it = reference.begin();
1666 it != reference.end();
1668 if (pcl::distances::l2(it->second, old.get_centroid())
1670 free_ids_.push_back(old.get_id());
1680 TabletopObjectsThread::remove_high_centroids(Eigen::Vector4f table_centroid, CentroidMap centroids)
1687 input_->header.frame_id);
1690 for (CentroidMap::iterator it = centroids.begin(); it != centroids.end();) {
1697 float d = sp_baserel_centroid.z() - sp_baserel_table.z();
1698 if (d > cfg_centroid_max_height_) {
1700 free_ids_.push_back(it->first);
1701 centroids.erase(it++);
1715 TabletopObjectsThread::fit_cylinder(ColorCloudConstPtr obj_in_base_frame,
1716 Eigen::Vector4f
const ¢roid,
1717 uint
const & centroid_i)
1719 Eigen::Vector4f new_centroid(centroid);
1720 ColorPointType pnt_min, pnt_max;
1721 Eigen::Vector3f obj_dim;
1722 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> obj_size_scores;
1723 pcl::getMinMax3D(*obj_in_base_frame, pnt_min, pnt_max);
1724 obj_dim[0] = fabs(pnt_max.x - pnt_min.x);
1725 obj_dim[1] = fabs(pnt_max.y - pnt_min.y);
1726 obj_dim[2] = fabs(pnt_max.z - pnt_min.z);
1727 compute_bounding_box_scores(obj_dim, obj_size_scores);
1730 name(),
"Computed object dimensions: %f %f %f", obj_dim[0], obj_dim[1], obj_dim[2]);
1732 for (
int os = 0; os < NUM_KNOWN_OBJS_; os++) {
1734 "** Cup %i: %f in x, %f in y, %f in z.",
1736 obj_size_scores[os][0],
1737 obj_size_scores[os][1],
1738 obj_size_scores[os][2]);
1739 obj_likelihoods_[centroid_i][os] =
1740 (double)obj_size_scores[os][0] * obj_size_scores[os][1] * obj_size_scores[os][2];
1744 pcl::NormalEstimation<ColorPointType, pcl::Normal> ne;
1745 pcl::SACSegmentationFromNormals<ColorPointType, pcl::Normal> seg;
1746 pcl::ExtractIndices<ColorPointType> extract;
1747 pcl::ExtractIndices<pcl::Normal> extract_normals;
1749 pcl::search::KdTree<ColorPointType>::Ptr tree_cyl(
new pcl::search::KdTree<ColorPointType>());
1750 pcl::ModelCoefficients::Ptr coefficients_cylinder(
new pcl::ModelCoefficients);
1751 pcl::PointIndices::Ptr inliers_cylinder(
new pcl::PointIndices);
1754 ne.setSearchMethod(tree_cyl);
1755 ne.setInputCloud(obj_in_base_frame);
1757 ne.compute(*obj_normals);
1761 seg.setOptimizeCoefficients(
true);
1762 seg.setModelType(pcl::SACMODEL_CYLINDER);
1763 seg.setMethodType(pcl::SAC_RANSAC);
1764 seg.setNormalDistanceWeight(0.1);
1765 seg.setMaxIterations(1000);
1766 seg.setDistanceThreshold(0.07);
1767 seg.setRadiusLimits(0, 0.12);
1769 seg.setInputCloud(obj_in_base_frame);
1770 seg.setInputNormals(obj_normals);
1774 seg.segment(*inliers_cylinder, *coefficients_cylinder);
1777 extract.setInputCloud(obj_in_base_frame);
1778 extract.setIndices(inliers_cylinder);
1779 extract.setNegative(
false);
1782 extract.filter(*cloud_cylinder_baserel);
1784 cylinder_params_[centroid_i][0] = 0;
1785 cylinder_params_[centroid_i][1] = 0;
1786 if (cloud_cylinder_baserel->points.empty()) {
1788 obj_shape_confidence_[centroid_i] = 0.0;
1794 obj_shape_confidence_[centroid_i] =
1795 (double)(cloud_cylinder_baserel->points.size()) / (obj_in_base_frame->points.size() * 1.0);
1797 "Cylinder fit confidence = %zu/%zu = %f",
1798 cloud_cylinder_baserel->points.size(),
1799 obj_in_base_frame->points.size(),
1800 obj_shape_confidence_[centroid_i]);
1802 ColorPointType pnt_min;
1803 ColorPointType pnt_max;
1804 pcl::getMinMax3D(*cloud_cylinder_baserel, pnt_min, pnt_max);
1805 if (cfg_verbose_cylinder_fitting_) {
1807 "Cylinder height according to cylinder inliers: %f",
1808 pnt_max.z - pnt_min.z);
1811 "Cylinder radius according to cylinder fitting: %f",
1812 (*coefficients_cylinder).values[6]);
1813 logger->
log_debug(
name(),
"Cylinder radius according to bounding box y: %f", obj_dim[1] / 2);
1817 cylinder_params_[centroid_i][0] = obj_dim[1] / 2;
1820 cylinder_params_[centroid_i][1] = obj_dim[2];
1825 new_centroid[0] = pnt_min.x + 0.5 * (pnt_max.x - pnt_min.x);
1826 new_centroid[1] = pnt_min.y + 0.5 * (pnt_max.y - pnt_min.y);
1827 new_centroid[2] = pnt_min.z + 0.5 * (pnt_max.z - pnt_min.z);
1830 signed int detected_obj_id = -1;
1831 double best_confidence = 0.0;
1832 if (cfg_verbose_cylinder_fitting_) {
1835 for (
int os = 0; os < NUM_KNOWN_OBJS_; os++) {
1836 if (cfg_verbose_cylinder_fitting_) {
1839 obj_likelihoods_[centroid_i][os] =
1840 (0.6 * obj_likelihoods_[centroid_i][os]) + (0.4 * obj_shape_confidence_[centroid_i]);
1843 if (obj_likelihoods_[centroid_i][os] > best_confidence) {
1844 best_confidence = obj_likelihoods_[centroid_i][os];
1845 detected_obj_id = os;
1848 if (cfg_verbose_cylinder_fitting_) {
1851 if (best_confidence > 0.6) {
1852 best_obj_guess_[centroid_i] = detected_obj_id;
1854 if (cfg_verbose_cylinder_fitting_) {
1856 "MATCH FOUND!! -------------------------> Cup number %i",
1860 best_obj_guess_[centroid_i] = -1;
1861 if (cfg_verbose_cylinder_fitting_) {
1865 if (cfg_verbose_cylinder_fitting_) {
1869 return new_centroid;
1880 std::map<unsigned int, int>
1881 TabletopObjectsThread::track_objects(
1882 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids)
1884 std::map<uint, int> final_assignment;
1887 for (
unsigned int i = 0; i < new_centroids.size(); i++) {
1888 final_assignment[i] = next_id();
1890 return final_assignment;
1892 TIMETRACK_START(ttc_hungarian_);
1893 hungarian_problem_t hp;
1895 std::vector<unsigned int> obj_ids(centroids_.size());
1899 hp.num_rows = new_centroids.size();
1900 hp.num_cols = centroids_.size();
1901 hp.cost = (
int **)calloc(hp.num_rows,
sizeof(
int *));
1902 for (
int i = 0; i < hp.num_rows; i++)
1903 hp.cost[i] = (
int *)calloc(hp.num_cols,
sizeof(
int));
1904 for (
int row = 0; row < hp.num_rows; row++) {
1905 unsigned int col = 0;
1906 for (CentroidMap::iterator col_it = centroids_.begin(); col_it != centroids_.end();
1908 double distance = pcl::distances::l2(new_centroids[row], col_it->second);
1909 hp.cost[row][col] = (int)(distance * 1000);
1910 obj_ids[col] = col_it->first;
1914 solver.
init(hp.cost, hp.num_rows, hp.num_cols, HUNGARIAN_MODE_MINIMIZE_COST);
1917 int assignment_size;
1920 for (
int row = 0; row < assignment_size; row++) {
1921 if (row >= hp.num_rows) {
1922 id = obj_ids.at(assignment[row]);
1923 old_centroids_.push_back(
OldCentroid(
id, centroids_.at(
id)));
1925 }
else if (assignment[row] >= hp.num_cols) {
1926 bool assigned =
false;
1928 for (OldCentroidVector::iterator it = old_centroids_.begin(); it != old_centroids_.end();
1930 if (pcl::distances::l2(new_centroids[row], it->get_centroid())
1931 <= cfg_centroid_max_distance_) {
1933 old_centroids_.erase(it);
1943 id = obj_ids[assignment[row]];
1947 if (pcl::distances::l2(centroids_[
id], new_centroids[row]) > cfg_centroid_max_distance_) {
1949 old_centroids_.push_back(
OldCentroid(
id, centroids_[
id]));
1953 final_assignment[row] = id;
1955 return final_assignment;
1959 #ifdef HAVE_VISUAL_DEBUGGING
1963 visthread_ = visthread;
This class is used to save old centroids in order to check for reappearance.
virtual void finalize()
Finalize the thread.
virtual ~TabletopObjectsThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
TabletopObjectsThread()
Constructor.
Base class for virtualization thread.
std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > V_Vector4f
Aligned vector of vectors/points.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
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.
Base class for exceptions in Fawkes.
Hungarian method assignment solver.
int * get_assignment(int &size)
Get assignment and size.
int init(int **cost_matrix, int rows, int cols, int mode)
Initialize hungarian method.
void solve()
Solve the assignment problem.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void msgq_pop()
Erase first message from queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
void remove_pointcloud(const char *id)
Remove the point cloud.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT >> cloud)
Add point cloud.
bool exists_pointcloud(const char *id)
Check if point cloud exists.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
Position3DInterface Fawkes BlackBoard Interface.
int32_t visibility_history() const
Get visibility_history value.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void set_frame(const char *new_frame)
Set frame value.
RefPtr<> is a reference-counting shared smartpointer.
void reset()
Reset pointer.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
bool is_enabled() const
Get enabled value.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
A class for handling time.
Compare points' distance to a plane.
pcl::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
Check if point is inside or outside a given polygon.
pcl::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.
Wrapper class to add time stamp and frame ID to base types.
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
float deg2rad(float deg)
Convert an angle given in degrees to radians.