MRPT  1.9.9
PlaneInferredInfo.cpp
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 #include "pbmap-precomp.h" // Precompiled headers
17 
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(pair<unsigned, unsigned>(
132  planeIndices[j] % frame->width,
133  planeIndices[j] / frame->width));
134  break;
135  }
136 
137  // cout << "polyImg size " << polyImg.size() << endl;
138 
139  // Calc center
140  unsigned left = polyImg[0].first, right = polyImg[0].first,
141  up = polyImg[0].second, down = polyImg[0].second;
142  for (unsigned i = 1; i < polyImg.size(); i++)
143  {
144  if (polyImg[i].first < left)
145  left = polyImg[i].first;
146  else if (polyImg[i].first > right)
147  right = polyImg[i].first;
148  if (polyImg[i].second < down)
149  down = polyImg[i].second;
150  else if (polyImg[i].second > up)
151  up = polyImg[i].second;
152  }
153  pair<int, int> center((left + right) / 2, (up + down) / 2);
154 
155  // cout << "center " << center.first << " " << center.second << endl;
156 
157  vector<Eigen::Vector2i> outerPolygon;
158  unsigned Threshold = threshold * frame->width / 640;
159  for (unsigned i = 1; i < polyImg.size(); i++)
160  {
161  Eigen::Vector2f dir;
162  double normDir = sqrt(double(
163  (polyImg[i].first - center.first) *
164  (polyImg[i].first - center.first) +
165  (polyImg[i].second - center.second) *
166  (polyImg[i].second - center.second)));
167  dir[0] = (polyImg[i].first - center.first) / normDir;
168  dir[1] = (polyImg[i].second - center.second) / normDir;
169  // cout << "polyImg[i] " << polyImg[i].first << " " <<
170  // polyImg[i].second << endl;
171  // cout << "dir " << dir << endl;
172  // cout << "normDir " << normDir << endl;
173  Eigen::Vector2i outVertex;
174  outVertex[0] = dir[0] * Threshold + polyImg[i].first;
175  outVertex[1] = dir[1] * Threshold + polyImg[i].second;
176  if (outVertex[0] < 0)
177  outVertex[0] = 0;
178  else if (outVertex[0] >= static_cast<int>(frame->width))
179  outVertex[0] = frame->width - 1;
180  if (outVertex[1] < 0)
181  outVertex[1] = 0;
182  else if (outVertex[1] >= static_cast<int>(frame->height))
183  outVertex[1] = frame->height - 1;
184  // cout << "outVertex " << outVertex << endl;
185  outerPolygon.push_back(outVertex);
186  }
187 
188  // cout << "Fill outer pc\n";
189  // Fill outerPolygonPtr point cloud
190  for (unsigned i = 1; i < outerPolygon.size(); i++)
191  {
192  double edgeLenght = std::sqrt(static_cast<double>(
193  (outerPolygon[i][0] - outerPolygon[i - 1][0]) *
194  (outerPolygon[i][0] - outerPolygon[i - 1][0]) +
195  (outerPolygon[i][1] - outerPolygon[i - 1][1]) *
196  (outerPolygon[i][1] - outerPolygon[i - 1][1])));
197  Eigen::Vector2f direction;
198  direction[0] =
199  (outerPolygon[i][0] - outerPolygon[i - 1][0]) / edgeLenght;
200  direction[1] =
201  (outerPolygon[i][1] - outerPolygon[i - 1][1]) / edgeLenght;
202  // cout << "outerPolygon[i] " << outerPolygon[i] << " outerPolygon[i-1]
203  // " << outerPolygon[i-1] << endl;
204  // cout << "direction " << direction << endl;
205  // cout << "edgeLenght " << edgeLenght << endl;
206  // Eigen::Vector2f direction = normalize(outerPolygon[i] -
207  // outerPolygon[i-1]);
208  Eigen::Vector2i outContour;
209  for (unsigned j = 0; j < edgeLenght; j++)
210  {
211  outContour[0] = outerPolygon[i - 1][0] + int(direction[0] * j);
212  outContour[1] = outerPolygon[i - 1][1] + int(direction[1] * j);
213  // cout << "outContour " << outContour << endl;
214  pcl::PointXYZRGBA& outerPt =
215  frame->points[outContour[0] + outContour[1] * frame->width];
216  if (!pcl_isfinite(outerPt.x)) continue;
217 
218  // Fill pointCloud corresponding to the outerPolygon
219  plane.outerPolygonPtr->points.push_back(outerPt);
220  double dist2Plane = plane.v3normal.dot(
221  getVector3fromPointXYZ(outerPt) - plane.v3center);
222  if (dist2Plane > 0.1) return false;
223  }
224  }
225 
226  // cout << "outerPolygonPtr size " << plane.outerPolygonPtr->size() <<
227  // endl;
228 
229  return true;
230 }
231 
232 /**!
233  * Check if the area of input plane is stable and bounded (e.g < 1 m2) along the
234  * last keyframes that observe it
235  */
236 void PlaneInferredInfo::isFullExtent(Plane& plane, double newArea)
237 {
238  // cout << "Plane " << plane.id << " newArea " << newArea << " limit " <<
239  // 1.1*plane.areaVoxels<< endl;
240  if (plane.areaVoxels > 1.0 || newArea > 1.0)
241  {
242  plane.bFullExtent = false;
243  return;
244  }
245 
246  if (newArea < 1.1 * plane.areaVoxels)
247  plane.nFramesAreaIsStable++;
248  else
249  plane.nFramesAreaIsStable = 0;
250 
251  if (plane.nFramesAreaIsStable > 2)
252  {
253  plane.bFullExtent = true;
254  }
255 }
256 
257 #endif
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr polygonContourPtr
Definition: Plane.h:179
A class used to store a planar feature (Plane for short).
Definition: Plane.h:43
GLint * first
Definition: glext.h:3833
STL namespace.
bool bFullExtent
Definition: Plane.h:149
float areaVoxels
Definition: Plane.h:147
Eigen::Vector3f v3normal
Definition: Plane.h:139
bool bFromStructure
Definition: Plane.h:150
std::string label
Definition: Plane.h:131
auto dir
const GLdouble * v
Definition: glext.h:3684
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr outerPolygonPtr
Definition: Plane.h:181
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:124
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:138
unsigned nFramesAreaIsStable
Definition: Plane.h:151
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Definition: Miscellaneous.h:38



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019