22 #include "laser-cluster-thread.h" 23 #include "cluster_colors.h" 25 #include <pcl_utils/utils.h> 26 #include <pcl_utils/comparisons.h> 27 #include <utils/time/wait.h> 28 #include <utils/math/angle.h> 29 #include <baseapp/run.h> 30 #ifdef USE_TIMETRACKER 31 # include <utils/time/tracker.h> 33 #include <utils/time/tracker_macros.h> 35 #include <pcl/point_cloud.h> 36 #include <pcl/point_types.h> 37 #include <pcl/sample_consensus/method_types.h> 38 #include <pcl/sample_consensus/model_types.h> 39 #include <pcl/segmentation/extract_clusters.h> 40 #include <pcl/filters/extract_indices.h> 41 #include <pcl/surface/convex_hull.h> 42 #include <pcl/search/kdtree.h> 43 #include <pcl/filters/passthrough.h> 44 #include <pcl/filters/project_inliers.h> 45 #include <pcl/filters/conditional_removal.h> 46 #include <pcl/common/centroid.h> 47 #include <pcl/common/transforms.h> 48 #include <pcl/common/distances.h> 50 #include <interfaces/Position3DInterface.h> 51 #include <interfaces/SwitchInterface.h> 52 #include <interfaces/LaserClusterInterface.h> 73 :
Thread(
"LaserClusterThread",
Thread::OPMODE_WAITFORWAKEUP),
77 set_name(
"LaserClusterThread(%s)", cfg_name.c_str());
79 cfg_prefix_ = cfg_prefix;
92 cfg_line_removal_ =
config->
get_bool(cfg_prefix_+
"line_removal/enable");
93 if (cfg_line_removal_) {
94 cfg_segm_max_iterations_ =
95 config->
get_uint(cfg_prefix_+
"line_removal/segmentation_max_iterations");
96 cfg_segm_distance_threshold_ =
97 config->
get_float(cfg_prefix_+
"line_removal/segmentation_distance_threshold");
98 cfg_segm_min_inliers_ =
99 config->
get_uint(cfg_prefix_+
"line_removal/segmentation_min_inliers");
100 cfg_segm_sample_max_dist_ =
101 config->
get_float(cfg_prefix_+
"line_removal/segmentation_sample_max_dist");
102 cfg_line_min_length_ =
105 cfg_switch_tolerance_ =
config->
get_float(cfg_prefix_+
"switch_tolerance");
106 cfg_cluster_tolerance_ =
config->
get_float(cfg_prefix_+
"clustering/tolerance");
107 cfg_cluster_min_size_ =
config->
get_uint(cfg_prefix_+
"clustering/min_size");
108 cfg_cluster_max_size_ =
config->
get_uint(cfg_prefix_+
"clustering/max_size");
112 cfg_use_bbox_ =
false;
113 cfg_bbox_min_x_ = 0.0;
114 cfg_bbox_max_x_ = 0.0;
120 cfg_use_bbox_ =
true;
125 cfg_max_num_clusters_ =
config->
get_uint(cfg_prefix_+
"max_num_clusters");
127 cfg_selection_mode_ = SELECT_MIN_ANGLE;
129 std::string selmode =
config->
get_string(cfg_prefix_+
"cluster_selection_mode");
130 if (selmode ==
"min-angle") {
131 cfg_selection_mode_ = SELECT_MIN_ANGLE;
132 }
else if (selmode ==
"min-dist") {
133 cfg_selection_mode_ = SELECT_MIN_DIST;
139 current_max_x_ = cfg_bbox_max_x_;
142 input_ = pcl_utils::cloudptr_from_refptr(finput_);
145 double rotation[4] = {0., 0., 0., 1.};
146 cluster_pos_ifs_.resize(cfg_max_num_clusters_, NULL);
147 for (
unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
148 cluster_pos_ifs_[i] =
150 cfg_name_.c_str(), i + 1);
151 cluster_pos_ifs_[i]->set_rotation(rotation);
152 cluster_pos_ifs_[i]->write();
163 config_if_->set_max_x(current_max_x_);
164 if (cfg_selection_mode_ == SELECT_MIN_DIST) {
165 config_if_->set_selection_mode(LaserClusterInterface::SELMODE_MIN_DIST);
167 config_if_->set_selection_mode(LaserClusterInterface::SELMODE_MIN_ANGLE);
171 bool autostart =
true;
178 for (
size_t i = 0; i < cluster_pos_ifs_.size(); ++i) {
187 fclusters_->header.frame_id = finput_->header.frame_id;
188 fclusters_->is_dense =
false;
189 char *output_cluster_name;
190 if (asprintf(&output_cluster_name,
"/laser-cluster/%s", cfg_name_.c_str()) != -1) {
191 output_cluster_name_ = output_cluster_name;
192 free(output_cluster_name);
195 clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_);
198 fclusters_labeled_->header.frame_id = finput_->header.frame_id;
199 fclusters_labeled_->is_dense =
false;
200 char *output_cluster_labeled_name;
201 if (asprintf(&output_cluster_labeled_name,
202 "/laser-cluster/%s-labeled", cfg_name_.c_str()) != -1) {
203 output_cluster_labeled_name_ = output_cluster_labeled_name;
204 free(output_cluster_labeled_name);
208 clusters_labeled_ = pcl_utils::cloudptr_from_refptr(fclusters_labeled_);
210 seg_.setOptimizeCoefficients(
true);
211 seg_.setModelType(pcl::SACMODEL_LINE);
212 seg_.setMethodType(pcl::SAC_RANSAC);
213 seg_.setMaxIterations(cfg_segm_max_iterations_);
214 seg_.setDistanceThreshold(cfg_segm_distance_threshold_);
218 #ifdef USE_TIMETRACKER 221 ttc_full_loop_ = tt_->add_class(
"Full Loop");
222 ttc_msgproc_ = tt_->add_class(
"Message Processing");
223 ttc_extract_lines_ = tt_->add_class(
"Line Extraction");
224 ttc_clustering_ = tt_->add_class(
"Clustering");
234 clusters_labeled_.reset();
238 for (
size_t i = 0; i < cluster_pos_ifs_.size(); ++i) {
246 fclusters_labeled_.
reset();
252 TIMETRACK_START(ttc_full_loop_);
256 TIMETRACK_START(ttc_msgproc_);
277 if (msg->max_x() < 0.0) {
278 logger->
log_info(
name(),
"Got cluster max x less than zero, setting config default %f",
280 current_max_x_ = cfg_bbox_max_x_;
282 current_max_x_ = msg->max_x();
289 if (msg->selection_mode() == LaserClusterInterface::SELMODE_MIN_DIST) {
290 cfg_selection_mode_ = SELECT_MIN_DIST;
291 }
else if (msg->selection_mode() == LaserClusterInterface::SELMODE_MIN_ANGLE) {
292 cfg_selection_mode_ = SELECT_MIN_ANGLE;
305 for (
unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
306 set_position(cluster_pos_ifs_[i],
false);
311 TIMETRACK_INTER(ttc_msgproc_, ttc_extract_lines_);
314 if (input_->points.size() <= 10) {
323 CloudPtr noline_cloud(
new Cloud());
326 pcl::PassThrough<PointType> passthrough;
327 if (current_max_x_ > 0.) {
328 passthrough.setFilterFieldName(
"x");
329 passthrough.setFilterLimits(cfg_bbox_min_x_, current_max_x_);
331 passthrough.setInputCloud(input_);
332 passthrough.filter(*noline_cloud);
337 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
338 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
340 if (cfg_line_removal_) {
341 std::list<CloudPtr> restore_pcls;
343 while (noline_cloud->points.size () > cfg_cluster_min_size_) {
348 pcl::search::KdTree<PointType>::Ptr
349 search(
new pcl::search::KdTree<PointType>);
350 search->setInputCloud(noline_cloud);
351 seg_.setSamplesMaxDist(cfg_segm_sample_max_dist_, search);
352 seg_.setInputCloud(noline_cloud);
353 seg_.segment(*inliers, *coeff);
354 if (inliers->indices.size () == 0) {
360 if ((
double)inliers->indices.size() < cfg_segm_min_inliers_) {
366 float length = calc_line_length(noline_cloud, inliers, coeff);
368 if (length < cfg_line_min_length_) {
372 CloudPtr cloud_line(
new Cloud());
373 pcl::ExtractIndices<PointType> extract;
374 extract.setInputCloud(noline_cloud);
375 extract.setIndices(inliers);
376 extract.setNegative(
false);
377 extract.filter(*cloud_line);
378 restore_pcls.push_back(cloud_line);
383 CloudPtr cloud_f(
new Cloud());
384 pcl::ExtractIndices<PointType> extract;
385 extract.setInputCloud(noline_cloud);
386 extract.setIndices(inliers);
387 extract.setNegative(
true);
388 extract.filter(*cloud_f);
389 *noline_cloud = *cloud_f;
392 for (CloudPtr cloud : restore_pcls) {
393 *noline_cloud += *cloud;
398 CloudPtr tmp_cloud(
new Cloud());
400 pcl::PassThrough<PointType> passthrough;
401 passthrough.setInputCloud(noline_cloud);
402 passthrough.filter(*tmp_cloud);
404 if (noline_cloud->points.size() != tmp_cloud->points.size()) {
407 *noline_cloud = *tmp_cloud;
413 TIMETRACK_INTER(ttc_extract_lines_, ttc_clustering_);
415 clusters_->points.resize(noline_cloud->points.size());
416 clusters_->height = 1;
417 clusters_->width = noline_cloud->points.size();
419 clusters_labeled_->points.resize(noline_cloud->points.size());
420 clusters_labeled_->height = 1;
421 clusters_labeled_->width = noline_cloud->points.size();
424 for (
size_t p = 0; p < clusters_->points.size(); ++p) {
425 ColorPointType &out_point = clusters_->points[p];
426 LabelPointType &out_lab_point = clusters_labeled_->points[p];
427 PointType &in_point = noline_cloud->points[p];
428 out_point.x = out_lab_point.x = in_point.x;
429 out_point.y = out_lab_point.y = in_point.y;
430 out_point.z = out_lab_point.z = in_point.z;
431 out_point.r = out_point.g = out_point.b = 1.0;
432 out_lab_point.label = 0;
438 std::vector<pcl::PointIndices> cluster_indices;
439 if (noline_cloud->points.size() > 0) {
441 pcl::search::KdTree<PointType>::Ptr
442 kdtree_cl(
new pcl::search::KdTree<PointType>());
443 kdtree_cl->setInputCloud(noline_cloud);
445 pcl::EuclideanClusterExtraction<PointType> ec;
446 ec.setClusterTolerance(cfg_cluster_tolerance_);
447 ec.setMinClusterSize(cfg_cluster_min_size_);
448 ec.setMaxClusterSize(cfg_cluster_max_size_);
449 ec.setSearchMethod(kdtree_cl);
450 ec.setInputCloud(noline_cloud);
451 ec.extract(cluster_indices);
456 for (
auto cluster : cluster_indices) {
464 for (
auto ci : cluster.indices) {
465 ColorPointType &out_point = clusters_->points[ci];
466 out_point.r = ignored_cluster_color[0];
467 out_point.g = ignored_cluster_color[1];;
468 out_point.b = ignored_cluster_color[2];;
479 if (! cluster_indices.empty()) {
480 std::vector<ClusterInfo> cinfos;
482 for (
unsigned int i = 0; i < cluster_indices.size(); ++i) {
483 Eigen::Vector4f centroid;
484 pcl::compute3DCentroid(*noline_cloud, cluster_indices[i].indices, centroid);
485 if ( !cfg_use_bbox_ ||
486 ((centroid.x() >= cfg_bbox_min_x_) && (centroid.x() <= cfg_bbox_max_x_) &&
487 (centroid.y() >= cfg_bbox_min_y_) && (centroid.y() <= cfg_bbox_max_y_)))
490 info.angle = std::atan2(centroid.y(), centroid.x());
491 info.dist = centroid.norm();
493 info.centroid = centroid;
494 cinfos.push_back(info);
506 if (! cinfos.empty()) {
507 if (cfg_selection_mode_ == SELECT_MIN_ANGLE) {
508 std::sort(cinfos.begin(), cinfos.end(),
509 [](
const ClusterInfo & a,
const ClusterInfo & b) ->
bool 511 return a.angle < b.angle;
513 }
else if (cfg_selection_mode_ == SELECT_MIN_DIST) {
514 std::sort(cinfos.begin(), cinfos.end(),
515 [](
const ClusterInfo & a,
const ClusterInfo & b) ->
bool 517 return a.dist < b.dist;
525 for (i = 0; i < std::min(cinfos.size(), (size_t)cfg_max_num_clusters_); ++i) {
527 for (
auto ci : cluster_indices[cinfos[i].index].indices) {
528 ColorPointType &out_point = clusters_->points[ci];
529 LabelPointType &out_lab_point = clusters_labeled_->points[ci];
530 out_point.r = cluster_colors[i][0];
531 out_point.g = cluster_colors[i][1];;
532 out_point.b = cluster_colors[i][2];;
533 out_lab_point.label = i;
536 set_position(cluster_pos_ifs_[i],
true, cinfos[i].centroid);
538 for (
unsigned int j = i; j < cfg_max_num_clusters_; ++j) {
539 set_position(cluster_pos_ifs_[j],
false);
544 for (
unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
545 set_position(cluster_pos_ifs_[i],
false);
551 for (
unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
552 set_position(cluster_pos_ifs_[i],
false);
557 if (finput_->header.frame_id ==
"") {
560 fclusters_->header.frame_id = finput_->header.frame_id;
561 pcl_utils::copy_time(finput_, fclusters_);
562 fclusters_labeled_->header.frame_id = finput_->header.frame_id;
563 pcl_utils::copy_time(finput_, fclusters_labeled_);
565 TIMETRACK_END(ttc_clustering_);
566 TIMETRACK_END(ttc_full_loop_);
568 #ifdef USE_TIMETRACKER 569 if (++tt_loopcount_ >= 5) {
571 tt_->print_to_stdout();
579 bool is_visible,
const Eigen::Vector4f ¢roid,
580 const Eigen::Quaternionf &attitude)
584 if (input_->header.frame_id.empty()) {
593 spose(tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()),
594 tf::Vector3(centroid[0], centroid[1], centroid[2])),
597 iface->
set_frame(cfg_result_frame_.c_str());
610 tf::Vector3 &origin = baserel_pose.getOrigin();
611 tf::Quaternion quat = baserel_pose.getRotation();
613 Eigen::Vector4f baserel_centroid;
614 baserel_centroid[0] = origin.x();
615 baserel_centroid[1] = origin.y();
616 baserel_centroid[2] = origin.z();
617 baserel_centroid[3] = 0.;
621 Eigen::Vector4f last_centroid(iface->
translation(0) - cfg_offset_x_,
624 bool different_cluster =
625 fabs((last_centroid - baserel_centroid).norm()) > cfg_switch_tolerance_;
627 if (! different_cluster && visibility_history >= 0) {
634 double translation[3] = { origin.x() + cfg_offset_x_,
635 origin.y() + cfg_offset_y_,
636 origin.z() + cfg_offset_z_ };
637 double rotation[4] = { quat.x(), quat.y(), quat.z(), quat.w() };
642 if (visibility_history <= 0) {
646 double translation[3] = { 0, 0, 0 };
647 double rotation[4] = { 0, 0, 0, 1 };
657 LaserClusterThread::calc_line_length(CloudPtr cloud, pcl::PointIndices::Ptr inliers,
658 pcl::ModelCoefficients::Ptr coeff)
660 if (inliers->indices.size() < 2)
return 0.;
662 CloudPtr cloud_line(
new Cloud());
663 CloudPtr cloud_line_proj(
new Cloud());
665 pcl::ExtractIndices<PointType> extract;
666 extract.setInputCloud(cloud);
667 extract.setIndices(inliers);
668 extract.setNegative(
false);
669 extract.filter(*cloud_line);
672 pcl::ProjectInliers<PointType> proj;
673 proj.setModelType(pcl::SACMODEL_LINE);
674 proj.setInputCloud(cloud_line);
675 proj.setModelCoefficients(coeff);
676 proj.filter(*cloud_line_proj);
678 Eigen::Vector3f point_on_line, line_dir;
679 point_on_line[0] = cloud_line_proj->points[0].x;
680 point_on_line[1] = cloud_line_proj->points[0].y;
681 point_on_line[2] = cloud_line_proj->points[0].z;
682 line_dir[0] = coeff->values[3];
683 line_dir[1] = coeff->values[4];
684 line_dir[2] = coeff->values[5];
686 ssize_t idx_1 = 0, idx_2 = 0;
687 float max_dist_1 = 0.f, max_dist_2 = 0.f;
689 for (
size_t i = 1; i < cloud_line_proj->points.size(); ++i) {
690 const PointType &pt = cloud_line_proj->points[i];
691 Eigen::Vector3f ptv(pt.x, pt.y, pt.z);
692 Eigen::Vector3f diff(ptv - point_on_line);
693 float dist = diff.norm();
694 float dir = line_dir.dot(diff);
696 if (dist > max_dist_1) {
702 if (dist > max_dist_2) {
709 if (idx_1 >= 0 && idx_2 >= 0) {
710 const PointType &pt_1 = cloud_line_proj->points[idx_1];
711 const PointType &pt_2 = cloud_line_proj->points[idx_2];
713 Eigen::Vector3f ptv_1(pt_1.x, pt_1.y, pt_1.z);
714 Eigen::Vector3f ptv_2(pt_2.x, pt_2.y, pt_2.z);
716 return (ptv_1 - ptv_2).norm();
void set_frame(const char *new_frame)
Set frame value.
void set_selection_mode(const SelectionMode new_selection_mode)
Set selection_mode value.
bool msgq_empty()
Check if queue is empty.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void remove_pointcloud(const char *id)
Remove the point cloud.
LaserClusterInterface Fawkes BlackBoard Interface.
virtual ~LaserClusterThread()
Destructor.
This class tries to translate the found plan to interpreteable data for the rest of the program...
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
SetSelectionModeMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
LaserClusterThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
SetMaxXMessage Fawkes BlackBoard Interface Message.
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.
void write()
Write from local copy into BlackBoard memory.
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.
void reset()
Reset pointer.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
virtual void init()
Initialize the thread.
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Position3DInterface Fawkes BlackBoard Interface.
void set_name(const char *format,...)
Set name of thread.
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.
Base class for exceptions in Fawkes.
void set_enabled(const bool new_enabled)
Set enabled value.
virtual Interface * open_for_writing_f(const char *interface_type, const char *identifier,...)
Open interface for writing with identifier format string.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
virtual void finalize()
Finalize the 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.
double * translation() const
Get translation value.
Wrapper class to add time stamp and frame ID to base types.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
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.
Configuration * config
This is the Configuration member used to access the configuration.
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.
virtual void close(Interface *interface)=0
Close interface.