22 #include "tabletop_objects_thread.h" 23 #include "cluster_colors.h" 24 #ifdef HAVE_VISUAL_DEBUGGING 25 # include "visualization_thread_base.h" 28 #include <pcl_utils/utils.h> 29 #include <pcl_utils/comparisons.h> 30 #include <utils/time/wait.h> 31 #include <utils/math/angle.h> 32 #ifdef USE_TIMETRACKER 33 # include <utils/time/tracker.h> 35 #include <utils/time/tracker_macros.h> 37 #include <pcl/point_cloud.h> 38 #include <pcl/point_types.h> 39 #include <pcl/sample_consensus/method_types.h> 40 #include <pcl/sample_consensus/model_types.h> 41 #include <pcl/segmentation/extract_clusters.h> 42 #include <pcl/filters/extract_indices.h> 43 #include <pcl/surface/convex_hull.h> 44 #include <pcl/kdtree/kdtree.h> 45 #include <pcl/kdtree/kdtree_flann.h> 46 #include <pcl/filters/project_inliers.h> 47 #include <pcl/filters/conditional_removal.h> 48 #include <pcl/common/centroid.h> 49 #include <pcl/common/transforms.h> 50 #include <pcl/common/distances.h> 51 #include <pcl/registration/distances.h> 53 #include <utils/hungarian_method/hungarian.h> 56 #include <pcl/features/normal_3d.h> 57 #include <pcl/ModelCoefficients.h> 58 #include <pcl/filters/extract_indices.h> 59 #include <pcl/filters/passthrough.h> 60 #include <pcl/sample_consensus/method_types.h> 61 #include <pcl/sample_consensus/model_types.h> 63 #include <pcl/filters/statistical_outlier_removal.h> 65 #include <interfaces/Position3DInterface.h> 66 #include <interfaces/SwitchInterface.h> 72 #define CFG_PREFIX "/perception/tabletop-objects/" 84 :
Thread(
"TabletopObjectsThread",
Thread::OPMODE_CONTINUOUS),
87 #ifdef HAVE_VISUAL_DEBUGGING 102 cfg_depth_filter_min_x_ =
config->
get_float(CFG_PREFIX
"depth_filter_min_x");
103 cfg_depth_filter_max_x_ =
config->
get_float(CFG_PREFIX
"depth_filter_max_x");
105 cfg_segm_max_iterations_ =
107 cfg_segm_distance_threshold_ =
109 cfg_segm_inlier_quota_ =
111 cfg_table_min_cluster_quota_ =
113 cfg_table_downsample_leaf_size_ =
115 cfg_table_cluster_tolerance_ =
117 cfg_max_z_angle_deviation_ =
config->
get_float(CFG_PREFIX
"max_z_angle_deviation");
118 cfg_table_min_height_ =
config->
get_float(CFG_PREFIX
"table_min_height");
119 cfg_table_max_height_ =
config->
get_float(CFG_PREFIX
"table_max_height");
120 cfg_table_model_enable_ =
config->
get_bool(CFG_PREFIX
"table_model_enable");
121 cfg_table_model_length_ =
config->
get_float(CFG_PREFIX
"table_model_length");
122 cfg_table_model_width_ =
config->
get_float(CFG_PREFIX
"table_model_width");
123 cfg_table_model_step_ =
config->
get_float(CFG_PREFIX
"table_model_step");
126 cfg_cluster_tolerance_ =
config->
get_float(CFG_PREFIX
"cluster_tolerance");
127 cfg_cluster_min_size_ =
config->
get_uint(CFG_PREFIX
"cluster_min_size");
128 cfg_cluster_max_size_ =
config->
get_uint(CFG_PREFIX
"cluster_max_size");
132 cfg_centroid_max_age_ =
config->
get_uint(CFG_PREFIX
"centroid_max_age");
133 cfg_centroid_max_distance_ =
config->
get_float(CFG_PREFIX
"centroid_max_distance");
134 cfg_centroid_min_distance_ =
config->
get_float(CFG_PREFIX
"centroid_min_distance");
135 cfg_centroid_max_height_ =
config->
get_float(CFG_PREFIX
"centroid_max_height");
136 cfg_cylinder_fitting_ =
config->
get_bool(CFG_PREFIX
"enable_cylinder_fitting");
137 cfg_track_objects_ =
config->
get_bool(CFG_PREFIX
"enable_object_tracking");
139 cfg_verbose_cylinder_fitting_ =
config->
get_bool(CFG_PREFIX
"verbose_cylinder_fitting");
141 cfg_verbose_cylinder_fitting_ =
false;
146 input_ = pcl_utils::cloudptr_from_refptr(finput_);
151 colored_input_ = pcl_utils::cloudptr_from_refptr(fcoloredinput_);
152 converted_input_.reset(
new Cloud());
153 input_ = converted_input_;
154 converted_input_->header.frame_id = colored_input_->header.frame_id;
155 converted_input_->header.stamp = colored_input_->header.stamp;
157 throw Exception(
"Point cloud '%s' does not exist or not XYZ or XYZ/RGB PCL",
158 cfg_input_pointcloud_.c_str());
162 double rotation[4] = {0., 0., 0., 1.};
163 table_pos_if_ = NULL;
168 table_pos_if_->
write();
175 pos_ifs_.resize(MAX_CENTROIDS, NULL);
176 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
178 if (asprintf(&tmp,
"Tabletop Object %u", i + 1) != -1) {
180 std::string
id = tmp;
192 for (
unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
201 fclusters_->header.frame_id = input_->header.frame_id;
202 fclusters_->is_dense =
false;
204 clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_);
209 for (
int i = 0; i < MAX_CENTROIDS; i++) {
211 f_tmp_cloud->header.frame_id = input_->header.frame_id;
212 f_tmp_cloud->is_dense =
false;
214 if (asprintf(&tmp_name,
"obj_cluster_%u", i) != -1) {
219 f_obj_clusters_.push_back(f_tmp_cloud);
220 tmp_cloud = pcl_utils::cloudptr_from_refptr(f_tmp_cloud);
221 obj_clusters_.push_back(tmp_cloud);
228 std::vector<double> init_likelihoods;
229 init_likelihoods.resize(NUM_KNOWN_OBJS_ + 1, 0.0);
234 known_obj_dimensions_.resize(NUM_KNOWN_OBJS_);
237 known_obj_dimensions_[0][0] = 0.07;
238 known_obj_dimensions_[0][1] = 0.07;
239 known_obj_dimensions_[0][2] = 0.104;
241 known_obj_dimensions_[1][0] = 0.088;
242 known_obj_dimensions_[1][1] = 0.088;
243 known_obj_dimensions_[1][2] = 0.155;
245 known_obj_dimensions_[2][0] = 0.106;
246 known_obj_dimensions_[2][1] = 0.106;
247 known_obj_dimensions_[2][2] = 0.277;
251 table_inclination_ = 0.0;
253 ftable_model_ =
new Cloud();
254 table_model_ = pcl_utils::cloudptr_from_refptr(ftable_model_);
255 table_model_->header.frame_id = input_->header.frame_id;
259 fsimplified_polygon_ =
new Cloud();
260 simplified_polygon_ = pcl_utils::cloudptr_from_refptr(fsimplified_polygon_);
261 simplified_polygon_->header.frame_id = input_->header.frame_id;
265 grid_.setFilterFieldName(
"x");
266 grid_.setFilterLimits(cfg_depth_filter_min_x_, cfg_depth_filter_max_x_);
267 grid_.setLeafSize(cfg_voxel_leaf_size_, cfg_voxel_leaf_size_, cfg_voxel_leaf_size_);
269 seg_.setOptimizeCoefficients(
true);
270 seg_.setModelType(pcl::SACMODEL_PLANE);
271 seg_.setMethodType(pcl::SAC_RANSAC);
272 seg_.setMaxIterations(cfg_segm_max_iterations_);
273 seg_.setDistanceThreshold(cfg_segm_distance_threshold_);
281 old_centroids_.clear();
282 for (
unsigned int i = 0; i < MAX_CENTROIDS; i++)
283 free_ids_.push_back(i);
285 #ifdef USE_TIMETRACKER 288 ttc_full_loop_ = tt_->add_class(
"Full Loop");
289 ttc_msgproc_ = tt_->add_class(
"Message Processing");
290 ttc_convert_ = tt_->add_class(
"Input Conversion");
291 ttc_voxelize_ = tt_->add_class(
"Downsampling");
292 ttc_plane_ = tt_->add_class(
"Plane Segmentation");
293 ttc_extract_plane_ = tt_->add_class(
"Plane Extraction");
294 ttc_plane_downsampling_ = tt_->add_class(
"Plane Downsampling");
295 ttc_cluster_plane_ = tt_->add_class(
"Plane Clustering");
296 ttc_convex_hull_ = tt_->add_class(
"Convex Hull");
297 ttc_simplify_polygon_ = tt_->add_class(
"Polygon Simplification");
298 ttc_find_edge_ = tt_->add_class(
"Polygon Edge");
299 ttc_transform_ = tt_->add_class(
"Transform");
300 ttc_transform_model_ = tt_->add_class(
"Model Transformation");
301 ttc_extract_non_plane_ = tt_->add_class(
"Non-Plane Extraction");
302 ttc_polygon_filter_ = tt_->add_class(
"Polygon Filter");
303 ttc_table_to_output_ = tt_->add_class(
"Table to Output");
304 ttc_cluster_objects_ = tt_->add_class(
"Object Clustering");
305 ttc_visualization_ = tt_->add_class(
"Visualization");
306 ttc_hungarian_ = tt_->add_class(
"Hungarian Method (centroids)");
307 ttc_old_centroids_ = tt_->add_class(
"Old Centroid Removal");
308 ttc_obj_extraction_ = tt_->add_class(
"Object Extraction");
318 simplified_polygon_.reset();
326 for (PosIfsVector::iterator it = pos_ifs_.begin(); it != pos_ifs_.end(); it++) {
333 ftable_model_.reset();
334 fsimplified_polygon_.reset();
337 template <
typename Po
intType>
339 comparePoints2D(
const PointType &p1,
const PointType &p2)
341 double angle1 = atan2(p1.y, p1.x) + M_PI;
342 double angle2 = atan2(p2.y, p2.x) + M_PI;
343 return (angle1 > angle2);
351 TabletopObjectsThread::is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p,
352 PointType &br_p1p, PointType &br_p2p)
355 Eigen::Vector3f cb_br_p1(cb_br_p1p.x, cb_br_p1p.y, cb_br_p1p.z);
356 Eigen::Vector3f cb_br_p2(cb_br_p2p.x, cb_br_p2p.y, cb_br_p2p.z);
357 Eigen::Vector3f cb_br_p1_p2_center = (cb_br_p1 + cb_br_p2) * 0.5;
359 Eigen::Vector3f br_p1(br_p1p.x, br_p1p.y, br_p1p.z);
360 Eigen::Vector3f br_p2(br_p2p.x, br_p2p.y, br_p2p.z);
361 Eigen::Vector3f br_p1_p2_center = (br_p2 + br_p1) * 0.5;
363 double dist_x = (cb_br_p1_p2_center[0] - br_p1_p2_center[0]);
368 if ( (dist_x < -0.25) ||
369 ((abs(dist_x) <= 0.25) && ((br_p2 - br_p1).norm() < (cb_br_p2 - cb_br_p1).norm())) )
379 TIMETRACK_START(ttc_full_loop_);
383 TIMETRACK_START(ttc_msgproc_);
402 TimeWait::wait(250000);
403 TIMETRACK_ABORT(ttc_full_loop_);
407 TIMETRACK_END(ttc_msgproc_);
410 if (colored_input_) {
411 pcl_utils::get_time(colored_input_, pcl_time);
413 pcl_utils::get_time(input_, pcl_time);
415 if (*last_pcl_time_ == pcl_time) {
416 TimeWait::wait(20000);
417 TIMETRACK_ABORT(ttc_full_loop_);
420 *last_pcl_time_ = pcl_time;
422 if (colored_input_) {
423 TIMETRACK_START(ttc_convert_);
424 convert_colored_input();
425 TIMETRACK_END(ttc_convert_);
428 TIMETRACK_START(ttc_voxelize_);
430 CloudPtr temp_cloud(
new Cloud);
431 CloudPtr temp_cloud2(
new Cloud);
432 pcl::ExtractIndices<PointType> extract_;
433 CloudPtr cloud_hull_;
434 CloudPtr model_cloud_hull_;
435 CloudPtr cloud_proj_;
436 CloudPtr cloud_filt_;
437 CloudPtr cloud_above_;
438 CloudPtr cloud_objs_;
439 pcl::search::KdTree<PointType> kdtree_;
441 grid_.setInputCloud(input_);
442 grid_.filter(*temp_cloud);
444 if (temp_cloud->points.size() <= 10) {
449 TimeWait::wait(50000);
453 TIMETRACK_INTER(ttc_voxelize_, ttc_plane_)
455 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
456 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
457 Eigen::Vector4f baserel_table_centroid(0,0,0,0);
466 bool happy_with_plane =
false;
467 while (! happy_with_plane) {
468 happy_with_plane =
true;
470 if (temp_cloud->points.size() <= 10) {
471 logger->
log_warn(
name(),
"[L %u] no more points for plane detection, skipping loop", loop_count_);
472 set_position(table_pos_if_,
false);
473 TIMETRACK_ABORT(ttc_plane_);
474 TIMETRACK_ABORT(ttc_full_loop_);
475 TimeWait::wait(50000);
479 seg_.setInputCloud(temp_cloud);
480 seg_.segment(*inliers, *coeff);
483 if ((
double)inliers->indices.size() < (cfg_segm_inlier_quota_ * (double)temp_cloud->points.size())) {
484 logger->
log_warn(
name(),
"[L %u] no table in scene, skipping loop (%zu inliers, required %f, voxelized size %zu)",
485 loop_count_, inliers->indices.size(),
486 (cfg_segm_inlier_quota_ * temp_cloud->points.size()), temp_cloud->points.size());
487 set_position(table_pos_if_,
false);
488 TIMETRACK_ABORT(ttc_plane_);
489 TIMETRACK_ABORT(ttc_full_loop_);
490 TimeWait::wait(50000);
498 table_normal(tf::Vector3(coeff->values[0], coeff->values[1], coeff->values[2]),
503 tf::Vector3 z_axis(0, 0, copysign(1.0, baserel_normal.z()));
504 table_inclination_ = z_axis.angle(baserel_normal);
505 if (fabs(z_axis.angle(baserel_normal)) > cfg_max_z_angle_deviation_) {
506 happy_with_plane =
false;
507 logger->
log_warn(
name(),
"[L %u] table normal (%f,%f,%f) Z angle deviation |%f| > %f, excluding",
508 loop_count_, baserel_normal.x(), baserel_normal.y(), baserel_normal.z(),
509 z_axis.angle(baserel_normal), cfg_max_z_angle_deviation_);
514 happy_with_plane =
false;
517 if (happy_with_plane) {
523 pcl::compute3DCentroid(*temp_cloud, *inliers, table_centroid);
525 centroid(tf::Point(table_centroid[0], table_centroid[1], table_centroid[2]),
529 baserel_table_centroid[0] = baserel_centroid.x();
530 baserel_table_centroid[1] = baserel_centroid.y();
531 baserel_table_centroid[2] = baserel_centroid.z();
533 if ((baserel_centroid.z() < cfg_table_min_height_) ||
534 (baserel_centroid.z() > cfg_table_max_height_))
536 happy_with_plane =
false;
538 loop_count_, baserel_centroid.z(),
539 cfg_table_min_height_, 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);
572 pcl::ProjectInliers<PointType> proj;
573 proj.setModelType(pcl::SACMODEL_PLANE);
574 proj.setInputCloud(temp_cloud2);
575 proj.setModelCoefficients(coeff);
576 cloud_proj_.reset(
new Cloud());
577 proj.filter(*cloud_proj_);
579 TIMETRACK_INTER(ttc_extract_plane_, ttc_plane_downsampling_);
591 CloudPtr cloud_table_voxelized(
new Cloud());
592 pcl::VoxelGrid<PointType> table_grid;
593 table_grid.setLeafSize(cfg_table_downsample_leaf_size_,
594 cfg_table_downsample_leaf_size_,
595 cfg_table_downsample_leaf_size_);
596 table_grid.setInputCloud(cloud_proj_);
597 table_grid.filter(*cloud_table_voxelized);
599 TIMETRACK_INTER(ttc_plane_downsampling_, ttc_cluster_plane_);
602 pcl::search::KdTree<PointType>::Ptr
603 kdtree_table(
new pcl::search::KdTree<PointType>());
604 kdtree_table->setInputCloud(cloud_table_voxelized);
606 std::vector<pcl::PointIndices> table_cluster_indices;
607 pcl::EuclideanClusterExtraction<PointType> table_ec;
608 table_ec.setClusterTolerance(cfg_table_cluster_tolerance_);
609 table_ec.setMinClusterSize(cfg_table_min_cluster_quota_
610 * cloud_table_voxelized->points.size());
611 table_ec.setMaxClusterSize(cloud_table_voxelized->points.size());
612 table_ec.setSearchMethod(kdtree_table);
613 table_ec.setInputCloud(cloud_table_voxelized);
614 table_ec.extract(table_cluster_indices);
616 if (! table_cluster_indices.empty()) {
618 CloudPtr cloud_table_extracted(
new Cloud());
619 pcl::PointIndices::ConstPtr table_cluster_indices_ptr(
new pcl::PointIndices(table_cluster_indices[0]));
620 pcl::ExtractIndices<PointType> table_cluster_extract;
621 table_cluster_extract.setNegative(
false);
622 table_cluster_extract.setInputCloud(cloud_table_voxelized);
623 table_cluster_extract.setIndices(table_cluster_indices_ptr);
624 table_cluster_extract.filter(*cloud_table_extracted);
625 *cloud_proj_ = *cloud_table_extracted;
628 pcl::compute3DCentroid(*cloud_proj_, table_centroid);
632 logger->
log_info(
name(),
"[L %u] table plane clustering did not generate any clusters", loop_count_);
635 TIMETRACK_INTER(ttc_cluster_plane_, ttc_convex_hull_)
639 pcl::ConvexHull<PointType> hr;
640 #ifdef PCL_VERSION_COMPARE 641 #if PCL_VERSION_COMPARE(>=,1,5,0) 647 hr.setInputCloud(cloud_proj_);
648 cloud_hull_.reset(
new Cloud());
649 hr.reconstruct(*cloud_hull_);
652 if (cloud_hull_->points.empty()) {
653 logger->
log_warn(
name(),
"[L %u] convex hull of table empty, skipping loop", loop_count_);
654 TIMETRACK_ABORT(ttc_convex_hull_);
655 TIMETRACK_ABORT(ttc_full_loop_);
656 set_position(table_pos_if_,
false);
660 TIMETRACK_INTER(ttc_convex_hull_, ttc_simplify_polygon_)
662 CloudPtr simplified_polygon = simplify_polygon(cloud_hull_, 0.02);
663 *simplified_polygon_ = *simplified_polygon;
666 *cloud_hull_ = *simplified_polygon;
668 TIMETRACK_INTER(ttc_simplify_polygon_, ttc_find_edge_)
670 #ifdef HAVE_VISUAL_DEBUGGING 672 good_hull_edges.resize(cloud_hull_->points.size() * 2);
684 tf::Quaternion q = t.getRotation();
685 Eigen::Affine3f affine_cloud =
686 Eigen::Translation3f(t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z())
687 * Eigen::Quaternionf(q.w(), q.x(), q.y(), q.z());
690 CloudPtr baserel_polygon_cloud(
new Cloud());
691 pcl::transformPointCloud(*cloud_hull_, *baserel_polygon_cloud, affine_cloud);
695 Eigen::Vector3f left_frustrum_normal =
696 Eigen::AngleAxisf( cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
697 * (-1. * Eigen::Vector3f::UnitY());
699 Eigen::Vector3f right_frustrum_normal =
700 Eigen::AngleAxisf(- cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
701 * Eigen::Vector3f::UnitY();
703 Eigen::Vector3f lower_frustrum_normal =
704 Eigen::AngleAxisf(cfg_vertical_va_ * 0.5, Eigen::Vector3f::UnitY())
705 * Eigen::Vector3f::UnitZ();
709 #ifdef HAVE_VISUAL_DEBUGGING 710 size_t geidx1 = std::numeric_limits<size_t>::max();
711 size_t geidx2 = std::numeric_limits<size_t>::max();
714 size_t lf_pidx1, lf_pidx2;
715 pidx1 = pidx2 = lf_pidx1 = lf_pidx2 = std::numeric_limits<size_t>::max();
726 const size_t psize = cloud_hull_->points.size();
727 #ifdef HAVE_VISUAL_DEBUGGING 728 size_t good_edge_points = 0;
730 for (
size_t i = 0; i < psize; ++i) {
732 PointType &p1p = cloud_hull_->points[i ];
733 PointType &p2p = cloud_hull_->points[(i+1) % psize];
735 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
736 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
738 PointType &br_p1p = baserel_polygon_cloud->points[i ];
739 PointType &br_p2p = baserel_polygon_cloud->points[(i + 1) % psize];
742 if ( ! (((left_frustrum_normal.dot(p1) < 0.03) &&
743 (left_frustrum_normal.dot(p2) < 0.03)) ||
744 ((right_frustrum_normal.dot(p1) < 0.03) &&
745 (right_frustrum_normal.dot(p2) < 0.03)) ) )
750 if ((lower_frustrum_normal.dot(p1) < 0.01) &&
751 (lower_frustrum_normal.dot(p2) < 0.01)) {
754 if ( (lf_pidx1 == std::numeric_limits<size_t>::max()) ||
755 is_polygon_edge_better(br_p1p, br_p2p,
756 baserel_polygon_cloud->points[lf_pidx1], baserel_polygon_cloud->points[lf_pidx2]))
761 lf_pidx2 = (i + 1) % psize;
767 #ifdef HAVE_VISUAL_DEBUGGING 769 for (
unsigned int j = 0; j < 3; ++j)
770 good_hull_edges[good_edge_points][j] = p1[j];
771 good_hull_edges[good_edge_points][3] = 0.;
773 for (
unsigned int j = 0; j < 3; ++j)
774 good_hull_edges[good_edge_points][j] = p2[j];
775 good_hull_edges[good_edge_points][3] = 0.;
779 if (pidx1 != std::numeric_limits<size_t>::max()) {
781 PointType &cb_br_p1p = baserel_polygon_cloud->points[pidx1];
782 PointType &cb_br_p2p = baserel_polygon_cloud->points[pidx2];
784 if (! is_polygon_edge_better(cb_br_p1p, cb_br_p2p, br_p1p, br_p2p))
801 pidx2 = (i + 1) % psize;
802 #ifdef HAVE_VISUAL_DEBUGGING 803 geidx1 = good_edge_points - 2;
804 geidx2 = good_edge_points - 1;
817 if (lf_pidx1 != std::numeric_limits<size_t>::max()) {
818 PointType &lp1p = baserel_polygon_cloud->points[lf_pidx1];
819 PointType &lp2p = baserel_polygon_cloud->points[lf_pidx2];
821 Eigen::Vector4f lp1(lp1p.x, lp1p.y, lp1p.z, 0.);
822 Eigen::Vector4f lp2(lp2p.x, lp2p.y, lp2p.z, 0.);
825 if (pidx1 == std::numeric_limits<size_t>::max()) {
829 #ifdef HAVE_VISUAL_DEBUGGING 830 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
831 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
832 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
833 geidx1 = good_edge_points++;
835 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
836 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
837 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
838 geidx2 = good_edge_points++;
843 PointType &p1p = baserel_polygon_cloud->points[pidx1];
844 PointType &p2p = baserel_polygon_cloud->points[pidx2];
846 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0.);
847 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0.);
851 (p1[0] > baserel_table_centroid[0]) || (p2[0] > baserel_table_centroid[0]))
858 #ifdef HAVE_VISUAL_DEBUGGING 859 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
860 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
861 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
862 geidx1 = good_edge_points++;
864 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
865 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
866 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
867 geidx2 = good_edge_points++;
875 #ifdef HAVE_VISUAL_DEBUGGING 876 if (good_edge_points > 0) {
877 good_hull_edges[geidx1][3] = 1.0;
878 good_hull_edges[geidx2][3] = 1.0;
880 good_hull_edges.resize(good_edge_points);
883 TIMETRACK_END(ttc_find_edge_);
885 model_cloud_hull_.reset(
new Cloud());
886 if ( cfg_table_model_enable_ &&
887 (pidx1 != std::numeric_limits<size_t>::max()) &&
888 (pidx2 != std::numeric_limits<size_t>::max()) )
890 TIMETRACK_START(ttc_transform_);
894 PointType &p1p = cloud_hull_->points[pidx1];
895 PointType &p2p = cloud_hull_->points[pidx2];
897 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
898 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
901 Eigen::Vector3f model_normal = Eigen::Vector3f::UnitZ();
902 Eigen::Vector3f normal(coeff->values[0], coeff->values[1], coeff->values[2]);
905 Eigen::Vector3f table_centroid_3f =
906 Eigen::Vector3f(table_centroid[0], table_centroid[1], table_centroid[2]);
909 Eigen::Vector3f p1_p2 = p2 - p1;
910 Eigen::Vector3f p1_p2_center = (p2 + p1) * 0.5;
912 Eigen::Vector3f p1_p2_normal_cross = p1_p2.cross(normal);
913 p1_p2_normal_cross.normalize();
917 double nD = - p1_p2_normal_cross.dot(p1_p2_center);
918 double p1_p2_centroid_dist = p1_p2_normal_cross.dot(table_centroid_3f) + nD;
919 if (p1_p2_centroid_dist < 0) {
921 p1_p2_normal_cross *= -1;
924 Eigen::Vector3f table_center =
925 p1_p2_center + p1_p2_normal_cross * (cfg_table_model_width_ * 0.5);
927 for (
unsigned int i = 0; i < 3; ++i) table_centroid[i] = table_center[i];
928 table_centroid[3] = 0.;
931 std::vector<Eigen::Vector3f> tpoints(4);
932 tpoints[0] = p1_p2_center + p1_p2 * (cfg_table_model_length_ * 0.5);
933 tpoints[1] = tpoints[0] + p1_p2_normal_cross * cfg_table_model_width_;
934 tpoints[3] = p1_p2_center - p1_p2 * (cfg_table_model_length_ * 0.5);
935 tpoints[2] = tpoints[3] + p1_p2_normal_cross * cfg_table_model_width_;
937 model_cloud_hull_->points.resize(4);
938 model_cloud_hull_->height = 1;
939 model_cloud_hull_->width = 4;
940 model_cloud_hull_->is_dense =
true;
941 for (
int i = 0; i < 4; ++i) {
942 model_cloud_hull_->points[i].x = tpoints[i][0];
943 model_cloud_hull_->points[i].y = tpoints[i][1];
944 model_cloud_hull_->points[i].z = tpoints[i][2];
951 Eigen::Vector3f rotaxis = model_normal.cross(normal);
953 double angle = acos(normal.dot(model_normal));
956 Eigen::Affine3f affine =
957 Eigen::Translation3f(table_centroid.x(), table_centroid.y(),
959 * Eigen::AngleAxisf(angle, rotaxis);
962 model_p1(-cfg_table_model_width_ * 0.5, cfg_table_model_length_ * 0.5, 0.),
963 model_p2(-cfg_table_model_width_ * 0.5, -cfg_table_model_length_ * 0.5, 0.);
964 model_p1 = affine * model_p1;
965 model_p2 = affine * model_p2;
968 Eigen::Vector3f model_p1_p2 = model_p2 - model_p1;
969 model_p1_p2.normalize();
971 Eigen::Vector3f model_rotaxis = model_p1_p2.cross(p1_p2);
972 model_rotaxis.normalize();
973 double angle_p1_p2 = acos(model_p1_p2.dot(p1_p2));
980 Eigen::Translation3f(table_centroid.x(), table_centroid.y(),
982 * Eigen::AngleAxisf(angle_p1_p2, model_rotaxis)
983 * Eigen::AngleAxisf(angle, rotaxis);
987 Eigen::Quaternionf qt;
988 qt = Eigen::AngleAxisf(angle_p1_p2, model_rotaxis)
989 * Eigen::AngleAxisf(angle, rotaxis);
992 set_position(table_pos_if_,
true, table_centroid, qt);
994 TIMETRACK_INTER(ttc_transform_, ttc_transform_model_)
997 CloudPtr table_model = generate_table_model(cfg_table_model_length_, cfg_table_model_width_, cfg_table_model_step_);
998 pcl::transformPointCloud(*table_model, *table_model_, affine.matrix());
1001 table_model_->header.frame_id = input_->header.frame_id;
1003 TIMETRACK_END(ttc_transform_model_);
1007 set_position(table_pos_if_,
false);
1008 logger->
log_warn(
name(),
"Failed to transform convex hull cloud, exception follows");
1010 TIMETRACK_ABORT(ttc_find_edge_);
1013 TIMETRACK_START(ttc_extract_non_plane_);
1015 cloud_filt_.reset(
new Cloud());
1016 extract_.setNegative(
true);
1017 extract_.filter(*cloud_filt_);
1019 TIMETRACK_INTER(ttc_extract_non_plane_, ttc_polygon_filter_);
1027 bool viewpoint_above =
true;
1030 origin(tf::Point(0, 0, 0),
fawkes::Time(0, 0), input_->header.frame_id);
1034 viewpoint_above = (baserel_viewpoint.z() > table_centroid[2]);
1052 pcl::ComparisonOps::CompareOp op =
1054 ? (coeff->values[3] > 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT)
1055 : (coeff->values[3] < 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT);
1058 pcl::ConditionAnd<PointType>::Ptr
1059 above_cond(
new pcl::ConditionAnd<PointType>());
1060 above_cond->addComparison(above_comp);
1061 pcl::ConditionalRemoval<PointType> above_condrem(above_cond);
1062 above_condrem.setInputCloud(cloud_filt_);
1063 cloud_above_.reset(
new Cloud());
1064 above_condrem.filter(*cloud_above_);
1068 if (cloud_filt_->points.size() < cfg_cluster_min_size_) {
1070 TIMETRACK_ABORT(ttc_polygon_filter_);
1071 TIMETRACK_ABORT(ttc_full_loop_);
1076 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
1078 pcl::ConditionAnd<PointType>::Ptr
1079 polygon_cond(
new pcl::ConditionAnd<PointType>());
1083 (model_cloud_hull_ && ! model_cloud_hull_->points.empty()) ? *model_cloud_hull_ : *cloud_hull_));
1084 polygon_cond->addComparison(inpoly_comp);
1087 pcl::ConditionalRemoval<PointType> condrem(polygon_cond);
1088 condrem.setInputCloud(cloud_above_);
1090 cloud_objs_.reset(
new Cloud());
1091 condrem.filter(*cloud_objs_);
1100 TIMETRACK_INTER(ttc_polygon_filter_, ttc_table_to_output_);
1102 std::vector<int> indices(cloud_proj_->points.size());
1103 for (uint i = 0; i < indices.size(); i++)
1105 ColorCloudPtr tmp_clusters = colorize_cluster(cloud_proj_, indices, table_color);
1106 tmp_clusters->height = 1;
1107 tmp_clusters->is_dense =
false;
1108 tmp_clusters->width = cloud_proj_->points.size();
1110 TIMETRACK_INTER(ttc_table_to_output_, ttc_cluster_objects_);
1112 unsigned int object_count = 0;
1113 if (cloud_objs_->points.size() > 0) {
1116 pcl::StatisticalOutlierRemoval<PointType> sor;
1117 sor.setInputCloud(cloud_objs_);
1119 sor.setStddevMulThresh(0.2);
1120 sor.filter(*cloud_objs_);
1123 std::vector<pcl::PointCloud<ColorPointType>::Ptr> tmp_obj_clusters(MAX_CENTROIDS);
1124 object_count = cluster_objects(cloud_objs_, tmp_clusters, tmp_obj_clusters);
1125 if (object_count == 0) {
1129 TIMETRACK_INTER(ttc_hungarian_, ttc_old_centroids_)
1132 for (OldCentroidVector::iterator it = old_centroids_.begin();
1133 it != old_centroids_.end(); it++) {
1137 delete_old_centroids(old_centroids_, cfg_centroid_max_age_);
1139 delete_near_centroids(centroids_, old_centroids_, cfg_centroid_min_distance_);
1141 TIMETRACK_END(ttc_old_centroids_);
1144 for (
unsigned int i = 0; i < pos_ifs_.size(); i++) {
1145 if (!centroids_.count(i)) {
1146 set_position(pos_ifs_[i],
false);
1151 for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); it++) {
1152 set_position(pos_ifs_[it->first],
true, it->second);
1155 TIMETRACK_INTER(ttc_cluster_objects_, ttc_visualization_)
1157 *clusters_ = *tmp_clusters;
1158 fclusters_->header.frame_id = input_->header.frame_id;
1159 pcl_utils::copy_time(input_, fclusters_);
1160 pcl_utils::copy_time(input_, ftable_model_);
1161 pcl_utils::copy_time(input_, fsimplified_polygon_);
1163 for (
unsigned int i = 0; i < f_obj_clusters_.size(); i++) {
1164 if (centroids_.count(i)) {
1165 *obj_clusters_[i] = *tmp_obj_clusters[i];
1168 obj_clusters_[i]->clear();
1173 pcl_utils::copy_time(input_, f_obj_clusters_[i]);
1176 #ifdef HAVE_VISUAL_DEBUGGING 1178 Eigen::Vector4f normal;
1179 normal[0] = coeff->values[0];
1180 normal[1] = coeff->values[1];
1181 normal[2] = coeff->values[2];
1185 hull_vertices.resize(cloud_hull_->points.size());
1186 for (
unsigned int i = 0; i < cloud_hull_->points.size(); ++i) {
1187 hull_vertices[i][0] = cloud_hull_->points[i].x;
1188 hull_vertices[i][1] = cloud_hull_->points[i].y;
1189 hull_vertices[i][2] = cloud_hull_->points[i].z;
1190 hull_vertices[i][3] = 0.;
1194 if (model_cloud_hull_ && ! model_cloud_hull_->points.empty()) {
1195 model_vertices.resize(model_cloud_hull_->points.size());
1196 for (
unsigned int i = 0; i < model_cloud_hull_->points.size(); ++i) {
1197 model_vertices[i][0] = model_cloud_hull_->points[i].x;
1198 model_vertices[i][1] = model_cloud_hull_->points[i].y;
1199 model_vertices[i][2] = model_cloud_hull_->points[i].z;
1200 model_vertices[i][3] = 0.;
1204 visthread_->visualize(input_->header.frame_id,
1205 table_centroid, normal, hull_vertices, model_vertices,
1206 good_hull_edges, centroids_, cylinder_params_, obj_shape_confidence_, best_obj_guess_);
1210 TIMETRACK_END(ttc_visualization_);
1211 TIMETRACK_END(ttc_full_loop_);
1213 #ifdef USE_TIMETRACKER 1214 if (++tt_loopcount_ >= 5) {
1216 tt_->print_to_stdout();
1221 std::vector<pcl::PointIndices>
1222 TabletopObjectsThread::extract_object_clusters(CloudConstPtr input) {
1223 TIMETRACK_START(ttc_obj_extraction_);
1224 std::vector<pcl::PointIndices> cluster_indices;
1225 if (input->empty()) {
1226 TIMETRACK_ABORT(ttc_obj_extraction_);
1227 return cluster_indices;
1231 pcl::search::KdTree<PointType>::Ptr
1232 kdtree_cl(
new pcl::search::KdTree<PointType>());
1233 kdtree_cl->setInputCloud(input);
1235 pcl::EuclideanClusterExtraction<PointType> ec;
1236 ec.setClusterTolerance(cfg_cluster_tolerance_);
1237 ec.setMinClusterSize(cfg_cluster_min_size_);
1238 ec.setMaxClusterSize(cfg_cluster_max_size_);
1239 ec.setSearchMethod(kdtree_cl);
1240 ec.setInputCloud(input);
1241 ec.extract(cluster_indices);
1244 TIMETRACK_END(ttc_obj_extraction_);
1245 return cluster_indices;
1248 int TabletopObjectsThread::next_id() {
1249 if (free_ids_.empty()) {
1253 int id = free_ids_.front();
1254 free_ids_.pop_front();
1259 TabletopObjectsThread::cluster_objects(CloudConstPtr input_cloud,
1260 ColorCloudPtr tmp_clusters,
1261 std::vector<ColorCloudPtr> &tmp_obj_clusters) {
1262 unsigned int object_count = 0;
1263 std::vector<pcl::PointIndices> cluster_indices = extract_object_clusters(input_cloud);
1264 std::vector<pcl::PointIndices>::const_iterator it;
1265 unsigned int num_points = 0;
1266 for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
1267 num_points += it->indices.size();
1269 CentroidMap tmp_centroids;
1271 if (num_points > 0) {
1272 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids(MAX_CENTROIDS);
1275 unsigned int centroid_i = 0;
1277 std::vector<double> init_likelihoods;
1278 init_likelihoods.resize(NUM_KNOWN_OBJS_ + 1, 0.0);
1279 for (uint i = 0; i < MAX_CENTROIDS; i++)
1280 obj_likelihoods_[i] = init_likelihoods;
1282 for (it = cluster_indices.begin();
1283 it != cluster_indices.end() && centroid_i < MAX_CENTROIDS;
1294 ColorCloudPtr single_cluster =
1295 colorize_cluster(input_cloud, it->indices, cluster_colors[centroid_i]);
1296 single_cluster->header.frame_id = input_cloud->header.frame_id;
1297 single_cluster->width = it->indices.size();
1298 single_cluster->height = 1;
1300 ColorCloudPtr obj_in_base_frame(
new ColorCloud());
1301 obj_in_base_frame->header.frame_id = cfg_base_frame_;
1302 obj_in_base_frame->width = it->indices.size();
1303 obj_in_base_frame->height = 1;
1304 obj_in_base_frame->points.resize(it->indices.size());
1309 pcl_utils::transform_pointcloud(cfg_base_frame_, *single_cluster,
1312 pcl::compute3DCentroid(*obj_in_base_frame, new_centroids[centroid_i]);
1314 if (cfg_cylinder_fitting_) {
1315 new_centroids[centroid_i] = fit_cylinder(obj_in_base_frame,
1316 new_centroids[centroid_i], centroid_i);
1321 object_count = centroid_i;
1322 new_centroids.resize(object_count);
1327 CentroidMap cylinder_params(cylinder_params_);
1328 std::map<uint, signed int> best_obj_guess(best_obj_guess_);
1329 std::map<uint, double> obj_shape_confidence(obj_shape_confidence_);
1330 std::map<uint, std::vector<double> > obj_likelihoods(obj_likelihoods_);
1331 cylinder_params_.clear();
1332 best_obj_guess_.clear();
1333 obj_shape_confidence_.clear();
1334 obj_likelihoods_.clear();
1336 std::map<uint, int> assigned_ids;
1337 if (cfg_track_objects_) {
1338 assigned_ids = track_objects(new_centroids);
1341 for (
unsigned int i = 0; i < new_centroids.size(); i++) {
1342 assigned_ids[i] = i;
1347 for (uint i = 0; i < new_centroids.size(); i++) {
1350 assigned_id = assigned_ids.at(i);
1352 catch (
const std::out_of_range& e) {
1357 if (assigned_id == -1)
1359 tmp_centroids[assigned_id] = new_centroids[i];
1360 cylinder_params_[assigned_id] = cylinder_params[i];
1361 obj_shape_confidence_[assigned_id] = obj_shape_confidence[i];
1362 best_obj_guess_[assigned_id] = best_obj_guess[i];
1363 obj_likelihoods_[assigned_id] = obj_likelihoods[i];
1364 ColorCloudPtr colorized_cluster = colorize_cluster(input_cloud,
1365 cluster_indices[i].indices,
1366 cluster_colors[assigned_id % MAX_CENTROIDS]);
1367 *tmp_clusters += *colorized_cluster;
1368 tmp_obj_clusters[assigned_id] = colorized_cluster;
1373 remove_high_centroids(table_centroid, tmp_centroids);
1375 if (object_count > 0)
1381 for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); it++) {
1382 old_centroids_.push_back(
OldCentroid(it->first, it->second));
1385 centroids_ = tmp_centroids;
1386 return object_count;
1389 TabletopObjectsThread::ColorCloudPtr TabletopObjectsThread::colorize_cluster (
1390 CloudConstPtr input_cloud,
1391 const std::vector<int> &cluster,
1392 const uint8_t color[]) {
1394 result->resize(cluster.size());
1395 result->header.frame_id = input_cloud->header.frame_id;
1397 for (std::vector<int>::const_iterator it = cluster.begin(); it != cluster.end(); ++it, ++i) {
1398 ColorPointType &p1 = result->points.at(i);
1399 const PointType &p2 = input_cloud->points.at(*it);
1411 TabletopObjectsThread::compute_bounding_box_scores(
1412 Eigen::Vector3f& cluster_dim,
1413 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >& scores)
1416 scores.resize(NUM_KNOWN_OBJS_);
1418 for (
int i = 0; i < NUM_KNOWN_OBJS_; i++) {
1419 scores[i][0] = compute_similarity(cluster_dim[0],
1420 known_obj_dimensions_[i][0]);
1421 scores[i][1] = compute_similarity(cluster_dim[1],
1422 known_obj_dimensions_[i][1]);
1423 scores[i][2] = compute_similarity(cluster_dim[2],
1424 known_obj_dimensions_[i][2]);
1430 TabletopObjectsThread::compute_similarity(
double d1,
double d2)
1432 return exp(-50.0 * ((d1 - d2) * (d1 - d2)));
1437 bool is_visible,
const Eigen::Vector4f ¢roid,
1438 const Eigen::Quaternionf &attitude)
1443 spose(tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()),
1444 tf::Vector3(centroid[0], centroid[1], centroid[2])),
1447 iface->
set_frame(cfg_result_frame_.c_str());
1454 if (visibility_history >= 0) {
1459 tf::Vector3 &origin = baserel_pose.getOrigin();
1460 tf::Quaternion quat = baserel_pose.getRotation();
1461 double translation[3] = { origin.x(), origin.y(), origin.z() };
1462 double rotation[4] = { quat.x(), quat.y(), quat.z(), quat.w() };
1467 if (visibility_history <= 0) {
1471 double translation[3] = { 0, 0, 0 };
1472 double rotation[4] = { 0, 0, 0, 1 };
1481 TabletopObjectsThread::CloudPtr
1482 TabletopObjectsThread::generate_table_model(
const float length,
const float width,
1483 const float thickness,
const float step,
1484 const float max_error)
1486 CloudPtr c(
new Cloud());
1488 const float length_2 = fabs(length) * 0.5;
1489 const float width_2 = fabs(width) * 0.5;
1490 const float thickness_2 = fabs(thickness) * 0.5;
1493 const unsigned int l_base_num = std::max(2u, (
unsigned int)floor(length / step));
1494 const unsigned int num_w = l_base_num +
1495 ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1496 const unsigned int w_base_num = std::max(2u, (
unsigned int)floor(width / step));
1497 const unsigned int num_h = w_base_num +
1498 ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1499 const unsigned int t_base_num = std::max(2u, (
unsigned int)floor(thickness / step));
1500 const unsigned int num_t = t_base_num +
1501 ((thickness < t_base_num * step) ? 0 : ((thickness - t_base_num * step) > max_error ? 2 : 1));
1508 c->width = num_t * num_w * num_h;
1510 c->points.resize(num_t * num_w * num_h);
1512 unsigned int idx = 0;
1513 for (
unsigned int t = 0; t < num_t; ++t) {
1514 for (
unsigned int w = 0; w < num_w; ++w) {
1515 for (
unsigned int h = 0; h < num_h; ++h) {
1516 PointType &p = c->points[idx++];
1518 p.x = h * step - width_2;
1519 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error) p.x = width_2;
1521 p.y = w * step - length_2;
1522 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error) p.y = length_2;
1524 p.z = t * step - thickness_2;
1525 if ((t == num_t - 1) && fabs(p.z - thickness_2) > max_error) p.z = thickness_2;
1533 TabletopObjectsThread::CloudPtr
1534 TabletopObjectsThread::generate_table_model(
const float length,
const float width,
1535 const float step,
const float max_error)
1537 CloudPtr c(
new Cloud());
1539 const float length_2 = fabs(length) * 0.5;
1540 const float width_2 = fabs(width) * 0.5;
1543 const unsigned int l_base_num = std::max(2u, (
unsigned int)floor(length / step));
1544 const unsigned int num_w = l_base_num +
1545 ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1546 const unsigned int w_base_num = std::max(2u, (
unsigned int)floor(width / step));
1547 const unsigned int num_h = w_base_num +
1548 ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1554 c->width = num_w * num_h;
1556 c->points.resize(num_w * num_h);
1558 unsigned int idx = 0;
1559 for (
unsigned int w = 0; w < num_w; ++w) {
1560 for (
unsigned int h = 0; h < num_h; ++h) {
1561 PointType &p = c->points[idx++];
1563 p.x = h * step - width_2;
1564 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error) p.x = width_2;
1566 p.y = w * step - length_2;
1567 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error) p.y = length_2;
1577 TabletopObjectsThread::CloudPtr
1578 TabletopObjectsThread::simplify_polygon(CloudPtr polygon,
float dist_threshold)
1580 const float sqr_dist_threshold = dist_threshold * dist_threshold;
1581 CloudPtr result(
new Cloud());
1582 const size_t psize = polygon->points.size();
1583 result->points.resize(psize);
1584 size_t res_points = 0;
1586 for (
size_t i = 1; i <= psize; ++i) {
1587 PointType &p1p = polygon->points[i - i_dist ];
1590 if (result->points.empty()) {
1596 PointType &p2p = polygon->points[i % psize];
1597 PointType &p3p = (i == psize) ? result->points[0] : polygon->points[(i + 1) % psize];
1599 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0);
1600 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0);
1601 Eigen::Vector4f p3(p3p.x, p3p.y, p3p.z, 0);
1603 Eigen::Vector4f line_dir = p3 - p1;
1605 double sqr_dist = pcl::sqrPointToLineDistance(p2, p1, line_dir);
1606 if (sqr_dist < sqr_dist_threshold) {
1610 result->points[res_points++] = p2p;
1614 result->header.frame_id = polygon->header.frame_id;
1615 result->header.stamp = polygon->header.stamp;
1616 result->width = res_points;
1618 result->is_dense =
false;
1619 result->points.resize(res_points);
1625 TabletopObjectsThread::convert_colored_input()
1627 converted_input_->header.seq = colored_input_->header.seq;
1628 converted_input_->header.frame_id = colored_input_->header.frame_id;
1629 converted_input_->header.stamp = colored_input_->header.stamp;
1630 converted_input_->width = colored_input_->width;
1631 converted_input_->height = colored_input_->height;
1632 converted_input_->is_dense = colored_input_->is_dense;
1634 const size_t size = colored_input_->points.size();
1635 converted_input_->points.resize(size);
1636 for (
size_t i = 0; i < size; ++i) {
1637 const ColorPointType &in = colored_input_->points[i];
1638 PointType &out = converted_input_->points[i];
1646 void TabletopObjectsThread::delete_old_centroids(OldCentroidVector centroids,
1654 if (centroid.getAge() > age) {
1655 free_ids_.push_back(centroid.getId());
1659 }), centroids.end());
1662 void TabletopObjectsThread::delete_near_centroids(CentroidMap reference,
1663 OldCentroidVector centroids,
float min_distance)
1670 for (CentroidMap::const_iterator it = reference.begin(); it != reference.end(); it++) {
1671 if (pcl::distances::l2(it->second, old.getCentroid()) < min_distance) {
1672 free_ids_.push_back(old.getId());
1677 }), centroids.end());
1681 TabletopObjectsThread::remove_high_centroids(Eigen::Vector4f table_centroid,
1682 CentroidMap centroids) {
1685 tf::Point(table_centroid[0], table_centroid[1], table_centroid[2]),
1689 for (CentroidMap::iterator it = centroids.begin(); it != centroids.end();) {
1692 tf::Point(it->second[0], it->second[1], it->second[2]),
1694 float d = sp_baserel_centroid.z() - sp_baserel_table.z();
1695 if (d > cfg_centroid_max_height_) {
1697 free_ids_.push_back(it->first);
1698 centroids.erase(it++);
1711 Eigen::Vector4f TabletopObjectsThread::fit_cylinder(
1712 ColorCloudConstPtr obj_in_base_frame, Eigen::Vector4f
const ¢roid,
1713 uint
const ¢roid_i)
1715 Eigen::Vector4f new_centroid(centroid);
1716 ColorPointType pnt_min, pnt_max;
1717 Eigen::Vector3f obj_dim;
1718 std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>
1720 pcl::getMinMax3D(*obj_in_base_frame, pnt_min, pnt_max);
1721 obj_dim[0] = fabs(pnt_max.x - pnt_min.x);
1722 obj_dim[1] = fabs(pnt_max.y - pnt_min.y);
1723 obj_dim[2] = fabs(pnt_max.z - pnt_min.z);
1724 compute_bounding_box_scores(obj_dim, obj_size_scores);
1727 obj_dim[1], obj_dim[2]);
1729 for (
int os = 0; os < NUM_KNOWN_OBJS_; os++) {
1731 obj_size_scores[os][0], obj_size_scores[os][1], obj_size_scores[os][2]);
1732 obj_likelihoods_[centroid_i][os] = obj_size_scores[os][0]
1733 * obj_size_scores[os][1] * obj_size_scores[os][2];
1739 pcl::NormalEstimation < ColorPointType, pcl::Normal > ne;
1740 pcl::SACSegmentationFromNormals < ColorPointType, pcl::Normal > seg;
1741 pcl::ExtractIndices < ColorPointType > extract;
1742 pcl::ExtractIndices < pcl::Normal > extract_normals;
1745 pcl::search::KdTree<ColorPointType>::Ptr tree_cyl(
1746 new pcl::search::KdTree<ColorPointType>());
1747 pcl::ModelCoefficients::Ptr coefficients_cylinder(
new pcl::ModelCoefficients);
1748 pcl::PointIndices::Ptr inliers_cylinder(
new pcl::PointIndices);
1751 ne.setSearchMethod(tree_cyl);
1752 ne.setInputCloud(obj_in_base_frame);
1754 ne.compute(*obj_normals);
1758 seg.setOptimizeCoefficients(
true);
1759 seg.setModelType(pcl::SACMODEL_CYLINDER);
1760 seg.setMethodType(pcl::SAC_RANSAC);
1761 seg.setNormalDistanceWeight(0.1);
1762 seg.setMaxIterations(1000);
1763 seg.setDistanceThreshold(0.07);
1764 seg.setRadiusLimits(0, 0.12);
1766 seg.setInputCloud(obj_in_base_frame);
1767 seg.setInputNormals(obj_normals);
1771 seg.segment(*inliers_cylinder, *coefficients_cylinder);
1774 extract.setInputCloud(obj_in_base_frame);
1775 extract.setIndices(inliers_cylinder);
1776 extract.setNegative(
false);
1779 extract.filter(*cloud_cylinder_baserel);
1781 cylinder_params_[centroid_i][0] = 0;
1782 cylinder_params_[centroid_i][1] = 0;
1783 if (cloud_cylinder_baserel->points.empty()) {
1785 obj_shape_confidence_[centroid_i] = 0.0;
1793 obj_shape_confidence_[centroid_i] = (double) (cloud_cylinder_baserel->points
1794 .size()) / (obj_in_base_frame->points.size() * 1.0);
1796 cloud_cylinder_baserel->points.size(), obj_in_base_frame->points.size(),
1797 obj_shape_confidence_[centroid_i]);
1799 ColorPointType pnt_min;
1800 ColorPointType pnt_max;
1801 pcl::getMinMax3D(*cloud_cylinder_baserel, pnt_min, pnt_max);
1802 if (cfg_verbose_cylinder_fitting_) {
1804 "Cylinder height according to cylinder inliers: %f",
1805 pnt_max.z - pnt_min.z);
1809 "Cylinder radius according to cylinder fitting: %f",
1810 (*coefficients_cylinder).values[6]);
1812 "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_) {
1833 obj_shape_confidence_[centroid_i]);
1835 for (
int os = 0; os < NUM_KNOWN_OBJS_; os++) {
1836 if (cfg_verbose_cylinder_fitting_) {
1839 obj_likelihoods_[centroid_i][os]);
1840 obj_likelihoods_[centroid_i][os] =
1841 (0.6 * obj_likelihoods_[centroid_i][os])
1842 + (0.4 * obj_shape_confidence_[centroid_i]);
1844 obj_likelihoods_[centroid_i][os]);
1846 if (obj_likelihoods_[centroid_i][os] > best_confidence) {
1847 best_confidence = obj_likelihoods_[centroid_i][os];
1848 detected_obj_id = os;
1851 if (cfg_verbose_cylinder_fitting_) {
1853 "********************Object Result********************");
1855 if (best_confidence > 0.6) {
1856 best_obj_guess_[centroid_i] = detected_obj_id;
1858 if (cfg_verbose_cylinder_fitting_) {
1860 "MATCH FOUND!! -------------------------> Cup number %i",
1865 best_obj_guess_[centroid_i] = -1;
1866 if (cfg_verbose_cylinder_fitting_) {
1870 if (cfg_verbose_cylinder_fitting_) {
1872 "*****************************************************");
1875 return new_centroid;
1886 std::map<unsigned int, int>
1887 TabletopObjectsThread::track_objects(
1888 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids)
1890 std::map<uint, int> final_assignment;
1893 for (
unsigned int i = 0; i < new_centroids.size(); i++) {
1894 final_assignment[i] = next_id();
1896 return final_assignment;
1899 TIMETRACK_START(ttc_hungarian_);
1900 hungarian_problem_t hp;
1902 std::vector<unsigned int> obj_ids(centroids_.size());
1906 hp.num_rows = new_centroids.size();
1907 hp.num_cols = centroids_.size();
1908 hp.cost = (
int**) calloc(hp.num_rows,
sizeof(
int*));
1909 for (
int i = 0; i < hp.num_rows; i++)
1910 hp.cost[i] = (
int*) calloc(hp.num_cols,
sizeof(
int));
1911 for (
int row = 0; row < hp.num_rows; row++) {
1912 unsigned int col = 0;
1913 for (CentroidMap::iterator col_it = centroids_.begin();
1914 col_it != centroids_.end(); col_it++, col++) {
1915 double distance = pcl::distances::l2(new_centroids[row],
1917 hp.cost[row][col] = (int) (distance * 1000);
1918 obj_ids[col] = col_it->first;
1922 solver.
init(hp.cost, hp.num_rows, hp.num_cols,
1923 HUNGARIAN_MODE_MINIMIZE_COST);
1926 int assignment_size;
1929 for (
int row = 0; row < assignment_size; row++) {
1930 if (row >= hp.num_rows) {
1931 id = obj_ids.at(assignment[row]);
1932 old_centroids_.push_back(
OldCentroid(
id, centroids_.at(
id)));
1935 else if (assignment[row] >= hp.num_cols) {
1936 bool assigned =
false;
1938 for (OldCentroidVector::iterator it = old_centroids_.begin();
1939 it != old_centroids_.end(); it++) {
1940 if (pcl::distances::l2(new_centroids[row], it->getCentroid())
1941 <= cfg_centroid_max_distance_) {
1943 old_centroids_.erase(it);
1954 id = obj_ids[assignment[row]];
1958 if (pcl::distances::l2(centroids_[
id], new_centroids[row])
1959 > cfg_centroid_max_distance_) {
1961 old_centroids_.push_back(
OldCentroid(
id, centroids_[
id]));
1965 final_assignment[row] = id;
1967 return final_assignment;
1971 #ifdef HAVE_VISUAL_DEBUGGING 1975 visthread_ = visthread;
void set_frame(const char *new_frame)
Set frame value.
Compare points' distance to a plane.
bool msgq_empty()
Check if queue is empty.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
void remove_pointcloud(const char *id)
Remove the point cloud.
void solve()
Solve the assignment problem.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > V_Vector4f
Aligned vector of vectors/points.
A class for handling time.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Check if point is inside or outside a given polygon.
void write()
Write from local copy into BlackBoard memory.
Base class for virtualization thread.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Logger * logger
This is the Logger member used to access the logger.
virtual ~TabletopObjectsThread()
Destructor.
Clock * clock
By means of this member access to the clock is given.
void reset()
Reset pointer.
void msgq_pop()
Erase first message from queue.
boost::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Hungarian method assignment solver.
Position3DInterface Fawkes BlackBoard Interface.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
void set_enabled(const bool new_enabled)
Set enabled value.
TabletopObjectsThread()
Constructor.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
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.
bool is_enabled() const
Get enabled value.
Wrapper class to add time stamp and frame ID to base types.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
RefPtr<> is a reference-counting shared smartpointer.
int init(int **cost_matrix, int rows, int cols, int mode)
Initialize hungarian method.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void init()
Initialize the thread.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
This class is used to save old centroids in order to check for reappearance.
int * get_assignment(int &size)
Get assignment and size.
virtual void loop()
Code to execute in the thread.
int32_t visibility_history() const
Get visibility_history value.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
bool exists_pointcloud(const char *id)
Check if point cloud exists.
virtual void close(Interface *interface)=0
Close interface.
boost::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.