MRPT  1.9.9
ConsistencyTest.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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 #pragma once
17 
18 #include <mrpt/config.h>
19 #if MRPT_HAS_PCL
20 
21 //#include <mrpt/math/types_math.h> // Eigen
22 #include <mrpt/pbmap/PbMap.h>
23 
24 namespace mrpt::pbmap
25 {
26 /*! This class computes the rigid transformation between two sets of matched
27  * planes,
28  * and provides a measure of their rigid adjustment
29  *
30  * \ingroup mrpt_pbmap_grp
31  */
33 {
34  public:
35  /*!Constructor */
36  ConsistencyTest(PbMap& PBM_source, PbMap& PBM_target);
37 
38  // /**! Get diamond of points around the center. This is used to calculate
39  // the adjustment error with a model plane */
40  // void calcDiamondPlane(Plane& plane);
41 
42  /*!Calculate the alignment error between two sets of matched planes.
43  The input rigid transformation "se3rigidTransfInv" is used to project the
44  centroids of one set of planes into their
45  matched planes and returns the sum of cuadratic distances */
46  double calcAlignmentError(
47  std::map<unsigned, unsigned>& matched_planes,
48  Eigen::Matrix4f& rigidTransf);
49 
50  /*!Return an initial guess for the rigid transformation which aligns two
51  matched places.
52  The translation is calculated from the planes centroids and the rotation
53  from the alignment of the plane's normals.*/
54  Eigen::Matrix4f initPose(std::map<unsigned, unsigned>& matched_planes);
55  Eigen::Matrix4f estimatePose(std::map<unsigned, unsigned>&
56  matched_planes); // Weighted with the area
58  std::map<unsigned, unsigned>& matched_planes,
59  Eigen::Matrix4f& rigidTransf, Eigen::Matrix<float, 6, 6>& covarianceM);
60 
61  /*!Return an initial guess for the rigid transformation which aligns two
62  matched places.
63  The translation is calculated from the planes centroids and the rotation
64  from the alignment of the plane's normals.
65  A planar movement is assumed (wheeled robot)*/
66  Eigen::Matrix4f initPose2D(std::map<unsigned, unsigned>& matched_planes);
67 
68  /*!Return the estimated rigid transformation which aligns two matched
69  subgraphs (i.e. neighborhoods of planes).
70  This function iteratively minimizes the alignment error of the matched
71  planes wrt the rigid transformation.*/
72  Eigen::Matrix4f getRTwithModel(
73  std::map<unsigned, unsigned>& matched_planes);
74 
75  // Eigen::Matrix4f getAlignment( const
76  // mrpt::math::CMatrixFixed<float,3,8> &matched_planes );
77 
78  Eigen::Matrix4f estimatePoseRANSAC(
79  std::map<unsigned, unsigned>& matched_planes);
80 
81  private:
82  /*!One of the subgraphs matched by SubgraphMatcher.*/
84 
85  /*!The other subgraph matched by SubgraphMatcher.*/
87 
88  /*!List of pairs of matched planes from the PbMaps PBMSource with those from
89  * PBMTarget*/
90  std::map<unsigned, unsigned> matched_planes;
91 
92  //// Ransac functions to detect outliers in the plane matching
93  // void ransacPlaneAlignment_fit( const mrpt::math::CMatrixFloat
94  // &planeCorresp,
95  // const std::vector<size_t> &useIndices,
96  //// vector< Eigen::Matrix4f > &fitModels );
97  // vector< mrpt::math::CMatrixFloat44 >
98  // &fitModels );
99  //
100  // void ransac3Dplane_distance( const mrpt::math::CMatrixFloat
101  // &planeCorresp,
102  // const vector< Eigen::Matrix4f > &
103  // testModels,
104  // const double distanceThreshold,
105  // unsigned int & out_bestModelIndex,
106  // std::vector<size_t> & out_inlierIndices );
107  //
108  // bool ransac3Dplane_degenerate( const mrpt::math::CMatrixFloat
109  // &planeCorresp,
110  // const std::vector<size_t> &useIndices );
111  //
112  // void TestRANSAC();
113 };
114 } // namespace mrpt::pbmap
115 #endif
Eigen::Matrix4f estimatePose(std::map< unsigned, unsigned > &matched_planes)
Eigen::Matrix4f estimatePoseRANSAC(std::map< unsigned, unsigned > &matched_planes)
Eigen::Matrix4f initPose2D(std::map< unsigned, unsigned > &matched_planes)
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.
Eigen::Matrix4f initPose(std::map< unsigned, unsigned > &matched_planes)
std::map< unsigned, unsigned > matched_planes
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:44
bool estimatePoseWithCovariance(std::map< unsigned, unsigned > &matched_planes, Eigen::Matrix4f &rigidTransf, Eigen::Matrix< float, 6, 6 > &covarianceM)
Eigen::Matrix4f getRTwithModel(std::map< unsigned, unsigned > &matched_planes)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019