23 #ifndef _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_MERGE_PIPELINE_H_
24 #define _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_MERGE_PIPELINE_H_
26 #include "mongodb_tf_transformer.h"
27 #include "pcl_db_pipeline.h"
29 #include <pcl_utils/comparisons.h>
30 #include <pcl_utils/transforms.h>
31 #include <pcl_utils/utils.h>
32 #include <tf/transformer.h>
33 #ifdef USE_TIMETRACKER
34 # include <utils/time/tracker.h>
36 #include <utils/time/tracker_macros.h>
39 #define USE_ICP_ALIGNMENT
42 #define CFG_PREFIX_MERGE "/perception/pcl-db-merge/"
45 #include <pcl/filters/approximate_voxel_grid.h>
46 #include <pcl/filters/conditional_removal.h>
47 #include <pcl/filters/extract_indices.h>
48 #include <pcl/filters/passthrough.h>
49 #include <pcl/filters/voxel_grid.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/point_types.h>
52 #include <pcl/segmentation/sac_segmentation.h>
53 #include <pcl/surface/convex_hull.h>
56 #ifdef USE_ICP_ALIGNMENT
57 # include <pcl/registration/icp.h>
59 #ifdef USE_NDT_ALIGNMENT
60 # if not defined(PCL_VERSION_COMPARE) || PCL_VERSION_COMPARE(<, 1, 7, 0)
61 # error NDT alignment requires PCL 1.7.0 or higher
63 # include <pcl/registration/ndt.h>
66 #include <Eigen/StdVector>
67 #include <mongocxx/client.hpp>
74 template <
typename Po
intType>
93 this->
name_ =
"PCL_DB_MergePL";
95 cfg_transform_to_sensor_frame_ = config->
get_bool(CFG_PREFIX_MERGE
"transform-to-sensor-frame");
96 if (cfg_transform_to_sensor_frame_) {
97 cfg_fixed_frame_ = config->
get_string(CFG_PREFIX_MERGE
"fixed-frame");
98 cfg_sensor_frame_ = config->
get_string(CFG_PREFIX_MERGE
"sensor-frame");
101 cfg_global_frame_ = config->
get_string(CFG_PREFIX_MERGE
"global-frame");
102 cfg_passthrough_filter_axis_ = config->
get_string(CFG_PREFIX_MERGE
"passthrough-filter/axis");
103 std::vector<float> passthrough_filter_limits =
104 config->
get_floats(CFG_PREFIX_MERGE
"passthrough-filter/limits");
105 if (passthrough_filter_limits.size() != 2) {
107 "with exactly two elements");
109 if (passthrough_filter_limits[1] < passthrough_filter_limits[0]) {
110 throw fawkes::Exception(
"Passthrough filter limits start cannot be smaller than end");
112 cfg_passthrough_filter_limits_[0] = passthrough_filter_limits[0];
113 cfg_passthrough_filter_limits_[1] = passthrough_filter_limits[1];
114 cfg_downsample_leaf_size_ = config->
get_float(CFG_PREFIX_MERGE
"downsample-leaf-size");
115 cfg_plane_rem_max_iter_ =
116 config->
get_float(CFG_PREFIX_MERGE
"plane-removal/segmentation-max-iterations");
117 cfg_plane_rem_dist_thresh_ =
118 config->
get_float(CFG_PREFIX_MERGE
"plane-removal/segmentation-distance-threshold");
119 cfg_icp_ransac_iterations_ = config->
get_uint(CFG_PREFIX_MERGE
"icp/ransac-iterations");
120 cfg_icp_max_correspondance_distance_ =
121 config->
get_float(CFG_PREFIX_MERGE
"icp/max-correspondance-distance");
122 cfg_icp_max_iterations_ = config->
get_uint(CFG_PREFIX_MERGE
"icp/max-iterations");
123 cfg_icp_transformation_eps_ = config->
get_float(CFG_PREFIX_MERGE
"icp/transformation-epsilon");
124 cfg_icp_euclidean_fitness_eps_ =
125 config->
get_float(CFG_PREFIX_MERGE
"icp/euclidean-fitness-epsilon");
128 "Age Tolerance: %li "
129 "Limits: [%f, %f] tf range: [%li, %li]",
131 cfg_passthrough_filter_limits_[0],
132 cfg_passthrough_filter_limits_[1],
136 use_alignment_ =
true;
137 #ifdef USE_TIMETRACKER
140 ttc_merge_ = tt_->add_class(
"Full Merge");
141 ttc_retrieval_ = tt_->add_class(
"Retrieval");
142 ttc_transform_global_ = tt_->add_class(
"Transform to Map");
143 ttc_downsample_ = tt_->add_class(
"Downsampling");
144 ttc_align_1_ = tt_->add_class(
"First ICP");
145 ttc_transform_1_ = tt_->add_class(
"Apply 1st TF");
146 ttc_remove_planes_ = tt_->add_class(
"Plane Removal");
147 ttc_align_2_ = tt_->add_class(
"Second ICP");
148 ttc_transform_final_ = tt_->add_class(
"Apply final TF");
149 ttc_output_ = tt_->add_class(
"Output");
156 #ifdef USE_TIMETRACKER
167 merge(std::vector<long> ×, std::string &database, std::string &collection)
169 TIMETRACK_START(ttc_merge_);
170 const unsigned int num_clouds = times.size();
172 std::vector<long> actual_times(num_clouds);
177 this->
output_->is_dense =
false;
179 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(num_clouds);
180 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> non_transformed(num_clouds);
181 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> non_aligned(num_clouds);
182 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> non_aligned_downsampled(
184 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> aligned_downsampled(num_clouds);
185 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> aligned_downsampled_remplane(
187 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transforms(num_clouds
190 for (
unsigned int i = 0; i < num_clouds; ++i) {
203 TIMETRACK_START(ttc_retrieval_);
209 TIMETRACK_ABORT(ttc_retrieval_);
210 TIMETRACK_ABORT(ttc_merge_);
214 TIMETRACK_INTER(ttc_retrieval_, ttc_transform_global_);
216 for (
unsigned int i = 0; i < num_clouds; ++i) {
223 "Restored transforms for %zu frames "
224 "for range (%li..%li)",
226 actual_times[i] + this->cfg_transform_range_[0],
227 actual_times[i] + this->cfg_transform_range_[1]);
231 fawkes::pcl_utils::transform_pointcloud(cfg_global_frame_, *pcls[i], transformer);
234 "Failed to transform from %s to %s",
235 pcls[i]->header.frame_id.c_str(),
236 cfg_global_frame_.c_str());
239 *non_aligned[i] = *pcls[i];
244 TIMETRACK_END(ttc_transform_global_);
246 if (use_alignment_) {
249 TIMETRACK_START(ttc_downsample_);
255 pcl::PassThrough<PointType> pass;
256 pass.setFilterFieldName(cfg_passthrough_filter_axis_.c_str());
257 pass.setFilterLimits(cfg_passthrough_filter_limits_[0], cfg_passthrough_filter_limits_[1]);
260 pcl::VoxelGrid<PointType> downsample;
261 downsample.setLeafSize(cfg_downsample_leaf_size_,
262 cfg_downsample_leaf_size_,
263 cfg_downsample_leaf_size_);
268 for (
unsigned int i = 0; i < num_clouds; ++i) {
270 pass.setInputCloud(pcls[i]);
271 pass.filter(*filtered_z);
273 downsample.setInputCloud(filtered_z);
274 downsample.filter(*non_aligned_downsampled[i]);
276 "Filtered cloud %u contains %zu points",
278 non_aligned_downsampled[i]->points.size());
280 TIMETRACK_INTER(ttc_downsample_, ttc_align_1_);
283 for (
unsigned int i = 1; i < num_clouds; ++i) {
285 Eigen::Matrix4f transform;
288 source = non_aligned_downsampled[i];
289 target = non_aligned_downsampled[i - 1];
291 #ifdef USE_ICP_ALIGNMENT
292 align_icp(source, target, transform);
294 #elif defined(USE_NDT_ALIGNMENT)
295 align_ndt(source, target);
298 transforms[i - 1] = transform;
301 TIMETRACK_INTER(ttc_align_1_, ttc_transform_1_);
305 *aligned_downsampled[0] = *non_aligned_downsampled[0];
306 for (
unsigned int i = 1; i < num_clouds; ++i) {
307 pcl::transformPointCloud(*non_aligned_downsampled[i],
308 *aligned_downsampled[i],
312 TIMETRACK_INTER(ttc_transform_1_, ttc_remove_planes_);
314 for (
unsigned int i = 0; i < num_clouds; ++i) {
315 *aligned_downsampled_remplane[i] = *aligned_downsampled[i];
316 remove_plane(aligned_downsampled_remplane[i]);
318 "Removed plane from cloud %u, "
319 "%zu of %zu points remain",
321 aligned_downsampled_remplane[i]->points.size(),
322 aligned_downsampled[i]->points.size());
325 TIMETRACK_INTER(ttc_remove_planes_, ttc_align_2_);
327 for (
unsigned int i = 1; i < num_clouds; ++i) {
328 Eigen::Matrix4f transform;
331 source = aligned_downsampled_remplane[i];
332 target = aligned_downsampled_remplane[i - 1];
334 align_icp(source, target, transform);
337 pcl::transformPointCloud(*aligned_downsampled_remplane[i], tmp, transform);
338 *aligned_downsampled_remplane[i] = tmp;
340 transforms[i - 1] *= transform;
343 TIMETRACK_INTER(ttc_align_2_, ttc_transform_final_);
345 for (
unsigned int i = 1; i < num_clouds; ++i) {
347 pcl::transformPointCloud(*pcls[i], tmp, transforms[i - 1]);
351 TIMETRACK_END(ttc_transform_final_);
354 TIMETRACK_END(ttc_merge_);
355 TIMETRACK_START(ttc_output_);
360 merge_output(database, non_transformed, num_clouds);
362 fawkes::pcl_utils::set_time(this->
output_, now);
365 merge_output(database, non_aligned, num_clouds);
367 fawkes::pcl_utils::set_time(this->
output_, now);
370 merge_output(database, non_aligned_downsampled, num_clouds);
372 fawkes::pcl_utils::set_time(this->
output_, now);
375 merge_output(database, aligned_downsampled, num_clouds);
377 fawkes::pcl_utils::set_time(this->
output_, now);
380 merge_output(database, aligned_downsampled_remplane, num_clouds);
382 fawkes::pcl_utils::set_time(this->
output_, now);
386 merge_output(database, pcls, actual_times);
388 TIMETRACK_END(ttc_output_);
390 #ifdef USE_TIMETRACKER
391 if (++tt_loopcount_ >= 5) {
393 tt_->print_to_stdout();
402 pcl::SACSegmentation<PointType> tablesegm;
403 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
404 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
406 tablesegm.setOptimizeCoefficients(
true);
407 tablesegm.setModelType(pcl::SACMODEL_PLANE);
408 tablesegm.setMethodType(pcl::SAC_RANSAC);
409 tablesegm.setMaxIterations(1000);
410 tablesegm.setDistanceThreshold(0.022);
412 tablesegm.setInputCloud(cloud);
413 tablesegm.segment(*inliers, *coeff);
415 if (!coeff || coeff->values.empty()) {
419 pcl::ExtractIndices<PointType> extract;
421 extract.setNegative(
true);
422 extract.setInputCloud(cloud);
423 extract.setIndices(inliers);
424 extract.filter(extracted);
427 pcl::ConvexHull<PointType> convex_hull;
428 convex_hull.setDimension(2);
429 convex_hull.setInputCloud(cloud);
432 convex_hull.reconstruct(*hull);
448 pcl::ComparisonOps::CompareOp op =
449 coeff->values[3] < 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT;
452 typename pcl::ConditionAnd<PointType>::Ptr above_cond(
new pcl::ConditionAnd<PointType>());
453 above_cond->addComparison(above_comp);
454 pcl::ConditionalRemoval<PointType> above_condrem;
455 above_condrem.setCondition(above_cond);
456 above_condrem.setInputCloud(cloud);
459 above_condrem.filter(*cloud_above);
462 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
464 typename pcl::ConditionAnd<PointType>::Ptr polygon_cond(
new pcl::ConditionAnd<PointType>());
468 polygon_cond->addComparison(inpoly_comp);
471 pcl::ConditionalRemoval<PointType> condrem;
472 condrem.setCondition(polygon_cond);
473 condrem.setInputCloud(cloud_above);
474 condrem.filter(*cloud);
477 #ifdef USE_ICP_ALIGNMENT
481 Eigen::Matrix4f & transform)
487 pcl::IterativeClosestPoint<PointType, PointType> icp;
488 icp.setInputCloud(source);
489 icp.setInputTarget(target);
491 icp.setRANSACIterations(cfg_icp_ransac_iterations_);
495 icp.setMaxCorrespondenceDistance(cfg_icp_max_correspondance_distance_);
497 icp.setMaximumIterations(cfg_icp_max_iterations_);
499 icp.setTransformationEpsilon(cfg_icp_transformation_eps_);
501 icp.setEuclideanFitnessEpsilon(cfg_icp_euclidean_fitness_eps_);
508 transform = icp.getFinalTransformation();
511 return icp.hasConverged();
515 #ifdef USE_NDT_ALIGNMENT
522 pcl::NormalDistributionsTransform<PointType, PointType> ndt;
523 ndt.setInputCloud(source);
524 ndt.setInputTarget(target);
528 ndt.setTransformationEpsilon(0.01);
530 ndt.setStepSize(0.1);
532 ndt.setResolution(1.0);
535 ndt.setMaximumIterations(5);
538 transform = ndt.getFinalTransformation();
539 return ndt.hasConverged();
544 merge_output(std::string & database,
546 std::vector<long> & actual_times)
548 size_t num_points = 0;
549 const size_t num_clouds = clouds.size();
550 for (
unsigned int i = 0; i < num_clouds; ++i) {
551 num_points += clouds[i]->points.size();
553 this->
output_->header.frame_id =
554 cfg_transform_to_sensor_frame_ ? cfg_sensor_frame_ : cfg_global_frame_;
555 this->
output_->points.resize(num_points);
557 this->
output_->width = num_points;
559 for (
unsigned int i = 0; i < num_clouds; ++i) {
561 const size_t cldn = lpcl->points.size();
565 for (
size_t p = 0; p < cldn; ++p, ++out_p) {
566 const PointType & ip = lpcl->points[p];
573 op.r = cluster_colors[i][0];
574 op.g = cluster_colors[i][1];
575 op.b = cluster_colors[i][2];
579 if (cfg_transform_to_sensor_frame_) {
583 unsigned int ref_pos = clouds.size() - 1;
588 "Restored transforms for %zu frames for range (%li..%li)",
589 transformer.get_frame_caches().size(),
590 actual_times[ref_pos] + this->cfg_transform_range_[0],
591 actual_times[ref_pos] + this->cfg_transform_range_[1]);
594 fawkes::pcl_utils::get_time(clouds[ref_pos], source_time);
596 transformer.lookup_transform(cfg_fixed_frame_,
602 tf_->
lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
604 fawkes::tf::Transform transform = transform_current * transform_recorded;
607 fawkes::pcl_utils::transform_pointcloud(*(this->
output_), transform);
618 std::string cfg_global_frame_;
619 bool cfg_transform_to_sensor_frame_;
620 std::string cfg_sensor_frame_;
621 std::string cfg_fixed_frame_;
622 std::string cfg_passthrough_filter_axis_;
623 float cfg_passthrough_filter_limits_[2];
624 float cfg_downsample_leaf_size_;
625 float cfg_plane_rem_max_iter_;
626 float cfg_plane_rem_dist_thresh_;
627 unsigned int cfg_icp_ransac_iterations_;
628 float cfg_icp_max_correspondance_distance_;
629 unsigned int cfg_icp_max_iterations_;
630 float cfg_icp_transformation_eps_;
631 float cfg_icp_euclidean_fitness_eps_;
633 #ifdef USE_TIMETRACKER
635 unsigned int tt_loopcount_;
636 unsigned int ttc_merge_;
637 unsigned int ttc_retrieval_;
638 unsigned int ttc_transform_global_;
639 unsigned int ttc_downsample_;
640 unsigned int ttc_align_1_;
641 unsigned int ttc_transform_1_;
642 unsigned int ttc_remove_planes_;
643 unsigned int ttc_align_2_;
644 unsigned int ttc_transform_final_;
645 unsigned int ttc_output_;