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 <tf/transformer.h> 30 #include <pcl_utils/utils.h> 31 #include <pcl_utils/transforms.h> 32 #include <pcl_utils/comparisons.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/" 46 #include <pcl/point_cloud.h> 47 #include <pcl/point_types.h> 48 #include <pcl/filters/approximate_voxel_grid.h> 49 #include <pcl/filters/voxel_grid.h> 50 #include <pcl/filters/passthrough.h> 51 #include <pcl/filters/extract_indices.h> 52 #include <pcl/filters/conditional_removal.h> 53 #include <pcl/segmentation/sac_segmentation.h> 54 #include <pcl/surface/convex_hull.h> 55 #ifdef USE_ICP_ALIGNMENT 56 # include <pcl/registration/icp.h> 58 #ifdef USE_NDT_ALIGNMENT 59 # if not defined(PCL_VERSION_COMPARE) || PCL_VERSION_COMPARE(<,1,7,0) 60 # error NDT alignment requires PCL 1.7.0 or higher 62 # include <pcl/registration/ndt.h> 65 #include <Eigen/StdVector> 66 #include <mongo/client/dbclient.h> 73 template <
typename Po
intType>
92 this->
name_ =
"PCL_DB_MergePL";
94 cfg_transform_to_sensor_frame_ =
95 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");
102 config->
get_string(CFG_PREFIX_MERGE
"global-frame");
103 cfg_passthrough_filter_axis_ =
104 config->
get_string(CFG_PREFIX_MERGE
"passthrough-filter/axis");
105 std::vector<float> passthrough_filter_limits =
106 config->
get_floats(CFG_PREFIX_MERGE
"passthrough-filter/limits");
107 if (passthrough_filter_limits.size() != 2) {
109 "with exactly two elements");
111 if (passthrough_filter_limits[1] < passthrough_filter_limits[0]) {
112 throw fawkes::Exception(
"Passthrough filter limits start cannot be smaller than end");
114 cfg_passthrough_filter_limits_[0] = passthrough_filter_limits[0];
115 cfg_passthrough_filter_limits_[1] = passthrough_filter_limits[1];
116 cfg_downsample_leaf_size_ = config->
get_float(CFG_PREFIX_MERGE
"downsample-leaf-size");
117 cfg_plane_rem_max_iter_ =
118 config->
get_float(CFG_PREFIX_MERGE
"plane-removal/segmentation-max-iterations");
119 cfg_plane_rem_dist_thresh_ =
120 config->
get_float(CFG_PREFIX_MERGE
"plane-removal/segmentation-distance-threshold");
121 cfg_icp_ransac_iterations_ =
122 config->
get_uint(CFG_PREFIX_MERGE
"icp/ransac-iterations");
123 cfg_icp_max_correspondance_distance_ =
124 config->
get_float(CFG_PREFIX_MERGE
"icp/max-correspondance-distance");
125 cfg_icp_max_iterations_ =
126 config->
get_uint(CFG_PREFIX_MERGE
"icp/max-iterations");
127 cfg_icp_transformation_eps_ =
128 config->
get_float(CFG_PREFIX_MERGE
"icp/transformation-epsilon");
129 cfg_icp_euclidean_fitness_eps_ =
130 config->
get_float(CFG_PREFIX_MERGE
"icp/euclidean-fitness-epsilon");
133 "Limits: [%f, %f] tf range: [%lli, %lli]",
138 use_alignment_ =
true;
139 #ifdef USE_TIMETRACKER 142 ttc_merge_ = tt_->add_class(
"Full Merge");
143 ttc_retrieval_ = tt_->add_class(
"Retrieval");
144 ttc_transform_global_ = tt_->add_class(
"Transform to Map");
145 ttc_downsample_ = tt_->add_class(
"Downsampling");
146 ttc_align_1_ = tt_->add_class(
"First ICP");
147 ttc_transform_1_ = tt_->add_class(
"Apply 1st TF");
148 ttc_remove_planes_ = tt_->add_class(
"Plane Removal");
149 ttc_align_2_ = tt_->add_class(
"Second ICP");
150 ttc_transform_final_ = tt_->add_class(
"Apply final TF");
151 ttc_output_ = tt_->add_class(
"Output");
158 #ifdef USE_TIMETRACKER 169 merge(std::vector<long long> ×, std::string &database, std::string &collection)
171 TIMETRACK_START(ttc_merge_);
172 const unsigned int num_clouds = times.size();
174 std::vector<long long> actual_times(num_clouds);
179 this->
output_->is_dense =
false;
181 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(num_clouds);
182 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> non_transformed(num_clouds);
183 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> non_aligned(num_clouds);
184 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> non_aligned_downsampled(num_clouds);
185 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> aligned_downsampled(num_clouds);
186 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> aligned_downsampled_remplane(num_clouds);
187 std::vector<Eigen::Matrix4f,Eigen::aligned_allocator<Eigen::Matrix4f> >
188 transforms(num_clouds-1);
190 for (
unsigned int i = 0; i < num_clouds; ++i) {
203 TIMETRACK_START(ttc_retrieval_);
208 TIMETRACK_ABORT(ttc_retrieval_);
209 TIMETRACK_ABORT(ttc_merge_);
213 TIMETRACK_INTER(ttc_retrieval_, ttc_transform_global_);
215 for (
unsigned int i = 0; i < num_clouds; ++i) {
223 "for range (%li..%li)",
226 actual_times[i] + this->cfg_transform_range_[1]);
230 fawkes::pcl_utils::transform_pointcloud(cfg_global_frame_, *pcls[i], transformer);
233 pcls[i]->header.frame_id.c_str(),
234 cfg_global_frame_.c_str());
237 *non_aligned[i] = *pcls[i];
242 TIMETRACK_END(ttc_transform_global_);
244 if (use_alignment_) {
247 TIMETRACK_START(ttc_downsample_);
253 pcl::PassThrough<PointType> pass;
254 pass.setFilterFieldName(cfg_passthrough_filter_axis_.c_str());
255 pass.setFilterLimits(cfg_passthrough_filter_limits_[0],
256 cfg_passthrough_filter_limits_[1]);
259 pcl::VoxelGrid<PointType> downsample;
260 downsample.setLeafSize(cfg_downsample_leaf_size_,
261 cfg_downsample_leaf_size_,
262 cfg_downsample_leaf_size_);
266 for (
unsigned int i = 0; i < num_clouds; ++i) {
269 pass.setInputCloud(pcls[i]);
270 pass.filter(*filtered_z);
272 downsample.setInputCloud(filtered_z);
273 downsample.filter(*non_aligned_downsampled[i]);
275 i, non_aligned_downsampled[i]->points.size ());
277 TIMETRACK_INTER(ttc_downsample_, ttc_align_1_);
280 for (
unsigned int i = 1; i < num_clouds; ++i) {
282 Eigen::Matrix4f transform;
285 source = non_aligned_downsampled[i];
286 target = non_aligned_downsampled[i-1];
288 # ifdef USE_ICP_ALIGNMENT 289 align_icp(source, target, transform);
291 # elif defined(USE_NDT_ALIGNMENT) 292 align_ndt(source, target);
295 transforms[i-1] = transform;
298 TIMETRACK_INTER(ttc_align_1_, ttc_transform_1_);
302 *aligned_downsampled[0] = *non_aligned_downsampled[0];
303 for (
unsigned int i = 1; i < num_clouds; ++i) {
304 pcl::transformPointCloud(*non_aligned_downsampled[i],
305 *aligned_downsampled[i], transforms[i-1]);
308 TIMETRACK_INTER(ttc_transform_1_, ttc_remove_planes_);
310 for (
unsigned int i = 0; i < num_clouds; ++i) {
311 *aligned_downsampled_remplane[i] = *aligned_downsampled[i];
312 remove_plane(aligned_downsampled_remplane[i]);
314 "%zu of %zu points remain",
315 i, aligned_downsampled_remplane[i]->points.size(),
316 aligned_downsampled[i]->points.size());
319 TIMETRACK_INTER(ttc_remove_planes_, ttc_align_2_);
321 for (
unsigned int i = 1; i < num_clouds; ++i) {
322 Eigen::Matrix4f transform;
325 source = aligned_downsampled_remplane[i];
326 target = aligned_downsampled_remplane[i-1];
328 align_icp(source, target, transform);
331 pcl::transformPointCloud(*aligned_downsampled_remplane[i], tmp, transform);
332 *aligned_downsampled_remplane[i] = tmp;
334 transforms[i-1] *= transform;
337 TIMETRACK_INTER(ttc_align_2_, ttc_transform_final_);
339 for (
unsigned int i = 1; i < num_clouds; ++i) {
341 pcl::transformPointCloud(*pcls[i], tmp, transforms[i-1]);
345 TIMETRACK_END(ttc_transform_final_);
349 TIMETRACK_END(ttc_merge_);
350 TIMETRACK_START(ttc_output_);
356 merge_output(database, non_transformed, num_clouds);
357 now.
stamp(); fawkes::pcl_utils::set_time(this->
output_, now);
360 merge_output(database, non_aligned, num_clouds);
361 now.
stamp(); fawkes::pcl_utils::set_time(this->
output_, now);
364 merge_output(database, non_aligned_downsampled, num_clouds);
365 now.
stamp(); fawkes::pcl_utils::set_time(this->
output_, now);
368 merge_output(database, aligned_downsampled, num_clouds);
369 now.
stamp(); fawkes::pcl_utils::set_time(this->
output_, now);
372 merge_output(database, aligned_downsampled_remplane, num_clouds);
373 now.
stamp(); fawkes::pcl_utils::set_time(this->
output_, now);
377 merge_output(database, pcls, actual_times);
379 TIMETRACK_END(ttc_output_);
381 #ifdef USE_TIMETRACKER 382 if (++tt_loopcount_ >= 5) {
384 tt_->print_to_stdout();
392 pcl::SACSegmentation<PointType> tablesegm;
393 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
394 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
396 tablesegm.setOptimizeCoefficients(
true);
397 tablesegm.setModelType(pcl::SACMODEL_PLANE);
398 tablesegm.setMethodType(pcl::SAC_RANSAC);
399 tablesegm.setMaxIterations(1000);
400 tablesegm.setDistanceThreshold(0.022);
402 tablesegm.setInputCloud(cloud);
403 tablesegm.segment(*inliers, *coeff);
405 if (! coeff || coeff->values.empty()) {
409 pcl::ExtractIndices<PointType> extract;
411 extract.setNegative(
true);
412 extract.setInputCloud(cloud);
413 extract.setIndices(inliers);
414 extract.filter(extracted);
417 pcl::ConvexHull<PointType> convex_hull;
418 convex_hull.setDimension(2);
419 convex_hull.setInputCloud(cloud);
422 convex_hull.reconstruct(*hull);
438 pcl::ComparisonOps::CompareOp op =
439 coeff->values[3] < 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT;
442 typename pcl::ConditionAnd<PointType>::Ptr
443 above_cond(
new pcl::ConditionAnd<PointType>());
444 above_cond->addComparison(above_comp);
445 pcl::ConditionalRemoval<PointType> above_condrem(above_cond);
446 above_condrem.setInputCloud(cloud);
449 above_condrem.filter(*cloud_above);
452 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
454 typename pcl::ConditionAnd<PointType>::Ptr
455 polygon_cond(
new pcl::ConditionAnd<PointType>());
459 polygon_cond->addComparison(inpoly_comp);
462 pcl::ConditionalRemoval<PointType> condrem(polygon_cond);
463 condrem.setInputCloud(cloud_above);
464 condrem.filter(*cloud);
468 #ifdef USE_ICP_ALIGNMENT 471 Eigen::Matrix4f &transform)
477 pcl::IterativeClosestPoint<PointType, PointType> icp;
478 icp.setInputCloud(source);
479 icp.setInputTarget(target);
481 icp.setRANSACIterations(cfg_icp_ransac_iterations_);
485 icp.setMaxCorrespondenceDistance(cfg_icp_max_correspondance_distance_);
487 icp.setMaximumIterations(cfg_icp_max_iterations_);
489 icp.setTransformationEpsilon(cfg_icp_transformation_eps_);
491 icp.setEuclideanFitnessEpsilon(cfg_icp_euclidean_fitness_eps_);
498 transform = icp.getFinalTransformation();
501 return icp.hasConverged();
505 #ifdef USE_NDT_ALIGNMENT 511 pcl::NormalDistributionsTransform<PointType, PointType> ndt;
512 ndt.setInputCloud(source);
513 ndt.setInputTarget(target);
517 ndt.setTransformationEpsilon(0.01);
519 ndt.setStepSize(0.1);
521 ndt.setResolution(1.0);
524 ndt.setMaximumIterations(5);
527 transform = ndt.getFinalTransformation();
528 return ndt.hasConverged();
533 merge_output(std::string &database,
535 std::vector<long long> &actual_times)
537 size_t num_points = 0;
538 const size_t num_clouds = clouds.size();
539 for (
unsigned int i = 0; i < num_clouds; ++i) {
540 num_points += clouds[i]->points.size();
542 this->
output_->header.frame_id =
543 cfg_transform_to_sensor_frame_ ? cfg_sensor_frame_ : cfg_global_frame_;
544 this->
output_->points.resize(num_points);
546 this->
output_->width = num_points;
548 for (
unsigned int i = 0; i < num_clouds; ++i) {
550 const size_t cldn = lpcl->points.size();
551 if (cldn == 0)
continue;
553 for (
size_t p = 0; p < cldn; ++p, ++out_p) {
554 const PointType &ip = lpcl->points[p];
562 op.r = cluster_colors[i][0];
563 op.g = cluster_colors[i][1];
564 op.b = cluster_colors[i][2];
568 if (cfg_transform_to_sensor_frame_) {
573 unsigned int ref_pos = clouds.size() - 1;
578 "Restored transforms for %zu frames for range (%li..%li)",
581 actual_times[ref_pos] + this->cfg_transform_range_[1]);
584 fawkes::pcl_utils::get_time(clouds[ref_pos], source_time);
587 source_time, transform_recorded);
590 tf_->
lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
592 fawkes::tf::Transform transform = transform_current * transform_recorded;
595 fawkes::pcl_utils::transform_pointcloud(*(this->
output_), transform);
598 "Failed to transform point cloud, exception follows");
609 std::string cfg_global_frame_;
610 bool cfg_transform_to_sensor_frame_;
611 std::string cfg_sensor_frame_;
612 std::string cfg_fixed_frame_;
613 std::string cfg_passthrough_filter_axis_;
614 float cfg_passthrough_filter_limits_[2];
615 float cfg_downsample_leaf_size_;
616 float cfg_plane_rem_max_iter_;
617 float cfg_plane_rem_dist_thresh_;
618 unsigned int cfg_icp_ransac_iterations_;
619 float cfg_icp_max_correspondance_distance_;
620 unsigned int cfg_icp_max_iterations_;
621 float cfg_icp_transformation_eps_;
622 float cfg_icp_euclidean_fitness_eps_;
624 #ifdef USE_TIMETRACKER 626 unsigned int tt_loopcount_;
627 unsigned int ttc_merge_;
628 unsigned int ttc_retrieval_;
629 unsigned int ttc_transform_global_;
630 unsigned int ttc_downsample_;
631 unsigned int ttc_align_1_;
632 unsigned int ttc_transform_1_;
633 unsigned int ttc_remove_planes_;
634 unsigned int ttc_align_2_;
635 unsigned int ttc_transform_final_;
636 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.
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.