util3d_mapping.hpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef UTIL3D_MAPPING_HPP_
29 #define UTIL3D_MAPPING_HPP_
30 
31 #include <rtabmap/core/util3d_filtering.h>
32 #include <rtabmap/core/util3d.h>
33 #include <pcl/common/common.h>
34 #include <pcl/common/centroid.h>
35 #include <pcl/common/io.h>
36 
37 namespace rtabmap{
38 namespace util3d{
39 
40 template<typename PointT>
41 typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
42  const typename pcl::PointCloud<PointT> & cloud)
43 {
44  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
45  *output = cloud;
46  for(unsigned int i=0; i<output->size(); ++i)
47  {
48  output->at(i).z = 0;
49  }
50  return output;
51 }
52 
53 template<typename PointT>
54 void segmentObstaclesFromGround(
55  const typename pcl::PointCloud<PointT>::Ptr & cloud,
56  const typename pcl::IndicesPtr & indices,
57  pcl::IndicesPtr & ground,
58  pcl::IndicesPtr & obstacles,
59  int normalKSearch,
60  float groundNormalAngle,
61  float clusterRadius,
62  int minClusterSize,
63  bool segmentFlatObstacles,
64  float maxGroundHeight,
65  pcl::IndicesPtr * flatObstacles,
66  const Eigen::Vector4f & viewPoint)
67 {
68  ground.reset(new std::vector<int>);
69  obstacles.reset(new std::vector<int>);
70  if(flatObstacles)
71  {
72  flatObstacles->reset(new std::vector<int>);
73  }
74 
75  if(cloud->size())
76  {
77  // Find the ground
78  pcl::IndicesPtr flatSurfaces = normalFiltering(
79  cloud,
80  indices,
81  groundNormalAngle,
82  Eigen::Vector4f(0,0,1,0),
83  normalKSearch,
84  viewPoint);
85 
86  if(segmentFlatObstacles && flatSurfaces->size())
87  {
88  int biggestFlatSurfaceIndex;
89  std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
90  cloud,
91  flatSurfaces,
92  clusterRadius,
93  minClusterSize,
94  std::numeric_limits<int>::max(),
95  &biggestFlatSurfaceIndex);
96 
97  // cluster all surfaces for which the centroid is in the Z-range of the bigger surface
98  if(clusteredFlatSurfaces.size())
99  {
100  Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
101  if(maxGroundHeight != 0.0f)
102  {
103  // Search for biggest surface under max ground height
104  size_t points = 0;
105  biggestFlatSurfaceIndex = -1;
106  for(size_t i=0;i<clusteredFlatSurfaces.size();++i)
107  {
108  Eigen::Vector4f min,max;
109  pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
110  if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
111  {
112  points = clusteredFlatSurfaces.at(i)->size();
113  biggestFlatSurfaceIndex = i;
114  biggestSurfaceMin = min;
115  biggestSurfaceMax = max;
116  }
117  }
118  }
119  else
120  {
121  pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), biggestSurfaceMin, biggestSurfaceMax);
122  }
123  if(biggestFlatSurfaceIndex>=0)
124  {
125  ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
126  }
127 
128  if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
129  {
130  for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
131  {
132  if((int)i!=biggestFlatSurfaceIndex)
133  {
134  Eigen::Vector4f centroid(0,0,0,1);
135  pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
136  if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
137  {
138  ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
139  }
140  else if(flatObstacles)
141  {
142  *flatObstacles = util3d::concatenate(*flatObstacles, clusteredFlatSurfaces.at(i));
143  }
144  }
145  }
146  }
147  else
148  {
149  // reject ground!
150  ground.reset(new std::vector<int>);
151  if(flatObstacles)
152  {
153  *flatObstacles = flatSurfaces;
154  }
155  }
156  }
157  }
158  else
159  {
160  ground = flatSurfaces;
161  }
162 
163  if(ground->size() != cloud->size())
164  {
165  // Remove ground
166  pcl::IndicesPtr notObstacles = ground;
167  if(indices->size())
168  {
169  notObstacles = util3d::extractIndices(cloud, indices, true);
170  notObstacles = util3d::concatenate(notObstacles, ground);
171  }
172  pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
173 
174  // If ground height is set, remove obstacles under it
175  if(maxGroundHeight != 0.0f)
176  {
177  otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
178  }
179 
180  //Cluster remaining stuff (obstacles)
181  if(otherStuffIndices->size())
182  {
183  std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
184  cloud,
185  otherStuffIndices,
186  clusterRadius,
187  minClusterSize);
188 
189  // merge indices
190  obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
191  }
192  }
193  }
194 }
195 
196 template<typename PointT>
197 void segmentObstaclesFromGround(
198  const typename pcl::PointCloud<PointT>::Ptr & cloud,
199  pcl::IndicesPtr & ground,
200  pcl::IndicesPtr & obstacles,
201  int normalKSearch,
202  float groundNormalAngle,
203  float clusterRadius,
204  int minClusterSize,
205  bool segmentFlatObstacles,
206  float maxGroundHeight,
207  pcl::IndicesPtr * flatObstacles,
208  const Eigen::Vector4f & viewPoint)
209 {
210  pcl::IndicesPtr indices(new std::vector<int>);
211  segmentObstaclesFromGround<PointT>(
212  cloud,
213  indices,
214  ground,
215  obstacles,
216  normalKSearch,
217  groundNormalAngle,
218  clusterRadius,
219  minClusterSize,
220  segmentFlatObstacles,
221  maxGroundHeight,
222  flatObstacles,
223  viewPoint);
224 }
225 
226 template<typename PointT>
227 void occupancy2DFromGroundObstacles(
228  const typename pcl::PointCloud<PointT>::Ptr & cloud,
229  const pcl::IndicesPtr & groundIndices,
230  const pcl::IndicesPtr & obstaclesIndices,
231  cv::Mat & ground,
232  cv::Mat & obstacles,
233  float cellSize)
234 {
235  typename pcl::PointCloud<PointT>::Ptr groundCloud(new pcl::PointCloud<PointT>);
236  typename pcl::PointCloud<PointT>::Ptr obstaclesCloud(new pcl::PointCloud<PointT>);
237 
238  if(groundIndices->size())
239  {
240  pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
241  }
242 
243  if(obstaclesIndices->size())
244  {
245  pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
246  }
247 
248  occupancy2DFromGroundObstacles<PointT>(
249  groundCloud,
250  obstaclesCloud,
251  ground,
252  obstacles,
253  cellSize);
254 }
255 
256 template<typename PointT>
257 void occupancy2DFromGroundObstacles(
258  const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
259  const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
260  cv::Mat & ground,
261  cv::Mat & obstacles,
262  float cellSize)
263 {
264  ground = cv::Mat();
265  if(groundCloud->size())
266  {
267  //project on XY plane
268  typename pcl::PointCloud<PointT>::Ptr groundCloudProjected;
269  groundCloudProjected = util3d::projectCloudOnXYPlane(*groundCloud);
270  //voxelize to grid cell size
271  groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
272 
273  ground = cv::Mat(1, (int)groundCloudProjected->size(), CV_32FC2);
274  for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
275  {
276  cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
277  ptr[i][0] = groundCloudProjected->at(i).x;
278  ptr[i][1] = groundCloudProjected->at(i).y;
279  }
280  }
281 
282  obstacles = cv::Mat();
283  if(obstaclesCloud->size())
284  {
285  //project on XY plane
286  typename pcl::PointCloud<PointT>::Ptr obstaclesCloudProjected;
287  obstaclesCloudProjected = util3d::projectCloudOnXYPlane(*obstaclesCloud);
288  //voxelize to grid cell size
289  obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
290 
291  obstacles = cv::Mat(1, (int)obstaclesCloudProjected->size(), CV_32FC2);
292  for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
293  {
294  cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();
295  ptr[i][0] = obstaclesCloudProjected->at(i).x;
296  ptr[i][1] = obstaclesCloudProjected->at(i).y;
297  }
298  }
299 }
300 
301 template<typename PointT>
302 void occupancy2DFromCloud3D(
303  const typename pcl::PointCloud<PointT>::Ptr & cloud,
304  const pcl::IndicesPtr & indices,
305  cv::Mat & ground,
306  cv::Mat & obstacles,
307  float cellSize,
308  float groundNormalAngle,
309  int minClusterSize,
310  bool segmentFlatObstacles,
311  float maxGroundHeight)
312 {
313  if(cloud->size() == 0)
314  {
315  return;
316  }
317  pcl::IndicesPtr groundIndices, obstaclesIndices;
318 
319  segmentObstaclesFromGround<PointT>(
320  cloud,
321  indices,
322  groundIndices,
323  obstaclesIndices,
324  20,
325  groundNormalAngle,
326  cellSize*2.0f,
327  minClusterSize,
328  segmentFlatObstacles,
329  maxGroundHeight);
330 
331  occupancy2DFromGroundObstacles<PointT>(
332  cloud,
333  groundIndices,
334  obstaclesIndices,
335  ground,
336  obstacles,
337  cellSize);
338 }
339 
340 template<typename PointT>
341 void occupancy2DFromCloud3D(
342  const typename pcl::PointCloud<PointT>::Ptr & cloud,
343  cv::Mat & ground,
344  cv::Mat & obstacles,
345  float cellSize,
346  float groundNormalAngle,
347  int minClusterSize,
348  bool segmentFlatObstacles,
349  float maxGroundHeight)
350 {
351  pcl::IndicesPtr indices(new std::vector<int>);
352  occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize, segmentFlatObstacles, maxGroundHeight);
353 }
354 
355 }
356 }
357 
358 #endif /* UTIL3D_MAPPING_HPP_ */
rtabmap::util3d::extractClusters
std::vector< pcl::IndicesPtr > RTABMAP_EXP extractClusters(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)
Definition: util3d_filtering.cpp:1685
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::util3d::passThrough
pcl::IndicesPtr RTABMAP_EXP passThrough(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
Definition: util3d_filtering.cpp:619
rtabmap::util3d::getMinMax3D
void RTABMAP_EXP getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2666
f
f
rtabmap::util3d::extractIndices
pcl::IndicesPtr RTABMAP_EXP extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
Definition: util3d_filtering.cpp:1813
util3d_filtering.h
rtabmap::util3d::segmentObstaclesFromGround
void segmentObstaclesFromGround(const typename pcl::PointCloud< PointT >::Ptr &cloud, const typename pcl::IndicesPtr &indices, pcl::IndicesPtr &ground, pcl::IndicesPtr &obstacles, int normalKSearch, float groundNormalAngle, float clusterRadius, int minClusterSize, bool segmentFlatObstacles, float maxGroundHeight, pcl::IndicesPtr *flatObstacles, const Eigen::Vector4f &viewPoint)
Definition: util3d_mapping.hpp:54
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::util3d::occupancy2DFromCloud3D
void occupancy2DFromCloud3D(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, cv::Mat &ground, cv::Mat &obstacles, float cellSize, float groundNormalAngle, int minClusterSize, bool segmentFlatObstacles, float maxGroundHeight)
Definition: util3d_mapping.hpp:302
rtabmap::util3d::concatenate
pcl::IndicesPtr RTABMAP_EXP concatenate(const std::vector< pcl::IndicesPtr > &indices)
Concatenate a vector of indices to a single vector.
Definition: util3d.cpp:3133
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:519
rtabmap::util3d::projectCloudOnXYPlane
pcl::PointCloud< PointT >::Ptr projectCloudOnXYPlane(const typename pcl::PointCloud< PointT > &cloud)
Definition: util3d_mapping.hpp:41
rtabmap::util3d::occupancy2DFromGroundObstacles
void occupancy2DFromGroundObstacles(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &groundIndices, const pcl::IndicesPtr &obstaclesIndices, cv::Mat &ground, cv::Mat &obstacles, float cellSize)
Definition: util3d_mapping.hpp:227
rtabmap::util3d::normalFiltering
pcl::IndicesPtr RTABMAP_EXP normalFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
Definition: util3d_filtering.cpp:1518

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:37:06