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