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>
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};
645LaserClusterThread::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();
virtual ~LaserClusterThread()
Destructor.
LaserClusterThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_writing_f(const char *interface_type, const char *identifier,...)
Open interface for writing with identifier format string.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
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.
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.
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
SetMaxXMessage Fawkes BlackBoard Interface Message.
SetSelectionModeMessage Fawkes BlackBoard Interface Message.
LaserClusterInterface Fawkes BlackBoard Interface.
void set_selection_mode(const SelectionMode new_selection_mode)
Set selection_mode value.
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.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add 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.
double * translation() const
Get translation value.
void set_frame(const char *new_frame)
Set frame value.
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.
void set_name(const char *format,...)
Set name of thread.
A class for handling time.
Wrapper class to add time stamp and frame ID to base types.
This class tries to translate the found plan to interpreteable data for the rest of the program.
Fawkes library namespace.