22 #include "laser-cluster-thread.h"
24 #include "cluster_colors.h"
26 #include <baseapp/run.h>
27 #include <pcl_utils/comparisons.h>
28 #include <pcl_utils/utils.h>
29 #include <utils/math/angle.h>
30 #include <utils/time/wait.h>
31 #ifdef USE_TIMETRACKER
32 # include <utils/time/tracker.h>
34 #include <interfaces/LaserClusterInterface.h>
35 #include <interfaces/Position3DInterface.h>
36 #include <interfaces/SwitchInterface.h>
37 #include <pcl/common/centroid.h>
38 #include <pcl/common/distances.h>
39 #include <pcl/common/transforms.h>
40 #include <pcl/filters/conditional_removal.h>
41 #include <pcl/filters/extract_indices.h>
42 #include <pcl/filters/passthrough.h>
43 #include <pcl/filters/project_inliers.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_types.h>
46 #include <pcl/sample_consensus/method_types.h>
47 #include <pcl/sample_consensus/model_types.h>
48 #include <pcl/search/kdtree.h>
49 #include <pcl/segmentation/extract_clusters.h>
50 #include <pcl/surface/convex_hull.h>
51 #include <utils/time/tracker_macros.h>
71 :
Thread(
"LaserClusterThread",
Thread::OPMODE_WAITFORWAKEUP),
75 set_name(
"LaserClusterThread(%s)", cfg_name.c_str());
77 cfg_prefix_ = cfg_prefix;
88 cfg_line_removal_ =
config->
get_bool(cfg_prefix_ +
"line_removal/enable");
89 if (cfg_line_removal_) {
90 cfg_segm_max_iterations_ =
91 config->
get_uint(cfg_prefix_ +
"line_removal/segmentation_max_iterations");
92 cfg_segm_distance_threshold_ =
93 config->
get_float(cfg_prefix_ +
"line_removal/segmentation_distance_threshold");
94 cfg_segm_min_inliers_ =
config->
get_uint(cfg_prefix_ +
"line_removal/segmentation_min_inliers");
95 cfg_segm_sample_max_dist_ =
96 config->
get_float(cfg_prefix_ +
"line_removal/segmentation_sample_max_dist");
97 cfg_line_min_length_ =
config->
get_float(cfg_prefix_ +
"line_removal/min_length");
99 cfg_switch_tolerance_ =
config->
get_float(cfg_prefix_ +
"switch_tolerance");
100 cfg_cluster_tolerance_ =
config->
get_float(cfg_prefix_ +
"clustering/tolerance");
101 cfg_cluster_min_size_ =
config->
get_uint(cfg_prefix_ +
"clustering/min_size");
102 cfg_cluster_max_size_ =
config->
get_uint(cfg_prefix_ +
"clustering/max_size");
106 cfg_use_bbox_ =
false;
107 cfg_bbox_min_x_ = 0.0;
108 cfg_bbox_max_x_ = 0.0;
110 cfg_bbox_min_x_ =
config->
get_float(cfg_prefix_ +
"bounding-box/min_x");
111 cfg_bbox_max_x_ =
config->
get_float(cfg_prefix_ +
"bounding-box/max_x");
112 cfg_bbox_min_y_ =
config->
get_float(cfg_prefix_ +
"bounding-box/min_y");
113 cfg_bbox_max_y_ =
config->
get_float(cfg_prefix_ +
"bounding-box/max_y");
114 cfg_use_bbox_ =
true;
120 cfg_max_num_clusters_ =
config->
get_uint(cfg_prefix_ +
"max_num_clusters");
122 cfg_selection_mode_ = SELECT_MIN_ANGLE;
124 std::string selmode =
config->
get_string(cfg_prefix_ +
"cluster_selection_mode");
125 if (selmode ==
"min-angle") {
126 cfg_selection_mode_ = SELECT_MIN_ANGLE;
127 }
else if (selmode ==
"min-dist") {
128 cfg_selection_mode_ = SELECT_MIN_DIST;
135 current_max_x_ = cfg_bbox_max_x_;
138 input_ = pcl_utils::cloudptr_from_refptr(finput_);
141 double rotation[4] = {0., 0., 0., 1.};
142 cluster_pos_ifs_.resize(cfg_max_num_clusters_, NULL);
143 for (
unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
144 cluster_pos_ifs_[i] =
148 cluster_pos_ifs_[i]->set_rotation(rotation);
149 cluster_pos_ifs_[i]->write();
160 config_if_->set_max_x(current_max_x_);
161 if (cfg_selection_mode_ == SELECT_MIN_DIST) {
162 config_if_->set_selection_mode(LaserClusterInterface::SELMODE_MIN_DIST);
164 config_if_->set_selection_mode(LaserClusterInterface::SELMODE_MIN_ANGLE);
168 bool autostart =
true;
176 for (
size_t i = 0; i < cluster_pos_ifs_.size(); ++i) {
185 fclusters_->header.frame_id = finput_->header.frame_id;
186 fclusters_->is_dense =
false;
187 char *output_cluster_name;
188 if (asprintf(&output_cluster_name,
"/laser-cluster/%s", cfg_name_.c_str()) != -1) {
189 output_cluster_name_ = output_cluster_name;
190 free(output_cluster_name);
193 clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_);
196 fclusters_labeled_->header.frame_id = finput_->header.frame_id;
197 fclusters_labeled_->is_dense =
false;
198 char *output_cluster_labeled_name;
199 if (asprintf(&output_cluster_labeled_name,
"/laser-cluster/%s-labeled", cfg_name_.c_str())
201 output_cluster_labeled_name_ = output_cluster_labeled_name;
202 free(output_cluster_labeled_name);
206 clusters_labeled_ = pcl_utils::cloudptr_from_refptr(fclusters_labeled_);
208 seg_.setOptimizeCoefficients(
true);
209 seg_.setModelType(pcl::SACMODEL_LINE);
210 seg_.setMethodType(pcl::SAC_RANSAC);
211 seg_.setMaxIterations(cfg_segm_max_iterations_);
212 seg_.setDistanceThreshold(cfg_segm_distance_threshold_);
216 #ifdef USE_TIMETRACKER
219 ttc_full_loop_ = tt_->add_class(
"Full Loop");
220 ttc_msgproc_ = tt_->add_class(
"Message Processing");
221 ttc_extract_lines_ = tt_->add_class(
"Line Extraction");
222 ttc_clustering_ = tt_->add_class(
"Clustering");
231 clusters_labeled_.reset();
235 for (
size_t i = 0; i < cluster_pos_ifs_.size(); ++i) {
243 fclusters_labeled_.
reset();
249 TIMETRACK_START(ttc_full_loop_);
253 TIMETRACK_START(ttc_msgproc_);
269 if (msg->max_x() < 0.0) {
271 "Got cluster max x less than zero, setting config default %f",
273 current_max_x_ = cfg_bbox_max_x_;
275 current_max_x_ = msg->max_x();
281 if (msg->selection_mode() == LaserClusterInterface::SELMODE_MIN_DIST) {
282 cfg_selection_mode_ = SELECT_MIN_DIST;
283 }
else if (msg->selection_mode() == LaserClusterInterface::SELMODE_MIN_ANGLE) {
284 cfg_selection_mode_ = SELECT_MIN_ANGLE;
297 for (
unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
298 set_position(cluster_pos_ifs_[i],
false);
303 TIMETRACK_INTER(ttc_msgproc_, ttc_extract_lines_);
305 if (input_->points.size() <= 10) {
314 CloudPtr noline_cloud(
new Cloud());
317 pcl::PassThrough<PointType> passthrough;
318 if (current_max_x_ > 0.) {
319 passthrough.setFilterFieldName(
"x");
320 passthrough.setFilterLimits(cfg_bbox_min_x_, current_max_x_);
322 passthrough.setInputCloud(input_);
323 passthrough.filter(*noline_cloud);
328 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
329 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
331 if (cfg_line_removal_) {
332 std::list<CloudPtr> restore_pcls;
334 while (noline_cloud->points.size() > cfg_cluster_min_size_) {
339 pcl::search::KdTree<PointType>::Ptr
search(
new pcl::search::KdTree<PointType>);
340 search->setInputCloud(noline_cloud);
341 seg_.setSamplesMaxDist(cfg_segm_sample_max_dist_,
search);
342 seg_.setInputCloud(noline_cloud);
343 seg_.segment(*inliers, *coeff);
344 if (inliers->indices.size() == 0) {
350 if ((
double)inliers->indices.size() < cfg_segm_min_inliers_) {
356 float length = calc_line_length(noline_cloud, inliers, coeff);
358 if (length < cfg_line_min_length_) {
362 CloudPtr cloud_line(
new Cloud());
363 pcl::ExtractIndices<PointType> extract;
364 extract.setInputCloud(noline_cloud);
365 extract.setIndices(inliers);
366 extract.setNegative(
false);
367 extract.filter(*cloud_line);
368 restore_pcls.push_back(cloud_line);
372 CloudPtr cloud_f(
new Cloud());
373 pcl::ExtractIndices<PointType> extract;
374 extract.setInputCloud(noline_cloud);
375 extract.setIndices(inliers);
376 extract.setNegative(
true);
377 extract.filter(*cloud_f);
378 *noline_cloud = *cloud_f;
381 for (CloudPtr cloud : restore_pcls) {
382 *noline_cloud += *cloud;
387 CloudPtr tmp_cloud(
new Cloud());
389 pcl::PassThrough<PointType> passthrough;
390 passthrough.setInputCloud(noline_cloud);
391 passthrough.filter(*tmp_cloud);
393 if (noline_cloud->points.size() != tmp_cloud->points.size()) {
396 *noline_cloud = *tmp_cloud;
402 TIMETRACK_INTER(ttc_extract_lines_, ttc_clustering_);
404 clusters_->points.resize(noline_cloud->points.size());
405 clusters_->height = 1;
406 clusters_->width = noline_cloud->points.size();
408 clusters_labeled_->points.resize(noline_cloud->points.size());
409 clusters_labeled_->height = 1;
410 clusters_labeled_->width = noline_cloud->points.size();
413 for (
size_t p = 0; p < clusters_->points.size(); ++p) {
414 ColorPointType &out_point = clusters_->points[p];
415 LabelPointType &out_lab_point = clusters_labeled_->points[p];
416 PointType & in_point = noline_cloud->points[p];
417 out_point.x = out_lab_point.x = in_point.x;
418 out_point.y = out_lab_point.y = in_point.y;
419 out_point.z = out_lab_point.z = in_point.z;
420 out_point.r = out_point.g = out_point.b = 1.0;
421 out_lab_point.label = 0;
427 std::vector<pcl::PointIndices> cluster_indices;
428 if (noline_cloud->points.size() > 0) {
430 pcl::search::KdTree<PointType>::Ptr kdtree_cl(
new pcl::search::KdTree<PointType>());
431 kdtree_cl->setInputCloud(noline_cloud);
433 pcl::EuclideanClusterExtraction<PointType> ec;
434 ec.setClusterTolerance(cfg_cluster_tolerance_);
435 ec.setMinClusterSize(cfg_cluster_min_size_);
436 ec.setMaxClusterSize(cfg_cluster_max_size_);
437 ec.setSearchMethod(kdtree_cl);
438 ec.setInputCloud(noline_cloud);
439 ec.extract(cluster_indices);
444 for (
auto cluster : cluster_indices) {
453 for (
auto ci : cluster.indices) {
454 ColorPointType &out_point = clusters_->points[ci];
455 out_point.r = ignored_cluster_color[0];
456 out_point.g = ignored_cluster_color[1];
458 out_point.b = ignored_cluster_color[2];
467 if (!cluster_indices.empty()) {
468 std::vector<ClusterInfo> cinfos;
470 for (
unsigned int i = 0; i < cluster_indices.size(); ++i) {
471 Eigen::Vector4f centroid;
472 pcl::compute3DCentroid(*noline_cloud, cluster_indices[i].indices, centroid);
474 || ((centroid.x() >= cfg_bbox_min_x_) && (centroid.x() <= cfg_bbox_max_x_)
475 && (centroid.y() >= cfg_bbox_min_y_) && (centroid.y() <= cfg_bbox_max_y_))) {
477 info.angle = std::atan2(centroid.y(), centroid.x());
478 info.dist = centroid.norm();
480 info.centroid = centroid;
481 cinfos.push_back(info);
493 if (!cinfos.empty()) {
494 if (cfg_selection_mode_ == SELECT_MIN_ANGLE) {
495 std::sort(cinfos.begin(),
497 [](
const ClusterInfo &a,
const ClusterInfo &b) ->
bool {
498 return a.angle < b.angle;
500 }
else if (cfg_selection_mode_ == SELECT_MIN_DIST) {
501 std::sort(cinfos.begin(),
503 [](
const ClusterInfo &a,
const ClusterInfo &b) ->
bool {
504 return a.dist < b.dist;
511 for (i = 0; i < std::min(cinfos.size(), (size_t)cfg_max_num_clusters_); ++i) {
513 for (
auto ci : cluster_indices[cinfos[i].index].indices) {
514 ColorPointType &out_point = clusters_->points[ci];
515 LabelPointType &out_lab_point = clusters_labeled_->points[ci];
516 out_point.r = cluster_colors[i][0];
517 out_point.g = cluster_colors[i][1];
519 out_point.b = cluster_colors[i][2];
521 out_lab_point.label = i;
524 set_position(cluster_pos_ifs_[i],
true, cinfos[i].centroid);
526 for (
unsigned int j = i; j < cfg_max_num_clusters_; ++j) {
527 set_position(cluster_pos_ifs_[j],
false);
532 for (
unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
533 set_position(cluster_pos_ifs_[i],
false);
539 for (
unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
540 set_position(cluster_pos_ifs_[i],
false);
545 if (finput_->header.frame_id ==
"") {
548 fclusters_->header.frame_id = finput_->header.frame_id;
549 pcl_utils::copy_time(finput_, fclusters_);
550 fclusters_labeled_->header.frame_id = finput_->header.frame_id;
551 pcl_utils::copy_time(finput_, fclusters_labeled_);
553 TIMETRACK_END(ttc_clustering_);
554 TIMETRACK_END(ttc_full_loop_);
556 #ifdef USE_TIMETRACKER
557 if (++tt_loopcount_ >= 5) {
559 tt_->print_to_stdout();
567 const Eigen::Vector4f & centroid,
568 const Eigen::Quaternionf & attitude)
572 if (input_->header.frame_id.empty()) {
581 tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()),
582 tf::Vector3(centroid[0], centroid[1], centroid[2])),
584 input_->header.frame_id);
586 iface->
set_frame(cfg_result_frame_.c_str());
598 tf::Vector3 & origin = baserel_pose.getOrigin();
599 tf::Quaternion quat = baserel_pose.getRotation();
601 Eigen::Vector4f baserel_centroid;
602 baserel_centroid[0] = origin.x();
603 baserel_centroid[1] = origin.y();
604 baserel_centroid[2] = origin.z();
605 baserel_centroid[3] = 0.;
609 Eigen::Vector4f last_centroid(iface->
translation(0) - cfg_offset_x_,
613 bool different_cluster =
614 fabs((last_centroid - baserel_centroid).norm()) > cfg_switch_tolerance_;
616 if (!different_cluster && visibility_history >= 0) {
623 double translation[3] = {origin.x() + cfg_offset_x_,
624 origin.y() + cfg_offset_y_,
625 origin.z() + cfg_offset_z_};
626 double rotation[4] = {quat.x(), quat.y(), quat.z(), quat.w()};
631 if (visibility_history <= 0) {
635 double translation[3] = {0, 0, 0};
636 double rotation[4] = {0, 0, 0, 1};
645 LaserClusterThread::calc_line_length(CloudPtr cloud,
646 pcl::PointIndices::Ptr inliers,
647 pcl::ModelCoefficients::Ptr coeff)
649 if (inliers->indices.size() < 2)
652 CloudPtr cloud_line(
new Cloud());
653 CloudPtr cloud_line_proj(
new Cloud());
655 pcl::ExtractIndices<PointType> extract;
656 extract.setInputCloud(cloud);
657 extract.setIndices(inliers);
658 extract.setNegative(
false);
659 extract.filter(*cloud_line);
662 pcl::ProjectInliers<PointType> proj;
663 proj.setModelType(pcl::SACMODEL_LINE);
664 proj.setInputCloud(cloud_line);
665 proj.setModelCoefficients(coeff);
666 proj.filter(*cloud_line_proj);
668 Eigen::Vector3f point_on_line, line_dir;
669 point_on_line[0] = cloud_line_proj->points[0].x;
670 point_on_line[1] = cloud_line_proj->points[0].y;
671 point_on_line[2] = cloud_line_proj->points[0].z;
672 line_dir[0] = coeff->values[3];
673 line_dir[1] = coeff->values[4];
674 line_dir[2] = coeff->values[5];
676 ssize_t idx_1 = 0, idx_2 = 0;
677 float max_dist_1 = 0.f, max_dist_2 = 0.f;
679 for (
size_t i = 1; i < cloud_line_proj->points.size(); ++i) {
680 const PointType &pt = cloud_line_proj->points[i];
681 Eigen::Vector3f ptv(pt.x, pt.y, pt.z);
682 Eigen::Vector3f diff(ptv - point_on_line);
683 float dist = diff.norm();
684 float dir = line_dir.dot(diff);
686 if (dist > max_dist_1) {
692 if (dist > max_dist_2) {
699 if (idx_1 >= 0 && idx_2 >= 0) {
700 const PointType &pt_1 = cloud_line_proj->points[idx_1];
701 const PointType &pt_2 = cloud_line_proj->points[idx_2];
703 Eigen::Vector3f ptv_1(pt_1.x, pt_1.y, pt_1.z);
704 Eigen::Vector3f ptv_2(pt_2.x, pt_2.y, pt_2.z);
706 return (ptv_1 - ptv_2).norm();