Main MRPT website > C++ reference for MRPT 1.9.9
PlaneInferredInfo.cpp
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 #include "pbmap-precomp.h" // Precompiled headers
17 
18 #include <mrpt/pbmap/PlaneInferredInfo.h>
19 
20 #if MRPT_HAS_PCL
21 
22 using namespace std;
23 using namespace mrpt::pbmap;
24 
25 /**!
26  * Check if the input plane correpond to the floor. For that we assume that:
27  * a) the vertical axis of the sensor's reference frame and the gravity axis are
28  * simmilarly oriented,
29  * b) the sensor is placed between 0.5 and 2 m above the floor plane
30  * c) the input plane is big enough > 5m2.
31  * d) ***we are not checking this now*** Most of the map planes are above the
32  * floor plane
33 */
34 
35 bool PlaneInferredInfo::searchTheFloor(
36  Eigen::Matrix4f& poseSensor, Plane& plane)
37 {
38  if (plane.areaVoxels < 2.0) return false;
39 
40  // The angle between the Y axis of the camera and the normal of the plane
41  // will normally be around: a) -1 for the floor b) 1 for the ceiling c) 0
42  // for the walls
43  double cosGravityDir = poseSensor.col(1).head(3).dot(plane.v3normal);
44  if (cosGravityDir < -0.94) // cos 20º = 0.9397
45  {
46  double sensorHeight =
47  (plane.v3normal.dot(poseSensor.col(3).head(3) - plane.v3center));
48 
49  if (sensorHeight < 0.7 || sensorHeight > 2.0) return false;
50 
51  mPbMap.FloorPlane = plane.id;
52  plane.label = mrpt::format("Floor%u", plane.id);
53  }
54  else if (cosGravityDir > 0.94)
55  plane.label = mrpt::format("Ceiling%u", plane.id);
56  else if (fabs(cosGravityDir) < 0.34)
57  plane.label = mrpt::format("Wall%u", plane.id);
58  else
59  return false;
60 
61  // For bounding planes (e.g. floor) -> Check that the rest of planes in the
62  // map are above this one
63  for (unsigned i = 0; i < mPbMap.vPlanes.size(); i++)
64  {
65  if (plane.id == mPbMap.vPlanes[i].id) continue;
66  if (plane.v3normal.dot(mPbMap.vPlanes[i].v3center - plane.v3center) <
67  -0.1)
68  {
69  plane.label = "";
70  return false;
71  }
72  }
73 
74  plane.bFromStructure = true;
75 
76  return true;
77 }
78 
79 /**!
80  * Check if the input plane has points too close to the image limits. Assume
81  * squared cells downsampling
82  * Check that the convex hull vertex are 'dist_threshold' away from the image
83  * limits.
84 */
85 bool PlaneInferredInfo::isPlaneCutbyImage(
86  vector<int>& planeIndices, unsigned& widthSampledImage,
87  unsigned& heightSampledImage, unsigned threshold)
88 {
89  unsigned upperLimWidth = widthSampledImage - threshold;
90  unsigned upperLimHeight = heightSampledImage - threshold;
91  unsigned u, v;
92  // cout << "threshold " << threshold << " upperLimWidth " << upperLimWidth
93  // << " upperLimHeight " << upperLimHeight << endl;
94  for (unsigned i = 0; i < planeIndices.size(); i++)
95  {
96  u = planeIndices[i] % widthSampledImage;
97  v = planeIndices[i] / widthSampledImage;
98  // cout << "idx " << planeIndices[i] << " u " << u << " v " << v <<
99  // endl;
100  if (u < threshold || u > upperLimWidth || v < threshold ||
101  v > upperLimHeight)
102  { // cout << "\nRETURN TRUE " << u << " " << v << endl;
103  return true;
104  }
105  }
106  // cout << "\n\n\nRETURN FALSE\n";
107  return false;
108 }
109 
110 /**!
111  * Check if the surrounding points of the input plane in the input frame are
112  * behind the plane
113 */
114 bool PlaneInferredInfo::isSurroundingBackground(
115  Plane& plane, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& frame,
116  vector<int>& planeIndices, unsigned threshold)
117 {
118  // cout << "isSurroundingBackground\n";
119  // Find in the frame the 2D position of the convex hull vertex by brute
120  // force // Innefficient
121  vector<pair<unsigned, unsigned>> polyImg;
122  for (unsigned i = 0; i < plane.polygonContourPtr->size(); i++)
123  for (unsigned j = 0; j < planeIndices.size(); j++)
124  if (frame->points[planeIndices[j]].x ==
125  plane.polygonContourPtr->points[i].x &&
126  frame->points[planeIndices[j]].y ==
127  plane.polygonContourPtr->points[i].y &&
128  frame->points[planeIndices[j]].z ==
129  plane.polygonContourPtr->points[i].z)
130  {
131  polyImg.push_back(
132  pair<unsigned, unsigned>(
133  planeIndices[j] % frame->width,
134  planeIndices[j] / frame->width));
135  break;
136  }
137 
138  // cout << "polyImg size " << polyImg.size() << endl;
139 
140  // Calc center
141  unsigned left = polyImg[0].first, right = polyImg[0].first,
142  up = polyImg[0].second, down = polyImg[0].second;
143  for (unsigned i = 1; i < polyImg.size(); i++)
144  {
145  if (polyImg[i].first < left)
146  left = polyImg[i].first;
147  else if (polyImg[i].first > right)
148  right = polyImg[i].first;
149  if (polyImg[i].second < down)
150  down = polyImg[i].second;
151  else if (polyImg[i].second > up)
152  up = polyImg[i].second;
153  }
154  pair<int, int> center((left + right) / 2, (up + down) / 2);
155 
156  // cout << "center " << center.first << " " << center.second << endl;
157 
158  vector<Eigen::Vector2i> outerPolygon;
159  unsigned Threshold = threshold * frame->width / 640;
160  for (unsigned i = 1; i < polyImg.size(); i++)
161  {
162  Eigen::Vector2f dir;
163  double normDir = sqrt(
164  double(
165  (polyImg[i].first - center.first) *
166  (polyImg[i].first - center.first) +
167  (polyImg[i].second - center.second) *
168  (polyImg[i].second - center.second)));
169  dir[0] = (polyImg[i].first - center.first) / normDir;
170  dir[1] = (polyImg[i].second - center.second) / normDir;
171  // cout << "polyImg[i] " << polyImg[i].first << " " <<
172  // polyImg[i].second << endl;
173  // cout << "dir " << dir << endl;
174  // cout << "normDir " << normDir << endl;
175  Eigen::Vector2i outVertex;
176  outVertex[0] = dir[0] * Threshold + polyImg[i].first;
177  outVertex[1] = dir[1] * Threshold + polyImg[i].second;
178  if (outVertex[0] < 0)
179  outVertex[0] = 0;
180  else if (outVertex[0] >= static_cast<int>(frame->width))
181  outVertex[0] = frame->width - 1;
182  if (outVertex[1] < 0)
183  outVertex[1] = 0;
184  else if (outVertex[1] >= static_cast<int>(frame->height))
185  outVertex[1] = frame->height - 1;
186  // cout << "outVertex " << outVertex << endl;
187  outerPolygon.push_back(outVertex);
188  }
189 
190  // cout << "Fill outer pc\n";
191  // Fill outerPolygonPtr point cloud
192  for (unsigned i = 1; i < outerPolygon.size(); i++)
193  {
194  double edgeLenght = std::sqrt(
195  static_cast<double>(
196  (outerPolygon[i][0] - outerPolygon[i - 1][0]) *
197  (outerPolygon[i][0] - outerPolygon[i - 1][0]) +
198  (outerPolygon[i][1] - outerPolygon[i - 1][1]) *
199  (outerPolygon[i][1] - outerPolygon[i - 1][1])));
200  Eigen::Vector2f direction;
201  direction[0] =
202  (outerPolygon[i][0] - outerPolygon[i - 1][0]) / edgeLenght;
203  direction[1] =
204  (outerPolygon[i][1] - outerPolygon[i - 1][1]) / edgeLenght;
205  // cout << "outerPolygon[i] " << outerPolygon[i] << " outerPolygon[i-1]
206  // " << outerPolygon[i-1] << endl;
207  // cout << "direction " << direction << endl;
208  // cout << "edgeLenght " << edgeLenght << endl;
209  // Eigen::Vector2f direction = normalize(outerPolygon[i] -
210  // outerPolygon[i-1]);
211  Eigen::Vector2i outContour;
212  for (unsigned j = 0; j < edgeLenght; j++)
213  {
214  outContour[0] = outerPolygon[i - 1][0] + int(direction[0] * j);
215  outContour[1] = outerPolygon[i - 1][1] + int(direction[1] * j);
216  // cout << "outContour " << outContour << endl;
217  pcl::PointXYZRGBA& outerPt =
218  frame->points[outContour[0] + outContour[1] * frame->width];
219  if (!pcl_isfinite(outerPt.x)) continue;
220 
221  // Fill pointCloud corresponding to the outerPolygon
222  plane.outerPolygonPtr->points.push_back(outerPt);
223  double dist2Plane = plane.v3normal.dot(
224  getVector3fromPointXYZ(outerPt) - plane.v3center);
225  if (dist2Plane > 0.1) return false;
226  }
227  }
228 
229  // cout << "outerPolygonPtr size " << plane.outerPolygonPtr->size() <<
230  // endl;
231 
232  return true;
233 }
234 
235 /**!
236  * Check if the area of input plane is stable and bounded (e.g < 1 m2) along the
237  * last keyframes that observe it
238 */
239 void PlaneInferredInfo::isFullExtent(Plane& plane, double newArea)
240 {
241  // cout << "Plane " << plane.id << " newArea " << newArea << " limit " <<
242  // 1.1*plane.areaVoxels<< endl;
243  if (plane.areaVoxels > 1.0 || newArea > 1.0)
244  {
245  plane.bFullExtent = false;
246  return;
247  }
248 
249  if (newArea < 1.1 * plane.areaVoxels)
250  plane.nFramesAreaIsStable++;
251  else
252  plane.nFramesAreaIsStable = 0;
253 
254  if (plane.nFramesAreaIsStable > 2)
255  {
256  plane.bFullExtent = true;
257  }
258 }
259 
260 #endif
mrpt::pbmap::Plane::bFromStructure
bool bFromStructure
Definition: Plane.h:153
mrpt::pbmap::Plane::areaVoxels
float areaVoxels
Definition: Plane.h:150
mrpt::pbmap::Plane::nFramesAreaIsStable
unsigned nFramesAreaIsStable
Definition: Plane.h:154
mrpt::pbmap::Plane::bFullExtent
bool bFullExtent
Definition: Plane.h:152
PlaneInferredInfo.h
pbmap-precomp.h
v
const GLdouble * v
Definition: glext.h:3678
mrpt::pbmap::Plane::v3center
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:141
mrpt::pbmap::Plane::outerPolygonPtr
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr outerPolygonPtr
Definition: Plane.h:184
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::pbmap::Plane
A class used to store a planar feature (Plane for short).
Definition: Plane.h:46
mrpt::pbmap::Plane::polygonContourPtr
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr polygonContourPtr
Definition: Plane.h:182
mrpt::pbmap
Definition: ConsistencyTest.h:27
mrpt::pbmap::Plane::v3normal
Eigen::Vector3f v3normal
Definition: Plane.h:142
mrpt::pbmap::Plane::id
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:127
mrpt::pbmap::getVector3fromPointXYZ
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Definition: Miscellaneous.h:40
mrpt::pbmap::Plane::label
std::string label
Definition: Plane.h:134
first
GLint * first
Definition: glext.h:3827


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