167 float *runningTime = NULL,
175 float *runningTime = NULL,
184 float kernel(
const float &x2,
const float &rho2);
Declares a virtual base class for all metric maps storage classes.
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
The ICP algorithm configuration data.
unsigned int maxIterations
Maximum number of iterations to run.
bool skip_quality_calculation
Skip the (sometimes) expensive evaluation of the term 'quality' at ICP output (Default=true)
float Axy_aprox_derivatives
[LM method only] The size of the perturbance in x & y used to estimate the Jacobians of the square er...
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic). See http://www.mrpt.org/tutorials/programming/scan-matchi...
uint32_t corresponding_points_decimation
Decimation of the point cloud being registered against the reference one (default=5) - set to 1 to ha...
float ransac_fuseMaxDiffPhi
float smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough.
float normalizationStd
RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used t...
bool onlyClosestCorrespondences
The usual approach: to consider only the closest correspondence for each local point (Default to true...
float thresholdAng
Initial threshold distance for two points to become a correspondence.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
bool skip_cov_calculation
Skip the computation of the covariance (saves some time) (default=false)
float minAbsStep_trans
If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters),...
TICPCovarianceMethod ICP_covariance_method
The method to use for covariance estimation (Default: icpCovFiniteDifferences)
float ALFA
The scale factor for threshold everytime convergence is achieved.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
TConfigParams()
Initializer for default values:
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
bool ransac_fuseByCorrsMatch
float kernel_rho
Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0....
float covariance_varPoints
This is the normalization constant that is used to scale the whole 3x3 covariance.
bool use_kernel
Whether to use kernel_rho to smooth distances, or use distances directly (default=true)
float LM_initial_lambda
[LM method only] The initial value of the lambda parameter in the LM method (default=1e-4)
float ransac_mahalanobisDistanceThreshold
unsigned int ransac_maxSetSize
float minAbsStep_rot
If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians),...
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
CICP()
Constructor with the default options.
mrpt::poses::CPosePDFPtr ICP_Method_LM(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
mrpt::poses::CPose3DPDFPtr ICP3D_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
float kernel(const float &x2, const float &rho2)
Computes:
mrpt::poses::CPosePDFPtr ICP_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
mrpt::poses::CPosePDFPtr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=NULL, void *info=NULL)
An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/...
CICP(const TConfigParams &icpParams)
Constructor that directly set the ICP params from a given struct.
virtual ~CICP()
Destructor.
TConfigParams options
The options employed by the ICP align.
mrpt::poses::CPose3DPDFPtr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, float *runningTime=NULL, void *info=NULL)
The virtual method for aligning a pair of metric maps, aligning the full 6D pose.
A base class for any algorithm able of maps alignment.
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
TICPCovarianceMethod
ICP covariance estimation methods, used in mrpt::slam::CICP::options <>
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options <>
@ icpCovLinealMSE
Use the covariance of the optimal registration, disregarding uncertainty in data association.
@ icpCovFiniteDifferences
Covariance of a least-squares optimizer (includes data association uncertainty)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The ICP algorithm return information.
float quality
A measure of the 'quality' of the local minimum of the sqr. error found by the method....
unsigned short nIterations
The number of executed iterations until convergence.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
static void fill(bimap< enum_t, std::string > &m_map)
slam::TICPAlgorithm enum_t
static void fill(bimap< enum_t, std::string > &m_map)
slam::TICPCovarianceMethod enum_t
Only specializations of this class are defined for each enum type of interest.