Go to the documentation of this file.
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.4.3"
74 #define POINTMATCHER_VERSION_INT 10403
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;
238 for(
size_t i=0; i<labels.size(); i++)
240 stream << labels[i].text;
241 if(i != (labels.size() -1))
274 void save(
const std::string& fileName,
bool binary =
false,
unsigned precision = 7)
const;
340 template<
typename MatrixType>
342 template<
typename MatrixType>
344 template<
typename MatrixType>
346 template<
typename MatrixType>
348 template<
typename MatrixType>
350 template<
typename MatrixType>
356 template<
typename MatrixType>
382 Matches(
const int knn,
const int pointsCount);
451 virtual void inPlaceFilter(
DataPoints& cloud) = 0;
482 void resetVisitCount();
483 unsigned long getVisitCount()
const;
486 virtual void init(
const DataPoints& filteredReference) = 0;
553 T getPointUsedRatio()
const;
554 T getWeightedPointUsedRatio()
const;
556 virtual T getOverlap()
const;
557 virtual Matrix getCovariance()
const;
602 const Vector& getLimits()
const;
603 const Vector& getConditionVariables()
const;
637 virtual void dumpStats(std::ostream& stream);
638 virtual void dumpStatsHeader(std::ostream& stream);
642 virtual void finish(
const size_t iterationCount);
671 virtual void setDefault();
673 virtual void loadFromYaml(std::istream& in);
674 virtual void loadFromYamlNode(
const YAML::Node& doc);
675 unsigned getPrefilteredReadingPtsCount()
const;
676 unsigned getPrefilteredReferencePtsCount()
const;
678 bool getMaxNumIterationsReached()
const;
689 virtual void loadAdditionalYAMLContent(YAML::Node& doc);
693 const std::string& createModulesFromRegistrar(
const std::string& regName,
const YAML::Node& doc,
const R& registrar, std::vector<std::shared_ptr<typename R::TargetType> >& modules);
697 const std::string& createModuleFromRegistrar(
const std::string& regName,
const YAML::Node& doc,
const R& registrar, std::shared_ptr<typename R::TargetType>& module);
753 "Function now always returns map with filter chain applied. "
754 "This may have altered your program behavior."
755 "Reasons for this stated here and in associated PR: "
756 "https://github.com/norlab-ulaval/libpointmatcher/issues/209.")
760 "Function now always returns map with filter chain applied. "
761 "This may have altered your program behavior."
762 "Reasons for this stated here and in associated PR: "
763 "https://github.com/norlab-ulaval/libpointmatcher/issues/209")
882 #endif // __POINTMATCHER_CORE_H
virtual void setDefault()
Construct an ICP algorithm that works in most of the cases.
void clearMap()
Clear the map (reset to same state as after the object is created)
bool hasMap() const
Return whether the object currently holds a valid map.
bool timeExists(const std::string &name) const
Look if a time with a given name exist.
PM_DEPRECATED("Use getPrefilteredInternalMap instead. " "Function now always returns map with filter chain applied. " "This may have altered your program behavior." "Reasons for this stated here and in associated PR: " "https://github.com/norlab-ulaval/libpointmatcher/issues/209.") const DataPoints &getInternalMap() const
int nbRejectedPoints
number of points with all matches set to zero weights
DataPointsFilters()
Construct an empty chain.
unsigned getNbGroupedDescriptors() const
Return the number of grouped descriptors (e.g., normals can have 3 components but would count as only...
A chain of OutlierFilter.
Parametrizable::Parameters Parameters
alias
DataPointsFilters::iterator DataPointsFiltersIt
alias
static void addEmpty3dPointFields(const Index numberOfPoints, DataPoints &pointCloud)
Adds 3D coordinates and normals fields to a point cloud.
typename DataPoints::Index Index
Matrix Dists
Squared distances to closest points, dense matrix of ScalarType.
bool operator==(const Label &that) const
Return whether two labels are equals.
Dists dists
squared distances to closest points
Labels descriptorLabels
labels of descriptors
PM::DataPointsFilter DataPointsFilter
TransformationParameters compute(const DataPoints &cloudIn, const TransformationParameters &initialTransformationParameters)
Apply ICP to cloud cloudIn, with initial guess.
void swapCols(Index iCol, Index jCol)
Swap column i and j in the point cloud, swap also features and descriptors if any....
virtual void finishInfoEntry(const char *file, unsigned line, const char *func)
Finish the entry into the info channel.
std::string text
name of the label
void validateFile(const std::string &fileName)
Throw a runtime_error exception if fileName cannot be opened.
OutlierFilters::const_iterator OutlierFiltersConstIt
alias
std::vector< Vector, Eigen::aligned_allocator< Vector > > VectorVector
A vector of vector over ScalarType, not a matrix.
unsigned getEuclideanDim() const
Return the dimension of the point cloud.
T getMedianAbsDeviation() const
Calculate the Median of Absolute Deviation(MAD), which is median(|x-median(x)|), a kind of robust sta...
void init()
Init the chain.
Ids ids
identifiers of closest points
static DataPoints generateUniformlySampledCylinder(const ScalarType radius, const ScalarType height, const Index numberOfPoints, const StaticCoordVector &translation, const Quaternion &rotation)
Generates a uniformly sampled cylinder (with no filling), with a given number of points and pose.
Matrix OutlierWeights
Weights of the associations between the points in Matches and the points in the reference.
ErrorMinimizer::ErrorElements ErrorElements
static DataPoints generateUniformlySampledBox(const ScalarType length, const ScalarType width, const ScalarType height, const Index numberOfPoints, const StaticCoordVector &translation, const Quaternion &rotation)
Generates a uniformly sampled box (with no filling), with a given number of points and pose.
virtual void beginInfoEntry(const char *file, unsigned line, const char *func)
Start a new entry into the info channel.
static constexpr Eigen::Index kPointDimension
The dimension of the point clouds that libpointmatcher will process.
void addTime(const std::string &name, const Int64Matrix &newTime)
Add a time by name, remove first if already exists.
void assertDescriptorConsistency() const
Assert if descriptors are not consistent with features.
TransformationCheckers transformationCheckers
transformation checkers
T pointUsedRatio
the ratio of how many points were used for error minimization
TransformationParameters compute(const DataPoints &readingIn, const DataPoints &referenceIn, const TransformationParameters &initialTransformationParameters)
Perform ICP from initial guess and return optimised transformation matrix.
void removeFeature(const std::string &name)
Remove a feature by name, the whole matrix will be copied.
void concatenate(const DataPoints &dp)
Add another point cloud after the current one. Faster merge will be achieved if all descriptor and ti...
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...
std::map< std::string, std::vector< std::string > > CsvElements
Data from a CSV file.
virtual std::ostream * infoStream()
Return the info stream, 0 if hasInfoChannel() returns false.
void allocateDescriptor(const std::string &name, const unsigned dim)
Makes sure a descriptor of a given name exists, if present, check its dimensions.
bool descriptorExists(const std::string &name) const
Look if a descriptor with a given name exist.
PointMatcherSupport::Parametrizable Parametrizable
alias
TransformationCheckers::iterator TransformationCheckersIt
alias
Eigen::Block< MatrixType > getViewByName(const std::string &name, const Labels &labels, MatrixType &data, const int viewRow=-1) const
static AffineTransform buildUpTransformation(const StaticCoordVector &translation, const Quaternion &rotation)
Builds a 3D affine transformation with a given translation and rotation.
T ScalarType
The scalar type.
void addDescriptor(const std::string &name, const Matrix &newDescriptor)
Add a descriptor by name, remove first if already exists.
const Eigen::Block< const MatrixType > getConstViewByName(const std::string &name, const Labels &labels, const MatrixType &data, const int viewRow=-1) const
An expception thrown when the yaml config file contains invalid configuration (e.g....
void allocateFields(const Labels &newLabels, Labels &labels, MatrixType &data) const
Make sure a vector of fields of given names exist.
bool setMap(const DataPoints &map)
Set the map using inputCloud.
An outlier filter removes or weights links between points in reading and their matched points in refe...
Eigen::Array< T, Eigen::Dynamic, Eigen::Dynamic > Array
A dense array over ScalarType.
Functions and classes that are dependant on scalar type are defined in this templatized class.
Eigen::Matrix< T, kPointDimension, 1 > StaticCoordVector
A vector over ScalarType of kPointDimension.
DataPointsFilters referenceDataPointsFilters
filters for reference
Point matcher did not converge.
void setLogger(std::shared_ptr< Logger > newLogger)
Set a new logger, protected by a mutex.
A chain of DataPointsFilter.
TransformationCheckers::const_iterator TransformationCheckersConstIt
alias
The logger interface, used to output warnings and informations.
virtual bool hasWarningChannel() const
Return whether this logger provides the warning channel.
bool maxNumIterationsReached
store if we reached the maximum number of iterations last time compute was called
Stuff common to all ICP algorithms.
Matrix getDescriptorCopyByName(const std::string &name) const
Get descriptor by name, return a matrix containing a copy of the requested descriptor.
unsigned getTimeDim() const
Return the total number of times.
Int64Matrix getTimeCopyByName(const std::string &name) const
Get time by name, return a matrix containing a copy of the requested time.
void allocateFeatures(const Labels &newLabels)
Make sure a vector of features of given names exist.
Eigen::Transform< T, kPointDimension, Eigen::Affine > AffineTransform
An affine transform over ScalarType of kPointDimension.
DataPoints mapPointCloud
point cloud of the map, always in global frame (frame of first point cloud)
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.
std::vector< Quaternion, Eigen::aligned_allocator< Quaternion > > QuaternionVector
A vector of quaternions over ScalarType.
unsigned getHomogeneousDim() const
Return the dimension of the point cloud in homogeneous coordinates (one more than Euclidean dimension...
A matcher links points in the reading to points in the reference.
static StaticCoordVector computeNormalOfAxisAlignedPlane(const StaticCoordVector &axisAlignedPlaneDimensions)
Computes a normal vector from a vector that contains the dimensions of a 2D shape in at-most 2 direct...
Labels timeLabels
labels of times.
TransformationParameters operator()(const DataPoints &cloudIn)
Apply ICP to cloud cloudIn, with identity as initial guess.
friend std::ostream & operator<<(std::ostream &stream, const Labels &labels)
Parametrizable::ParametersDoc ParametersDoc
alias
bool operator==(const DataPoints &that) const
Return whether two point-clouds are identical (same data and same labels)
An exception thrown when one tries to use a module type that does not exist.
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.
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.
#define DEF_REGISTRAR_IFACE(name, ifaceName)
#define DEF_REGISTRAR(name)
const typedef Eigen::Block< const Int64Matrix > TimeConstView
a view on a const time
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
ConstView getFeatureViewByName(const std::string &name) const
Get a const view on a feature by name, throw an exception if it does not exist.
OutlierWeights compute(const DataPoints &filteredReading, const DataPoints &filteredReference, const Matches &input)
Apply outlier-detection chain.
const DataPoints & getPrefilteredInternalMap() const
Return the map, in internal coordinates (fast)
Eigen::Matrix< std::int64_t, Eigen::Dynamic, Eigen::Dynamic > Int64Matrix
A dense signed 64-bits matrix.
DataPointsFilters readingDataPointsFilters
filters for reading, applied once
DataPointsFilters::const_iterator DataPointsFiltersConstIt
alias
static DataPoints generateUniformlySampledPlane(const StaticCoordVector &dimensions, const Index numberOfPoints, const StaticCoordVector &translation, const Quaternion &rotation)
Generates a uniformly sampled plane, with a given number of points and pose.
bool featureExists(const std::string &name) const
Look if a feature with a given name exist.
const DataPoints & getReadingFiltered() const
Return the filtered point cloud reading used in the ICP chain.
OutlierFilters::iterator OutlierFiltersIt
alias
PM::TransformationChecker TransformationChecker
A structure holding data ready for minimization. The data are "normalized", for instance there are no...
void removeField(const std::string &name, Labels &labels, MatrixType &data) const
Remove a descriptor or feature by name, no copy is done.
void allocateTime(const std::string &name, const unsigned dim)
Makes sure a time of a given name exists, if present, check its dimensions.
void allocateFeature(const std::string &name, const unsigned dim)
Makes sure a feature of a given name exists, if present, check its dimensions.
Matches()
In case of too few matches the dists are filled with InvalidDist.
virtual std::ostream * warningStream()
Return the warning stream, 0 if hasWarningChannel() returns false.
std::vector< ParameterDoc > ParametersDoc
The documentation of all parameters.
PointMatcher()
Constructor, populates the registrars.
unsigned getFieldStartingRow(const std::string &name, const Labels &labels) const
Return the starting row of a feature or a descriptor with a given name. Return 0 if the name is not f...
Labels featureLabels
labels of features
OutlierFilters outlierFilters
outlier filters
virtual void loadFromYaml(std::istream &in)
Construct an ICP algorithm from a YAML file.
Matches matches
associations
void setColFrom(Index thisCol, const DataPoints &that, Index thatCol)
Set column thisCol equal to column thatCol of that, copy features and descriptors if any....
DataPoints createSimilarEmpty() const
Create an empty DataPoints of similar dimensions and labels for features, descriptors and times.
std::shared_ptr< ErrorMinimizer > errorMinimizer
error minimizer
void concatenateLabelledMatrix(Labels *labels, MatrixType *data, const Labels extraLabels, const MatrixType extraData)
Add another matrix after the current one. Faster merge will be achieved if all labels are the same....
T weightedPointUsedRatio
the ratio of how many points were used (with weight) for error minimization
Matrix descriptors
descriptors of points in the cloud, might be empty
Parameters parameters
parameters with their values encoded in string
Eigen::Block< Int64Matrix > TimeView
A view on a time.
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Logger()
Construct without parameter.
DataPoints reading
reading point cloud
A data filter takes a point cloud as input, transforms it, and produces another point cloud as output...
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.
TimeConstView getTimeViewByName(const std::string &name) const
Get a const view on a time by name, throw an exception if it does not exist.
PM::ErrorMinimizer ErrorMinimizer
IntMatrix Ids
Identifiers of closest points, dense matrix of integers.
void assertTimesConsistency() const
Assert if times are not consistent with features.
Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic > IntMatrix
A dense integer matrix.
An exception thrown when one tries to fetch the value of an unexisting parameter.
virtual bool hasInfoChannel() const
Return whether this logger provides the info channel.
unsigned getNbPoints() const
Return the number of points contained in the point cloud.
void allocateField(const std::string &name, const unsigned dim, Labels &labels, MatrixType &data) const
Make sure a field of a given name exists, if present, check its dimensions.
TransformationParameters operator()(const DataPoints &readingIn, const DataPoints &referenceIn)
Perform ICP and return optimised transformation matrix.
Label(const std::string &text="", const size_t span=0)
Construct a label from a given name and number of data dimensions it spans.
static void swapDataPoints(DataPoints &a, DataPoints &b)
Exchange in place point clouds a and b, with no data copy.
Matrix features
features of points in the cloud
DataPoints reference
reference point cloud
Eigen::Matrix< T, Eigen::Dynamic, 1 > Vector
A vector over ScalarType.
const typedef Eigen::Block< const Matrix > ConstView
A view on a const feature or const descriptor.
Transformations::iterator TransformationsIt
alias
Matrix::Index Index
An index to a row or a column.
std::shared_ptr< Matcher > matcher
matcher
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.
virtual void beginWarningEntry(const char *file, unsigned line, const char *func)
Start a new entry into the warning channel.
bool contains(const std::string &text) const
Return whether there is a label named text.
static DataPoints generateUniformlySampledSphere(const ScalarType radius, const Index numberOfPoints, const StaticCoordVector &translation, const Quaternion &rotation)
Generates a uniformly sampled sphere (with no filling), with a given number of points and pose.
Parametrizable::InvalidParameter InvalidParameter
alias
Result of the data-association step (Matcher::findClosests), before outlier rejection.
std::shared_ptr< Inspector > inspector
inspector
Int64Matrix times
time associated to each points, might be empty
void allocateTimes(const Labels &newLabels)
Make sure a vector of time of given names exist.
static DataPoints generateUniformlySampledCircle(const ScalarType radius, const Index numberOfPoints, const StaticCoordVector &translation, const Quaternion &rotation)
Generates a uniformly sampled circle, with a given number of points and pose.
The superclass of classes that are constructed using generic parameters. This class provides the para...
void removeTime(const std::string &name)
Remove a descriptor by name, the whole matrix will be copied.
void assertConsistency(const std::string &dataName, const int dataRows, const int dataCols, const Labels &labels) const
Assert if a matrix is not consistent with features.
void removeDescriptor(const std::string &name)
Remove a descriptor by name, the whole matrix will be copied.
static constexpr int InvalidId
unsigned prefilteredReadingPtsCount
remaining number of points after prefiltering but before the iterative process
void save(const std::string &fileName, bool binary=false, unsigned precision=7) const
Save a point cloud to a file, determine format from extension.
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.
OutlierWeights weights
weights for every association
size_t span
number of data dimensions the label spans
Labels()
Construct empty Labels.
unsigned long visitCounter
number of points visited
The documentation of a parameter.
virtual ~Logger()
Virtual destructor, do nothing.
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.
unsigned getDescriptorDim() const
Return the total number of descriptors.
Eigen::Block< Matrix > View
A view on a feature or descriptor.
An inspector allows to log data at the different steps, for analysis.
void conservativeResize(Index pointCount)
Resize the cloud to pointCount points, conserving existing ones.
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.
static const PointMatcher & get()
Return instances.
int nbRejectedMatches
number of matches with zero weights
size_t totalDim() const
Return the sum of the spans of each label.
virtual void loadFromYamlNode(const YAML::Node &doc)
Construct an ICP algorithm from a YAML file.
T getStandardDeviation() const
Transformations transformations
transformations
virtual void finishWarningEntry(const char *file, unsigned line, const char *func)
Finish the entry into the warning channel.
DataPoints()
Construct an empty point cloud.
Parametrizable::ParameterDoc ParameterDoc
alias
An error minimizer will compute a transformation matrix such as to minimize the error between the rea...
static constexpr ScalarType pi
Pi.
Functions and classes that are not dependant on scalar type are defined in this namespace.
unsigned getFieldDimension(const std::string &name, const Labels &labels) const
Return the dimension of a feature or a descriptor with a given name. Return 0 if the name is not foun...
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.
bool fieldExists(const std::string &name, const unsigned dim, const Labels &labels) const
Look if a descriptor or a feature with a given name and dimension exist.
ConfigurationError(const std::string &reason)
return an exception when a transformation has invalid parameters
T getDistsQuantile(const T quantile) const
Get the distance at the T-ratio closest point.
const DataPoints getPrefilteredMap() const
Return the map, in global coordinates (slow)
static void applyTransformation(const StaticCoordVector &translation, const Quaternion &rotation, DataPoints &pointCloud)
Transforms a point cloud by translating and rotating it a given amount.
TransformationParameters T_refIn_refMean
offset for centered map
static DataPoints load(const std::string &fileName)
Load a point cloud from a file, determine format from extension.
void apply(DataPoints &cloud)
Apply this chain to cloud, mutates cloud.
void addField(const std::string &name, const MatrixType &newField, Labels &labels, MatrixType &data) const
Add a descriptor or feature by name, remove first if already exists.
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
InvalidField(const std::string &reason)
Construct the exception with an error message.
ConvergenceError(const std::string &reason)
Construct the exception with an error message.
PM::OutlierFilter OutlierFilter
const std::string className
name of the class
ErrorElements lastErrorElements
memory of the last computed error
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.
Matrix getFeatureCopyByName(const std::string &name) const
Get feature by name, return a matrix containing a copy of the requested feature.
DataPoints readingFiltered
reading point cloud after the filters were applied
unsigned prefilteredReferencePtsCount
remaining number of points after prefiltering but before the iterative process
void allocateDescriptors(const Labels &newLabels)
Make sure a vector of descriptors of given names exist.
Transformations::const_iterator TransformationsConstIt
alias
Eigen::Quaternion< T > Quaternion
A quaternion over ScalarType.
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
DataPointsFilters readingStepDataPointsFilters
filters for reading, applied at each step
std::vector< Label >::const_iterator const_iterator
alias
Matrix TransformationParameters
A matrix holding the parameters a transformation.
InvalidModuleType(const std::string &reason)
Construct an invalid–module-type exception.
The name for a certain number of dim.
static constexpr T InvalidDist
In case of too few matches the ids are filled with InvalidId.