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  */
74 template <typename PointType>
76 {
77 public:
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 
265  typename PointCloudDBPipeline<PointType>::CloudPtr filtered_z(
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;
286  typename PointCloudDBPipeline<PointType>::CloudConstPtr source, target;
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;
329  typename PointCloudDBPipeline<PointType>::CloudConstPtr source, target;
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 
398 private: // 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;
420  typename PointCloudDBPipeline<PointType>::Cloud extracted;
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);
457  typename PointCloudDBPipeline<PointType>::CloudPtr cloud_above(
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.setInputCloud(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 
615 private: // 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
634  fawkes::TimeTracker *tt_;
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
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:98
PointCloudDBPipeline::CloudConstPtr
Cloud::ConstPtr CloudConstPtr
Shared pointer to constant cloud.
Definition: pcl_db_pipeline.h:103
PointCloudDBMergePipeline::PointCloudDBMergePipeline
PointCloudDBMergePipeline(mongocxx::client *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, fawkes::tf::Transformer *transformer, typename PointCloudDBPipeline< PointType >::ColorCloudPtr output)
Constructor.
Definition: pcl_db_merge_pipeline.h:86
PointCloudDBPipeline
Database point cloud pipeline base class.
Definition: pcl_db_pipeline.h:90
fawkes::tf::MongoDBTransformer
Definition: mongodb_tf_transformer.h:42
PointCloudDBPipeline::output_
ColorCloudPtr output_
The final (colored) output of the pipeline.
Definition: pcl_db_pipeline.h:331
fawkes::pcl_utils::PolygonComparison::ConstPtr
boost::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:61
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
fawkes::tf::Transformer
Definition: transformer.h:71
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
PointCloudDBMergePipeline
Point cloud merging pipeline.
Definition: pcl_db_merge_pipeline.h:75
PointCloudDBMergePipeline::merge
void merge(std::vector< long > &times, std::string &database, std::string &collection)
Merge point clouds.
Definition: pcl_db_merge_pipeline.h:167
fawkes::Configuration
Definition: config.h:68
fawkes::tf::MongoDBTransformer::restore
void restore(fawkes::Time &start, fawkes::Time &end)
Restore transforms from database.
Definition: mongodb_tf_transformer.h:59
PointCloudDBPipeline::logger_
fawkes::Logger * logger_
Logger for informative messages.
Definition: pcl_db_pipeline.h:329
fawkes::Configuration::get_floats
virtual std::vector< float > get_floats(const char *path)=0
PointCloudDBMergePipeline::~PointCloudDBMergePipeline
virtual ~PointCloudDBMergePipeline()
Destructor.
Definition: pcl_db_merge_pipeline.h:154
fawkes::Logger
Definition: logger.h:40
PointCloudDBPipeline::ColorPointType
pcl::PointXYZRGB ColorPointType
Colored point type.
Definition: pcl_db_pipeline.h:97
PointCloudDBPipeline::retrieve_clouds
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.
Definition: pcl_db_pipeline.h:267
PointCloudDBPipeline::mongodb_client_
mongocxx::client * mongodb_client_
MongoDB client to retrieve data.
Definition: pcl_db_pipeline.h:327
fawkes::tf::Transformer::get_frame_caches
std::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.
Definition: transformer.cpp:194
pcl::PointCloud< PointType >
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
PointCloudDBPipeline::name_
const char * name_
Name of the pipeline.
Definition: pcl_db_pipeline.h:322
PointCloudDBPipeline::cfg_pcl_age_tolerance_
long cfg_pcl_age_tolerance_
Age tolerance for retrieved point clouds.
Definition: pcl_db_pipeline.h:324
fawkes::pcl_utils::PlaneDistanceComparison::ConstPtr
boost::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:113
fawkes::tf::Transformer::lookup_transform
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
Definition: transformer.cpp:272
fawkes::pcl_utils::PlaneDistanceComparison
Compare points' distance to a plane.
Definition: comparisons.h:105
fawkes::pcl_utils::PolygonComparison
Check if point is inside or outside a given polygon.
Definition: comparisons.h:49
PointCloudDBPipeline::Cloud
pcl::PointCloud< PointType > Cloud
Basic point cloud type.
Definition: pcl_db_pipeline.h:94
fawkes::Time
Definition: time.h:96
PointCloudDBPipeline::cfg_transform_range_
long cfg_transform_range_[2]
Transform range start and end times.
Definition: pcl_db_pipeline.h:325
fawkes::TimeTracker
Definition: tracker.h:40
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
PointCloudDBPipeline::CloudPtr
Cloud::Ptr CloudPtr
Shared pointer to cloud.
Definition: pcl_db_pipeline.h:101
PointCloudDBPipeline::ColorCloudPtr
ColorCloud::Ptr ColorCloudPtr
Shared pointer to colored cloud.
Definition: pcl_db_pipeline.h:106
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::Time::stamp
Time & stamp()
Set this time to the current time.
Definition: time.cpp:709
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Exception
Definition: exception.h:39