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 32 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 37 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

)

float trans_error = fabs(-PBMTarget.vPlanes[it->second].d + translation.dot(PBMTarget.vPlanes[it->second].v3normal) + PBMSource.vPlanes[it->first].d);

Definition at line 190 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 763 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 330 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 483 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.

float trans_error = fabs(-PBMTarget.vPlanes[it->second].d + translation.dot(PBMTarget.vPlanes[it->second].v3normal) + PBMSource.vPlanes[it->first].d);

Definition at line 62 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 419 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 92 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 85 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 88 of file ConsistencyTest.h.

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