libpointmatcher  1.3.1
PointMatcher.h
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 Francois 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 #ifndef __POINTMATCHER_CORE_H
37 #define __POINTMATCHER_CORE_H
38 
39 #ifndef EIGEN_USE_NEW_STDVECTOR
40 #define EIGEN_USE_NEW_STDVECTOR
41 #endif // EIGEN_USE_NEW_STDVECTOR
42 //#define EIGEN2_SUPPORT
43 
44 //TODO: investigate if that causes more problems down the road
45 #define EIGEN_NO_DEBUG
46 
47 #include "Eigen/StdVector"
48 #include "Eigen/Core"
49 #include "Eigen/Geometry"
50 
51 
52 #include <boost/thread/mutex.hpp>
53 
54 #include <stdexcept>
55 #include <limits>
56 #include <iostream>
57 #include <ostream>
58 #include <memory>
59 //#include <cstdint>
60 #include <boost/cstdint.hpp>
61 
62 #include "DeprecationWarnings.h"
63 #include "Parametrizable.h"
64 #include "Registrar.h"
65 
72 #define POINTMATCHER_VERSION "1.3.1"
74 #define POINTMATCHER_VERSION_INT 10301
76 
79 {
80  // TODO: gather all exceptions
81 
83  struct InvalidModuleType: std::runtime_error
84  {
85  InvalidModuleType(const std::string& reason);
86  };
87 
89  struct TransformationError: std::runtime_error
90  {
92  TransformationError(const std::string& reason);
93  };
94 
96  struct Logger: public Parametrizable
97  {
98  Logger();
99  Logger(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
100 
101  virtual ~Logger();
102  virtual bool hasInfoChannel() const;
103  virtual void beginInfoEntry(const char *file, unsigned line, const char *func);
104  virtual std::ostream* infoStream();
105  virtual void finishInfoEntry(const char *file, unsigned line, const char *func);
106  virtual bool hasWarningChannel() const;
107  virtual void beginWarningEntry(const char *file, unsigned line, const char *func);
108  virtual std::ostream* warningStream();
109  virtual void finishWarningEntry(const char *file, unsigned line, const char *func);
110  };
111 
112  void setLogger(std::shared_ptr<Logger> newLogger);
113 
114  void validateFile(const std::string& fileName);
115 
117  typedef std::map<std::string, std::vector<std::string> > CsvElements;
118 }
119 
121 template<typename T>
123 {
124  // ---------------------------------
125  // macros for constants
126  // ---------------------------------
127 
129  #define ZERO_PLUS_EPS (0. + std::numeric_limits<double>::epsilon())
130  #define ONE_MINUS_EPS (1. - std::numeric_limits<double>::epsilon())
132 
133  // ---------------------------------
134  // exceptions
135  // ---------------------------------
136 
137  //TODO: gather exceptions here and in Exceptions.cpp
138 
140  struct ConvergenceError: std::runtime_error
141  {
142  ConvergenceError(const std::string& reason);
143  };
144 
145 
146  // ---------------------------------
147  // eigen and nabo-based types
148  // ---------------------------------
149 
151  typedef T ScalarType;
153  typedef typename Eigen::Matrix<T, Eigen::Dynamic, 1> Vector;
155  typedef std::vector<Vector, Eigen::aligned_allocator<Vector> > VectorVector;
157  typedef typename Eigen::Quaternion<T> Quaternion;
159  typedef std::vector<Quaternion, Eigen::aligned_allocator<Quaternion> > QuaternionVector;
161  typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
163  typedef typename Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> IntMatrix;
165  typedef typename Eigen::Matrix<std::int64_t, Eigen::Dynamic, Eigen::Dynamic> Int64Matrix;
167  typedef typename Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> Array;
168 
169 
171 
175 
176  // alias for scope reasons
182 
183  // ---------------------------------
184  // input types
185  // ---------------------------------
186 
188 
199  struct DataPoints
200  {
202  typedef Eigen::Block<Matrix> View;
204  typedef Eigen::Block<Int64Matrix> TimeView;
206  typedef const Eigen::Block<const Matrix> ConstView;
208  typedef const Eigen::Block<const Int64Matrix> TimeConstView;
210  typedef typename Matrix::Index Index;
211 
213  struct Label
214  {
215  std::string text;
216  size_t span;
217  Label(const std::string& text = "", const size_t span = 0);
218  bool operator ==(const Label& that) const;
219  };
221  struct Labels: std::vector<Label>
222  {
223  typedef typename std::vector<Label>::const_iterator const_iterator;
224  Labels();
225  Labels(const Label& label);
226  bool contains(const std::string& text) const;
227  size_t totalDim() const;
228  friend std::ostream& operator<< (std::ostream& stream, const Labels& labels)
229  {
230  for(size_t i=0; i<labels.size(); i++)
231  {
232  stream << labels[i].text;
233  if(i != (labels.size() -1))
234  stream << ", ";
235  }
236 
237  return stream;
238  };
239  };
240 
242  struct InvalidField: std::runtime_error
243  {
244  InvalidField(const std::string& reason);
245  };
246 
247  // Constructors from descriptions (reserve memory)
248  DataPoints();
249  DataPoints(const Labels& featureLabels, const Labels& descriptorLabels, const size_t pointCount);
250  DataPoints(const Labels& featureLabels, const Labels& descriptorLabels, const Labels& timeLabels, const size_t pointCount);
251 
252  // Copy constructors from partial data
256 
257  bool operator ==(const DataPoints& that) const;
258 
259  unsigned getNbPoints() const;
260  unsigned getEuclideanDim() const;
261  unsigned getHomogeneousDim() const;
262  unsigned getNbGroupedDescriptors() const;
263  unsigned getDescriptorDim() const;
264  unsigned getTimeDim() const;
265 
266  void save(const std::string& fileName, bool binary = false) const;
267  static DataPoints load(const std::string& fileName);
268 
269  void concatenate(const DataPoints& dp);
270  void conservativeResize(Index pointCount);
272  DataPoints createSimilarEmpty(Index pointCount) const;
273  void setColFrom(Index thisCol, const DataPoints& that, Index thatCol);
274  void swapCols(Index iCol,Index jCol);
275 
276  // methods related to features
277  void allocateFeature(const std::string& name, const unsigned dim);
278  void allocateFeatures(const Labels& newLabels);
279  void addFeature(const std::string& name, const Matrix& newFeature);
280  void removeFeature(const std::string& name);
281  Matrix getFeatureCopyByName(const std::string& name) const;
282  ConstView getFeatureViewByName(const std::string& name) const;
283  View getFeatureViewByName(const std::string& name);
284  ConstView getFeatureRowViewByName(const std::string& name, const unsigned row) const;
285  View getFeatureRowViewByName(const std::string& name, const unsigned row);
286  bool featureExists(const std::string& name) const;
287  bool featureExists(const std::string& name, const unsigned dim) const;
288  unsigned getFeatureDimension(const std::string& name) const;
289  unsigned getFeatureStartingRow(const std::string& name) const;
290 
291  // methods related to descriptors
292  void allocateDescriptor(const std::string& name, const unsigned dim);
293  void allocateDescriptors(const Labels& newLabels);
294  void addDescriptor(const std::string& name, const Matrix& newDescriptor);
295  void removeDescriptor(const std::string& name);
296  Matrix getDescriptorCopyByName(const std::string& name) const;
297  ConstView getDescriptorViewByName(const std::string& name) const;
298  View getDescriptorViewByName(const std::string& name);
299  ConstView getDescriptorRowViewByName(const std::string& name, const unsigned row) const;
300  View getDescriptorRowViewByName(const std::string& name, const unsigned row);
301  bool descriptorExists(const std::string& name) const;
302  bool descriptorExists(const std::string& name, const unsigned dim) const;
303  unsigned getDescriptorDimension(const std::string& name) const;
304  unsigned getDescriptorStartingRow(const std::string& name) const;
305  void assertDescriptorConsistency() const;
306 
307  // methods related to times
308  void allocateTime(const std::string& name, const unsigned dim);
309  void allocateTimes(const Labels& newLabels);
310  void addTime(const std::string& name, const Int64Matrix& newTime);
311  void removeTime(const std::string& name);
312  Int64Matrix getTimeCopyByName(const std::string& name) const;
313  TimeConstView getTimeViewByName(const std::string& name) const;
314  TimeView getTimeViewByName(const std::string& name);
315  TimeConstView getTimeRowViewByName(const std::string& name, const unsigned row) const;
316  TimeView getTimeRowViewByName(const std::string& name, const unsigned row);
317  bool timeExists(const std::string& name) const;
318  bool timeExists(const std::string& name, const unsigned dim) const;
319  unsigned getTimeDimension(const std::string& name) const;
320  unsigned getTimeStartingRow(const std::string& name) const;
321  void assertTimesConsistency() const;
322 
329 
330  private:
331  void assertConsistency(const std::string& dataName, const int dataRows, const int dataCols, const Labels& labels) const;
332  template<typename MatrixType>
333  void allocateFields(const Labels& newLabels, Labels& labels, MatrixType& data) const;
334  template<typename MatrixType>
335  void allocateField(const std::string& name, const unsigned dim, Labels& labels, MatrixType& data) const;
336  template<typename MatrixType>
337  void addField(const std::string& name, const MatrixType& newField, Labels& labels, MatrixType& data) const;
338  template<typename MatrixType>
339  void removeField(const std::string& name, Labels& labels, MatrixType& data) const;
340  template<typename MatrixType>
341  const Eigen::Block<const MatrixType> getConstViewByName(const std::string& name, const Labels& labels, const MatrixType& data, const int viewRow = -1) const;
342  template<typename MatrixType>
343  Eigen::Block<MatrixType> getViewByName(const std::string& name, const Labels& labels, MatrixType& data, const int viewRow = -1) const;
344  bool fieldExists(const std::string& name, const unsigned dim, const Labels& labels) const;
345  unsigned getFieldDimension(const std::string& name, const Labels& labels) const;
346  unsigned getFieldStartingRow(const std::string& name, const Labels& labels) const;
347 
348  template<typename MatrixType>
349  void concatenateLabelledMatrix(Labels* labels, MatrixType* data, const Labels extraLabels, const MatrixType extraData);
350  };
351 
352  static void swapDataPoints(DataPoints& a, DataPoints& b);
353 
354  // ---------------------------------
355  // intermediate types
356  // ---------------------------------
357 
359 
363  struct Matches
364  {
365  typedef Matrix Dists;
366  typedef IntMatrix Ids;
367 
368 
369  static constexpr int InvalidId = -1;
370  static constexpr T InvalidDist = std::numeric_limits<T>::infinity();
371 
372  Matches();
373  Matches(const Dists& dists, const Ids ids);
374  Matches(const int knn, const int pointsCount);
375 
378 
379  T getDistsQuantile(const T quantile) const;
380  T getMedianAbsDeviation() const;
381  T getStandardDeviation() const;
382 
383  };
384 
386 
390 
391  // ---------------------------------
392  // types of processing bricks
393  // ---------------------------------
394 
397  {
398  Transformation();
399  Transformation(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
400  virtual ~Transformation();
401 
403  virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const = 0;
404 
406  virtual bool checkParameters(const TransformationParameters& parameters) const = 0;
407 
410 
411  };
412 
414  struct Transformations: public std::vector<std::shared_ptr<Transformation> >
415  {
416  void apply(DataPoints& cloud, const TransformationParameters& parameters) const;
417  };
418  typedef typename Transformations::iterator TransformationsIt;
419  typedef typename Transformations::const_iterator TransformationsConstIt;
420 
421  DEF_REGISTRAR(Transformation)
422 
423  // ---------------------------------
424 
425 
426 
430  {
432  DataPointsFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
433  virtual ~DataPointsFilter();
434  virtual void init();
435 
437  virtual DataPoints filter(const DataPoints& input) = 0;
438 
440  virtual void inPlaceFilter(DataPoints& cloud) = 0;
441  };
442 
444  struct DataPointsFilters: public std::vector<std::shared_ptr<DataPointsFilter> >
445  {
447  DataPointsFilters(std::istream& in);
448  void init();
449  void apply(DataPoints& cloud);
450  };
451  typedef typename DataPointsFilters::iterator DataPointsFiltersIt;
452  typedef typename DataPointsFilters::const_iterator DataPointsFiltersConstIt;
453 
454  DEF_REGISTRAR(DataPointsFilter)
455 
456  // ---------------------------------
457 
458 
459 
462  struct Matcher: public Parametrizable
463  {
464  unsigned long visitCounter;
465 
466  Matcher();
467  Matcher(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
468  virtual ~Matcher();
469 
470  void resetVisitCount();
471  unsigned long getVisitCount() const;
472 
474  virtual void init(const DataPoints& filteredReference) = 0;
476  virtual Matches findClosests(const DataPoints& filteredReading) = 0;
477  };
478 
479  DEF_REGISTRAR(Matcher)
480 
481  // ---------------------------------
482 
483 
484 
489  {
490  OutlierFilter();
491  OutlierFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
492 
493  virtual ~OutlierFilter();
494 
496  virtual OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input) = 0;
497  };
498 
499 
501  struct OutlierFilters: public std::vector<std::shared_ptr<OutlierFilter> >
502  {
503 
504  OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input);
505 
506  };
507 
508  typedef typename OutlierFilters::const_iterator OutlierFiltersConstIt;
509  typedef typename OutlierFilters::iterator OutlierFiltersIt;
510 
511  DEF_REGISTRAR(OutlierFilter)
512 
513  // ---------------------------------
514 
515 
516 
520  {
523  {
532 
533  ErrorElements();
534  ErrorElements(const DataPoints& requestedPts, const DataPoints& sourcePts, const OutlierWeights& outlierWeights, const Matches& matches);
535  };
536 
537  ErrorMinimizer();
538  ErrorMinimizer(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
539  virtual ~ErrorMinimizer();
540 
541  T getPointUsedRatio() const;
542  T getWeightedPointUsedRatio() const;
543  ErrorElements getErrorElements() const; //TODO: ensure that is return a usable value
544  virtual T getOverlap() const;
545  virtual Matrix getCovariance() const;
546  virtual T getResidualError(const DataPoints& filteredReading, const DataPoints& filteredReference, const OutlierWeights& outlierWeights, const Matches& matches) const;
547 
549  virtual TransformationParameters compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const OutlierWeights& outlierWeights, const Matches& matches);
551  virtual TransformationParameters compute(const ErrorElements& matchedPoints) = 0;
552 
553  // helper functions
554  static Matrix crossProduct(const Matrix& A, const Matrix& B);//TODO: this might go in pointmatcher_support namespace
555 
556  protected:
557  //T pointUsedRatio; //!< the ratio of how many points were used for error minimization
558  //T weightedPointUsedRatio; //!< the ratio of how many points were used (with weight) for error minimization
559  //TODO: standardize the use of this variable
561  };
562 
563  DEF_REGISTRAR(ErrorMinimizer)
564 
565  // ---------------------------------
566 
567 
568 
573  {
574  protected:
575  typedef std::vector<std::string> StringVector;
580 
581  public:
583  TransformationChecker(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
584  virtual ~TransformationChecker();
586  virtual void init(const TransformationParameters& parameters, bool& iterate) = 0;
588  virtual void check(const TransformationParameters& parameters, bool& iterate) = 0;
589 
590  const Vector& getLimits() const;
591  const Vector& getConditionVariables() const;
592  const StringVector& getLimitNames() const;
593  const StringVector& getConditionVariableNames() const;
594 
595  protected:
596  static Vector matrixToAngles(const TransformationParameters& parameters);
597  };
598 
600  struct TransformationCheckers: public std::vector<std::shared_ptr<TransformationChecker> >
601  {
602  void init(const TransformationParameters& parameters, bool& iterate);
603  void check(const TransformationParameters& parameters, bool& iterate);
604  };
605  typedef typename TransformationCheckers::iterator TransformationCheckersIt;
606  typedef typename TransformationCheckers::const_iterator TransformationCheckersConstIt;
607 
608  DEF_REGISTRAR(TransformationChecker)
609 
610  // ---------------------------------
611 
612 
613  struct Inspector: public Parametrizable
614  {
615 
616  Inspector();
617  Inspector(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
618 
619  //
620  virtual ~Inspector();
621  virtual void init();
622 
623  // performance statistics
624  virtual void addStat(const std::string& name, double data);
625  virtual void dumpStats(std::ostream& stream);
626  virtual void dumpStatsHeader(std::ostream& stream);
627 
628  // data statistics
629  virtual void dumpIteration(const size_t iterationNumber, const TransformationParameters& parameters, const DataPoints& filteredReference, const DataPoints& reading, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationCheckers& transformationCheckers);
630  virtual void finish(const size_t iterationCount);
631  };
632 
633  DEF_REGISTRAR(Inspector)
634 
635  // ---------------------------------
636 
637  DEF_REGISTRAR_IFACE(Logger, PointMatcherSupport::Logger)
638 
639  // ---------------------------------
640 
641  // algorithms
642 
643 
645  {
646  public:
651  std::shared_ptr<Matcher> matcher;
653  std::shared_ptr<ErrorMinimizer> errorMinimizer;
655  std::shared_ptr<Inspector> inspector;
656 
657  virtual ~ICPChainBase();
658 
659  virtual void setDefault();
660 
661  void loadFromYaml(std::istream& in);
662  unsigned getPrefilteredReadingPtsCount() const;
663  unsigned getPrefilteredReferencePtsCount() const;
664 
665  bool getMaxNumIterationsReached() const;
666 
667  protected:
671 
672  ICPChainBase();
673 
674  void cleanup();
675 
676  virtual void loadAdditionalYAMLContent(PointMatcherSupport::YAML::Node& doc);
677 
679  template<typename R>
680  const std::string& createModulesFromRegistrar(const std::string& regName, const PointMatcherSupport::YAML::Node& doc, const R& registrar, std::vector<std::shared_ptr<typename R::TargetType> >& modules);
681 
683  template<typename R>
684  const std::string& createModuleFromRegistrar(const std::string& regName, const PointMatcherSupport::YAML::Node& doc, const R& registrar, std::shared_ptr<typename R::TargetType>& module);
685 
687  std::string nodeVal(const std::string& regName, const PointMatcherSupport::YAML::Node& doc);
688  };
689 
692  {
694  const DataPoints& readingIn,
695  const DataPoints& referenceIn);
696 
698  const DataPoints& readingIn,
699  const DataPoints& referenceIn,
700  const TransformationParameters& initialTransformationParameters);
701 
703  const DataPoints& readingIn,
704  const DataPoints& referenceIn,
705  const TransformationParameters& initialTransformationParameters);
706 
708  const DataPoints& getReadingFiltered() const { return readingFiltered; }
709 
710  protected:
712  const DataPoints& readingIn,
713  const DataPoints& reference,
714  const TransformationParameters& T_refIn_refMean,
715  const TransformationParameters& initialTransformationParameters);
716 
718  };
719 
722  struct ICPSequence: public ICP
723  {
725  const DataPoints& cloudIn);
727  const DataPoints& cloudIn,
728  const TransformationParameters& initialTransformationParameters);
730  const DataPoints& cloudIn,
731  const TransformationParameters& initialTransformationParameters);
732 
733  bool hasMap() const;
734  bool setMap(const DataPoints& map);
735  void clearMap();
736  PM_DEPRECATED("Use getPrefilteredInternalMap instead. "
737  "Function now always returns map with filter chain applied. "
738  "This may have altered your program behavior."
739  "Reasons for this stated here and in associated PR: "
740  "https://github.com/ethz-asl/libpointmatcher/issues/209.")
741  const DataPoints& getInternalMap() const;
742  const DataPoints& getPrefilteredInternalMap() const;
743  PM_DEPRECATED("Use getPrefilteredMap instead. "
744  "Function now always returns map with filter chain applied. "
745  "This may have altered your program behavior."
746  "Reasons for this stated here and in associated PR: "
747  "https://github.com/ethz-asl/libpointmatcher/issues/209")
748  const DataPoints getMap() const;
749  const DataPoints getPrefilteredMap() const;
750 
751  protected:
754  };
755 
756  // ---------------------------------
757  // Instance-related functions
758  // ---------------------------------
759 
760  PointMatcher();
761 
762  static const PointMatcher& get();
763 
764 
765 }; // PointMatcher<T>
766 
767 #endif // __POINTMATCHER_CORE_H
768 
PointMatcher::ICPSequence
ICP alogrithm, taking a sequence of clouds and using a map Warning: used with caution,...
Definition: PointMatcher.h:722
PointMatcher::ICPSequence::clearMap
void clearMap()
Clear the map (reset to same state as after the object is created)
Definition: ICP.cpp:512
PointMatcher::ICPSequence::hasMap
bool hasMap() const
Return whether the object currently holds a valid map.
Definition: ICP.cpp:457
PointMatcher::DataPoints::timeExists
bool timeExists(const std::string &name) const
Look if a time with a given name exist.
Definition: DataPoints.cpp:687
PointMatcher::ErrorMinimizer::ErrorElements::nbRejectedPoints
int nbRejectedPoints
number of points with all matches set to zero weights
Definition: PointMatcher.h:529
PointMatcher::DataPointsFilters::DataPointsFilters
DataPointsFilters()
Construct an empty chain.
Definition: DataPointsFilter.cpp:73
PointMatcher::DataPoints::getNbGroupedDescriptors
unsigned getNbGroupedDescriptors() const
Return the number of grouped descriptors (e.g., normals can have 3 components but would count as only...
Definition: DataPoints.cpp:179
PointMatcher::OutlierFilters
A chain of OutlierFilter.
Definition: PointMatcher.h:501
PointMatcher::Parameters
Parametrizable::Parameters Parameters
alias
Definition: PointMatcher.h:178
PointMatcher::DataPointsFiltersIt
DataPointsFilters::iterator DataPointsFiltersIt
alias
Definition: PointMatcher.h:451
PointMatcher::Matches::Dists
Matrix Dists
Squared distances to closest points, dense matrix of ScalarType.
Definition: PointMatcher.h:365
PointMatcher::DataPoints::Label::operator==
bool operator==(const Label &that) const
Return whether two labels are equals.
Definition: DataPoints.cpp:57
PointMatcher::Matches::dists
Dists dists
squared distances to closest points
Definition: PointMatcher.h:376
PointMatcher::DataPoints::descriptorLabels
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:326
PointMatcher::ICPSequence::compute
TransformationParameters compute(const DataPoints &cloudIn, const TransformationParameters &initialTransformationParameters)
Apply ICP to cloud cloudIn, with initial guess.
Definition: ICP.cpp:573
PointMatcher::DataPoints::swapCols
void swapCols(Index iCol, Index jCol)
Swap column i and j in the point cloud, swap also features and descriptors if any....
Definition: DataPoints.cpp:405
PointMatcherSupport::Logger::finishInfoEntry
virtual void finishInfoEntry(const char *file, unsigned line, const char *func)
Finish the entry into the info channel.
Definition: Logger.cpp:74
PointMatcher::DataPoints::Label::text
std::string text
name of the label
Definition: PointMatcher.h:215
PointMatcherSupport::validateFile
void validateFile(const std::string &fileName)
Throw a runtime_error exception if fileName cannot be opened.
Definition: IO.cpp:355
PointMatcher::OutlierFiltersConstIt
OutlierFilters::const_iterator OutlierFiltersConstIt
alias
Definition: PointMatcher.h:508
PointMatcher::VectorVector
std::vector< Vector, Eigen::aligned_allocator< Vector > > VectorVector
A vector of vector over ScalarType, not a matrix.
Definition: PointMatcher.h:155
PointMatcher::DataPoints::getEuclideanDim
unsigned getEuclideanDim() const
Return the dimension of the point cloud.
Definition: DataPoints.cpp:165
PointMatcher::Matches::getMedianAbsDeviation
T getMedianAbsDeviation() const
Calculate the Median of Absolute Deviation(MAD), which is median(|x-median(x)|), a kind of robust sta...
Definition: Matches.cpp:91
PointMatcher::DataPointsFilters::init
void init()
Init the chain.
Definition: DataPointsFilter.cpp:97
PointMatcher::Matches::ids
Ids ids
identifiers of closest points
Definition: PointMatcher.h:377
PointMatcher::OutlierWeights
Matrix OutlierWeights
Weights of the associations between the points in Matches and the points in the reference.
Definition: PointMatcher.h:389
PointMatcherSupport::Logger::beginInfoEntry
virtual void beginInfoEntry(const char *file, unsigned line, const char *func)
Start a new entry into the info channel.
Definition: Logger.cpp:64
PointMatcher::DataPoints::addTime
void addTime(const std::string &name, const Int64Matrix &newTime)
Add a time by name, remove first if already exists.
Definition: DataPoints.cpp:636
PointMatcher::Transformations
A chain of Transformation.
Definition: PointMatcher.h:414
PointMatcher::DataPoints::assertDescriptorConsistency
void assertDescriptorConsistency() const
Assert if descriptors are not consistent with features.
Definition: DataPoints.cpp:610
PointMatcher::ICPChainBase::transformationCheckers
TransformationCheckers transformationCheckers
transformation checkers
Definition: PointMatcher.h:654
PointMatcher::ErrorMinimizer::ErrorElements::pointUsedRatio
T pointUsedRatio
the ratio of how many points were used for error minimization
Definition: PointMatcher.h:530
PointMatcher::DataPoints::Labels
A vector of Label.
Definition: PointMatcher.h:221
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:265
PointMatcher::DataPoints::removeFeature
void removeFeature(const std::string &name)
Remove a feature by name, the whole matrix will be copied.
Definition: DataPoints.cpp:444
PointMatcher::DataPoints::concatenate
void concatenate(const DataPoints &dp)
Add another point cloud after the current one. Faster merge will be achieved if all descriptor and ti...
Definition: DataPoints.cpp:225
PointMatcher::DataPoints::addFeature
void addFeature(const std::string &name, const Matrix &newFeature)
Add a feature by name, remove first if already exists. The 'pad' field will stay at the end for homog...
Definition: DataPoints.cpp:435
PointMatcherSupport::CsvElements
std::map< std::string, std::vector< std::string > > CsvElements
Data from a CSV file.
Definition: PointMatcher.h:117
PointMatcherSupport::Logger::infoStream
virtual std::ostream * infoStream()
Return the info stream, 0 if hasInfoChannel() returns false.
Definition: Logger.cpp:68
PointMatcher::DataPoints::allocateDescriptor
void allocateDescriptor(const std::string &name, const unsigned dim)
Makes sure a descriptor of a given name exists, if present, check its dimensions.
Definition: DataPoints.cpp:518
PointMatcher::DataPoints::descriptorExists
bool descriptorExists(const std::string &name) const
Look if a descriptor with a given name exist.
Definition: DataPoints.cpp:582
PointMatcher::Parametrizable
PointMatcherSupport::Parametrizable Parametrizable
alias
Definition: PointMatcher.h:177
PointMatcher::TransformationCheckersIt
TransformationCheckers::iterator TransformationCheckersIt
alias
Definition: PointMatcher.h:605
PointMatcher::ScalarType
T ScalarType
The scalar type.
Definition: PointMatcher.h:151
PointMatcher::DataPoints::addDescriptor
void addDescriptor(const std::string &name, const Matrix &newDescriptor)
Add a descriptor by name, remove first if already exists.
Definition: DataPoints.cpp:532
PointMatcher::ICPSequence::setMap
bool setMap(const DataPoints &map)
Set the map using inputCloud.
Definition: ICP.cpp:464
PointMatcher::OutlierFilter
An outlier filter removes or weights links between points in reading and their matched points in refe...
Definition: PointMatcher.h:488
PointMatcher::Array
Eigen::Array< T, Eigen::Dynamic, Eigen::Dynamic > Array
A dense array over ScalarType.
Definition: PointMatcher.h:167
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:122
PointMatcher::ICPChainBase::referenceDataPointsFilters
DataPointsFilters referenceDataPointsFilters
filters for reference
Definition: PointMatcher.h:649
PointMatcher::ConvergenceError
Point matcher did not converge.
Definition: PointMatcher.h:140
PointMatcherSupport::setLogger
void setLogger(std::shared_ptr< Logger > newLogger)
Set a new logger, protected by a mutex.
Definition: Logger.cpp:98
PointMatcher::DataPointsFilters
A chain of DataPointsFilter.
Definition: PointMatcher.h:444
PointMatcher::TransformationCheckersConstIt
TransformationCheckers::const_iterator TransformationCheckersConstIt
alias
Definition: PointMatcher.h:606
PointMatcherSupport::Logger
The logger interface, used to output warnings and informations.
Definition: PointMatcher.h:96
PointMatcherSupport::Logger::hasWarningChannel
virtual bool hasWarningChannel() const
Return whether this logger provides the warning channel.
Definition: Logger.cpp:78
PointMatcher::ICPChainBase::maxNumIterationsReached
bool maxNumIterationsReached
store if we reached the maximum number of iterations last time compute was called
Definition: PointMatcher.h:670
PointMatcher::ICPChainBase
Stuff common to all ICP algorithms.
Definition: PointMatcher.h:644
PointMatcher::DataPoints::getDescriptorCopyByName
Matrix getDescriptorCopyByName(const std::string &name) const
Get descriptor by name, return a matrix containing a copy of the requested descriptor.
Definition: DataPoints.cpp:547
PointMatcher::DataPoints::getTimeDim
unsigned getTimeDim() const
Return the total number of times.
Definition: DataPoints.cpp:193
PointMatcher::DataPoints::getTimeCopyByName
Int64Matrix getTimeCopyByName(const std::string &name) const
Get time by name, return a matrix containing a copy of the requested time.
Definition: DataPoints.cpp:650
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:199
PointMatcher::DataPoints::allocateFeatures
void allocateFeatures(const Labels &newLabels)
Make sure a vector of features of given names exist.
Definition: DataPoints.cpp:428
PointMatcher::ICPSequence::mapPointCloud
DataPoints mapPointCloud
point cloud of the map, always in global frame (frame of first point cloud)
Definition: PointMatcher.h:752
PointMatcher::DataPoints::getDescriptorStartingRow
unsigned getDescriptorStartingRow(const std::string &name) const
Return the starting row of a descriptor with a given name. Return 0 if the name is not found.
Definition: DataPoints.cpp:603
PointMatcher::QuaternionVector
std::vector< Quaternion, Eigen::aligned_allocator< Quaternion > > QuaternionVector
A vector of quaternions over ScalarType.
Definition: PointMatcher.h:159
PointMatcher::DataPoints::getHomogeneousDim
unsigned getHomogeneousDim() const
Return the dimension of the point cloud in homogeneous coordinates (one more than Euclidean dimension...
Definition: DataPoints.cpp:172
PointMatcher::Matcher
A matcher links points in the reading to points in the reference.
Definition: PointMatcher.h:462
PointMatcher::DataPoints::timeLabels
Labels timeLabels
labels of times.
Definition: PointMatcher.h:328
PointMatcher::ICPSequence::operator()
TransformationParameters operator()(const DataPoints &cloudIn)
Apply ICP to cloud cloudIn, with identity as initial guess.
Definition: ICP.cpp:555
PointMatcher::Transformation::checkParameters
virtual bool checkParameters(const TransformationParameters &parameters) const =0
Return whether the given parameters respect the expected constraints.
PointMatcher::ParametersDoc
Parametrizable::ParametersDoc ParametersDoc
alias
Definition: PointMatcher.h:180
PointMatcher::DataPoints::operator==
bool operator==(const DataPoints &that) const
Return whether two point-clouds are identical (same data and same labels)
Definition: DataPoints.cpp:200
PointMatcherSupport::InvalidModuleType
An exception thrown when one tries to use a module type that does not exist.
Definition: PointMatcher.h:83
PointMatcher::TransformationChecker::limits
Vector limits
values of limits involved in conditions to stop ICP loop
Definition: PointMatcher.h:576
PointMatcher::DataPoints::getTimeDimension
unsigned getTimeDimension(const std::string &name) const
Return the dimension of a time with a given name. Return 0 if the name is not found.
Definition: DataPoints.cpp:701
PointMatcher::DataPoints::getFeatureDimension
unsigned getFeatureDimension(const std::string &name) const
Return the dimension of a feature with a given name. Return 0 if the name is not found.
Definition: DataPoints.cpp:500
PointMatcher::DataPoints::TimeConstView
const typedef Eigen::Block< const Int64Matrix > TimeConstView
a view on a const time
Definition: PointMatcher.h:208
PointMatcher::Transformation
A function that transforms points and their descriptors given a transformation matrix.
Definition: PointMatcher.h:396
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:242
PointMatcher::DataPoints::getFeatureViewByName
ConstView getFeatureViewByName(const std::string &name) const
Get a const view on a feature by name, throw an exception if it does not exist.
Definition: DataPoints.cpp:458
PointMatcher::OutlierFilters::compute
OutlierWeights compute(const DataPoints &filteredReading, const DataPoints &filteredReference, const Matches &input)
Apply outlier-detection chain.
Definition: OutlierFilter.cpp:64
PointMatcher::ICPSequence::getPrefilteredInternalMap
const DataPoints & getPrefilteredInternalMap() const
Return the map, in internal coordinates (fast)
Definition: ICP.cpp:542
PointMatcher::Int64Matrix
Eigen::Matrix< std::int64_t, Eigen::Dynamic, Eigen::Dynamic > Int64Matrix
A dense signed 64-bits matrix.
Definition: PointMatcher.h:165
PointMatcher::ICPChainBase::readingDataPointsFilters
DataPointsFilters readingDataPointsFilters
filters for reading, applied once
Definition: PointMatcher.h:647
PointMatcher::DataPointsFiltersConstIt
DataPointsFilters::const_iterator DataPointsFiltersConstIt
alias
Definition: PointMatcher.h:452
PointMatcher::TransformationCheckers::init
void init(const TransformationParameters &parameters, bool &iterate)
Init all transformation checkers, set iterate to false if iteration should stop.
Definition: TransformationChecker.cpp:112
PointMatcher::DataPoints::featureExists
bool featureExists(const std::string &name) const
Look if a feature with a given name exist.
Definition: DataPoints.cpp:486
PointMatcher::ICP::getReadingFiltered
const DataPoints & getReadingFiltered() const
Return the filtered point cloud reading used in the ICP chain.
Definition: PointMatcher.h:708
PointMatcher::OutlierFiltersIt
OutlierFilters::iterator OutlierFiltersIt
alias
Definition: PointMatcher.h:509
PointMatcher::ErrorMinimizer::ErrorElements
A structure holding data ready for minimization. The data are "normalized", for instance there are no...
Definition: PointMatcher.h:522
PointMatcher::DataPoints::allocateTime
void allocateTime(const std::string &name, const unsigned dim)
Makes sure a time of a given name exists, if present, check its dimensions.
Definition: DataPoints.cpp:621
PointMatcher::DataPoints::allocateFeature
void allocateFeature(const std::string &name, const unsigned dim)
Makes sure a feature of a given name exists, if present, check its dimensions.
Definition: DataPoints.cpp:421
PointMatcher::Matches::Matches
Matches()
In case of too few matches the dists are filled with InvalidDist.
Definition: Matches.cpp:43
PointMatcherSupport::Logger::warningStream
virtual std::ostream * warningStream()
Return the warning stream, 0 if hasWarningChannel() returns false.
Definition: Logger.cpp:88
PointMatcherSupport::Parametrizable::ParametersDoc
std::vector< ParameterDoc > ParametersDoc
The documentation of all parameters.
Definition: Parametrizable.h:144
PointMatcher::PointMatcher
PointMatcher()
Constructor, populates the registrars.
Definition: Registry.cpp:61
PointMatcher::DataPoints::featureLabels
Labels featureLabels
labels of features
Definition: PointMatcher.h:324
PointMatcher::Transformation::correctParameters
virtual TransformationParameters correctParameters(const TransformationParameters &parameters) const =0
Return a valid version of the given transformation.
PointMatcher::ICPChainBase::outlierFilters
OutlierFilters outlierFilters
outlier filters
Definition: PointMatcher.h:652
PointMatcher::ErrorMinimizer::ErrorElements::matches
Matches matches
associations
Definition: PointMatcher.h:527
PointMatcher::DataPoints::setColFrom
void setColFrom(Index thisCol, const DataPoints &that, Index thatCol)
Set column thisCol equal to column thatCol of that, copy features and descriptors if any....
Definition: DataPoints.cpp:392
PointMatcher::DataPoints::createSimilarEmpty
DataPoints createSimilarEmpty() const
Create an empty DataPoints of similar dimensions and labels for features, descriptors and times.
Definition: DataPoints.cpp:339
PointMatcher::ICPChainBase::errorMinimizer
std::shared_ptr< ErrorMinimizer > errorMinimizer
error minimizer
Definition: PointMatcher.h:653
PointMatcher::ErrorMinimizer::ErrorElements::weightedPointUsedRatio
T weightedPointUsedRatio
the ratio of how many points were used (with weight) for error minimization
Definition: PointMatcher.h:531
PointMatcher::DataPoints::descriptors
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:325
PointMatcherSupport::Parametrizable::parameters
Parameters parameters
parameters with their values encoded in string
Definition: Parametrizable.h:161
PointMatcher::DataPoints::TimeView
Eigen::Block< Int64Matrix > TimeView
A view on a time.
Definition: PointMatcher.h:204
PointMatcher::Matrix
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Definition: PointMatcher.h:161
PointMatcher::TransformationChecker
A transformation checker can stop the iteration depending on some conditions.
Definition: PointMatcher.h:572
PointMatcherSupport::Logger::Logger
Logger()
Construct without parameter.
Definition: Logger.cpp:45
PointMatcher::ErrorMinimizer::ErrorElements::reading
DataPoints reading
reading point cloud
Definition: PointMatcher.h:524
PointMatcher::DataPointsFilter
A data filter takes a point cloud as input, transforms it, and produces another point cloud as output...
Definition: PointMatcher.h:429
PointMatcher::DataPoints::getFeatureRowViewByName
ConstView getFeatureRowViewByName(const std::string &name, const unsigned row) const
Get a const view on a feature row by name and number, throw an exception if it does not exist.
Definition: DataPoints.cpp:472
PointMatcher::DataPoints::getTimeViewByName
TimeConstView getTimeViewByName(const std::string &name) const
Get a const view on a time by name, throw an exception if it does not exist.
Definition: DataPoints.cpp:658
PointMatcher::Matches::Ids
IntMatrix Ids
Identifiers of closest points, dense matrix of integers.
Definition: PointMatcher.h:366
PointMatcher::DataPoints::assertTimesConsistency
void assertTimesConsistency() const
Assert if times are not consistent with features.
Definition: DataPoints.cpp:715
PointMatcher::IntMatrix
Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic > IntMatrix
A dense integer matrix.
Definition: PointMatcher.h:163
PointMatcherSupport::Parametrizable::InvalidParameter
An exception thrown when one tries to fetch the value of an unexisting parameter.
Definition: Parametrizable.h:101
PointMatcherSupport::Logger::hasInfoChannel
virtual bool hasInfoChannel() const
Return whether this logger provides the info channel.
Definition: Logger.cpp:58
PointMatcher::DataPoints::getNbPoints
unsigned getNbPoints() const
Return the number of points contained in the point cloud.
Definition: DataPoints.cpp:158
PointMatcher::TransformationChecker::conditionVariables
Vector conditionVariables
values of variables involved in conditions to stop ICP loop
Definition: PointMatcher.h:577
PointMatcher::ICP::operator()
TransformationParameters operator()(const DataPoints &readingIn, const DataPoints &referenceIn)
Perform ICP and return optimised transformation matrix.
Definition: ICP.cpp:244
PointMatcher::DataPoints::Label::Label
Label(const std::string &text="", const size_t span=0)
Construct a label from a given name and number of data dimensions it spans.
Definition: DataPoints.cpp:44
PointMatcher::swapDataPoints
static void swapDataPoints(DataPoints &a, DataPoints &b)
Exchange in place point clouds a and b, with no data copy.
Definition: DataPoints.cpp:1022
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:323
PointMatcher::ErrorMinimizer::ErrorElements::reference
DataPoints reference
reference point cloud
Definition: PointMatcher.h:525
PointMatcher::Vector
Eigen::Matrix< T, Eigen::Dynamic, 1 > Vector
A vector over ScalarType.
Definition: PointMatcher.h:153
PointMatcher::DataPoints::ConstView
const typedef Eigen::Block< const Matrix > ConstView
A view on a const feature or const descriptor.
Definition: PointMatcher.h:206
PointMatcher::TransformationsIt
Transformations::iterator TransformationsIt
alias
Definition: PointMatcher.h:418
PointMatcher::DataPoints::Index
Matrix::Index Index
An index to a row or a column.
Definition: PointMatcher.h:210
PointMatcherSupport::TransformationError::TransformationError
TransformationError(const std::string &reason)
return an exception when a transformation has invalid parameters
PointMatcher::ICPChainBase::matcher
std::shared_ptr< Matcher > matcher
matcher
Definition: PointMatcher.h:651
PointMatcher::TransformationCheckers::check
void check(const TransformationParameters &parameters, bool &iterate)
Check using all transformation checkers, set iterate to false if iteration should stop.
Definition: TransformationChecker.cpp:120
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:317
PointMatcherSupport::Logger::beginWarningEntry
virtual void beginWarningEntry(const char *file, unsigned line, const char *func)
Start a new entry into the warning channel.
Definition: Logger.cpp:84
PointMatcher::DataPoints::Labels::contains
bool contains(const std::string &text) const
Return whether there is a label named text.
Definition: DataPoints.cpp:75
PointMatcher::InvalidParameter
Parametrizable::InvalidParameter InvalidParameter
alias
Definition: PointMatcher.h:181
PointMatcher::Matches
Result of the data-association step (Matcher::findClosests), before outlier rejection.
Definition: PointMatcher.h:363
PointMatcher::ICPChainBase::inspector
std::shared_ptr< Inspector > inspector
inspector
Definition: PointMatcher.h:655
PointMatcher::TransformationChecker::conditionVariableNames
StringVector conditionVariableNames
names of variables involved in conditions to stop ICP loop
Definition: PointMatcher.h:579
PointMatcher::DataPoints::times
Int64Matrix times
time associated to each points, might be empty
Definition: PointMatcher.h:327
PointMatcher::DataPoints::allocateTimes
void allocateTimes(const Labels &newLabels)
Make sure a vector of time of given names exist.
Definition: DataPoints.cpp:628
PointMatcherSupport::Parametrizable
The superclass of classes that are constructed using generic parameters. This class provides the para...
Definition: Parametrizable.h:98
PointMatcher::DataPoints::removeTime
void removeTime(const std::string &name)
Remove a descriptor by name, the whole matrix will be copied.
Definition: DataPoints.cpp:643
PointMatcher::DataPoints::removeDescriptor
void removeDescriptor(const std::string &name)
Remove a descriptor by name, the whole matrix will be copied.
Definition: DataPoints.cpp:539
PointMatcher::ICPChainBase::prefilteredReadingPtsCount
unsigned prefilteredReadingPtsCount
remaining number of points after prefiltering but before the iterative process
Definition: PointMatcher.h:668
PointMatcher::DataPoints::getTimeStartingRow
unsigned getTimeStartingRow(const std::string &name) const
Return the starting row of a time with a given name. Return 0 if the name is not found.
Definition: DataPoints.cpp:708
PointMatcher::ErrorMinimizer::ErrorElements::weights
OutlierWeights weights
weights for every association
Definition: PointMatcher.h:526
PointMatcher::DataPoints::Label::span
size_t span
number of data dimensions the label spans
Definition: PointMatcher.h:216
PointMatcher::DataPoints::Labels::Labels
Labels()
Construct empty Labels.
Definition: DataPoints.cpp:64
PointMatcher::Matcher::visitCounter
unsigned long visitCounter
number of points visited
Definition: PointMatcher.h:464
PointMatcherSupport::Parametrizable::ParameterDoc
The documentation of a parameter.
Definition: Parametrizable.h:117
PointMatcherSupport::Logger::~Logger
virtual ~Logger()
Virtual destructor, do nothing.
Definition: Logger.cpp:54
PointMatcher::DataPoints::getDescriptorDimension
unsigned getDescriptorDimension(const std::string &name) const
Return the dimension of a descriptor with a given name. Return 0 if the name is not found.
Definition: DataPoints.cpp:596
PointMatcher::TransformationCheckers
A chain of TransformationChecker.
Definition: PointMatcher.h:600
PointMatcher::DataPoints::getDescriptorDim
unsigned getDescriptorDim() const
Return the total number of descriptors.
Definition: DataPoints.cpp:186
PointMatcher::DataPoints::View
Eigen::Block< Matrix > View
A view on a feature or descriptor.
Definition: PointMatcher.h:202
PointMatcher::Inspector
An inspector allows to log data at the different steps, for analysis.
Definition: PointMatcher.h:613
PointMatcher::DataPoints::conservativeResize
void conservativeResize(Index pointCount)
Resize the cloud to pointCount points, conserving existing ones.
Definition: DataPoints.cpp:327
PointMatcher::TransformationChecker::StringVector
std::vector< std::string > StringVector
a vector of strings
Definition: PointMatcher.h:575
PointMatcher::DataPoints::getDescriptorRowViewByName
ConstView getDescriptorRowViewByName(const std::string &name, const unsigned row) const
Get a const view on a descriptor row by name and number, throw an exception if it does not exist.
Definition: DataPoints.cpp:568
PointMatcher::Transformations::apply
void apply(DataPoints &cloud, const TransformationParameters &parameters) const
Apply this chain to cloud, using parameters, mutates cloud.
Definition: Transformation.cpp:61
PointMatcher::get
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:141
PointMatcher::ErrorMinimizer::ErrorElements::nbRejectedMatches
int nbRejectedMatches
number of matches with zero weights
Definition: PointMatcher.h:528
PointMatcher::DataPoints::Labels::totalDim
size_t totalDim() const
Return the sum of the spans of each label.
Definition: DataPoints.cpp:87
PointMatcher::ICPChainBase::transformations
Transformations transformations
transformations
Definition: PointMatcher.h:650
PointMatcherSupport::Logger::finishWarningEntry
virtual void finishWarningEntry(const char *file, unsigned line, const char *func)
Finish the entry into the warning channel.
Definition: Logger.cpp:94
PointMatcher::DataPoints::DataPoints
DataPoints()
Construct an empty point cloud.
Definition: DataPoints.cpp:97
PointMatcher::ParameterDoc
Parametrizable::ParameterDoc ParameterDoc
alias
Definition: PointMatcher.h:179
PointMatcher::ErrorMinimizer
An error minimizer will compute a transformation matrix such as to minimize the error between the rea...
Definition: PointMatcher.h:519
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: PointMatcher.h:78
PointMatcher::Transformation::~Transformation
virtual ~Transformation()
virtual destructor
Definition: Transformation.cpp:52
PointMatcher::DataPoints::getTimeRowViewByName
TimeConstView getTimeRowViewByName(const std::string &name, const unsigned row) const
Get a const view on a time row by name and number, throw an exception if it does not exist.
Definition: DataPoints.cpp:672
PointMatcher::Transformation::Transformation
Transformation()
Construct without parameter.
Definition: Transformation.cpp:41
PointMatcher::Matches::getDistsQuantile
T getDistsQuantile(const T quantile) const
Get the distance at the T-ratio closest point.
Definition: Matches.cpp:61
PointMatcher::ICPSequence::getPrefilteredMap
const DataPoints getPrefilteredMap() const
Return the map, in global coordinates (slow)
Definition: ICP.cpp:521
PointMatcherSupport::TransformationError
An expection thrown when a transformation has invalid parameters.
Definition: PointMatcher.h:89
PointMatcher::Transformation::compute
virtual DataPoints compute(const DataPoints &input, const TransformationParameters &parameters) const =0
Transform input using the transformation matrix.
PointMatcher::ICPSequence::T_refIn_refMean
TransformationParameters T_refIn_refMean
offset for centered map
Definition: PointMatcher.h:753
PointMatcher::DataPoints::load
static DataPoints load(const std::string &fileName)
Load a point cloud from a file, determine format from extension.
Definition: IO.cpp:375
PointMatcher::TransformationChecker::limitNames
StringVector limitNames
names of limits involved in conditions to stop ICP loop
Definition: PointMatcher.h:578
PointMatcher::DataPointsFilters::apply
void apply(DataPoints &cloud)
Apply this chain to cloud, mutates cloud.
Definition: DataPointsFilter.cpp:107
PointMatcher::DataPoints::getDescriptorViewByName
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
Definition: DataPoints.cpp:554
PointMatcher::DataPoints::save
void save(const std::string &fileName, bool binary=false) const
Save a point cloud to a file, determine format from extension.
Definition: IO.cpp:809
PointMatcher::DataPoints::InvalidField::InvalidField
InvalidField(const std::string &reason)
Construct the exception with an error message.
Definition: DataPoints.cpp:51
PointMatcher::ConvergenceError::ConvergenceError
ConvergenceError(const std::string &reason)
Construct the exception with an error message.
Definition: Exceptions.cpp:42
PointMatcherSupport::Parametrizable::className
const std::string className
name of the class
Definition: Parametrizable.h:159
PointMatcher::ErrorMinimizer::lastErrorElements
ErrorElements lastErrorElements
memory of the last computed error
Definition: PointMatcher.h:560
PointMatcher::DataPoints::getFeatureStartingRow
unsigned getFeatureStartingRow(const std::string &name) const
Return the starting row of a feature with a given name. Return 0 if the name is not found.
Definition: DataPoints.cpp:507
PointMatcher::DataPoints::getFeatureCopyByName
Matrix getFeatureCopyByName(const std::string &name) const
Get feature by name, return a matrix containing a copy of the requested feature.
Definition: DataPoints.cpp:451
PointMatcher::ICP::readingFiltered
DataPoints readingFiltered
reading point cloud after the filters were applied
Definition: PointMatcher.h:717
PointMatcher::ICPChainBase::prefilteredReferencePtsCount
unsigned prefilteredReferencePtsCount
remaining number of points after prefiltering but before the iterative process
Definition: PointMatcher.h:669
PointMatcher::DataPoints::allocateDescriptors
void allocateDescriptors(const Labels &newLabels)
Make sure a vector of descriptors of given names exist.
Definition: DataPoints.cpp:525
PointMatcher::TransformationsConstIt
Transformations::const_iterator TransformationsConstIt
alias
Definition: PointMatcher.h:419
PointMatcher::ICP
ICP algorithm.
Definition: PointMatcher.h:691
PointMatcher::Quaternion
Eigen::Quaternion< T > Quaternion
A quaternion over ScalarType.
Definition: PointMatcher.h:157
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:156
PointMatcher::ICPChainBase::readingStepDataPointsFilters
DataPointsFilters readingStepDataPointsFilters
filters for reading, applied at each step
Definition: PointMatcher.h:648
PointMatcher::DataPoints::Labels::const_iterator
std::vector< Label >::const_iterator const_iterator
alias
Definition: PointMatcher.h:223
PointMatcher::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:174
PointMatcherSupport::InvalidModuleType::InvalidModuleType
InvalidModuleType(const std::string &reason)
Construct an invalid–module-type exception.
Definition: ICP.cpp:59
PointMatcher::DataPoints::Label
The name for a certain number of dim.
Definition: PointMatcher.h:213
PointMatcher::Matches::InvalidDist
static constexpr T InvalidDist
In case of too few matches the ids are filled with InvalidId.
Definition: PointMatcher.h:370