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