ICP.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2012,
6 Fran├žois Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 
36 #include "PointMatcher.h"
37 #include "PointMatcherPrivate.h"
38 #include "Timer.h"
39 
40 #include "LoggerImpl.h"
41 #include "TransformationsImpl.h"
42 #include "DataPointsFiltersImpl.h"
43 #include "MatchersImpl.h"
44 #include "OutlierFiltersImpl.h"
45 #include "ErrorMinimizersImpl.h"
47 #include "InspectorsImpl.h"
48 
49 #include <yaml-cpp/yaml.h>
50 
51 using namespace std;
52 using namespace PointMatcherSupport;
53 
55 InvalidModuleType::InvalidModuleType(const std::string& reason):
56  runtime_error(reason)
57 {}
58 
60 template<typename T>
62  prefilteredReadingPtsCount(0),
63  prefilteredReferencePtsCount(0),
64  maxNumIterationsReached(false)
65 {}
66 
68 template<typename T>
70 {
71 }
72 
74 template<typename T>
76 {
77  transformations.clear();
78  readingDataPointsFilters.clear();
79  readingStepDataPointsFilters.clear();
80  referenceDataPointsFilters.clear();
81  matcher.reset();
82  outlierFilters.clear();
83  errorMinimizer.reset();
84  transformationCheckers.clear();
85  inspector.reset();
86 }
87 
89 template<typename T>
91 {
92 }
93 
95 template<typename T>
97 {
98  this->cleanup();
99 
100  this->transformations.push_back(std::make_shared<typename TransformationsImpl<T>::RigidTransformation>());
101  this->readingDataPointsFilters.push_back(std::make_shared<typename DataPointsFiltersImpl<T>::RandomSamplingDataPointsFilter>());
102  this->referenceDataPointsFilters.push_back(std::make_shared<typename DataPointsFiltersImpl<T>::SamplingSurfaceNormalDataPointsFilter>());
103  this->outlierFilters.push_back(std::make_shared<typename OutlierFiltersImpl<T>::TrimmedDistOutlierFilter>());
104  this->matcher = std::make_shared<typename MatchersImpl<T>::KDTreeMatcher>();
105  this->errorMinimizer = std::make_shared<PointToPlaneErrorMinimizer<T> >();
106  this->transformationCheckers.push_back(std::make_shared<typename TransformationCheckersImpl<T>::CounterTransformationChecker>());
107  this->transformationCheckers.push_back(std::make_shared<typename TransformationCheckersImpl<T>::DifferentialTransformationChecker>());
108  this->inspector = std::make_shared<typename InspectorsImpl<T>::NullInspector>();
109 }
110 
112 template<typename T>
114 {
115  this->cleanup();
116 
117  YAML::Node doc = YAML::Load(in);
118  typedef set<string> StringSet;
119  StringSet usedModuleTypes;
120 
121  // Fix for issue #6: compilation on gcc 4.4.4
122  //PointMatcher<T> pm;
123  const PointMatcher & pm = PointMatcher::get();
124 
125  {
126  // NOTE: The logger needs to be initialize first to allow ouput from other contructors
127  boost::mutex::scoped_lock lock(loggerMutex);
128  usedModuleTypes.insert(createModuleFromRegistrar("logger", doc, pm.REG(Logger), logger));
129  }
130  usedModuleTypes.insert(createModulesFromRegistrar("readingDataPointsFilters", doc, pm.REG(DataPointsFilter), readingDataPointsFilters));
131  usedModuleTypes.insert(createModulesFromRegistrar("readingStepDataPointsFilters", doc, pm.REG(DataPointsFilter), readingStepDataPointsFilters));
132  usedModuleTypes.insert(createModulesFromRegistrar("referenceDataPointsFilters", doc, pm.REG(DataPointsFilter), referenceDataPointsFilters));
133  //usedModuleTypes.insert(createModulesFromRegistrar("transformations", doc, pm.REG(Transformation), transformations));
134  usedModuleTypes.insert(createModuleFromRegistrar("matcher", doc, pm.REG(Matcher), matcher));
135  usedModuleTypes.insert(createModulesFromRegistrar("outlierFilters", doc, pm.REG(OutlierFilter), outlierFilters));
136  usedModuleTypes.insert(createModuleFromRegistrar("errorMinimizer", doc, pm.REG(ErrorMinimizer), errorMinimizer));
137 
138  // See if to use a rigid transformation
139  if (nodeVal("errorMinimizer", doc) != "PointToPointSimilarityErrorMinimizer")
140  this->transformations.push_back(std::make_shared<typename TransformationsImpl<T>::RigidTransformation>());
141  else
142  this->transformations.push_back(std::make_shared<typename TransformationsImpl<T>::SimilarityTransformation>());
143 
144  usedModuleTypes.insert(createModulesFromRegistrar("transformationCheckers", doc, pm.REG(TransformationChecker), transformationCheckers));
145  usedModuleTypes.insert(createModuleFromRegistrar("inspector", doc, pm.REG(Inspector),inspector));
146 
147 
148  // FIXME: this line cause segfault when there is an error in the yaml file...
149  //loadAdditionalYAMLContent(doc);
150 
151  // check YAML entries that do not correspend to any module
152  for(YAML::const_iterator moduleTypeIt = doc.begin(); moduleTypeIt != doc.end(); ++moduleTypeIt)
153  {
154  std::string moduleType = moduleTypeIt->first.as<std::string>();
155 
156  if (usedModuleTypes.find(moduleType) == usedModuleTypes.end())
157  throw InvalidModuleType(
158  (boost::format("Module type %1% does not exist") % moduleType).str()
159  );
160  }
161 }
162 
164 template<typename T>
166 {
167  return prefilteredReadingPtsCount;
168 }
169 
171 template<typename T>
173 {
174  return prefilteredReferencePtsCount;
175 }
176 
178 template<typename T>
180 {
181  return maxNumIterationsReached;
182 }
183 
185 template<typename T>
186 template<typename R>
187 const std::string& PointMatcher<T>::ICPChainBase::createModulesFromRegistrar(const std::string& regName, const YAML::Node& doc, const R& registrar, std::vector<std::shared_ptr<typename R::TargetType> >& modules)
188 {
189  const YAML::Node reg = doc[regName];
190 
191  if (reg)
192  {
193  //cout << regName << endl;
194  for(YAML::const_iterator moduleIt = reg.begin(); moduleIt != reg.end(); ++moduleIt)
195  {
196  const YAML::Node& module(*moduleIt);
197  modules.push_back(registrar.createFromYAML(module));
198  }
199  }
200 
201  return regName;
202 }
203 
205 template<typename T>
206 template<typename R>
207 const std::string& PointMatcher<T>::ICPChainBase::createModuleFromRegistrar(const std::string& regName, const YAML::Node& doc, const R& registrar, std::shared_ptr<typename R::TargetType>& module)
208 {
209  const YAML::Node reg = doc[regName];
210 
211  if (reg)
212  {
213  //cout << regName << endl;
214  module = registrar.createFromYAML(reg);
215  }
216  else
217  module.reset();
218 
219  return regName;
220 }
221 
222 template<typename T>
223 std::string PointMatcher<T>::ICPChainBase::nodeVal(const std::string& regName, const YAML::Node& doc)
224 {
225  const YAML::Node reg = doc[regName];
226 
227  if (reg)
228  {
232 
233  return name;
234  }
235 
236  return "";
237 }
238 
239 template struct PointMatcher<float>::ICPChainBase;
240 template struct PointMatcher<double>::ICPChainBase;
241 
242 
244 template<typename T>
246  const DataPoints& readingIn,
247  const DataPoints& referenceIn)
248 {
249  const int dim = readingIn.features.rows();
250  const TransformationParameters identity = TransformationParameters::Identity(dim, dim);
251  return this->compute(readingIn, referenceIn, identity);
252 }
253 
255 template<typename T>
257  const DataPoints& readingIn,
258  const DataPoints& referenceIn,
259  const TransformationParameters& initialTransformationParameters)
260 {
261  return this->compute(readingIn, referenceIn, initialTransformationParameters);
262 }
263 
265 template<typename T>
267  const DataPoints& readingIn,
268  const DataPoints& referenceIn,
269  const TransformationParameters& T_refIn_dataIn)
270 {
271  // Ensuring minimum definition of components
272  if (!this->matcher)
273  throw runtime_error("You must setup a matcher before running ICP");
274  if (!this->errorMinimizer)
275  throw runtime_error("You must setup an error minimizer before running ICP");
276  if (!this->inspector)
277  throw runtime_error("You must setup an inspector before running ICP");
278 
279  this->inspector->init();
280 
281  timer t; // Print how long take the algo
282  const int dim(referenceIn.features.rows());
283 
284  // Apply reference filters
285  // reference is express in frame <refIn>
286  DataPoints reference(referenceIn);
287  this->referenceDataPointsFilters.init();
288  this->referenceDataPointsFilters.apply(reference);
289 
290  // Create intermediate frame at the center of mass of reference pts cloud
291  // this help to solve for rotations
292  const int nbPtsReference = reference.features.cols();
293  const Vector meanReference = reference.features.rowwise().sum() / nbPtsReference;
294  TransformationParameters T_refIn_refMean(Matrix::Identity(dim, dim));
295  T_refIn_refMean.block(0,dim-1, dim-1, 1) = meanReference.head(dim-1);
296 
297  // Reajust reference position:
298  // from here reference is express in frame <refMean>
299  // Shortcut to do T_refIn_refMean.inverse() * reference
300  reference.features.topRows(dim-1).colwise() -= meanReference.head(dim-1);
301 
302  // Init matcher with reference points center on its mean
303  this->matcher->init(reference);
304 
305  // statistics on last step
306  this->inspector->addStat("ReferencePreprocessingDuration", t.elapsed());
307  this->inspector->addStat("ReferenceInPointCount", referenceIn.features.cols());
308  this->inspector->addStat("ReferencePointCount", reference.features.cols());
309  LOG_INFO_STREAM("PointMatcher::icp - reference pre-processing took " << t.elapsed() << " [s]");
310  this->prefilteredReferencePtsCount = reference.features.cols();
311 
312  return computeWithTransformedReference(readingIn, reference, T_refIn_refMean, T_refIn_dataIn);
313 
314 }
315 
317 template<typename T>
319  const DataPoints& readingIn,
320  const DataPoints& reference,
321  const TransformationParameters& T_refIn_refMean,
322  const TransformationParameters& T_refIn_dataIn)
323 {
324  const int dim(reference.features.rows());
325 
326  if (T_refIn_dataIn.cols() != T_refIn_dataIn.rows()) {
327  throw runtime_error("The initial transformation matrix must be squared.");
328  }
329  if (dim != T_refIn_dataIn.cols()) {
330  throw runtime_error("The shape of initial transformation matrix must be NxN. "
331  "Where N is the number of rows in the read/reference scans.");
332  }
333 
334  timer t; // Print how long take the algo
335 
336  // Apply readings filters
337  // reading is express in frame <dataIn>
338  DataPoints reading(readingIn);
339  //const int nbPtsReading = reading.features.cols();
340  this->readingDataPointsFilters.init();
341  this->readingDataPointsFilters.apply(reading);
342  readingFiltered = reading;
343 
344  // Reajust reading position:
345  // from here reading is express in frame <refMean>
347  T_refMean_dataIn = T_refIn_refMean.inverse() * T_refIn_dataIn;
348  this->transformations.apply(reading, T_refMean_dataIn);
349 
350  // Prepare reading filters used in the loop
351  this->readingStepDataPointsFilters.init();
352 
353  // Since reading and reference are express in <refMean>
354  // the frame <refMean> is equivalent to the frame <iter(0)>
355  TransformationParameters T_iter = Matrix::Identity(dim, dim);
356 
357  bool iterate(true);
358  this->maxNumIterationsReached = false;
359  this->transformationCheckers.init(T_iter, iterate);
360 
361  size_t iterationCount(0);
362 
363  // statistics on last step
364  this->inspector->addStat("ReadingPreprocessingDuration", t.elapsed());
365  this->inspector->addStat("ReadingInPointCount", readingIn.features.cols());
366  this->inspector->addStat("ReadingPointCount", reading.features.cols());
367  LOG_INFO_STREAM("PointMatcher::icp - reading pre-processing took " << t.elapsed() << " [s]");
368  this->prefilteredReadingPtsCount = reading.features.cols();
369  t.restart();
370 
371  // iterations
372  while (iterate)
373  {
374  DataPoints stepReading(reading);
375 
376  //-----------------------------
377  // Apply step filter
378  this->readingStepDataPointsFilters.apply(stepReading);
379 
380  //-----------------------------
381  // Transform Readings
382  this->transformations.apply(stepReading, T_iter);
383 
384  //-----------------------------
385  // Match to closest point in Reference
386  const Matches matches(
387  this->matcher->findClosests(stepReading)
388  );
389 
390  //-----------------------------
391  // Detect outliers
392  const OutlierWeights outlierWeights(
393  this->outlierFilters.compute(stepReading, reference, matches)
394  );
395 
396  assert(outlierWeights.rows() == matches.ids.rows());
397  assert(outlierWeights.cols() == matches.ids.cols());
398 
399  //cout << "outlierWeights: " << outlierWeights << "\n";
400 
401 
402  //-----------------------------
403  // Dump
404  this->inspector->dumpIteration(
405  iterationCount, T_iter, reference, stepReading, matches, outlierWeights, this->transformationCheckers
406  );
407 
408  //-----------------------------
409  // Error minimization
410  // equivalent to:
411  // T_iter(i+1)_iter(0) = T_iter(i+1)_iter(i) * T_iter(i)_iter(0)
412  T_iter = this->errorMinimizer->compute(
413  stepReading, reference, outlierWeights, matches) * T_iter;
414 
415  // Old version
416  //T_iter = T_iter * this->errorMinimizer->compute(
417  // stepReading, reference, outlierWeights, matches);
418 
419  // in test
420  try
421  {
422  this->transformationCheckers.check(T_iter, iterate);
423  }
425  {
426  iterate = false;
427  this->maxNumIterationsReached = true;
428  }
429 
430  ++iterationCount;
431  }
432 
433  this->inspector->addStat("IterationsCount", iterationCount);
434  this->inspector->addStat("PointCountTouched", this->matcher->getVisitCount());
435  this->matcher->resetVisitCount();
436  this->inspector->addStat("OverlapRatio", this->errorMinimizer->getWeightedPointUsedRatio());
437  this->inspector->addStat("ConvergenceDuration", t.elapsed());
438  this->inspector->finish(iterationCount);
439 
440  LOG_INFO_STREAM("PointMatcher::icp - " << iterationCount << " iterations took " << t.elapsed() << " [s]");
441 
442  // Move transformation back to original coordinate (without center of mass)
443  // T_iter is equivalent to: T_iter(i+1)_iter(0)
444  // the frame <iter(0)> equals <refMean>
445  // so we have:
446  // T_iter(i+1)_dataIn = T_iter(i+1)_iter(0) * T_refMean_dataIn
447  // T_iter(i+1)_dataIn = T_iter(i+1)_iter(0) * T_iter(0)_dataIn
448  // T_refIn_refMean remove the temperary frame added during initialization
449  return (T_refIn_refMean * T_iter * T_refMean_dataIn);
450 }
451 
452 template struct PointMatcher<float>::ICP;
453 template struct PointMatcher<double>::ICP;
454 
455 
457 template<typename T>
459 {
460  return (mapPointCloud.features.cols() != 0);
461 }
462 
464 template<typename T>
466 {
467  // Ensuring minimum definition of components
468  if (!this->matcher)
469  throw runtime_error("You must setup a matcher before running ICP");
470  if (!this->inspector)
471  throw runtime_error("You must setup an inspector before running ICP");
472 
473  timer t; // Print how long take the algo
474  const int dim(inputCloud.features.rows());
475  const int ptCount(inputCloud.features.cols());
476 
477  // update keyframe
478  if (ptCount == 0)
479  {
480  LOG_WARNING_STREAM("Ignoring attempt to create a map from an empty cloud");
481  return false;
482  }
483 
484  this->inspector->addStat("MapPointCount", inputCloud.features.cols());
485 
486  // Set map
487  mapPointCloud = inputCloud;
488 
489  // Create intermediate frame at the center of mass of reference pts cloud
490  // this help to solve for rotations
491  const Vector meanMap = mapPointCloud.features.rowwise().sum() / ptCount;
492  T_refIn_refMean = Matrix::Identity(dim, dim);
493  T_refIn_refMean.block(0,dim-1, dim-1, 1) = meanMap.head(dim-1);
494 
495  // Reajust reference position (only translations):
496  // from here reference is express in frame <refMean>
497  // Shortcut to do T_refIn_refMean.inverse() * reference
498  mapPointCloud.features.topRows(dim-1).colwise() -= meanMap.head(dim-1);
499 
500  // Apply reference filters
501  this->referenceDataPointsFilters.init();
502  this->referenceDataPointsFilters.apply(mapPointCloud);
503 
504  this->matcher->init(mapPointCloud);
505 
506  this->inspector->addStat("SetMapDuration", t.elapsed());
507 
508  return true;
509 }
510 
512 template<typename T>
514 {
515  const int dim(mapPointCloud.features.rows());
516  T_refIn_refMean = Matrix::Identity(dim, dim);
517  mapPointCloud = DataPoints();
518 }
519 
520 template<typename T>
522 {
524 
525  if(mapPointCloud.getNbPoints() > 0)
526  {
527  this->matcher->init(mapPointCloud);
528  }
529 }
530 
531 template<typename T>
533 {
535 
536  if(mapPointCloud.getNbPoints() > 0)
537  {
538  this->matcher->init(mapPointCloud);
539  }
540 }
541 
543 template<typename T>
545 {
546  DataPoints globalMap(mapPointCloud);
547  if(this->hasMap())
548  {
549  const int dim(mapPointCloud.features.rows());
550  const Vector meanMapNonHomo(T_refIn_refMean.block(0,dim-1, dim-1, 1));
551  globalMap.features.topRows(dim-1).colwise() += meanMapNonHomo;
552  }
553 
554  return globalMap;
555 }
556 
558 template<typename T>
561 }
562 
564 template<typename T>
566 {
567  return mapPointCloud;
568 }
569 
571 template<typename T>
574 }
575 
577 template<typename T>
579  const DataPoints& cloudIn)
580 {
581  const int dim = cloudIn.features.rows();
582  const TransformationParameters identity = TransformationParameters::Identity(dim, dim);
583  return this->compute(cloudIn, identity);
584 }
585 
587 template<typename T>
589  const DataPoints& cloudIn, const TransformationParameters& T_dataInOld_dataInNew)
590 {
591  return this->compute(cloudIn, T_dataInOld_dataInNew);
592 }
593 
595 template<typename T>
597  const DataPoints& cloudIn, const TransformationParameters& T_refIn_dataIn)
598 {
599  // initial keyframe
600  if (!hasMap())
601  {
602  const int dim(cloudIn.features.rows());
603  LOG_WARNING_STREAM("Ignoring attempt to perform ICP with an empty map");
604  return Matrix::Identity(dim, dim);
605  }
606 
607  this->inspector->init();
608 
609  return this->computeWithTransformedReference(cloudIn, mapPointCloud, T_refIn_refMean, T_refIn_dataIn);
610 }
611 
612 template struct PointMatcher<float>::ICPSequence;
613 template struct PointMatcher<double>::ICPSequence;
PointMatcher::ICPSequence
Definition: PointMatcher.h:733
PointMatcher::ICPSequence::setDefault
virtual void setDefault()
Construct an ICP algorithm that works in most of the cases.
Definition: ICP.cpp:521
compute_overlap.reference
reference
Definition: compute_overlap.py:71
PointMatcher::ICPChainBase::cleanup
void cleanup()
Clean chain up, empty all filters and delete associated objects.
Definition: ICP.cpp:75
PointMatcher::ICPChainBase::loadFromYaml
virtual void loadFromYaml(std::istream &in)
Construct an ICP algorithm from a YAML file.
Definition: ICP.cpp:113
PointMatcher::ICPSequence::clearMap
void clearMap()
Clear the map (reset to same state as after the object is created)
Definition: ICP.cpp:513
PointMatcher::ICPSequence::hasMap
bool hasMap() const
Return whether the object currently holds a valid map.
Definition: ICP.cpp:458
PointMatcher::ICPChainBase::createModuleFromRegistrar
const std::string & createModuleFromRegistrar(const std::string &regName, const YAML::Node &doc, const R &registrar, std::shared_ptr< typename R::TargetType > &module)
Instantiate a module if its name is in the YAML file.
Definition: ICP.cpp:207
TransformationCheckersImpl::CounterTransformationChecker::MaxNumIterationsReached
Struct used to inform through an exeption that ICP reached max number of iterations.
Definition: TransformationCheckersImpl.h:61
PointMatcher::ICPSequence::compute
TransformationParameters compute(const DataPoints &cloudIn, const TransformationParameters &initialTransformationParameters)
Apply ICP to cloud cloudIn, with initial guess.
Definition: ICP.cpp:596
compute_overlap.transformations
transformations
Definition: compute_overlap.py:45
LoggerImpl.h
timer
PointMatcher::OutlierWeights
Matrix OutlierWeights
Weights of the associations between the points in Matches and the points in the reference.
Definition: PointMatcher.h:397
DataPoints
PM::DataPoints DataPoints
Definition: pypoint_matcher_helper.h:16
icp_customized.name
string name
Definition: icp_customized.py:45
PointMatcher::ICP::compute
TransformationParameters compute(const DataPoints &readingIn, const DataPoints &referenceIn, const TransformationParameters &initialTransformationParameters)
Perform ICP from initial guess and return optimised transformation matrix.
Definition: ICP.cpp:266
LOG_INFO_STREAM
#define LOG_INFO_STREAM(args)
Definition: PointMatcherPrivate.h:58
OutlierFiltersImpl.h
PointMatcher::ICPChainBase::createModulesFromRegistrar
const std::string & createModulesFromRegistrar(const std::string &regName, const YAML::Node &doc, const R &registrar, std::vector< std::shared_ptr< typename R::TargetType > > &modules)
Instantiate modules if their names are in the YAML file.
Definition: ICP.cpp:187
PointMatcher::ICPChainBase::loadAdditionalYAMLContent
virtual void loadAdditionalYAMLContent(YAML::Node &doc)
Hook to load addition subclass-specific content from the YAML file.
Definition: ICP.cpp:90
PointMatcherSupport::loggerMutex
boost::mutex loggerMutex
mutex to protect access to logging
Definition: Logger.cpp:41
PointMatcher::ICPChainBase::setDefault
virtual void setDefault()
Construct an ICP algorithm that works in most of the cases.
Definition: ICP.cpp:96
timer::restart
void restart()
PointMatcher::ICPSequence::setMap
bool setMap(const DataPoints &map)
Set the map using inputCloud.
Definition: ICP.cpp:465
PointMatcher::OutlierFilter
An outlier filter removes or weights links between points in reading and their matched points in refe...
Definition: PointMatcher.h:499
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
PointMatcherPrivate.h
PointMatcherSupport::Logger
The logger interface, used to output warnings and informations.
Definition: PointMatcher.h:104
PointMatcher::ICPChainBase
Stuff common to all ICP algorithms.
Definition: PointMatcher.h:655
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
PointMatcher::Matcher
A matcher links points in the reading to points in the reference.
Definition: PointMatcher.h:473
ErrorMinimizersImpl.h
testing::internal::string
::std::string string
Definition: gtest.h:1979
PointMatcher::ICPSequence::operator()
TransformationParameters operator()(const DataPoints &cloudIn)
Apply ICP to cloud cloudIn, with identity as initial guess.
Definition: ICP.cpp:578
PointMatcherSupport::InvalidModuleType
An exception thrown when one tries to use a module type that does not exist.
Definition: PointMatcher.h:83
PointMatcher::ICPChainBase::nodeVal
std::string nodeVal(const std::string &regName, const YAML::Node &doc)
Get the value of a field in a node.
Definition: ICP.cpp:223
PointMatcher::ICPSequence::getPrefilteredInternalMap
const DataPoints & getPrefilteredInternalMap() const
Return the map, in internal coordinates (fast)
Definition: ICP.cpp:565
TransformationCheckersImpl::DifferentialTransformationChecker
Definition: TransformationCheckersImpl.h:81
icp_customized.errorMinimizer
errorMinimizer
Definition: icp_customized.py:125
Timer.h
align_sequence.params
params
Definition: align_sequence.py:13
PointMatcher::ICPSequence::loadFromYaml
virtual void loadFromYaml(std::istream &in)
Construct an ICP algorithm from a YAML file.
Definition: ICP.cpp:532
TransformationsImpl::SimilarityTransformation
Definition: TransformationsImpl.h:68
OutlierFiltersImpl::TrimmedDistOutlierFilter
Definition: OutlierFiltersImpl.h:128
MatchersImpl.h
icp_advance_api.matches
matches
Definition: icp_advance_api.py:114
PointMatcher::TransformationChecker
A transformation checker can stop the iteration depending on some conditions.
Definition: PointMatcher.h:583
PointMatcher::DataPointsFilter
A data filter takes a point cloud as input, transforms it, and produces another point cloud as output...
Definition: PointMatcher.h:440
PointMatcherSupport::getNameParamsFromYAML
void getNameParamsFromYAML(const YAML::Node &module, std::string &name, Parametrizable::Parameters &params)
Retrieve name and parameters from a yaml node.
Definition: Registrar.cpp:7
LOG_WARNING_STREAM
#define LOG_WARNING_STREAM(args)
Definition: PointMatcherPrivate.h:68
TransformationsImpl::RigidTransformation
Definition: TransformationsImpl.h:54
icp_customized.matcher
matcher
Definition: icp_customized.py:121
PointMatcher::ICP::operator()
TransformationParameters operator()(const DataPoints &readingIn, const DataPoints &referenceIn)
Perform ICP and return optimised transformation matrix.
Definition: ICP.cpp:245
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
PointMatcher::Vector
Eigen::Matrix< T, Eigen::Dynamic, 1 > Vector
A vector over ScalarType.
Definition: PointMatcher.h:161
timer::elapsed
double elapsed() const
TransformationsImpl.h
PointMatcher::ICPChainBase::ICPChainBase
ICPChainBase()
Protected contstructor, to prevent the creation of this object.
Definition: ICP.cpp:61
PointMatcher::ICP::computeWithTransformedReference
TransformationParameters computeWithTransformedReference(const DataPoints &readingIn, const DataPoints &reference, const TransformationParameters &T_refIn_refMean, const TransformationParameters &initialTransformationParameters)
Perferm ICP using an already-transformed reference and with an already-initialized matcher.
Definition: ICP.cpp:318
DataPointsFiltersImpl::SamplingSurfaceNormalDataPointsFilter
::SamplingSurfaceNormalDataPointsFilter< T > SamplingSurfaceNormalDataPointsFilter
Definition: DataPointsFiltersImpl.h:82
PointMatcher::Matches
Result of the data-association step (Matcher::findClosests), before outlier rejection.
Definition: PointMatcher.h:371
TransformationCheckersImpl::CounterTransformationChecker
Definition: TransformationCheckersImpl.h:58
std
icp_advance_api.dim
dim
Definition: icp_advance_api.py:152
DataPointsFiltersImpl::RandomSamplingDataPointsFilter
::RandomSamplingDataPointsFilter< T > RandomSamplingDataPointsFilter
Definition: DataPointsFiltersImpl.h:85
TransformationCheckersImpl.h
InspectorsImpl.h
icp_customized.inspector
inspector
Definition: icp_customized.py:131
PointMatcher::Inspector
An inspector allows to log data at the different steps, for analysis.
Definition: PointMatcher.h:624
PointMatcher::get
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:146
PointMatcher::ErrorMinimizer
An error minimizer will compute a transformation matrix such as to minimize the error between the rea...
Definition: PointMatcher.h:530
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
PointMatcher::ICPChainBase::getMaxNumIterationsReached
bool getMaxNumIterationsReached() const
Return the flag that informs if we reached the maximum number of iterations during the last iterative...
Definition: ICP.cpp:179
PointMatcher::ICPSequence::getPrefilteredMap
const DataPoints getPrefilteredMap() const
Return the map, in global coordinates (slow)
Definition: ICP.cpp:544
PointMatcher::ICPChainBase::getPrefilteredReferencePtsCount
unsigned getPrefilteredReferencePtsCount() const
Return the remaining number of points in the reference after prefiltering but before the iterative pr...
Definition: ICP.cpp:172
PointMatcher.h
public interface
DataPointsFiltersImpl.h
PointMatcher::ICPChainBase::~ICPChainBase
virtual ~ICPChainBase()
virtual desctructor
Definition: ICP.cpp:69
PointMatcher::ICP
ICP algorithm.
Definition: PointMatcher.h:702
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:199
compute_overlap.reading
reading
Definition: compute_overlap.py:70
PointMatcherSupport::logger
std::shared_ptr< Logger > logger
the current logger
Definition: Logger.cpp:42
PointMatcher::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182
PointMatcher::ICPChainBase::getPrefilteredReadingPtsCount
unsigned getPrefilteredReadingPtsCount() const
Return the remaining number of points in reading after prefiltering but before the iterative process.
Definition: ICP.cpp:165


libpointmatcher
Author(s):
autogenerated on Mon Jan 1 2024 03:24:43