mp2p_icp library

Main library for ICP components and pipelines. More...

Classes
class	mp2p_icp::ICP
class	mp2p_icp::ICP_LibPointmatcher
class	mp2p_icp::LogRecord
struct	mp2p_icp::MatchContext
struct	mp2p_icp::matched_line_t
struct	mp2p_icp::matched_plane_t
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
struct	mp2p_icp::OptimalTF_GN_Parameters
struct	mp2p_icp::OptimalTF_Result
struct	mp2p_icp::OutlierIndices
struct	mp2p_icp::Pairings
struct	mp2p_icp::PairWeights
struct	mp2p_icp::Parameters
struct	mp2p_icp::point_line_pair_t
class	mp2p_icp::QualityEvaluator
class	mp2p_icp::QualityEvaluator_PairedRatio
class	mp2p_icp::QualityEvaluator_RangeImageSimilarity
class	mp2p_icp::QualityEvaluator_Voxels
struct	mp2p_icp::Results
class	mp2p_icp::Solver
class	mp2p_icp::Solver_GaussNewton
class	mp2p_icp::Solver_Horn
class	mp2p_icp::Solver_OLAE
struct	mp2p_icp::SolverContext
struct	mp2p_icp::VisitCorrespondencesStats
struct	mp2p_icp::WeightParameters

Typedefs
using	mp2p_icp::MatchedLineList = std::vector< matched_line_t >
using	mp2p_icp::MatchedPlaneList = std::vector< matched_plane_t >
using	mp2p_icp::MatchedPointLineList = std::vector< point_line_pair_t >

Enumerations
enum	mp2p_icp::IterTermReason : uint8_t {   mp2p_icp::IterTermReason::Undefined = 0, mp2p_icp::IterTermReason::NoPairings, mp2p_icp::IterTermReason::SolverError, mp2p_icp::IterTermReason::MaxIterations,   mp2p_icp::IterTermReason::Stalled, mp2p_icp::IterTermReason::QualityCheckpointFailed, mp2p_icp::IterTermReason::HookRequest }

Functions
mrpt::math::CMatrixDouble66	mp2p_icp::covariance (const Pairings &finalPairings, const mrpt::poses::CPose3D &finalAlignSolution, const CovarianceParameters &p)
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)
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< 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)
std::tuple< mrpt::math::TPoint3D, mrpt::math::TPoint3D >	mp2p_icp::eval_centroids_robust (const Pairings &in, const OutlierIndices &outliers)
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)
mrpt::serialization::CArchive &	mp2p_icp::operator<< (mrpt::serialization::CArchive &out, const Pairings &obj)
mrpt::serialization::CArchive &	mp2p_icp::operator<< (mrpt::serialization::CArchive &out, const Results &obj)
mrpt::serialization::CArchive &	mp2p_icp::operator>> (mrpt::serialization::CArchive &in, Pairings &obj)
mrpt::serialization::CArchive &	mp2p_icp::operator>> (mrpt::serialization::CArchive &in, Results &obj)
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)
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)
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)

Detailed Description

Main library for ICP components and pipelines.

Typedef Documentation

MatchedLineList

using mp2p_icp::MatchedLineList = typedef std::vector<matched_line_t>

Definition at line 50 of file Pairings.h.

MatchedPlaneList

using mp2p_icp::MatchedPlaneList = typedef std::vector<matched_plane_t>

Definition at line 41 of file Pairings.h.

MatchedPointLineList

using mp2p_icp::MatchedPointLineList = typedef std::vector<point_line_pair_t>

Definition at line 69 of file Pairings.h.

Enumeration Type Documentation

IterTermReason

enum mp2p_icp::IterTermReason : uint8_t

strong

Reason of iterating termination.

Enumerator
Undefined
NoPairings
SolverError
MaxIterations
Stalled
QualityCheckpointFailed
HookRequest

Definition at line 18 of file IterTermReason.h.

Function Documentation

covariance()

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

Covariance estimation methods for an ICP result.

Definition at line 22 of file covariance.cpp.

error_line2line()

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
	)

Definition at line 167 of file errorTerms.cpp.

error_plane2plane()

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
	)

Definition at line 337 of file errorTerms.cpp.

error_point2line()

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
	)

Definition at line 61 of file errorTerms.cpp.

error_point2plane()

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
	)

Definition at line 109 of file errorTerms.cpp.

error_point2point()

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
	)

Definition at line 28 of file errorTerms.cpp.

eval_centroids_robust()

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

Definition at line 63 of file Pairings.cpp.

icp_pipeline_from_yaml()

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

Definition at line 18 of file icp_pipeline_from_yaml.cpp.

operator<<() [1/2]

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

Definition at line 48 of file Pairings.cpp.

operator<<() [2/2]

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

Definition at line 36 of file Results.cpp.

operator>>() [1/2]

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

Definition at line 55 of file Pairings.cpp.

operator>>() [2/2]

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

Definition at line 43 of file Results.cpp.

optimal_tf_gauss_newton()

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.

Definition at line 28 of file optimal_tf_gauss_newton.cpp.

optimal_tf_olae()

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.

Definition at line 233 of file optimal_tf_olae.cpp.

run_matchers()

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.

Definition at line 39 of file mp2p_icp/src/Matcher.cpp.

visit_correspondences()

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

Definition at line 31 of file visit_correspondences.h.