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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019