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;
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.