36 #ifndef __POINTMATCHER_CORE_H 37 #define __POINTMATCHER_CORE_H 39 #ifndef EIGEN_USE_NEW_STDVECTOR 40 #define EIGEN_USE_NEW_STDVECTOR 41 #endif // EIGEN_USE_NEW_STDVECTOR 45 #define EIGEN_NO_DEBUG 47 #include "Eigen/StdVector" 49 #include "Eigen/Geometry" 52 #include <boost/thread/mutex.hpp> 60 #include <boost/cstdint.hpp> 72 #define POINTMATCHER_VERSION "1.3.1" 74 #define POINTMATCHER_VERSION_INT 10301 110 virtual bool hasInfoChannel()
const;
111 virtual void beginInfoEntry(
const char *
file,
unsigned line,
const char *func);
112 virtual std::ostream* infoStream();
113 virtual void finishInfoEntry(
const char *file,
unsigned line,
const char *func);
114 virtual bool hasWarningChannel()
const;
115 virtual void beginWarningEntry(
const char *file,
unsigned line,
const char *func);
116 virtual std::ostream* warningStream();
117 virtual void finishWarningEntry(
const char *file,
unsigned line,
const char *func);
120 void setLogger(std::shared_ptr<Logger> newLogger);
125 typedef std::map<std::string, std::vector<std::string> >
CsvElements;
137 #define ZERO_PLUS_EPS (0. + std::numeric_limits<double>::epsilon()) 138 #define ONE_MINUS_EPS (1. - std::numeric_limits<double>::epsilon()) 161 typedef typename Eigen::Matrix<T, Eigen::Dynamic, 1>
Vector;
163 typedef std::vector<Vector, Eigen::aligned_allocator<Vector> >
VectorVector;
169 typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
Matrix;
171 typedef typename Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic>
IntMatrix;
173 typedef typename Eigen::Matrix<std::int64_t, Eigen::Dynamic, Eigen::Dynamic>
Int64Matrix;
175 typedef typename Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>
Array;
210 typedef Eigen::Block<Matrix>
View;
218 typedef typename Matrix::Index
Index;
235 size_t totalDim()
const;
238 for(
size_t i=0; i<labels.size(); i++)
240 stream << labels[i].text;
241 if(i != (labels.size() -1))
262 DataPoints(
const Matrix& features,
const Labels& featureLabels,
const Matrix& descriptors,
const Labels& descriptorLabels);
263 DataPoints(
const Matrix& features,
const Labels& featureLabels,
const Matrix& descriptors,
const Labels& descriptorLabels,
const Int64Matrix& times,
const Labels& timeLabels);
267 unsigned getNbPoints()
const;
268 unsigned getEuclideanDim()
const;
269 unsigned getHomogeneousDim()
const;
270 unsigned getNbGroupedDescriptors()
const;
271 unsigned getDescriptorDim()
const;
272 unsigned getTimeDim()
const;
274 void save(
const std::string& fileName,
bool binary =
false)
const;
278 void conservativeResize(Index pointCount);
280 DataPoints createSimilarEmpty(Index pointCount)
const;
281 void setColFrom(Index thisCol,
const DataPoints& that, Index thatCol);
282 void swapCols(Index iCol,Index jCol);
286 void allocateFeatures(
const Labels& newLabels);
287 void addFeature(
const std::string& name,
const Matrix& newFeature);
289 Matrix getFeatureCopyByName(
const std::string& name)
const;
290 ConstView getFeatureViewByName(
const std::string& name)
const;
291 View getFeatureViewByName(
const std::string& name);
292 ConstView getFeatureRowViewByName(
const std::string& name,
const unsigned row)
const;
293 View getFeatureRowViewByName(
const std::string& name,
const unsigned row);
295 bool featureExists(
const std::string& name,
const unsigned dim)
const;
296 unsigned getFeatureDimension(
const std::string& name)
const;
297 unsigned getFeatureStartingRow(
const std::string& name)
const;
300 void allocateDescriptor(
const std::string& name,
const unsigned dim);
301 void allocateDescriptors(
const Labels& newLabels);
302 void addDescriptor(
const std::string& name,
const Matrix& newDescriptor);
304 Matrix getDescriptorCopyByName(
const std::string& name)
const;
305 ConstView getDescriptorViewByName(
const std::string& name)
const;
306 View getDescriptorViewByName(
const std::string& name);
307 ConstView getDescriptorRowViewByName(
const std::string& name,
const unsigned row)
const;
308 View getDescriptorRowViewByName(
const std::string& name,
const unsigned row);
309 bool descriptorExists(
const std::string& name)
const;
310 bool descriptorExists(
const std::string& name,
const unsigned dim)
const;
311 unsigned getDescriptorDimension(
const std::string& name)
const;
312 unsigned getDescriptorStartingRow(
const std::string& name)
const;
313 void assertDescriptorConsistency()
const;
316 void allocateTime(
const std::string& name,
const unsigned dim);
317 void allocateTimes(
const Labels& newLabels);
318 void addTime(
const std::string& name,
const Int64Matrix& newTime);
320 Int64Matrix getTimeCopyByName(
const std::string& name)
const;
321 TimeConstView getTimeViewByName(
const std::string& name)
const;
322 TimeView getTimeViewByName(
const std::string& name);
323 TimeConstView getTimeRowViewByName(
const std::string& name,
const unsigned row)
const;
324 TimeView getTimeRowViewByName(
const std::string& name,
const unsigned row);
326 bool timeExists(
const std::string& name,
const unsigned dim)
const;
327 unsigned getTimeDimension(
const std::string& name)
const;
328 unsigned getTimeStartingRow(
const std::string& name)
const;
329 void assertTimesConsistency()
const;
339 void assertConsistency(
const std::string& dataName,
const int dataRows,
const int dataCols,
const Labels& labels)
const;
340 template<
typename MatrixType>
341 void allocateFields(
const Labels& newLabels,
Labels& labels, MatrixType&
data)
const;
342 template<
typename MatrixType>
343 void allocateField(
const std::string& name,
const unsigned dim,
Labels& labels, MatrixType&
data)
const;
344 template<
typename MatrixType>
345 void addField(
const std::string& name,
const MatrixType& newField,
Labels& labels, MatrixType&
data)
const;
346 template<
typename MatrixType>
348 template<
typename MatrixType>
349 const Eigen::Block<const MatrixType> getConstViewByName(
const std::string& name,
const Labels& labels,
const MatrixType&
data,
const int viewRow = -1)
const;
350 template<
typename MatrixType>
351 Eigen::Block<MatrixType> getViewByName(
const std::string& name,
const Labels& labels, MatrixType&
data,
const int viewRow = -1)
const;
352 bool fieldExists(
const std::string& name,
const unsigned dim,
const Labels& labels)
const;
354 unsigned getFieldStartingRow(
const std::string& name,
const Labels& labels)
const;
356 template<
typename MatrixType>
357 void concatenateLabelledMatrix(
Labels* labels, MatrixType*
data,
const Labels extraLabels,
const MatrixType extraData);
377 static constexpr
int InvalidId = -1;
378 static constexpr
T InvalidDist = std::numeric_limits<T>::infinity();
381 Matches(
const Dists& dists,
const Ids ids);
382 Matches(
const int knn,
const int pointsCount);
387 T getDistsQuantile(
const T quantile)
const;
388 T getMedianAbsDeviation()
const;
389 T getStandardDeviation()
const;
411 virtual DataPoints compute(
const DataPoints& input,
const TransformationParameters& parameters)
const = 0;
414 virtual void inPlaceCompute(
const TransformationParameters& parameters,
DataPoints& cloud)
const = 0;
417 virtual bool checkParameters(
const TransformationParameters& parameters)
const = 0;
420 virtual TransformationParameters correctParameters(
const TransformationParameters& parameters)
const = 0;
427 void apply(
DataPoints& cloud,
const TransformationParameters& parameters)
const;
451 virtual void inPlaceFilter(
DataPoints& cloud) = 0;
481 void resetVisitCount();
482 unsigned long getVisitCount()
const;
485 virtual void init(
const DataPoints& filteredReference) = 0;
552 T getPointUsedRatio()
const;
553 T getWeightedPointUsedRatio()
const;
555 virtual T getOverlap()
const;
556 virtual Matrix getCovariance()
const;
560 virtual TransformationParameters compute(
const DataPoints& filteredReading,
const DataPoints& filteredReference,
const OutlierWeights& outlierWeights,
const Matches&
matches);
562 virtual TransformationParameters compute(
const ErrorElements& matchedPoints) = 0;
565 static Matrix crossProduct(
const Matrix& A,
const Matrix& B);
597 virtual void init(
const TransformationParameters& parameters,
bool& iterate) = 0;
599 virtual void check(
const TransformationParameters& parameters,
bool& iterate) = 0;
601 const Vector& getLimits()
const;
602 const Vector& getConditionVariables()
const;
603 const StringVector& getLimitNames()
const;
604 const StringVector& getConditionVariableNames()
const;
607 static Vector matrixToAngles(
const TransformationParameters& parameters);
613 void init(
const TransformationParameters& parameters,
bool& iterate);
614 void check(
const TransformationParameters& parameters,
bool& iterate);
636 virtual void dumpStats(std::ostream& stream);
637 virtual void dumpStatsHeader(std::ostream& stream);
641 virtual void finish(
const size_t iterationCount);
668 virtual ~ICPChainBase();
670 virtual void setDefault();
672 virtual void loadFromYaml(std::istream& in);
673 unsigned getPrefilteredReadingPtsCount()
const;
674 unsigned getPrefilteredReferencePtsCount()
const;
676 bool getMaxNumIterationsReached()
const;
687 virtual void loadAdditionalYAMLContent(PointMatcherSupport::YAML::Node& doc);
691 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);
695 const std::string& createModuleFromRegistrar(
const std::string& regName,
const PointMatcherSupport::YAML::Node& doc,
const R& registrar, std::shared_ptr<typename R::TargetType>& module);
747 virtual void setDefault();
748 virtual void loadFromYaml(std::istream& in);
750 "Function now always returns map with filter chain applied. " 751 "This may have altered your program behavior." 752 "Reasons for this stated here and in associated PR: " 753 "https://github.com/ethz-asl/libpointmatcher/issues/209.")
755 const DataPoints& getPrefilteredInternalMap()
const;
757 "Function now always returns map with filter chain applied. " 758 "This may have altered your program behavior." 759 "Reasons for this stated here and in associated PR: " 760 "https://github.com/ethz-asl/libpointmatcher/issues/209")
780 #endif // __POINTMATCHER_CORE_H Matrix OutlierWeights
Weights of the associations between the points in Matches and the points in the reference.
std::vector< Vector, Eigen::aligned_allocator< Vector > > VectorVector
A vector of vector over ScalarType, not a matrix.
PM::Transformation Transformation
A matcher links points in the reading to points in the reference.
const Eigen::Block< const Int64Matrix > TimeConstView
a view on a const time
Parametrizable::ParametersDoc ParametersDoc
alias
A structure holding data ready for minimization. The data are "normalized", for instance there are no...
DataPointsFilters::const_iterator DataPointsFiltersConstIt
alias
The logger interface, used to output warnings and informations.
Matrix descriptors
descriptors of points in the cloud, might be empty
Eigen::Block< Int64Matrix > TimeView
A view on a time.
InvalidModuleType(const std::string &reason)
Construct an invalid–module-type exception.
void setLogger(std::shared_ptr< Logger > newLogger)
Set a new logger, protected by a mutex.
Point matcher did not converge.
Matrix::Index Index
An index to a row or a column.
Eigen::Quaternion< T > Quaternion
A quaternion over ScalarType.
PM::DataPoints DataPoints
void validateFile(const std::string &fileName)
Throw a runtime_error exception if fileName cannot be opened.
PointMatcherSupport::Parametrizable Parametrizable
alias
TransformationCheckers::iterator TransformationCheckersIt
alias
DataPointsFilters referenceDataPointsFilters
filters for reference
T weightedPointUsedRatio
the ratio of how many points were used (with weight) for error minimization
Matrix Dists
Squared distances to closest points, dense matrix of ScalarType.
bool maxNumIterationsReached
store if we reached the maximum number of iterations last time compute was called ...
Labels featureLabels
labels of features
TransformationCheckers transformationCheckers
transformation checkers
DataPoints reference
reference point cloud
DataPoints mapPointCloud
point cloud of the map, always in global frame (frame of first point cloud)
std::ostream & operator<<(std::ostream &o, const Parametrizable::ParameterDoc &p)
Dump the documentation of this parameter to a stream.
Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic > IntMatrix
A dense integer matrix.
#define DEF_REGISTRAR_IFACE(name, ifaceName)
An error minimizer will compute a transformation matrix such as to minimize the error between the rea...
TransformationCheckers::const_iterator TransformationCheckersConstIt
alias
unsigned long visitCounter
number of points visited
Labels timeLabels
labels of times.
The name for a certain number of dim.
PM::ErrorMinimizer ErrorMinimizer
std::map< std::string, std::vector< std::string > > CsvElements
Data from a CSV file.
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
int nbRejectedPoints
number of points with all matches set to zero weights
Parametrizable::Parameters Parameters
alias
DataPointsFilters readingDataPointsFilters
filters for reading, applied once
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
DataPoints readingFiltered
reading point cloud after the filters were applied
OutlierWeights weights
weights for every association
int nbRejectedMatches
number of matches with zero weights
T ScalarType
The scalar type.
Functions and classes that are not dependant on scalar type are defined in this namespace.
Parametrizable::ParameterDoc ParameterDoc
alias
An inspector allows to log data at the different steps, for analysis.
An exception thrown when one tries to use a module type that does not exist.
const Eigen::Block< const Matrix > ConstView
A view on a const feature or const descriptor.
bool contains(const M &m, const typename M::key_type &k)
DataPoints::Labels Labels
Functions and classes that are dependant on scalar type are defined in this templatized class...
Transformations::const_iterator TransformationsConstIt
alias
OutlierFilters outlierFilters
outlier filters
const DataPoints & getReadingFiltered() const
Return the filtered point cloud reading used in the ICP chain.
Result of the data-association step (Matcher::findClosests), before outlier rejection.
std::vector< Quaternion, Eigen::aligned_allocator< Quaternion > > QuaternionVector
A vector of quaternions over ScalarType.
bool operator==(const GTEST_10_TUPLE_(T)&t, const GTEST_10_TUPLE_(U)&u)
A data filter takes a point cloud as input, transforms it, and produces another point cloud as output...
#define DEF_REGISTRAR(name)
std::shared_ptr< ErrorMinimizer > errorMinimizer
error minimizer
An outlier filter removes or weights links between points in reading and their matched points in refe...
Eigen::Matrix< std::int64_t, Eigen::Dynamic, Eigen::Dynamic > Int64Matrix
A dense signed 64-bits matrix.
The documentation of a parameter.
A chain of DataPointsFilter.
Parametrizable::InvalidParameter InvalidParameter
alias
PM::TransformationChecker TransformationChecker
The superclass of classes that are constructed using generic parameters. This class provides the para...
An exception thrown when one tries to fetch the value of an unexisting parameter. ...
Transformations::iterator TransformationsIt
alias
#define PM_DEPRECATED(msg)
std::vector< ParameterDoc > ParametersDoc
The documentation of all parameters.
A chain of OutlierFilter.
std::shared_ptr< Matcher > matcher
matcher
IntMatrix Ids
Identifiers of closest points, dense matrix of integers.
DataPointsFilters::iterator DataPointsFiltersIt
alias
size_t span
number of data dimensions the label spans
TransformationParameters T_refIn_refMean
offset for centered map
ErrorElements lastErrorElements
memory of the last computed error
std::shared_ptr< Inspector > inspector
inspector
Eigen::Block< Matrix > View
A view on a feature or descriptor.
Transformations transformations
transformations
unsigned prefilteredReadingPtsCount
remaining number of points after prefiltering but before the iterative process
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
T pointUsedRatio
the ratio of how many points were used for error minimization
Int64Matrix times
time associated to each points, might be empty
Dists dists
squared distances to closest points
std::vector< Label >::const_iterator const_iterator
alias
Eigen::Array< T, Eigen::Dynamic, Eigen::Dynamic > Array
A dense array over ScalarType.
Eigen::Matrix< T, Eigen::Dynamic, 1 > Vector
A vector over ScalarType.
Matches matches
associations
OutlierFilters::iterator OutlierFiltersIt
alias
NearestNeighbourSearch< T >::Matrix load(const char *fileName)
Ids ids
identifiers of closest points
Matrix features
features of points in the cloud
DataPointsFilters readingStepDataPointsFilters
filters for reading, applied at each step
PM::DataPointsFilter DataPointsFilter
Stuff common to all ICP algorithms.
OutlierFilters::const_iterator OutlierFiltersConstIt
alias
DataPoints reading
reading point cloud
std::string text
name of the label
Matrix TransformationParameters
A matrix holding the parameters a transformation.
PM::OutlierFilter OutlierFilter
An expception thrown when the yaml config file contains invalid configuration (e.g., mutually exclusive settings)
unsigned prefilteredReferencePtsCount
remaining number of points after prefiltering but before the iterative process
Labels descriptorLabels
labels of descriptors
PM::DataPointsFilters DataPointsFilters
ErrorMinimizer::ErrorElements ErrorElements