Main MRPT website
>
C++ reference for MRPT 1.9.9
mrpt
pbmap
ConsistencyTest.h
Go to the documentation of this file.
1
/* +------------------------------------------------------------------------+
2
| Mobile Robot Programming Toolkit (MRPT) |
3
| http://www.mrpt.org/ |
4
| |
5
| Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6
| See: http://www.mrpt.org/Authors - All rights reserved. |
7
| Released under BSD License. See details in http://www.mrpt.org/License |
8
+------------------------------------------------------------------------+ */
9
10
/* Plane-based Map (PbMap) library
11
* Construction of plane-based maps and localization in it from RGBD Images.
12
* Writen by Eduardo Fernandez-Moral. See docs for <a
13
* href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
14
*/
15
16
#ifndef __CONSISTENCYTEST_H
17
#define __CONSISTENCYTEST_H
18
19
#include <mrpt/config.h>
20
#if MRPT_HAS_PCL
21
22
#include <
mrpt/math/types_math.h
>
// Eigen
23
#include <
mrpt/pbmap/PbMap.h
>
24
25
namespace
mrpt
26
{
27
namespace
pbmap
28
{
29
/*! This class computes the rigid transformation between two sets of matched
30
* planes,
31
* and provides a measure of their rigid adjustment
32
*
33
* \ingroup mrpt_pbmap_grp
34
*/
35
class
ConsistencyTest
36
{
37
public
:
38
/*!Constructor */
39
ConsistencyTest
(
PbMap
& PBM_source,
PbMap
& PBM_target);
40
41
// /**! Get diamond of points around the center. This is used to calculate
42
// the adjustment error with a model plane */
43
// void calcDiamondPlane(Plane& plane);
44
45
/*!Calculate the alignment error between two sets of matched planes.
46
The input rigid transformation "se3rigidTransfInv" is used to project the
47
centroids of one set of planes into their
48
matched planes and returns the sum of cuadratic distances */
49
double
calcAlignmentError
(
50
std::map<unsigned, unsigned>&
matched_planes
,
51
Eigen::Matrix4f& rigidTransf);
52
53
/*!Return an initial guess for the rigid transformation which aligns two
54
matched places.
55
The translation is calculated from the planes centroids and the rotation
56
from the alignment of the plane's normals.*/
57
Eigen::Matrix4f
initPose
(std::map<unsigned, unsigned>&
matched_planes
);
58
Eigen::Matrix4f
estimatePose
(
59
std::map<unsigned, unsigned>&
60
matched_planes
);
// Weighted with the area
61
bool
estimatePoseWithCovariance
(
62
std::map<unsigned, unsigned>&
matched_planes
,
63
Eigen::Matrix4f& rigidTransf, Eigen::Matrix<float, 6, 6>& covarianceM);
64
65
/*!Return an initial guess for the rigid transformation which aligns two
66
matched places.
67
The translation is calculated from the planes centroids and the rotation
68
from the alignment of the plane's normals.
69
A planar movement is assumed (wheeled robot)*/
70
Eigen::Matrix4f
initPose2D
(std::map<unsigned, unsigned>&
matched_planes
);
71
72
/*!Return the estimated rigid transformation which aligns two matched
73
subgraphs (i.e. neighborhoods of planes).
74
This function iteratively minimizes the alignment error of the matched
75
planes wrt the rigid transformation.*/
76
Eigen::Matrix4f
getRTwithModel
(
77
std::map<unsigned, unsigned>&
matched_planes
);
78
79
// Eigen::Matrix4f getAlignment( const
80
// mrpt::math::CMatrixFixedNumeric<float,3,8> &matched_planes );
81
82
Eigen::Matrix4f
estimatePoseRANSAC
(
83
std::map<unsigned, unsigned>&
matched_planes
);
84
85
private
:
86
/*!One of the subgraphs matched by SubgraphMatcher.*/
87
PbMap
&
PBMSource
;
88
89
/*!The other subgraph matched by SubgraphMatcher.*/
90
PbMap
&
PBMTarget
;
91
92
/*!List of pairs of matched planes from the PbMaps PBMSource with those from
93
* PBMTarget*/
94
std::map<unsigned, unsigned>
matched_planes
;
95
96
//// Ransac functions to detect outliers in the plane matching
97
// void ransacPlaneAlignment_fit( const mrpt::math::CMatrixFloat
98
// &planeCorresp,
99
// const std::vector<size_t> &useIndices,
100
//// vector< Eigen::Matrix4f > &fitModels );
101
// vector< mrpt::math::CMatrixFloat44 >
102
// &fitModels );
103
//
104
// void ransac3Dplane_distance( const mrpt::math::CMatrixFloat
105
// &planeCorresp,
106
// const vector< Eigen::Matrix4f > &
107
// testModels,
108
// const double distanceThreshold,
109
// unsigned int & out_bestModelIndex,
110
// std::vector<size_t> & out_inlierIndices );
111
//
112
// bool ransac3Dplane_degenerate( const mrpt::math::CMatrixFloat
113
// &planeCorresp,
114
// const std::vector<size_t> &useIndices );
115
//
116
// void TestRANSAC();
117
};
118
}
119
}
// End of namespaces
120
121
#endif
122
#endif
mrpt::pbmap::ConsistencyTest::calcAlignmentError
double calcAlignmentError(std::map< unsigned, unsigned > &matched_planes, Eigen::Matrix4f &rigidTransf)
Definition:
ConsistencyTest.cpp:37
mrpt::pbmap::ConsistencyTest::initPose
Eigen::Matrix4f initPose(std::map< unsigned, unsigned > &matched_planes)
Definition:
ConsistencyTest.cpp:62
mrpt::pbmap::ConsistencyTest::matched_planes
std::map< unsigned, unsigned > matched_planes
Definition:
ConsistencyTest.h:94
mrpt::pbmap::ConsistencyTest::ConsistencyTest
ConsistencyTest(PbMap &PBM_source, PbMap &PBM_target)
Definition:
ConsistencyTest.cpp:32
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition:
CKalmanFilterCapable.h:30
mrpt::pbmap::ConsistencyTest::PBMTarget
PbMap & PBMTarget
Definition:
ConsistencyTest.h:90
mrpt::pbmap::ConsistencyTest::getRTwithModel
Eigen::Matrix4f getRTwithModel(std::map< unsigned, unsigned > &matched_planes)
Definition:
ConsistencyTest.cpp:483
mrpt::pbmap::ConsistencyTest::estimatePose
Eigen::Matrix4f estimatePose(std::map< unsigned, unsigned > &matched_planes)
Definition:
ConsistencyTest.cpp:190
mrpt::pbmap::ConsistencyTest::estimatePoseWithCovariance
bool estimatePoseWithCovariance(std::map< unsigned, unsigned > &matched_planes, Eigen::Matrix4f &rigidTransf, Eigen::Matrix< float, 6, 6 > &covarianceM)
Definition:
ConsistencyTest.cpp:330
mrpt::pbmap::PbMap
A class used to store a Plane-based Map (PbMap).
Definition:
pbmap/PbMap.h:47
mrpt::pbmap::ConsistencyTest::estimatePoseRANSAC
Eigen::Matrix4f estimatePoseRANSAC(std::map< unsigned, unsigned > &matched_planes)
Definition:
ConsistencyTest.cpp:763
mrpt::pbmap::ConsistencyTest::PBMSource
PbMap & PBMSource
Definition:
ConsistencyTest.h:87
PbMap.h
mrpt::pbmap::ConsistencyTest
Definition:
ConsistencyTest.h:35
mrpt::pbmap::ConsistencyTest::initPose2D
Eigen::Matrix4f initPose2D(std::map< unsigned, unsigned > &matched_planes)
Definition:
ConsistencyTest.cpp:419
types_math.h
Page generated by
Doxygen 1.8.17
for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST