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()