mrpt::pbmap::ConsistencyTest Class Reference

Detailed Description

This class computes the rigid transformation between two sets of matched planes, and provides a measure of their rigid adjustment

Definition at line 33 of file ConsistencyTest.h.

#include <mrpt/pbmap/ConsistencyTest.h>

Public Member Functions
	ConsistencyTest (PbMap &PBM_source, PbMap &PBM_target)
double	calcAlignmentError (std::map< unsigned, unsigned > &matched_planes, Eigen::Matrix4f &rigidTransf)
	! Get diamond of points around the center. More...
Eigen::Matrix4f	initPose (std::map< unsigned, unsigned > &matched_planes)
Eigen::Matrix4f	estimatePose (std::map< unsigned, unsigned > &matched_planes)
bool	estimatePoseWithCovariance (std::map< unsigned, unsigned > &matched_planes, Eigen::Matrix4f &rigidTransf, Eigen::Matrix< float, 6, 6 > &covarianceM)
Eigen::Matrix4f	initPose2D (std::map< unsigned, unsigned > &matched_planes)
Eigen::Matrix4f	getRTwithModel (std::map< unsigned, unsigned > &matched_planes)
Eigen::Matrix4f	estimatePoseRANSAC (std::map< unsigned, unsigned > &matched_planes)

Private Attributes
PbMap &	PBMSource
PbMap &	PBMTarget
std::map< unsigned, unsigned >	matched_planes

Constructor & Destructor Documentation

ConsistencyTest()

ConsistencyTest::ConsistencyTest	(	PbMap &	PBM_source,
		PbMap &	PBM_target
	)

Constructor

Definition at line 31 of file ConsistencyTest.cpp.

Member Function Documentation

calcAlignmentError()

double ConsistencyTest::calcAlignmentError	(	std::map< unsigned, unsigned > &	matched_planes,
		Eigen::Matrix4f &	rigidTransf
	)

! Get diamond of points around the center.

This is used to calculate the adjustment error with a model plane
Calculate the alignment error between two sets of matched planes. The input rigid transformation "se3rigidTransfInv" is used to project the centroids of one set of planes into their matched planes and returns the sum of cuadratic distances

Definition at line 36 of file ConsistencyTest.cpp.

References mrpt::pbmap::compose(), matched_planes, PBMSource, PBMTarget, and mrpt::pbmap::PbMap::vPlanes.

Referenced by getRTwithModel().

estimatePose()

Eigen::Matrix4f ConsistencyTest::estimatePose

(

std::map< unsigned, unsigned > &

matched_planes

)

Definition at line 153 of file ConsistencyTest.cpp.

References det(), matched_planes, PBMSource, PBMTarget, and mrpt::pbmap::PbMap::vPlanes.

estimatePoseRANSAC()

Eigen::Matrix4f ConsistencyTest::estimatePoseRANSAC

(

std::map< unsigned, unsigned > &

matched_planes

)

Definition at line 632 of file ConsistencyTest.cpp.

References mrpt::math::RANSAC_Template< NUMTYPE >::execute(), matched_planes, PBMSource, PBMTarget, ransac3Dplane_degenerate(), ransac3Dplane_distance(), ransacPlaneAlignment_fit(), and mrpt::pbmap::PbMap::vPlanes.

estimatePoseWithCovariance()

bool ConsistencyTest::estimatePoseWithCovariance	(	std::map< unsigned, unsigned > &	matched_planes,
		Eigen::Matrix4f &	rigidTransf,
		Eigen::Matrix< float, 6, 6 > &	covarianceM
	)

Definition at line 259 of file ConsistencyTest.cpp.

References det(), matched_planes, PBMSource, PBMTarget, and mrpt::pbmap::PbMap::vPlanes.

getRTwithModel()

Eigen::Matrix4f ConsistencyTest::getRTwithModel

(

std::map< unsigned, unsigned > &

matched_planes

)

Return the estimated rigid transformation which aligns two matched subgraphs (i.e. neighborhoods of planes). This function iteratively minimizes the alignment error of the matched planes wrt the rigid transformation.

Definition at line 386 of file ConsistencyTest.cpp.

References calcAlignmentError(), mrpt::pbmap::compose(), mrpt::poses::CPose3D::exp(), mrpt::poses::CPose3D::getHomogeneousMatrix(), initPose(), matched_planes, PBMSource, PBMTarget, and mrpt::pbmap::PbMap::vPlanes.

Referenced by mrpt::pbmap::PbMapLocaliser::searchPlaneContext().

initPose()

Eigen::Matrix4f ConsistencyTest::initPose

(

std::map< unsigned, unsigned > &

matched_planes

)

Return an initial guess for the rigid transformation which aligns two matched places. The translation is calculated from the planes centroids and the rotation from the alignment of the plane's normals.

Definition at line 52 of file ConsistencyTest.cpp.

References det(), matched_planes, PBMSource, PBMTarget, and mrpt::pbmap::PbMap::vPlanes.

Referenced by getRTwithModel().

initPose2D()

Eigen::Matrix4f ConsistencyTest::initPose2D

(

std::map< unsigned, unsigned > &

matched_planes

)

Return an initial guess for the rigid transformation which aligns two matched places. The translation is calculated from the planes centroids and the rotation from the alignment of the plane's normals. A planar movement is assumed (wheeled robot)

Definition at line 332 of file ConsistencyTest.cpp.

References matched_planes, PBMSource, PBMTarget, and mrpt::pbmap::PbMap::vPlanes.

Member Data Documentation

matched_planes

std::map<unsigned, unsigned> mrpt::pbmap::ConsistencyTest::matched_planes

private

List of pairs of matched planes from the PbMaps PBMSource with those from PBMTarget

Definition at line 77 of file ConsistencyTest.h.

Referenced by calcAlignmentError(), estimatePose(), estimatePoseRANSAC(), estimatePoseWithCovariance(), getRTwithModel(), initPose(), and initPose2D().

PBMSource

PbMap& mrpt::pbmap::ConsistencyTest::PBMSource

private

One of the subgraphs matched by SubgraphMatcher.

Definition at line 71 of file ConsistencyTest.h.

Referenced by calcAlignmentError(), estimatePose(), estimatePoseRANSAC(), estimatePoseWithCovariance(), getRTwithModel(), initPose(), and initPose2D().

PBMTarget

PbMap& mrpt::pbmap::ConsistencyTest::PBMTarget

private

The other subgraph matched by SubgraphMatcher.

Definition at line 74 of file ConsistencyTest.h.

Referenced by calcAlignmentError(), estimatePose(), estimatePoseRANSAC(), estimatePoseWithCovariance(), getRTwithModel(), initPose(), and initPose2D().