namespace mp2p_icp

Overview

namespace mp2p_icp {

// namespaces

namespace mp2p_icp::internal;

// typedefs

typedef std::vector<mp2p_icp::Matcher::Ptr> matcher_list_t;
typedef std::vector<matched_plane_t> MatchedPlaneList;
typedef std::vector<matched_line_t> MatchedLineList;
typedef std::vector<point_plane_pair_t> MatchedPointPlaneList;
typedef std::vector<point_line_pair_t> MatchedPointLineList;
typedef std::function<double(double)> robust_sqrt_weight_func_t;
typedef std::string layer_name_t;

// enums

enum Coordinate;
enum IterTermReason;
enum RobustKernel;

// structs

struct CovarianceParameters;
struct MatchContext;
struct MatchState;
struct OptimalTF_GN_Parameters;
struct OptimalTF_Result;
struct OutlierIndices;
struct PairWeights;
struct Pairings;
struct Parameters;
struct PointCloudEigen;
struct Results;
struct SolverContext;
struct VisitCorrespondencesStats;
struct WeightParameters;
struct color_mode_t;
struct matched_line_t;
struct matched_plane_t;
struct pairings_render_params_t;
struct plane_patch_t;
struct point_line_pair_t;
struct point_plane_pair_t;
struct pointcloud_bitfield_t;
struct render_params_lines_t;
struct render_params_pairings_pt2ln_t;
struct render_params_pairings_pt2pl_t;
struct render_params_pairings_pt2pt_t;
struct render_params_planes_t;
struct render_params_point_layer_t;
struct render_params_points_t;
struct render_params_t;

// classes

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

// global functions

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

mrpt::math::CVectorFixedDouble<3> 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> 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> 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> 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> 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> icp_pipeline_from_yaml(
    const mrpt::containers::yaml& config,
    const mrpt::system::VerbosityLevel& vLevel = mrpt::system::LVL_INFO
    );

Pairings 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 optimal_tf_gauss_newton(const Pairings& in, OptimalTF_Result& result, const OptimalTF_GN_Parameters& gnParams = OptimalTF_GN_Parameters());
bool optimal_tf_horn(const mp2p_icp::Pairings& in, const WeightParameters& wp, OptimalTF_Result& result);
bool optimal_tf_olae(const Pairings& in, const WeightParameters& wp, OptimalTF_Result& result);

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

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

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

Pairings pt2ln_pl_to_pt2pt(const Pairings& in, const SolverContext& sc);

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

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

robust_sqrt_weight_func_t create_robust_kernel(const RobustKernel kernel, const double kernelParam);

template <class LAMBDA, class LAMBDA2>
void 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
    );

PointCloudEigen estimate_points_eigen(
    const float* xs,
    const float* ys,
    const float* zs,
    mrpt::optional_ref<const std::vector<size_t>> indices,
    std::optional<size_t> totalCount = std::nullopt
    );

void vector_of_points_to_xyz(
    const std::vector<mrpt::math::TPoint3Df>& pts,
    std::vector<float>& xs,
    std::vector<float>& ys,
    std::vector<float>& zs
    );

mrpt::maps::CSimplePointsMap::Ptr load_xyz_file(const std::string& fil);
const mrpt::maps::CPointsMap* MapToPointsMap(const mrpt::maps::CMetricMap& map);
mrpt::maps::CPointsMap* MapToPointsMap(mrpt::maps::CMetricMap& map);

const mrpt::maps::NearestNeighborsCapable* MapToNN(
    const mrpt::maps::CMetricMap& map,
    bool throwIfNotImplemented
    );

template <typename T>
void AttachToParameterSource(
    std::vector<std::shared_ptr<T>>& setObjects,
    ParameterSource& source
    );

void AttachToParameterSource(Parameterizable& o, ParameterSource& source);

} // namespace mp2p_icp

Global Functions

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

Classic Horn’s solution for optimal SE(3) transformation, modified to accept point-to-point, line-to-line, plane-to-plane pairings. If you need point-to-line or point-to-plane pairings, use the wrapper mp2p_icp::Solver_Horn.

On MRPT naming convention: “this”=global; “other”=local.

Returns:

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

Pairings pt2ln_pl_to_pt2pt(const Pairings& in, const SolverContext& sc)

Returns a copy of the input pairings, adding new pt2pt pairings for those pt2ln and pt2pl pairings.

robust_sqrt_weight_func_t create_robust_kernel(const RobustKernel kernel, const double kernelParam)

Creates a functor with the sqrt of the weight function of a given kernel, or an empty functor if non-robust kernel is selected.

Implemented as inline to try to make the compiler to optimize.

Parameters:

kernel	Selected kernel type.
kernelParam	Parameter of the kernel.

Returns:

A functor.

void vector_of_points_to_xyz(
    const std::vector<mrpt::math::TPoint3Df>& pts,
    std::vector<float>& xs,
    std::vector<float>& ys,
    std::vector<float>& zs
    )

Auxiliary function that can be used to convert a vector of TPoint3Df into the format expected by estimate_points_eigen()

template <typename T>
void AttachToParameterSource(
    std::vector<std::shared_ptr<T>>& setObjects,
    ParameterSource& source
    )

Attach a vector of objects to the given source

void AttachToParameterSource(Parameterizable& o, ParameterSource& source)

Attach a single object to the given source. Provided for syntax consistency between single and vectors of objects.