Fawkes API Fawkes Development Version
pcl_db_merge_pipeline.h
1
2/***************************************************************************
3 * pcl_db_merge_pipeline.h - Restore and merge point clouds from MongoDB
4 * Template class for variying point types
5 *
6 * Created: Sat Dec 01 00:15:45 2012 (Freiburg)
7 * Copyright 2012 Tim Niemueller [www.niemueller.de]
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#ifndef _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_MERGE_PIPELINE_H_
24#define _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_MERGE_PIPELINE_H_
25
26#include "mongodb_tf_transformer.h"
27#include "pcl_db_pipeline.h"
28
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>
35#endif
36#include <utils/time/tracker_macros.h>
37
38#define USE_ALIGNMENT
39#define USE_ICP_ALIGNMENT
40// define USE_NDT_ALIGNMENT
41
42#define CFG_PREFIX_MERGE "/perception/pcl-db-merge/"
43
44// missing in Eigen3 causing a compiler error if not included here
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>
54
55#include <assert.h>
56#ifdef USE_ICP_ALIGNMENT
57# include <pcl/registration/icp.h>
58#endif
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
62# endif
63# include <pcl/registration/ndt.h>
64#endif
65
66#include <Eigen/StdVector>
67#include <mongocxx/client.hpp>
68
69/** Point cloud merging pipeline.
70 * This class can merge multiple point clouds which are restored from
71 * a MongoDB database created by mongodb-log.
72 * @author Tim Niemueller
73 */
74template <typename PointType>
76{
77public:
78 /** Constructor.
79 * @param mongodb_client MongoDB client
80 * @param config configuration
81 * @param logger Logger
82 * @param transformer TF transformer for point cloud transformations between
83 * coordinate reference frames
84 * @param output output point cloud
85 */
86 PointCloudDBMergePipeline(mongocxx::client * mongodb_client,
87 fawkes::Configuration * config,
88 fawkes::Logger * logger,
89 fawkes::tf::Transformer * transformer,
91 : PointCloudDBPipeline<PointType>(mongodb_client, config, logger, output), tf_(transformer)
92 {
93 this->name_ = "PCL_DB_MergePL";
94
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");
99 }
100
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) {
106 throw fawkes::Exception("Pasthrough filter limits must be a list "
107 "with exactly two elements");
108 }
109 if (passthrough_filter_limits[1] < passthrough_filter_limits[0]) {
110 throw fawkes::Exception("Passthrough filter limits start cannot be smaller than end");
111 }
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");
126
127 this->logger_->log_info(this->name_,
128 "Age Tolerance: %li "
129 "Limits: [%f, %f] tf range: [%li, %li]",
131 cfg_passthrough_filter_limits_[0],
132 cfg_passthrough_filter_limits_[1],
133 this->cfg_transform_range_[0],
134 this->cfg_transform_range_[1]);
135
136 use_alignment_ = true;
137#ifdef USE_TIMETRACKER
138 tt_ = new fawkes::TimeTracker();
139 tt_loopcount_ = 0;
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");
150#endif
151 }
152
153 /** Destructor. */
155 {
156#ifdef USE_TIMETRACKER
157 delete tt_;
158#endif
159 }
160
161 /** Merge point clouds.
162 * @param times times for which to retrieve the point clouds.
163 * @param database database to retrieve from
164 * @param collection collection from which to retrieve the data
165 */
166 void
167 merge(std::vector<long> &times, std::string &database, std::string &collection)
168 {
169 TIMETRACK_START(ttc_merge_);
170 const unsigned int num_clouds = times.size();
171
172 std::vector<long> actual_times(num_clouds);
173
174 this->output_->points.clear();
175 this->output_->height = 1;
176 this->output_->width = 0;
177 this->output_->is_dense = false;
178
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(
183 num_clouds);
184 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> aligned_downsampled(num_clouds);
185 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> aligned_downsampled_remplane(
186 num_clouds);
187 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transforms(num_clouds
188 - 1);
189
190 for (unsigned int i = 0; i < num_clouds; ++i) {
191 non_transformed[i] = typename PointCloudDBPipeline<PointType>::CloudPtr(
193 non_aligned[i] = typename PointCloudDBPipeline<PointType>::CloudPtr(
195 non_aligned_downsampled[i] = typename PointCloudDBPipeline<PointType>::CloudPtr(
197 aligned_downsampled[i] = typename PointCloudDBPipeline<PointType>::CloudPtr(
199 aligned_downsampled_remplane[i] = typename PointCloudDBPipeline<PointType>::CloudPtr(
201 }
202
203 TIMETRACK_START(ttc_retrieval_);
204
205 pcls =
206 PointCloudDBPipeline<PointType>::retrieve_clouds(times, actual_times, database, collection);
207 if (pcls.empty()) {
208 this->logger_->log_warn(this->name_, "No point clouds found for desired timestamps");
209 TIMETRACK_ABORT(ttc_retrieval_);
210 TIMETRACK_ABORT(ttc_merge_);
211 return;
212 }
213
214 TIMETRACK_INTER(ttc_retrieval_, ttc_transform_global_);
215
216 for (unsigned int i = 0; i < num_clouds; ++i) {
217 // retrieve transforms
218 fawkes::tf::MongoDBTransformer transformer(this->mongodb_client_, database);
219
220 transformer.restore(/* start */ actual_times[i] + this->cfg_transform_range_[0],
221 /* end */ actual_times[i] + this->cfg_transform_range_[1]);
222 this->logger_->log_debug(this->name_,
223 "Restored transforms for %zu frames "
224 "for range (%li..%li)",
225 transformer.get_frame_caches().size(),
226 /* start */ actual_times[i] + this->cfg_transform_range_[0],
227 /* end */ actual_times[i] + this->cfg_transform_range_[1]);
228
229 // transform point clouds to common frame
230 try {
231 fawkes::pcl_utils::transform_pointcloud(cfg_global_frame_, *pcls[i], transformer);
232 } catch (fawkes::Exception &e) {
233 this->logger_->log_warn(this->name_,
234 "Failed to transform from %s to %s",
235 pcls[i]->header.frame_id.c_str(),
236 cfg_global_frame_.c_str());
237 this->logger_->log_warn(this->name_, e);
238 }
239 *non_aligned[i] = *pcls[i];
240 }
241
242 // merge point clouds
243
244 TIMETRACK_END(ttc_transform_global_);
245
246 if (use_alignment_) {
247 // align point clouds, use the first as target
248
249 TIMETRACK_START(ttc_downsample_);
250
251 // ### 1: ALIGN including table points
252
253 // FILTER and DOWNSAMPLE
254
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]);
258
259 //pcl::ApproximateVoxelGrid<PointType> downsample;
260 pcl::VoxelGrid<PointType> downsample;
261 downsample.setLeafSize(cfg_downsample_leaf_size_,
262 cfg_downsample_leaf_size_,
263 cfg_downsample_leaf_size_);
264
267
268 for (unsigned int i = 0; i < num_clouds; ++i) {
269 // downsample for efficient registration/Alignment
270 pass.setInputCloud(pcls[i]);
271 pass.filter(*filtered_z);
272
273 downsample.setInputCloud(filtered_z);
274 downsample.filter(*non_aligned_downsampled[i]);
275 this->logger_->log_info(this->name_,
276 "Filtered cloud %u contains %zu points",
277 i,
278 non_aligned_downsampled[i]->points.size());
279 }
280 TIMETRACK_INTER(ttc_downsample_, ttc_align_1_);
281
282 // ALIGN using ICP including table
283 for (unsigned int i = 1; i < num_clouds; ++i) {
284 this->logger_->log_info(this->name_, "Aligning cloud %u to %u", i, i - 1);
285 Eigen::Matrix4f transform;
287
288 source = non_aligned_downsampled[i];
289 target = non_aligned_downsampled[i - 1];
290
291#ifdef USE_ICP_ALIGNMENT
292 align_icp(source, target, transform);
293
294#elif defined(USE_NDT_ALIGNMENT)
295 align_ndt(source, target);
296#endif
297
298 transforms[i - 1] = transform;
299 }
300
301 TIMETRACK_INTER(ttc_align_1_, ttc_transform_1_);
302
303 // ### 2: ALIGN excluding table points
304
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],
309 transforms[i - 1]);
310 }
311
312 TIMETRACK_INTER(ttc_transform_1_, ttc_remove_planes_);
313
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]);
317 this->logger_->log_info(this->name_,
318 "Removed plane from cloud %u, "
319 "%zu of %zu points remain",
320 i,
321 aligned_downsampled_remplane[i]->points.size(),
322 aligned_downsampled[i]->points.size());
323 }
324
325 TIMETRACK_INTER(ttc_remove_planes_, ttc_align_2_);
326
327 for (unsigned int i = 1; i < num_clouds; ++i) {
328 Eigen::Matrix4f transform;
330
331 source = aligned_downsampled_remplane[i];
332 target = aligned_downsampled_remplane[i - 1];
333
334 align_icp(source, target, transform);
335
337 pcl::transformPointCloud(*aligned_downsampled_remplane[i], tmp, transform);
338 *aligned_downsampled_remplane[i] = tmp;
339
340 transforms[i - 1] *= transform;
341 }
342
343 TIMETRACK_INTER(ttc_align_2_, ttc_transform_final_);
344
345 for (unsigned int i = 1; i < num_clouds; ++i) {
347 pcl::transformPointCloud(*pcls[i], tmp, transforms[i - 1]);
348 *pcls[i] = tmp;
349 }
350
351 TIMETRACK_END(ttc_transform_final_);
352 }
353
354 TIMETRACK_END(ttc_merge_);
355 TIMETRACK_START(ttc_output_);
356
357#ifdef DEBUG_OUTPUT
358 fawkes::Time now;
359
360 merge_output(database, non_transformed, num_clouds);
361 now.stamp();
362 fawkes::pcl_utils::set_time(this->output_, now);
363 usleep(1000000);
364
365 merge_output(database, non_aligned, num_clouds);
366 now.stamp();
367 fawkes::pcl_utils::set_time(this->output_, now);
368 usleep(1000000);
369
370 merge_output(database, non_aligned_downsampled, num_clouds);
371 now.stamp();
372 fawkes::pcl_utils::set_time(this->output_, now);
373 usleep(1000000);
374
375 merge_output(database, aligned_downsampled, num_clouds);
376 now.stamp();
377 fawkes::pcl_utils::set_time(this->output_, now);
378 usleep(1000000);
379
380 merge_output(database, aligned_downsampled_remplane, num_clouds);
381 now.stamp();
382 fawkes::pcl_utils::set_time(this->output_, now);
383 usleep(1000000);
384#endif
385
386 merge_output(database, pcls, actual_times);
387
388 TIMETRACK_END(ttc_output_);
389
390#ifdef USE_TIMETRACKER
391 if (++tt_loopcount_ >= 5) {
392 tt_loopcount_ = 0;
393 tt_->print_to_stdout();
394 }
395#endif
396 }
397
398private: // methods
399 void
400 remove_plane(typename PointCloudDBPipeline<PointType>::CloudPtr &cloud)
401 {
402 pcl::SACSegmentation<PointType> tablesegm;
403 pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients());
404 pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
405
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);
411
412 tablesegm.setInputCloud(cloud);
413 tablesegm.segment(*inliers, *coeff);
414
415 if (!coeff || coeff->values.empty()) {
416 return;
417 }
418
419 pcl::ExtractIndices<PointType> extract;
421 extract.setNegative(true);
422 extract.setInputCloud(cloud);
423 extract.setIndices(inliers);
424 extract.filter(extracted);
425 *cloud = extracted;
426
427 pcl::ConvexHull<PointType> convex_hull;
428 convex_hull.setDimension(2);
429 convex_hull.setInputCloud(cloud);
432 convex_hull.reconstruct(*hull);
433
434 // Use only points above tables
435 // Why coeff->values[3] < 0 ? ComparisonOps::GT : ComparisonOps::LT?
436 // The model coefficients are in Hessian Normal Form, hence coeff[0..2] are
437 // the normal vector. We need to distinguish the cases where the normal vector
438 // points towards the origin (camera) or away from it. This can be checked
439 // by calculating the distance towards the origin, which conveniently in
440 // dist = N * x + p is just p which is coeff[3]. Therefore, if coeff[3] is
441 // negative, the normal vector points towards the frame origin and we want all
442 // points with positive distance from the table plane, otherwise it points
443 // away from the origin and we want points with "negative distance".
444 // We make use of the fact that we only have a boring RGB-D camera and
445 // not an X-Ray...
446 // Note that this assumes that the global frame's XY plane is the ground support
447 // plane!
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);
460
461 // Extract only points on the table plane
462 pcl::PointIndices::Ptr polygon(new pcl::PointIndices());
463
464 typename pcl::ConditionAnd<PointType>::Ptr polygon_cond(new pcl::ConditionAnd<PointType>());
465
468 polygon_cond->addComparison(inpoly_comp);
469
470 // build the filter
471 pcl::ConditionalRemoval<PointType> condrem;
472 condrem.setCondition(polygon_cond);
473 condrem.setInputCloud(cloud_above);
474 condrem.filter(*cloud);
475 }
476
477#ifdef USE_ICP_ALIGNMENT
478 bool
479 align_icp(typename PointCloudDBPipeline<PointType>::CloudConstPtr source,
481 Eigen::Matrix4f & transform)
482 {
484
485 //pcl::console::VERBOSITY_LEVEL old_level = pcl::console::getVerbosityLevel();
486 //pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
487 pcl::IterativeClosestPoint<PointType, PointType> icp;
488 icp.setInputSource(source);
489 icp.setInputTarget(target);
490
491 icp.setRANSACIterations(cfg_icp_ransac_iterations_);
492
493 // Set the max correspondence distance to 5cm
494 // (e.g., correspondences with higher distances will be ignored)
495 icp.setMaxCorrespondenceDistance(cfg_icp_max_correspondance_distance_);
496 // Set the maximum number of iterations (criterion 1)
497 icp.setMaximumIterations(cfg_icp_max_iterations_);
498 // Set the transformation epsilon (criterion 2)
499 icp.setTransformationEpsilon(cfg_icp_transformation_eps_);
500 // Set the euclidean distance difference epsilon (criterion 3)
501 icp.setEuclideanFitnessEpsilon(cfg_icp_euclidean_fitness_eps_);
502
503 this->logger_->log_info(this->name_, "Aligning");
504 icp.align(final);
505 this->logger_->log_info(this->name_, "Aligning done");
506 //this->logger_->log_info(this->name_, "ICP %u -> %u did%s converge, score: %f",
507 // icp.hasConverged() ? "" : " NOT", icp.getFitnessScore());
508 transform = icp.getFinalTransformation();
509 //score = icp.getFitnessScore();
510 //pcl::console::setVerbosityLevel(old_level);
511 return icp.hasConverged();
512 }
513#endif
514
515#ifdef USE_NDT_ALIGNMENT
516 // untested
517 bool
518 align_ndt(CloudConstPtr source, CloudConstPtr target, Eigen::Matrix4f &transform)
519 {
520 Cloud final;
521
522 pcl::NormalDistributionsTransform<PointType, PointType> ndt;
523 ndt.setInputCloud(source);
524 ndt.setInputTarget(target);
525
526 // Setting scale dependent NDT parameters
527 // Setting minimum transformation difference for termination condition.
528 ndt.setTransformationEpsilon(0.01);
529 // Setting maximum step size for More-Thuente line search.
530 ndt.setStepSize(0.1);
531 //Setting Resolution of NDT grid structure (VoxelGridCovariance).
532 ndt.setResolution(1.0);
533
534 // Setting max number of registration iterations.
535 ndt.setMaximumIterations(5);
536
537 ndt.align(final);
538 transform = ndt.getFinalTransformation();
539 return ndt.hasConverged();
540 }
541#endif
542
543 void
544 merge_output(std::string & database,
545 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> clouds,
546 std::vector<long> & actual_times)
547 {
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();
552 }
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);
556 this->output_->height = 1;
557 this->output_->width = num_points;
558 size_t out_p = 0;
559 for (unsigned int i = 0; i < num_clouds; ++i) {
560 const typename PointCloudDBPipeline<PointType>::CloudPtr &lpcl = clouds[i];
561 const size_t cldn = lpcl->points.size();
562 if (cldn == 0)
563 continue;
564
565 for (size_t p = 0; p < cldn; ++p, ++out_p) {
566 const PointType & ip = lpcl->points[p];
567 typename PointCloudDBPipeline<PointType>::ColorPointType &op = this->output_->points[out_p];
568
569 op.x = ip.x;
570 op.y = ip.y;
571 op.z = ip.z;
572
573 op.r = cluster_colors[i][0];
574 op.g = cluster_colors[i][1];
575 op.b = cluster_colors[i][2];
576 }
577 }
578
579 if (cfg_transform_to_sensor_frame_) {
580 // retrieve transforms
581 fawkes::tf::MongoDBTransformer transformer(this->mongodb_client_, database);
582
583 unsigned int ref_pos = clouds.size() - 1;
584
585 transformer.restore(/* start */ actual_times[ref_pos] + this->cfg_transform_range_[0],
586 /* end */ actual_times[ref_pos] + this->cfg_transform_range_[1]);
587 this->logger_->log_debug(this->name_,
588 "Restored transforms for %zu frames for range (%li..%li)",
589 transformer.get_frame_caches().size(),
590 /* start */ actual_times[ref_pos] + this->cfg_transform_range_[0],
591 /* end */ actual_times[ref_pos] + this->cfg_transform_range_[1]);
592
593 fawkes::Time source_time;
594 fawkes::pcl_utils::get_time(clouds[ref_pos], source_time);
595 fawkes::tf::StampedTransform transform_recorded;
596 transformer.lookup_transform(cfg_fixed_frame_,
597 cfg_global_frame_,
598 source_time,
599 transform_recorded);
600
601 fawkes::tf::StampedTransform transform_current;
602 tf_->lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
603
604 fawkes::tf::Transform transform = transform_current * transform_recorded;
605
606 try {
607 fawkes::pcl_utils::transform_pointcloud(*(this->output_), transform);
608 } catch (fawkes::Exception &e) {
609 this->logger_->log_warn(this->name_, "Failed to transform point cloud, exception follows");
610 this->logger_->log_warn(this->name_, e);
611 }
612 }
613 }
614
615private: // members
617
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_;
632
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_;
646#endif
647
648 bool use_alignment_;
649};
650
651#endif
Point cloud merging pipeline.
PointCloudDBMergePipeline(mongocxx::client *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, fawkes::tf::Transformer *transformer, typename PointCloudDBPipeline< PointType >::ColorCloudPtr output)
Constructor.
void merge(std::vector< long > &times, std::string &database, std::string &collection)
Merge point clouds.
virtual ~PointCloudDBMergePipeline()
Destructor.
Database point cloud pipeline base class.
long cfg_transform_range_[2]
Transform range start and end times.
fawkes::Logger * logger_
Logger for informative messages.
ColorCloud::Ptr ColorCloudPtr
Shared pointer to colored cloud.
ColorCloudPtr output_
The final (colored) output of the pipeline.
pcl::PointXYZRGB ColorPointType
Colored point type.
mongocxx::client * mongodb_client_
MongoDB client to retrieve data.
long cfg_pcl_age_tolerance_
Age tolerance for retrieved point clouds.
Cloud::Ptr CloudPtr
Shared pointer to cloud.
pcl::PointCloud< PointType > Cloud
Basic point cloud type.
Cloud::ConstPtr CloudConstPtr
Shared pointer to constant cloud.
std::vector< CloudPtr > retrieve_clouds(std::vector< long > &times, std::vector< long > &actual_times, std::string &database, std::string &collection_name)
Retrieve point clouds from database.
const char * name_
Name of the pipeline.
Interface for configuration handling.
Definition: config.h:68
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::vector< float > get_floats(const char *path)=0
Get list of values 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.
Definition: exception.h:36
Interface for logging.
Definition: logger.h:42
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_info(const char *component, const char *format,...)=0
Log informational message.
Time tracking utility.
Definition: tracker.h:37
A class for handling time.
Definition: time.h:93
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
Compare points' distance to a plane.
Definition: comparisons.h:101
pcl::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:108
Check if point is inside or outside a given polygon.
Definition: comparisons.h:45
pcl::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:52
Read transforms from MongoDB and answer queries.
void restore(fawkes::Time &start, fawkes::Time &end)
Restore transforms from database.
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
std::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.