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 <mongo/client/dbclient.h> 68 #include <Eigen/StdVector> 75 template <
typename Po
intType>
94 this->
name_ =
"PCL_DB_MergePL";
96 cfg_transform_to_sensor_frame_ = config->
get_bool(CFG_PREFIX_MERGE
"transform-to-sensor-frame");
97 if (cfg_transform_to_sensor_frame_) {
98 cfg_fixed_frame_ = config->
get_string(CFG_PREFIX_MERGE
"fixed-frame");
99 cfg_sensor_frame_ = config->
get_string(CFG_PREFIX_MERGE
"sensor-frame");
102 cfg_global_frame_ = config->
get_string(CFG_PREFIX_MERGE
"global-frame");
103 cfg_passthrough_filter_axis_ = config->
get_string(CFG_PREFIX_MERGE
"passthrough-filter/axis");
104 std::vector<float> passthrough_filter_limits =
105 config->
get_floats(CFG_PREFIX_MERGE
"passthrough-filter/limits");
106 if (passthrough_filter_limits.size() != 2) {
108 "with exactly two elements");
110 if (passthrough_filter_limits[1] < passthrough_filter_limits[0]) {
111 throw fawkes::Exception(
"Passthrough filter limits start cannot be smaller than end");
113 cfg_passthrough_filter_limits_[0] = passthrough_filter_limits[0];
114 cfg_passthrough_filter_limits_[1] = passthrough_filter_limits[1];
115 cfg_downsample_leaf_size_ = config->
get_float(CFG_PREFIX_MERGE
"downsample-leaf-size");
116 cfg_plane_rem_max_iter_ =
117 config->
get_float(CFG_PREFIX_MERGE
"plane-removal/segmentation-max-iterations");
118 cfg_plane_rem_dist_thresh_ =
119 config->
get_float(CFG_PREFIX_MERGE
"plane-removal/segmentation-distance-threshold");
120 cfg_icp_ransac_iterations_ = config->
get_uint(CFG_PREFIX_MERGE
"icp/ransac-iterations");
121 cfg_icp_max_correspondance_distance_ =
122 config->
get_float(CFG_PREFIX_MERGE
"icp/max-correspondance-distance");
123 cfg_icp_max_iterations_ = config->
get_uint(CFG_PREFIX_MERGE
"icp/max-iterations");
124 cfg_icp_transformation_eps_ = config->
get_float(CFG_PREFIX_MERGE
"icp/transformation-epsilon");
125 cfg_icp_euclidean_fitness_eps_ =
126 config->
get_float(CFG_PREFIX_MERGE
"icp/euclidean-fitness-epsilon");
129 "Age Tolerance: %li " 130 "Limits: [%f, %f] tf range: [%li, %li]",
132 cfg_passthrough_filter_limits_[0],
133 cfg_passthrough_filter_limits_[1],
137 use_alignment_ =
true;
138 #ifdef USE_TIMETRACKER 141 ttc_merge_ = tt_->add_class(
"Full Merge");
142 ttc_retrieval_ = tt_->add_class(
"Retrieval");
143 ttc_transform_global_ = tt_->add_class(
"Transform to Map");
144 ttc_downsample_ = tt_->add_class(
"Downsampling");
145 ttc_align_1_ = tt_->add_class(
"First ICP");
146 ttc_transform_1_ = tt_->add_class(
"Apply 1st TF");
147 ttc_remove_planes_ = tt_->add_class(
"Plane Removal");
148 ttc_align_2_ = tt_->add_class(
"Second ICP");
149 ttc_transform_final_ = tt_->add_class(
"Apply final TF");
150 ttc_output_ = tt_->add_class(
"Output");
157 #ifdef USE_TIMETRACKER 168 merge(std::vector<long long> ×, std::string &database, std::string &collection)
170 TIMETRACK_START(ttc_merge_);
171 const unsigned int num_clouds = times.size();
173 std::vector<long long> actual_times(num_clouds);
178 this->
output_->is_dense =
false;
180 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(num_clouds);
181 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> non_transformed(num_clouds);
182 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> non_aligned(num_clouds);
183 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> non_aligned_downsampled(
185 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> aligned_downsampled(num_clouds);
186 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> aligned_downsampled_remplane(
188 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transforms(num_clouds
191 for (
unsigned int i = 0; i < num_clouds; ++i) {
204 TIMETRACK_START(ttc_retrieval_);
210 TIMETRACK_ABORT(ttc_retrieval_);
211 TIMETRACK_ABORT(ttc_merge_);
215 TIMETRACK_INTER(ttc_retrieval_, ttc_transform_global_);
217 for (
unsigned int i = 0; i < num_clouds; ++i) {
224 "Restored transforms for %zu frames " 225 "for range (%lli..%lli)",
228 actual_times[i] + this->cfg_transform_range_[1]);
232 fawkes::pcl_utils::transform_pointcloud(cfg_global_frame_, *pcls[i], transformer);
235 "Failed to transform from %s to %s",
236 pcls[i]->header.frame_id.c_str(),
237 cfg_global_frame_.c_str());
240 *non_aligned[i] = *pcls[i];
245 TIMETRACK_END(ttc_transform_global_);
247 if (use_alignment_) {
250 TIMETRACK_START(ttc_downsample_);
256 pcl::PassThrough<PointType> pass;
257 pass.setFilterFieldName(cfg_passthrough_filter_axis_.c_str());
258 pass.setFilterLimits(cfg_passthrough_filter_limits_[0], cfg_passthrough_filter_limits_[1]);
261 pcl::VoxelGrid<PointType> downsample;
262 downsample.setLeafSize(cfg_downsample_leaf_size_,
263 cfg_downsample_leaf_size_,
264 cfg_downsample_leaf_size_);
269 for (
unsigned int i = 0; i < num_clouds; ++i) {
271 pass.setInputCloud(pcls[i]);
272 pass.filter(*filtered_z);
274 downsample.setInputCloud(filtered_z);
275 downsample.filter(*non_aligned_downsampled[i]);
277 "Filtered cloud %u contains %zu points",
279 non_aligned_downsampled[i]->points.size());
281 TIMETRACK_INTER(ttc_downsample_, ttc_align_1_);
284 for (
unsigned int i = 1; i < num_clouds; ++i) {
286 Eigen::Matrix4f transform;
289 source = non_aligned_downsampled[i];
290 target = non_aligned_downsampled[i - 1];
292 #ifdef USE_ICP_ALIGNMENT 293 align_icp(source, target, transform);
295 #elif defined(USE_NDT_ALIGNMENT) 296 align_ndt(source, target);
299 transforms[i - 1] = transform;
302 TIMETRACK_INTER(ttc_align_1_, ttc_transform_1_);
306 *aligned_downsampled[0] = *non_aligned_downsampled[0];
307 for (
unsigned int i = 1; i < num_clouds; ++i) {
308 pcl::transformPointCloud(*non_aligned_downsampled[i],
309 *aligned_downsampled[i],
313 TIMETRACK_INTER(ttc_transform_1_, ttc_remove_planes_);
315 for (
unsigned int i = 0; i < num_clouds; ++i) {
316 *aligned_downsampled_remplane[i] = *aligned_downsampled[i];
317 remove_plane(aligned_downsampled_remplane[i]);
319 "Removed plane from cloud %u, " 320 "%zu of %zu points remain",
322 aligned_downsampled_remplane[i]->points.size(),
323 aligned_downsampled[i]->points.size());
326 TIMETRACK_INTER(ttc_remove_planes_, ttc_align_2_);
328 for (
unsigned int i = 1; i < num_clouds; ++i) {
329 Eigen::Matrix4f transform;
332 source = aligned_downsampled_remplane[i];
333 target = aligned_downsampled_remplane[i - 1];
335 align_icp(source, target, transform);
338 pcl::transformPointCloud(*aligned_downsampled_remplane[i], tmp, transform);
339 *aligned_downsampled_remplane[i] = tmp;
341 transforms[i - 1] *= transform;
344 TIMETRACK_INTER(ttc_align_2_, ttc_transform_final_);
346 for (
unsigned int i = 1; i < num_clouds; ++i) {
348 pcl::transformPointCloud(*pcls[i], tmp, transforms[i - 1]);
352 TIMETRACK_END(ttc_transform_final_);
355 TIMETRACK_END(ttc_merge_);
356 TIMETRACK_START(ttc_output_);
361 merge_output(database, non_transformed, num_clouds);
363 fawkes::pcl_utils::set_time(this->
output_, now);
366 merge_output(database, non_aligned, num_clouds);
368 fawkes::pcl_utils::set_time(this->
output_, now);
371 merge_output(database, non_aligned_downsampled, num_clouds);
373 fawkes::pcl_utils::set_time(this->
output_, now);
376 merge_output(database, aligned_downsampled, num_clouds);
378 fawkes::pcl_utils::set_time(this->
output_, now);
381 merge_output(database, aligned_downsampled_remplane, num_clouds);
383 fawkes::pcl_utils::set_time(this->
output_, now);
387 merge_output(database, pcls, actual_times);
389 TIMETRACK_END(ttc_output_);
391 #ifdef USE_TIMETRACKER 392 if (++tt_loopcount_ >= 5) {
394 tt_->print_to_stdout();
403 pcl::SACSegmentation<PointType> tablesegm;
404 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
405 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
407 tablesegm.setOptimizeCoefficients(
true);
408 tablesegm.setModelType(pcl::SACMODEL_PLANE);
409 tablesegm.setMethodType(pcl::SAC_RANSAC);
410 tablesegm.setMaxIterations(1000);
411 tablesegm.setDistanceThreshold(0.022);
413 tablesegm.setInputCloud(cloud);
414 tablesegm.segment(*inliers, *coeff);
416 if (!coeff || coeff->values.empty()) {
420 pcl::ExtractIndices<PointType> extract;
422 extract.setNegative(
true);
423 extract.setInputCloud(cloud);
424 extract.setIndices(inliers);
425 extract.filter(extracted);
428 pcl::ConvexHull<PointType> convex_hull;
429 convex_hull.setDimension(2);
430 convex_hull.setInputCloud(cloud);
433 convex_hull.reconstruct(*hull);
449 pcl::ComparisonOps::CompareOp op =
450 coeff->values[3] < 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT;
453 typename pcl::ConditionAnd<PointType>::Ptr above_cond(
new pcl::ConditionAnd<PointType>());
454 above_cond->addComparison(above_comp);
455 pcl::ConditionalRemoval<PointType> above_condrem;
456 above_condrem.setCondition(above_cond);
457 above_condrem.setInputCloud(cloud);
460 above_condrem.filter(*cloud_above);
463 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
465 typename pcl::ConditionAnd<PointType>::Ptr polygon_cond(
new pcl::ConditionAnd<PointType>());
469 polygon_cond->addComparison(inpoly_comp);
472 pcl::ConditionalRemoval<PointType> condrem;
473 condrem.setCondition(polygon_cond);
474 condrem.setInputCloud(cloud_above);
475 condrem.filter(*cloud);
478 #ifdef USE_ICP_ALIGNMENT 482 Eigen::Matrix4f & transform)
488 pcl::IterativeClosestPoint<PointType, PointType> icp;
489 icp.setInputCloud(source);
490 icp.setInputTarget(target);
492 icp.setRANSACIterations(cfg_icp_ransac_iterations_);
496 icp.setMaxCorrespondenceDistance(cfg_icp_max_correspondance_distance_);
498 icp.setMaximumIterations(cfg_icp_max_iterations_);
500 icp.setTransformationEpsilon(cfg_icp_transformation_eps_);
502 icp.setEuclideanFitnessEpsilon(cfg_icp_euclidean_fitness_eps_);
509 transform = icp.getFinalTransformation();
512 return icp.hasConverged();
516 #ifdef USE_NDT_ALIGNMENT 523 pcl::NormalDistributionsTransform<PointType, PointType> ndt;
524 ndt.setInputCloud(source);
525 ndt.setInputTarget(target);
529 ndt.setTransformationEpsilon(0.01);
531 ndt.setStepSize(0.1);
533 ndt.setResolution(1.0);
536 ndt.setMaximumIterations(5);
539 transform = ndt.getFinalTransformation();
540 return ndt.hasConverged();
545 merge_output(std::string & database,
547 std::vector<long long> & actual_times)
549 size_t num_points = 0;
550 const size_t num_clouds = clouds.size();
551 for (
unsigned int i = 0; i < num_clouds; ++i) {
552 num_points += clouds[i]->points.size();
554 this->
output_->header.frame_id =
555 cfg_transform_to_sensor_frame_ ? cfg_sensor_frame_ : cfg_global_frame_;
556 this->
output_->points.resize(num_points);
558 this->
output_->width = num_points;
560 for (
unsigned int i = 0; i < num_clouds; ++i) {
562 const size_t cldn = lpcl->points.size();
566 for (
size_t p = 0; p < cldn; ++p, ++out_p) {
567 const PointType & ip = lpcl->points[p];
574 op.r = cluster_colors[i][0];
575 op.g = cluster_colors[i][1];
576 op.b = cluster_colors[i][2];
580 if (cfg_transform_to_sensor_frame_) {
584 unsigned int ref_pos = clouds.size() - 1;
589 "Restored transforms for %zu frames for range (%lli..%lli)",
590 transformer.get_frame_caches().size(),
592 actual_times[ref_pos] + this->cfg_transform_range_[1]);
595 fawkes::pcl_utils::get_time(clouds[ref_pos], source_time);
597 transformer.lookup_transform(cfg_fixed_frame_,
603 tf_->
lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
605 fawkes::tf::Transform transform = transform_current * transform_recorded;
608 fawkes::pcl_utils::transform_pointcloud(*(this->
output_), transform);
619 std::string cfg_global_frame_;
620 bool cfg_transform_to_sensor_frame_;
621 std::string cfg_sensor_frame_;
622 std::string cfg_fixed_frame_;
623 std::string cfg_passthrough_filter_axis_;
624 float cfg_passthrough_filter_limits_[2];
625 float cfg_downsample_leaf_size_;
626 float cfg_plane_rem_max_iter_;
627 float cfg_plane_rem_dist_thresh_;
628 unsigned int cfg_icp_ransac_iterations_;
629 float cfg_icp_max_correspondance_distance_;
630 unsigned int cfg_icp_max_iterations_;
631 float cfg_icp_transformation_eps_;
632 float cfg_icp_euclidean_fitness_eps_;
634 #ifdef USE_TIMETRACKER 636 unsigned int tt_loopcount_;
637 unsigned int ttc_merge_;
638 unsigned int ttc_retrieval_;
639 unsigned int ttc_transform_global_;
640 unsigned int ttc_downsample_;
641 unsigned int ttc_align_1_;
642 unsigned int ttc_transform_1_;
643 unsigned int ttc_remove_planes_;
644 unsigned int ttc_align_2_;
645 unsigned int ttc_transform_final_;
646 unsigned int ttc_output_;
Compare points' distance to a plane.
PointCloudDBMergePipeline(mongo::DBClientBase *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, fawkes::tf::Transformer *transformer, typename PointCloudDBPipeline< PointType >::ColorCloudPtr output)
Constructor.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void merge(std::vector< long long > ×, std::string &database, std::string &collection)
Merge point clouds.
Cloud::Ptr CloudPtr
Shared pointer to cloud.
pcl::PointCloud< PointType > Cloud
Basic point cloud type.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
mongo::DBClientBase * mongodb_client_
MongoDB client to retrieve data.
A class for handling time.
long cfg_pcl_age_tolerance_
Age tolerance for retrieved point clouds.
pcl::PointXYZRGB ColorPointType
Colored point type.
Check if point is inside or outside a given polygon.
Cloud::ConstPtr CloudConstPtr
Shared pointer to constant cloud.
Database point cloud pipeline base class.
virtual ~PointCloudDBMergePipeline()
Destructor.
boost::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
fawkes::Logger * logger_
Logger for informative messages.
ColorCloud::Ptr ColorCloudPtr
Shared pointer to colored cloud.
Base class for exceptions in Fawkes.
const char * name_
Name of the pipeline.
Point cloud merging pipeline.
virtual std::vector< float > get_floats(const char *path)=0
Get list of values from configuration which is of type float.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
ColorCloudPtr output_
The final (colored) output of the pipeline.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Time & stamp()
Set this time to the current time.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
long cfg_transform_range_[2]
Transform range start and end times.
Interface for configuration handling.
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.
std::vector< CloudPtr > retrieve_clouds(std::vector< long long > ×, std::vector< long long > &actual_times, std::string &database, std::string &collection)
Retrieve point clouds from database.
boost::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.