Program Listing for File CICP.h
↰ Return to documentation for file (include/mrpt/slam/CICP.h)
/* _
| | Mobile Robot Programming Toolkit (MRPT)
_ __ ___ _ __ _ __ | |_
| '_ ` _ \| '__| '_ \| __| https://www.mrpt.org/
| | | | | | | | |_) | |_
|_| |_| |_|_| | .__/ \__| https://github.com/MRPT/mrpt/
| |
|_|
Copyright (c) 2005-2026, Individual contributors, see AUTHORS file
See: https://www.mrpt.org/Authors - All rights reserved.
SPDX-License-Identifier: BSD-3-Clause
*/
#pragma once
#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/slam/CMetricMapsAlignmentAlgorithm.h>
#include <mrpt/typemeta/TEnumType.h>
namespace mrpt::slam
{
enum TICPAlgorithm
{
icpClassic = 0,
icpLevenbergMarquardt
};
enum TICPCovarianceMethod : int
{
icpCovLinealMSE = 0,
icpCovFiniteDifferences
};
class CICP : public mrpt::slam::CMetricMapsAlignmentAlgorithm
{
public:
class TConfigParams : public mrpt::config::CLoadableOptions
{
public:
void loadFromConfigFile(
const mrpt::config::CConfigFileBase& source,
const std::string& section) override; // See base docs
void saveToConfigFile(mrpt::config::CConfigFileBase& c, const std::string& s) const override;
TICPAlgorithm ICP_algorithm{icpClassic};
TICPCovarianceMethod ICP_covariance_method{icpCovFiniteDifferences};
bool onlyUniqueRobust{false};
unsigned int maxIterations{40};
double minAbsStep_trans{1e-6};
double minAbsStep_rot{1e-6};
double thresholdDist{0.75}, thresholdAng{0.15 * M_PI / 180.0};
double ALFA{0.5};
double smallestThresholdDist{0.1};
double covariance_varPoints{0.02 * 0.02};
bool doRANSAC{false};
unsigned int ransac_minSetSize{3}, ransac_maxSetSize{20}, ransac_nSimulations{100};
double ransac_mahalanobisDistanceThreshold{3.0};
double normalizationStd{0.02};
bool ransac_fuseByCorrsMatch{true};
double ransac_fuseMaxDiffXY{0.01}, ransac_fuseMaxDiffPhi{0.1 * M_PI / 180.0};
double kernel_rho{0.07};
bool use_kernel{true};
double Axy_aprox_derivatives{0.05};
double LM_initial_lambda{1e-4};
bool skip_cov_calculation{false};
bool skip_quality_calculation{true};
uint32_t corresponding_points_decimation{5};
};
TConfigParams options;
CICP() : options() {}
CICP(const TConfigParams& icpParams) : options(icpParams) {}
~CICP() override = default;
struct TReturnInfo : public TMetricMapAlignmentResult
{
TReturnInfo() = default;
virtual ~TReturnInfo() override = default;
unsigned int nIterations = 0;
double goodness = 0;
double quality = 0;
};
mrpt::poses::CPosePDF::Ptr AlignPDF(
const mrpt::maps::CMetricMap* m1,
const mrpt::maps::CMetricMap* m2,
const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
mrpt::optional_ref<TMetricMapAlignmentResult> outInfo = std::nullopt) override;
// See base class for docs
mrpt::poses::CPose3DPDF::Ptr Align3DPDF(
const mrpt::maps::CMetricMap* m1,
const mrpt::maps::CMetricMap* m2,
const mrpt::poses::CPose3DPDFGaussian& initialEstimationPDF,
mrpt::optional_ref<TMetricMapAlignmentResult> outInfo = std::nullopt) override;
protected:
float kernel(float x2, float rho2);
mrpt::poses::CPosePDF::Ptr ICP_Method_Classic(
const mrpt::maps::CMetricMap* m1,
const mrpt::maps::CMetricMap* m2,
const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
TReturnInfo& outInfo);
mrpt::poses::CPosePDF::Ptr ICP_Method_LM(
const mrpt::maps::CMetricMap* m1,
const mrpt::maps::CMetricMap* m2,
const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
TReturnInfo& outInfo);
mrpt::poses::CPose3DPDF::Ptr ICP3D_Method_Classic(
const mrpt::maps::CMetricMap* m1,
const mrpt::maps::CMetricMap* m2,
const mrpt::poses::CPose3DPDFGaussian& initialEstimationPDF,
TReturnInfo& outInfo);
};
} // namespace mrpt::slam
MRPT_ENUM_TYPE_BEGIN(mrpt::slam::TICPAlgorithm)
using namespace mrpt::slam;
MRPT_FILL_ENUM(icpClassic);
MRPT_FILL_ENUM(icpLevenbergMarquardt);
MRPT_ENUM_TYPE_END()
MRPT_ENUM_TYPE_BEGIN(mrpt::slam::TICPCovarianceMethod)
using namespace mrpt::slam;
MRPT_FILL_ENUM(icpCovLinealMSE);
MRPT_FILL_ENUM(icpCovFiniteDifferences);
MRPT_ENUM_TYPE_END()