namespace mp2p_icp

Overview

namespace mp2p_icp {

// 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_line_pair_t> MatchedPointLineList;
typedef std::function<double(double)> robust_sqrt_weight_func_t;
typedef std::string layer_name_t;
typedef std::vector<point_plane_pair_t> MatchedPointPlaneList;
typedef std::vector<point_with_cov_pair_t> MatchedPointWithCovList;

// enums

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 point_with_cov_pair_t;
struct pointcloud_bitfield_t;
struct render_params_lines_t;
struct render_params_pairings_cov2cov_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 IcpPrepareCapable;
class LogRecord;
class Matcher;
class Matcher_Adaptive;
class Matcher_Cov2Cov;
class Matcher_Point2Line;
class Matcher_Point2Plane;
class Matcher_Points_Base;
class Matcher_Points_DistanceThreshold;
class Matcher_Points_InlierRatio;
class MetricMapMergeCapable;
class NearestPlaneCapable;
class NearestPointWithCovCapable;
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::math::TPoint3Df& local_point,
    const mrpt::math::TPoint3Df& global_point,
    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
    );

const mp2p_icp::NearestPlaneCapable* MapToNP(const mrpt::maps::CMetricMap& map, bool throwIfNotImplemented);

mrpt::serialization::CArchive& operator >> (
    mrpt::serialization::CArchive& in,
    std::optional<metric_map_t::Georeferencing>& g
    );

mrpt::serialization::CArchive& operator << (
    mrpt::serialization::CArchive& out,
    const std::optional<metric_map_t::Georeferencing>& g
    );

std::optional<metric_map_t::Georeferencing> FromYAML(const mrpt::containers::yaml& yaml_data);
mrpt::containers::yaml ToYAML(const std::optional<metric_map_t::Georeferencing>& geoRef);

bool warn_on_field_padding_mismatch(
    const mrpt::maps::CPointsMap& src,
    const mrpt::maps::CPointsMap& dst,
    const mrpt::system::COutputLogger& logger
    );

void rotateViewDirectionFields(mrpt::maps::CPointsMap& pts, const mrpt::poses::CPose3D& tf);
bool pointcloud_sanity_check(const mrpt::maps::CPointsMap& pc, bool printWarnings = true);

void update_velocity_buffer_from_obs(
    mola::imu::LocalVelocityBuffer& localVelocityBuffer,
    const mrpt::obs::CObservation::Ptr& obs
    );

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

bool warn_on_field_padding_mismatch(
    const mrpt::maps::CPointsMap& src,
    const mrpt::maps::CPointsMap& dst,
    const mrpt::system::COutputLogger& logger
    )

Checks whether copying points from src into dst via CPointsMap::prepareForInsertPointsFrom() + insertPointFrom() would result in any destination field being zero-padded (i.e. the field exists in dst but is absent in src).

This situation is benign on MRPT < 2.15.13 (those fields were simply skipped, which caused sanity-check crashes), and is handled correctly from MRPT 2.15.13 onwards by writing explicit zeros. However, silent zero-padding may still indicate a configuration problem, or sensor drops.

One WARN-level message is emitted per mismatched field name.

Parameters:

src	Source point cloud (the one being inserted FROM).
dst	Destination point cloud (the one being inserted INTO).
logger	Logger used for WARN messages.

Returns:

True if at least one mismatch was found.

void rotateViewDirectionFields(
    mrpt::maps::CPointsMap& pts,
    const mrpt::poses::CPose3D& tf
    )

Rotates the per-point view-direction unit vector fields (view_x, view_y, view_z) of pts in place by the rotation part of tf, leaving the point coordinates untouched.

This must be called whenever a point cloud carrying these fields is re-expressed in a different frame of reference (e.g. via CPointsMap::insertAnotherMap()), since that operation transforms the XYZ coordinates but copies all other registered fields verbatim, which would otherwise leave the view vectors expressed in the wrong frame.

If pts does not have all three fields, or tf is the identity transformation, this is a no-op.

Parameters:

pts	Point cloud to modify in place.
tf	Transformation whose rotation part is applied to the view vectors.

void update_velocity_buffer_from_obs(
    mola::imu::LocalVelocityBuffer& localVelocityBuffer,
    const mrpt::obs::CObservation::Ptr& obs
    )

Updates a LocalVelocityBuffer instance from a given observation.

This function inspects the provided MRPT observation and, if it contains a YAML-encoded comment with a "local_velocity_buffer" section, parses and loads that data into the given mola::LocalVelocityBuffer object.

This is a required step in sm2mm() and may be reused in other parts while parsing simple-maps and there is a need to run deskew filters.

• Observations of type mrpt::obs::CObservationComment may include auxiliary YAML data under the key "local_velocity_buffer".

• This function safely checks for that data, parses it, and updates the localVelocityBuffer contents accordingly.

• If the observation is not a comment, or does not contain valid YAML, or lacks the "local_velocity_buffer" entry, the function silently returns without modifying localVelocityBuffer.

Error handling:

• YAML parsing errors or malformed data structures are caught internally.

• In such cases, the function prints an error message to std::cerr and leaves localVelocityBuffer unmodified.

This functionality is only available if MP2P_ICP_HAS_MOLA_IMU_PREINTEGRATION is defined at compile time. Otherwise, the function performs no operation.

Parameters:

localVelocityBuffer	Reference to a mola::LocalVelocityBuffer instance that will be updated if valid velocity buffer data is found in the observation.
obs	Shared pointer to the MRPT observation to be inspected.