MRPT  1.9.9
Plane.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 
23 #include <pcl/common/pca.h>
24 #include <pcl/point_types.h>
25 #include <map>
26 #include <set>
27 
28 #define USE_COMPLETNESS_HEURISTICS 1
29 #define USE_INFERRED_STRUCTURE 1
30 
31 static std::vector<size_t> DEFAULT_VECTOR;
32 
33 namespace mrpt::pbmap
34 {
35 /** A class used to store a planar feature (Plane for short).
36  * It is described with geometric features representing the shape and relative
37  * location of the patch (area, normal vector, elongation, 3D-convex hull,
38  * etc.)
39  * and radiometric features (the most representative color).
40  *
41  * \ingroup mrpt_pbmap_grp
42  */
44 {
46 
47  public:
49  : elongation(1.0),
50  bFullExtent(false),
51  bFromStructure(false),
52  // contourPtr(new pcl::PointCloud<pcl::PointXYZRGBA>),
53  polygonContourPtr(new pcl::PointCloud<pcl::PointXYZRGBA>),
54  planePointCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>)
55  {
56  // vector< vector<int> > vec(4, vector<int>(4));
57  }
58 
59  /*!
60  * Force the plane inliers to lay on the plane
61  */
62  void forcePtsLayOnPlane();
63 
64  /**!
65  * Calculate the plane's convex hull with the monotone chain algorithm.
66  */
67  // void calcConvexHull(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
68  // &pointCloud );
69  void calcConvexHull(
70  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloud,
71  std::vector<size_t>& indices = DEFAULT_VECTOR);
72 
74  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloud,
75  std::vector<size_t>& indices = DEFAULT_VECTOR);
76 
77  /** \brief Compute the area of a 2D planar polygon patch - using a given
78 normal
79 // * \param polygonContourPtr the point cloud (planar)
80 // * \param normal the plane normal
81  */
83  /*pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &polygonContourPtr, Vector<3> &normal*/);
84 
85  /** \brief Compute the patch's convex-hull area and mass center
86  */
88 
89  /*!
90  * Calculate plane's elongation and principal direction
91  */
93 
94  /*!Returns true when the closest distance between the patches "this" and
95  * "plane" is under distThreshold.*/
96  bool isPlaneNearby(Plane& plane, const float distThreshold);
97 
98  /*! Returns true if the two input planes represent the same physical surface
99  * for some given angle and distance thresholds.
100  * If the planes are the same they are merged in this and the function
101  * returns true. Otherwise it returns false.*/
102  bool isSamePlane(
103  Plane& plane, const float& cosAngleThreshold,
104  const float& distThreshold, const float& proxThreshold);
105 
106  bool isSamePlane(
107  Eigen::Matrix4f& Rt, Plane& plane_, const float& cosAngleThreshold,
108  const float& distThreshold, const float& proxThreshold);
109 
110  bool hasSimilarDominantColor(Plane& plane, const float colorThreshold);
111 
112  /*! Merge the two input patches into "updatePlane".
113  * Recalculate center, normal vector, area, inlier points (filtered),
114  * convex hull, etc.
115  */
116  void mergePlane(Plane& plane);
117  void mergePlane2(Plane& plane); // Adaptation for RGBD360
118 
119  void transform(Eigen::Matrix4f& Rt);
120 
121  /**!
122  * Parameters to allow the plane-based representation of the map by a graph
123  */
124  unsigned id;
125  unsigned numObservations;
126  unsigned semanticGroup;
127  std::set<unsigned> nearbyPlanes;
128  std::map<unsigned, unsigned> neighborPlanes;
129 
130  /*!Labels to store semantic attributes*/
134 
135  /**!
136  * Geometric description
137  */
138  Eigen::Vector3f v3center;
139  Eigen::Vector3f v3normal;
140  float d;
141  Eigen::Matrix4f information; // Fisher information matrix (the inverse of
142  // the plane covariance)
143  float curvature;
144  Eigen::Vector3f v3PpalDir;
145  float elongation; // This is the reatio between the lengths of the plane in
146  // the two principal directions
147  float areaVoxels;
148  float areaHull;
152 
153  /**!
154  * Radiometric description
155  */
156  Eigen::Vector3f v3colorNrgb;
159  Eigen::Vector3f v3colorNrgbDev;
160 
161  Eigen::Vector3f v3colorC1C2C3; // Color paper
162  std::vector<float> hist_H; // Normalized, Saturated Hue histogram
163  // (including 2 bins for black and white)
164 
165  std::vector<double> prog_area;
166  std::vector<double> prog_elongation; // This is the reatio between the
167  // lengths of the plane in the two
168  // principal directions
169  std::vector<Eigen::Vector3f> prog_C1C2C3;
170  std::vector<Eigen::Vector3f> prog_Nrgb;
171  std::vector<float> prog_intensity;
172  std::vector<std::vector<float>> prog_hist_H;
173 
174  /**!
175  * Convex Hull
176  */
177  // pcl::PointCloud<pcl::PointXYZRGBA>::Ptr contour::Ptr;
178  std::vector<int32_t> inliers;
179  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr polygonContourPtr;
180  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
181  outerPolygonPtr; // This is going to be deprecated
182  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
183  planePointCloudPtr; // This is going to be deprecated
184 
185  /*!
186  * Calculate plane's main color using "MeanShift" method
187  */
188  void calcMainColor();
189  void calcMainColor2();
190  void calcPlaneHistH();
191 
192  private:
193  /*!
194  * Calculate plane's main color in normalized rgb space
195  */
196  void getPlaneNrgb();
197  std::vector<float> r;
198  std::vector<float> g;
199  std::vector<float> b;
200  std::vector<float> intensity;
201 
202  // Color paper
203  /*!
204  * Calculate plane's main color in C1C2C3 representation
205  */
206  std::vector<float> c1;
207  std::vector<float> c2;
208  std::vector<float> c3;
209  void getPlaneC1C2C3();
210 
211  /*!
212  * Calculate plane's main color in HSV representation
213  */
214  // vector<float> H;
215  // vector<float> S;
216  // vector<vector<float> > HSV;
217 };
218 } // namespace mrpt::pbmap
219 #endif
std::vector< float > c3
Definition: Plane.h:208
std::vector< double > prog_area
Definition: Plane.h:165
std::string label_context
Definition: Plane.h:133
std::vector< float > c2
Definition: Plane.h:207
std::vector< Eigen::Vector3f > prog_Nrgb
Definition: Plane.h:170
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr polygonContourPtr
Definition: Plane.h:179
void transform(Eigen::Matrix4f &Rt)
Definition: Plane.cpp:1179
A class used to store a planar feature (Plane for short).
Definition: Plane.h:43
Eigen::Vector3f v3PpalDir
Definition: Plane.h:144
Eigen::Vector3f v3colorNrgbDev
Definition: Plane.h:159
float dominantIntensity
Definition: Plane.h:157
std::map< unsigned, unsigned > neighborPlanes
Definition: Plane.h:128
void mergePlane2(Plane &plane)
Definition: Plane.cpp:1135
float elongation
Definition: Plane.h:145
static std::vector< size_t > DEFAULT_VECTOR
Definition: Plane.h:31
GLuint GLuint GLsizei GLenum const GLvoid * indices
Definition: glext.h:3532
void mergePlane(Plane &plane)
Definition: Plane.cpp:1082
Eigen::Vector3f v3colorNrgb
! Radiometric description
Definition: Plane.h:156
void calcConvexHullandParams(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr &pointCloud, std::vector< size_t > &indices=DEFAULT_VECTOR)
Definition: Plane.cpp:829
std::vector< float > g
Definition: Plane.h:198
void calcElongationAndPpalDir()
Definition: Plane.cpp:331
std::vector< Eigen::Vector3f > prog_C1C2C3
Definition: Plane.h:169
std::vector< float > prog_intensity
Definition: Plane.h:171
bool bFullExtent
Definition: Plane.h:149
float areaVoxels
Definition: Plane.h:147
void calcPlaneHistH()
Definition: Plane.cpp:406
Eigen::Vector3f v3normal
Definition: Plane.h:139
bool bFromStructure
Definition: Plane.h:150
std::set< unsigned > nearbyPlanes
Definition: Plane.h:127
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr planePointCloudPtr
Definition: Plane.h:183
unsigned semanticGroup
Definition: Plane.h:126
std::vector< float > intensity
Definition: Plane.h:200
float areaHull
Definition: Plane.h:148
std::string label
Definition: Plane.h:131
void forcePtsLayOnPlane()
Definition: Plane.cpp:220
std::vector< double > prog_elongation
Definition: Plane.h:166
std::vector< float > c1
Definition: Plane.h:206
GLsizei const GLchar ** string
Definition: glext.h:4116
bool isSamePlane(Plane &plane, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold)
Definition: Plane.cpp:1011
bool isPlaneNearby(Plane &plane, const float distThreshold)
Definition: Plane.cpp:960
std::string label_object
Definition: Plane.h:132
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void getPlaneC1C2C3()
Definition: Plane.cpp:380
void calcMainColor()
Definition: Plane.cpp:603
float compute2DPolygonalArea()
Compute the area of a 2D planar polygon patch - using a given normal.
Definition: Plane.cpp:249
std::vector< std::vector< float > > prog_hist_H
Definition: Plane.h:172
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr outerPolygonPtr
Definition: Plane.h:181
std::vector< float > b
Definition: Plane.h:199
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:124
void computeMassCenterAndArea()
Compute the patch&#39;s convex-hull area and mass center.
Definition: Plane.cpp:285
std::vector< int32_t > inliers
! Convex Hull
Definition: Plane.h:178
std::vector< float > hist_H
Definition: Plane.h:162
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:138
std::vector< float > r
Definition: Plane.h:197
void calcMainColor2()
Definition: Plane.cpp:532
unsigned numObservations
Definition: Plane.h:125
void getPlaneNrgb()
Definition: Plane.cpp:349
Eigen::Matrix4f information
Definition: Plane.h:141
float curvature
Definition: Plane.h:143
bool bDominantColor
Definition: Plane.h:158
unsigned nFramesAreaIsStable
Definition: Plane.h:151
bool hasSimilarDominantColor(Plane &plane, const float colorThreshold)
Definition: Plane.cpp:1068
Eigen::Vector3f v3colorC1C2C3
Definition: Plane.h:161
void calcConvexHull(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr &pointCloud, std::vector< size_t > &indices=DEFAULT_VECTOR)
! Calculate the plane&#39;s convex hull with the monotone chain algorithm.
Definition: Plane.cpp:761



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 034c2ee2a Tue Aug 20 02:15:02 2019 +0200 at mar ago 20 02:20:10 CEST 2019