mp2p_icp library

Overview

Main library for ICP components and pipelines.

// typedefs

typedef std::vector<matched_plane_t> mp2p_icp::MatchedPlaneList;
typedef std::vector<matched_line_t> mp2p_icp::MatchedLineList;
typedef std::vector<point_plane_pair_t> mp2p_icp::MatchedPointPlaneList;
typedef std::vector<point_line_pair_t> mp2p_icp::MatchedPointLineList;

// enums

enum mp2p_icp::IterTermReason;

// structs

struct mp2p_icp::MatchContext;
struct mp2p_icp::OptimalTF_GN_Parameters;
struct mp2p_icp::OptimalTF_Result;
struct mp2p_icp::OutlierIndices;
struct mp2p_icp::PairWeights;
struct mp2p_icp::Pairings;
struct mp2p_icp::Parameters;
struct mp2p_icp::Results;
struct mp2p_icp::SolverContext;
struct mp2p_icp::VisitCorrespondencesStats;
struct mp2p_icp::WeightParameters;
struct mp2p_icp::matched_line_t;
struct mp2p_icp::matched_plane_t;
struct mp2p_icp::point_line_pair_t;
struct mp2p_icp::point_plane_pair_t;

// classes

class mp2p_icp::ICP;
class mp2p_icp::ICP_LibPointmatcher;
class mp2p_icp::LogRecord;
class mp2p_icp::Matcher;
class mp2p_icp::Matcher_Adaptive;
class mp2p_icp::Matcher_Point2Line;
class mp2p_icp::Matcher_Point2Plane;
class mp2p_icp::Matcher_Points_Base;
class mp2p_icp::Matcher_Points_DistanceThreshold;
class mp2p_icp::Matcher_Points_InlierRatio;
class mp2p_icp::QualityEvaluator;
class mp2p_icp::QualityEvaluator_PairedRatio;
class mp2p_icp::QualityEvaluator_RangeImageSimilarity;
class mp2p_icp::QualityEvaluator_Voxels;
class mp2p_icp::Solver;
class mp2p_icp::Solver_GaussNewton;
class mp2p_icp::Solver_Horn;
class mp2p_icp::Solver_OLAE;

// global functions

mrpt::math::CMatrixDouble66 mp2p_icp::covariance(
    const Pairings& finalPairings,
    const mrpt::poses::CPose3D& finalAlignSolution,
    const CovarianceParameters& p
    );

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2point(
    const mrpt::tfest::TMatchingPair& pairing,
    const mrpt::poses::CPose3D& relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian = std::nullopt
    );

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2line(
    const mp2p_icp::point_line_pair_t& pairing,
    const mrpt::poses::CPose3D& relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian = std::nullopt
    );

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2plane(
    const mp2p_icp::point_plane_pair_t& pairing,
    const mrpt::poses::CPose3D& relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian = std::nullopt
    );

mrpt::math::CVectorFixedDouble<4> mp2p_icp::error_line2line(
    const mp2p_icp::matched_line_t& pairing,
    const mrpt::poses::CPose3D& relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 12>> jacobian = std::nullopt
    );

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_plane2plane(
    const mp2p_icp::matched_plane_t& pairing,
    const mrpt::poses::CPose3D& relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian = std::nullopt
    );

std::tuple<mp2p_icp::ICP::Ptr, mp2p_icp::Parameters> mp2p_icp::icp_pipeline_from_yaml(
    const mrpt::containers::yaml& config,
    const mrpt::system::VerbosityLevel& vLevel = mrpt::system::LVL_INFO
    );

Pairings mp2p_icp::run_matchers(
    const matcher_list_t& matchers,
    const metric_map_t& pcGlobal,
    const metric_map_t& pcLocal,
    const mrpt::poses::CPose3D& local_wrt_global,
    const MatchContext& mc,
    const mrpt::optional_ref<MatchState>& userProvidedMS = std::nullopt
    );

bool mp2p_icp::optimal_tf_gauss_newton(const Pairings& in, OptimalTF_Result& result, const OptimalTF_GN_Parameters& gnParams = OptimalTF_GN_Parameters());
bool mp2p_icp::optimal_tf_olae(const Pairings& in, const WeightParameters& wp, OptimalTF_Result& result);

mrpt::serialization::CArchive& mp2p_icp::operator << (
    mrpt::serialization::CArchive& out,
    const Pairings& obj
    );

mrpt::serialization::CArchive& mp2p_icp::operator >> (
    mrpt::serialization::CArchive& in,
    Pairings& obj
    );

