MRPT  1.9.9
PbMapMaker.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 
20 #if MRPT_HAS_PCL
21 
22 #include <mrpt/pbmap/PbMap.h>
24 #include <mrpt/pbmap/Plane.h>
27 #include <pcl/visualization/cloud_viewer.h>
28 #include <pcl/visualization/pcl_visualizer.h>
29 #include <set>
30 
31 using PointT = pcl::PointXYZRGBA;
32 
33 namespace mrpt::pbmap
34 {
35 /*!frameRGBDandPose stores a dupla containing a pointCloud (built from a RGBD
36  * frame) and a pose.
37  * \ingroup mrpt_pbmap_grp
38  */
40 {
41  pcl::PointCloud<PointT>::Ptr cloudPtr;
42  Eigen::Matrix4f pose;
43 };
44 
45 /*! This class construct the PbMap extracting planar segments from Range images,
46  * which pose must be also provided.
47  * The range images and their poses are communicated with the object
48  * frameQueue.
49  * PbMapMaker run its own thread, which is created at initialization.
50  *
51  * \ingroup mrpt_pbmap_grp
52  */
54 {
55  public:
56  /*!PbMapMaker's constructor sets some threshold for plane segmentation and
57  map growing from a configuration file (or default).
58  This constructor also starts PbMapMaker's own thread.*/
59  PbMapMaker(const std::string& config_file);
60 
61  /*!PbMapMaker's destructor is used to save some debugging info to file.*/
62  ~PbMapMaker();
63 
64  /*!Get the PbMap.*/
65  PbMap getPbMap() { return mPbMap; }
66  /*!Serialize the PbMap.*/
67  void serializePbMap(std::string path);
68 
69  /*!frameQueue is a vector containing the frameRGBDandPose (range image +
70  * pose) to be processed.*/
71  std::vector<frameRGBDandPose> frameQueue;
72 
73  /*!observedPlanes is a list containing the current observed planes.*/
74  std::set<unsigned> sQueueObservedPlanes;
75 
76  /*!PCL viewer. It runs in a different thread.*/
77  pcl::visualization::CloudViewer cloudViewer;
78 
79  private:
80  /*!Find planar patches in the input organised point cloud
81  "pointCloudPtr_arg", and update the PbMap with them (it update previous
82  planes and
83  initialize new ones when they are first observed), the input pose "poseInv"
84  is used to place the current observations into a common frame of
85  reference. Different thresholds are used to control the plane segmentation:
86  - "distThreshold" defines the maximum distance of an inlier to the plane
87  - "angleThreshold" defines the maximum angle between an inlier's normal and
88  the plane's normal
89  - "minInliersF" defines the minimum number of inliers as a fraction of the
90  total number of points in the input cloud
91  */
92  void detectPlanesCloud(
93  pcl::PointCloud<PointT>::Ptr& pointCloudPtr_arg,
94  Eigen::Matrix4f& poseKF, double distThreshold, double angleThreshold,
95  double minInliersF);
96 
97  /*!Returns true when the closest distance between the patches "plane1" and
98  * "plane2" is under distThreshold.*/
99  bool arePlanesNearby(
100  Plane& plane1, Plane& plane2, const float distThreshold);
101 
102  /*!Check for new graph connections of the input plane. These connections are
103  stablished when the minimum distance between two patches is under
104  the input threshold "proximity"*/
105  void checkProximity(Plane& plane, float proximity);
106 
107  /*! Returns true if the two input planes represent the same physical surface
108  * for some given angle and distance thresholds.
109  * If the planes are the same they are merged in this and the function
110  * returns true. Otherwise it returns false.*/
111  bool areSamePlane(
112  Plane& plane1, Plane& plane2, const float& cosAngleThreshold,
113  const float& distThreshold, const float& proxThreshold);
114 
115  /*! Merge the two input patches into "updatePlane".
116  * Recalculate center, normal vector, area, inlier points (filtered),
117  * convex hull, etc.
118  */
119  void mergePlanes(Plane& updatePlane, Plane& discardPlane);
120 
121  /*!File containing some paramteres and heuristic thresholds"*/
123 
124  /*!Object to detect previous places.*/
126 
127  /*!Object to cluster set of planes according to their co-visibility.*/
129 
130  boost::mutex mtx_pbmap_busy;
131 
132  protected:
133  /*!The current PbMap.*/
135 
136  /*!List of planes observed in that last frame introduced.*/
137  std::set<unsigned> observedPlanes;
138 
139  /*!Object to infer some knowledge in the map planes.*/
141 
142  /*!PCL visualizer callback*/
143  void viz_cb(pcl::visualization::PCLVisualizer& viz);
144 
145  /*!This executes the PbMapMaker's thread*/
146  void run();
147 
148  /*!PbMapMaker's thread handle*/
149  std::thread pbmaker_hd;
150 
151  /*!PbMapMaker's exit thread*/
152  bool stop_pbMapMaker();
153 
154  /*!PbMapMaker's stop controller*/
156 
157  /*!PbMapMaker's stop var*/
159 
160  // Unary
169 };
170 } // namespace mrpt::pbmap
171 #endif
PlaneInferredInfo * mpPlaneInferInfo
Definition: PbMapMaker.h:140
std::set< unsigned > observedPlanes
Definition: PbMapMaker.h:137
A class used to store a planar feature (Plane for short).
Definition: Plane.h:43
PbMapLocaliser * mpPbMapLocaliser
Definition: PbMapMaker.h:125
bool areSamePlane(Plane &plane1, Plane &plane2, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold)
Definition: PbMapMaker.cpp:742
void checkProximity(Plane &plane, float proximity)
Definition: PbMapMaker.cpp:270
void serializePbMap(std::string path)
boost::mutex mtx_pbmap_busy
Definition: PbMapMaker.h:130
pcl::PointXYZRGBA PointT
Definition: PbMapMaker.h:31
void viz_cb(pcl::visualization::PCLVisualizer &viz)
Definition: PbMapMaker.cpp:818
A class used to infer some semantic meaning to the planes of a PbMap.
void mergePlanes(Plane &updatePlane, Plane &discardPlane)
Definition: PbMapMaker.cpp:768
SemanticClustering * clusterize
Definition: PbMapMaker.h:128
GLsizei const GLchar ** string
Definition: glext.h:4116
std::thread pbmaker_hd
Definition: PbMapMaker.h:149
PbMapMaker(const std::string &config_file)
Definition: PbMapMaker.cpp:179
bool arePlanesNearby(Plane &plane1, Plane &plane2, const float distThreshold)
Definition: PbMapMaker.cpp:200
std::set< unsigned > sQueueObservedPlanes
Definition: PbMapMaker.h:74
void detectPlanesCloud(pcl::PointCloud< PointT >::Ptr &pointCloudPtr_arg, Eigen::Matrix4f &poseKF, double distThreshold, double angleThreshold, double minInliersF)
Definition: PbMapMaker.cpp:290
pcl::PointCloud< PointT >::Ptr cloudPtr
Definition: PbMapMaker.h:41
std::vector< frameRGBDandPose > frameQueue
Definition: PbMapMaker.h:71
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:44
pcl::visualization::CloudViewer cloudViewer
Definition: PbMapMaker.h:77



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: dad381fd7 Sun Oct 20 13:36:46 2019 +0200 at dom oct 20 13:40:10 CEST 2019