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