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  float groundNormalsUp)
68 {
69  ground.reset(new std::vector<int>);
70  obstacles.reset(new std::vector<int>);
71  if(flatObstacles)
72  {
73  flatObstacles->reset(new std::vector<int>);
74  }
75 
76  if(cloud->size())
77  {
78  // Find the ground
79  pcl::IndicesPtr flatSurfaces = normalFiltering(
80  cloud,
81  indices,
82  groundNormalAngle,
83  Eigen::Vector4f(0,0,1,0),
84  normalKSearch,
85  viewPoint,
86  groundNormalsUp);
87 
88  if(segmentFlatObstacles && flatSurfaces->size())
89  {
90  int biggestFlatSurfaceIndex;
91  std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
92  cloud,
93  flatSurfaces,
94  clusterRadius,
95  minClusterSize,
96  std::numeric_limits<int>::max(),
97  &biggestFlatSurfaceIndex);
98 
99  // cluster all surfaces for which the centroid is in the Z-range of the bigger surface
100  if(clusteredFlatSurfaces.size())
101  {
102  Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
103  if(maxGroundHeight != 0.0f)
104  {
105  // Search for biggest surface under max ground height
106  size_t points = 0;
107  biggestFlatSurfaceIndex = -1;
108  for(size_t i=0;i<clusteredFlatSurfaces.size();++i)
109  {
110  Eigen::Vector4f min,max;
111  pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
112  if(min[2]<maxGroundHeight && clusteredFlatSurfaces.at(i)->size() > points)
113  {
114  points = clusteredFlatSurfaces.at(i)->size();
115  biggestFlatSurfaceIndex = i;
116  biggestSurfaceMin = min;
117  biggestSurfaceMax = max;
118  }
119  }
120  }
121  else
122  {
123  pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), biggestSurfaceMin, biggestSurfaceMax);
124  }
125  if(biggestFlatSurfaceIndex>=0)
126  {
127  ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
128  }
129 
130  if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
131  {
132  for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
133  {
134  if((int)i!=biggestFlatSurfaceIndex)
135  {
136  Eigen::Vector4f centroid(0,0,0,1);
137  pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
138  if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
139  {
140  ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
141  }
142  else if(flatObstacles)
143  {
144  *flatObstacles = util3d::concatenate(*flatObstacles, clusteredFlatSurfaces.at(i));
145  }
146  }
147  }
148  }
149  else
150  {
151  // reject ground!
152  ground.reset(new std::vector<int>);
153  if(flatObstacles)
154  {
155  *flatObstacles = flatSurfaces;
156  }
157  }
158  }
159  }
160  else
161  {
162  ground = flatSurfaces;
163  }
164 
165  if(ground->size() != cloud->size())
166  {
167  // Remove ground
168  pcl::IndicesPtr notObstacles = ground;
169  if(indices->size())
170  {
171  notObstacles = util3d::extractIndices(cloud, indices, true);
172  notObstacles = util3d::concatenate(notObstacles, ground);
173  }
174  pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
175 
176  // If ground height is set, remove obstacles under it
177  if(maxGroundHeight != 0.0f)
178  {
179  otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
180  }
181 
182  //Cluster remaining stuff (obstacles)
183  if(otherStuffIndices->size())
184  {
185  std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
186  cloud,
187  otherStuffIndices,
188  clusterRadius,
189  minClusterSize);
190 
191  // merge indices
192  obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
193  }
194  }
195  }
196 }
197 
198 template<typename PointT>
199 void segmentObstaclesFromGround(
200  const typename pcl::PointCloud<PointT>::Ptr & cloud,
201  pcl::IndicesPtr & ground,
202  pcl::IndicesPtr & obstacles,
203  int normalKSearch,
204  float groundNormalAngle,
205  float clusterRadius,
206  int minClusterSize,
207  bool segmentFlatObstacles,
208  float maxGroundHeight,
209  pcl::IndicesPtr * flatObstacles,
210  const Eigen::Vector4f & viewPoint,
211  float groundNormalsUp)
212 {
213  pcl::IndicesPtr indices(new std::vector<int>);
214  segmentObstaclesFromGround<PointT>(
215  cloud,
216  indices,
217  ground,
218  obstacles,
219  normalKSearch,
220  groundNormalAngle,
221  clusterRadius,
222  minClusterSize,
223  segmentFlatObstacles,
224  maxGroundHeight,
225  flatObstacles,
226  viewPoint,
227  groundNormalsUp);
228 }
229 
230 template<typename PointT>
231 void occupancy2DFromGroundObstacles(
232  const typename pcl::PointCloud<PointT>::Ptr & cloud,
233  const pcl::IndicesPtr & groundIndices,
234  const pcl::IndicesPtr & obstaclesIndices,
235  cv::Mat & ground,
236  cv::Mat & obstacles,
237  float cellSize)
238 {
239  typename pcl::PointCloud<PointT>::Ptr groundCloud(new pcl::PointCloud<PointT>);
240  typename pcl::PointCloud<PointT>::Ptr obstaclesCloud(new pcl::PointCloud<PointT>);
241 
242  if(groundIndices->size())
243  {
244  pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
245  }
246 
247  if(obstaclesIndices->size())
248  {
249  pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
250  }
251 
252  occupancy2DFromGroundObstacles<PointT>(
253  groundCloud,
254  obstaclesCloud,
255  ground,
256  obstacles,
257  cellSize);
258 }
259 
260 template<typename PointT>
261 void occupancy2DFromGroundObstacles(
262  const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
263  const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
264  cv::Mat & ground,
265  cv::Mat & obstacles,
266  float cellSize)
267 {
268  ground = cv::Mat();
269  if(groundCloud->size())
270  {
271  //project on XY plane
272  typename pcl::PointCloud<PointT>::Ptr groundCloudProjected;
273  groundCloudProjected = util3d::projectCloudOnXYPlane(*groundCloud);
274  //voxelize to grid cell size
275  groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
276 
277  ground = cv::Mat(1, (int)groundCloudProjected->size(), CV_32FC2);
278  for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
279  {
280  cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
281  ptr[i][0] = groundCloudProjected->at(i).x;
282  ptr[i][1] = groundCloudProjected->at(i).y;
283  }
284  }
285 
286  obstacles = cv::Mat();
287  if(obstaclesCloud->size())
288  {
289  //project on XY plane
290  typename pcl::PointCloud<PointT>::Ptr obstaclesCloudProjected;
291  obstaclesCloudProjected = util3d::projectCloudOnXYPlane(*obstaclesCloud);
292  //voxelize to grid cell size
293  obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
294 
295  obstacles = cv::Mat(1, (int)obstaclesCloudProjected->size(), CV_32FC2);
296  for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
297  {
298  cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();
299  ptr[i][0] = obstaclesCloudProjected->at(i).x;
300  ptr[i][1] = obstaclesCloudProjected->at(i).y;
301  }
302  }
303 }
304 
305 template<typename PointT>
306 void occupancy2DFromCloud3D(
307  const typename pcl::PointCloud<PointT>::Ptr & cloud,
308  const pcl::IndicesPtr & indices,
309  cv::Mat & ground,
310  cv::Mat & obstacles,
311  float cellSize,
312  float groundNormalAngle,
313  int minClusterSize,
314  bool segmentFlatObstacles,
315  float maxGroundHeight)
316 {
317  if(cloud->size() == 0)
318  {
319  return;
320  }
321  pcl::IndicesPtr groundIndices, obstaclesIndices;
322 
323  segmentObstaclesFromGround<PointT>(
324  cloud,
325  indices,
326  groundIndices,
327  obstaclesIndices,
328  20,
329  groundNormalAngle,
330  cellSize*2.0f,
331  minClusterSize,
332  segmentFlatObstacles,
333  maxGroundHeight);
334 
335  occupancy2DFromGroundObstacles<PointT>(
336  cloud,
337  groundIndices,
338  obstaclesIndices,
339  ground,
340  obstacles,
341  cellSize);
342 }
343 
344 template<typename PointT>
345 void occupancy2DFromCloud3D(
346  const typename pcl::PointCloud<PointT>::Ptr & cloud,
347  cv::Mat & ground,
348  cv::Mat & obstacles,
349  float cellSize,
350  float groundNormalAngle,
351  int minClusterSize,
352  bool segmentFlatObstacles,
353  float maxGroundHeight)
354 {
355  pcl::IndicesPtr indices(new std::vector<int>);
356  occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize, segmentFlatObstacles, maxGroundHeight);
357 }
358 
359 }
360 }
361 
362 #endif /* UTIL3D_MAPPING_HPP_ */
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::util3d::passThrough
pcl::IndicesPtr RTABMAP_CORE_EXPORT 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:730
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:613
rtabmap::util3d::normalFiltering
pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint, float groundNormalsUp=0.0f)
Definition: util3d_filtering.cpp:1938
rtabmap::util3d::concatenate
pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate(const std::vector< pcl::IndicesPtr > &indices)
Concatenate a vector of indices to a single vector.
Definition: util3d.cpp:3352
util3d.h
indices
indices
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:231
util3d_filtering.h
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::util3d::extractIndices
pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
Definition: util3d_filtering.cpp:2299
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
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:306
rtabmap::util3d::projectCloudOnXYPlane
pcl::PointCloud< PointT >::Ptr projectCloudOnXYPlane(const typename pcl::PointCloud< PointT > &cloud)
Definition: util3d_mapping.hpp:41
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, float groundNormalsUp)
Definition: util3d_mapping.hpp:54
rtabmap::util3d::getMinMax3D
void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2613
rtabmap::util3d::extractClusters
std::vector< pcl::IndicesPtr > RTABMAP_CORE_EXPORT 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:2151
rtabmap
Definition: CameraARCore.cpp:35
i
int i

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:23