std::tuple<mrpt::math::TPoint3D, mrpt::math::TPoint3D> mp2p_icp::eval_centroids_robust(
    const Pairings& in,
    const OutlierIndices& outliers
    );

mrpt::serialization::CArchive& mp2p_icp::operator << (
    mrpt::serialization::CArchive& out,
    const Results& obj
    );

mrpt::serialization::CArchive& mp2p_icp::operator >> (
    mrpt::serialization::CArchive& in,
    Results& obj
    );

template <class LAMBDA, class LAMBDA2>
void mp2p_icp::visit_correspondences(
    const Pairings& in,
    const WeightParameters& wp,
    const mrpt::math::TPoint3D& ct_local,
    const mrpt::math::TPoint3D& ct_global,
    OutlierIndices& in_out_outliers,
    LAMBDA lambda_each_pair,
    LAMBDA2 lambda_final,
    bool normalize_relative_point_vectors,
    const mrpt::optional_ref<VisitCorrespondencesStats>& outStats = std::nullopt
    );

Global Functions

mrpt::math::CMatrixDouble66 mp2p_icp::covariance(
    const Pairings& finalPairings,
    const mrpt::poses::CPose3D& finalAlignSolution,
    const CovarianceParameters& p
    )

Covariance estimation methods for an ICP result.

std::tuple<mp2p_icp::ICP::Ptr, mp2p_icp::Parameters> mp2p_icp::icp_pipeline_from_yaml(
    const mrpt::containers::yaml& config,
    const mrpt::system::VerbosityLevel& vLevel = mrpt::system::LVL_INFO
    )

Sets up an ICP pipeline from a YAML configuration file.

The basic structure of the YAML configuration block is (see also example YAML files):

class_name: mp2p_icp::ICP

# See: mp2p_icp::Parameter
params:
  maxIterations: 100
  # ...

solvers:
  - class: mp2p_icp::Solver_GaussNewton
    params:
      maxIterations: 10

# Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher
# instances to pair geometric entities between pointclouds.
matchers:
  - class: mp2p_icp::Matcher_Points_DistanceThreshold
    params:
      threshold: 0.20
      maxLocalPointsPerLayer: 500

quality:
  - class: mp2p_icp::QualityEvaluator_PairedRatio
    params:
     thresholdDistance: 0.1

Pairings mp2p_icp::run_matchers(
    const matcher_list_t& matchers,
    const metric_map_t& pcGlobal,
    const metric_map_t& pcLocal,
    const mrpt::poses::CPose3D& local_wrt_global,
    const MatchContext& mc,
    const mrpt::optional_ref<MatchState>& userProvidedMS = std::nullopt
    )

Runs a sequence of matcher between two metric_map_t objects.

This is normally invoked by mp2p_icp::ICP, but users can use it as a standalone module as needed.

bool mp2p_icp::optimal_tf_gauss_newton(
    const Pairings& in,
    OptimalTF_Result& result,
    const OptimalTF_GN_Parameters& gnParams = OptimalTF_GN_Parameters()
    )

Gauss-Newton non-linear, iterative optimizer to find the SE(3) optimal transformation between a set of correspondences.

This method requires a linearization point in OptimalTF_GN_Parameters::linearizationPoint.

Returns:

false If the number of pairings is too small for a unique solution, true on success.

bool mp2p_icp::optimal_tf_olae(const Pairings& in, const WeightParameters& wp, OptimalTF_Result& result)

OLAE algorithm to find the SE(3) optimal transformation given a set of correspondences between points-points, lines-lines, planes-planes. Refer to technical report: Jose-Luis Blanco-Claraco. OLAE-ICP: Robust and fast alignment of geometric features with the optimal linear attitude estimator, Arxiv 2019.

Returns:

false If the number of pairings is too small for a unique solution, true on success.

std::tuple<mrpt::math::TPoint3D, mrpt::math::TPoint3D> mp2p_icp::eval_centroids_robust(
    const Pairings& in,
    const OutlierIndices& outliers
    )

Evaluates the centroids [ct_local, ct_global] for point-to-point correspondences only, taking into account the current guess for outliers

template <class LAMBDA, class LAMBDA2>
void mp2p_icp::visit_correspondences(
    const Pairings& in,
    const WeightParameters& wp,
    const mrpt::math::TPoint3D& ct_local,
    const mrpt::math::TPoint3D& ct_global,
    OutlierIndices& in_out_outliers,
    LAMBDA lambda_each_pair,
    LAMBDA2 lambda_final,
    bool normalize_relative_point_vectors,
    const mrpt::optional_ref<VisitCorrespondencesStats>& outStats = std::nullopt
    )

Visit each correspondence