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 32 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 33 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 38 of file ConsistencyTest.cpp.

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

Referenced by getRTwithModel().

Here is the call graph for this function:

Here is the caller graph for this function:

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 188 of file ConsistencyTest.cpp.

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

estimatePoseRANSAC()

Eigen::Matrix4f ConsistencyTest::estimatePoseRANSAC

(

std::map< unsigned, unsigned > &

matched_planes

)

Definition at line 750 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.

Here is the call graph for this function:

estimatePoseWithCovariance()

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

Definition at line 326 of file ConsistencyTest.cpp.

References 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 475 of file ConsistencyTest.cpp.

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

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 matched_planes, PBMSource, PBMTarget, and mrpt::pbmap::PbMap::vPlanes.

Referenced by getRTwithModel().

Here is the caller graph for this function:

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 413 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 90 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 83 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 86 of file ConsistencyTest.h.

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