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  ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
101  Eigen::Vector4f min,max;
102  pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
103 
104  if(maxGroundHeight == 0.0f || min[2] < maxGroundHeight)
105  {
106  for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
107  {
108  if((int)i!=biggestFlatSurfaceIndex)
109  {
110  Eigen::Vector4f centroid(0,0,0,1);
111  pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
112  if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= max[2]) // epsilon
113  {
114  ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
115  }
116  else if(flatObstacles)
117  {
118  *flatObstacles = util3d::concatenate(*flatObstacles, clusteredFlatSurfaces.at(i));
119  }
120  }
121  }
122  }
123  else
124  {
125  // reject ground!
126  ground.reset(new std::vector<int>);
127  if(flatObstacles)
128  {
129  *flatObstacles = flatSurfaces;
130  }
131  }
132  }
133  }
134  else
135  {
136  ground = flatSurfaces;
137  }
138 
139  if(ground->size() != cloud->size())
140  {
141  // Remove ground
142  pcl::IndicesPtr notObstacles = ground;
143  if(indices->size())
144  {
145  notObstacles = util3d::extractIndices(cloud, indices, true);
146  notObstacles = util3d::concatenate(notObstacles, ground);
147  }
148  pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
149 
150  // If ground height is set, remove obstacles under it
151  if(maxGroundHeight != 0.0f)
152  {
153  otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
154  }
155 
156  //Cluster remaining stuff (obstacles)
157  if(otherStuffIndices->size())
158  {
159  std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
160  cloud,
161  otherStuffIndices,
162  clusterRadius,
163  minClusterSize);
164 
165  // merge indices
166  obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
167  }
168  }
169  }
170 }
171 
172 template<typename PointT>
173 void segmentObstaclesFromGround(
174  const typename pcl::PointCloud<PointT>::Ptr & cloud,
175  pcl::IndicesPtr & ground,
176  pcl::IndicesPtr & obstacles,
177  int normalKSearch,
178  float groundNormalAngle,
179  float clusterRadius,
180  int minClusterSize,
181  bool segmentFlatObstacles,
182  float maxGroundHeight,
183  pcl::IndicesPtr * flatObstacles,
184  const Eigen::Vector4f & viewPoint)
185 {
186  pcl::IndicesPtr indices(new std::vector<int>);
187  segmentObstaclesFromGround<PointT>(
188  cloud,
189  indices,
190  ground,
191  obstacles,
192  normalKSearch,
193  groundNormalAngle,
194  clusterRadius,
195  minClusterSize,
196  segmentFlatObstacles,
197  maxGroundHeight,
198  flatObstacles,
199  viewPoint);
200 }
201 
202 template<typename PointT>
203 void occupancy2DFromGroundObstacles(
204  const typename pcl::PointCloud<PointT>::Ptr & cloud,
205  const pcl::IndicesPtr & groundIndices,
206  const pcl::IndicesPtr & obstaclesIndices,
207  cv::Mat & ground,
208  cv::Mat & obstacles,
209  float cellSize)
210 {
211  typename pcl::PointCloud<PointT>::Ptr groundCloud(new pcl::PointCloud<PointT>);
212  typename pcl::PointCloud<PointT>::Ptr obstaclesCloud(new pcl::PointCloud<PointT>);
213 
214  if(groundIndices->size())
215  {
216  pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
217  }
218 
219  if(obstaclesIndices->size())
220  {
221  pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
222  }
223 
224  occupancy2DFromGroundObstacles<PointT>(
225  groundCloud,
226  obstaclesCloud,
227  ground,
228  obstacles,
229  cellSize);
230 }
231 
232 template<typename PointT>
233 void occupancy2DFromGroundObstacles(
234  const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
235  const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
236  cv::Mat & ground,
237  cv::Mat & obstacles,
238  float cellSize)
239 {
240  ground = cv::Mat();
241  if(groundCloud->size())
242  {
243  //project on XY plane
244  typename pcl::PointCloud<PointT>::Ptr groundCloudProjected;
245  groundCloudProjected = util3d::projectCloudOnXYPlane(*groundCloud);
246  //voxelize to grid cell size
247  groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
248 
249  ground = cv::Mat(1, (int)groundCloudProjected->size(), CV_32FC2);
250  for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
251  {
252  cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
253  ptr[i][0] = groundCloudProjected->at(i).x;
254  ptr[i][1] = groundCloudProjected->at(i).y;
255  }
256  }
257 
258  obstacles = cv::Mat();
259  if(obstaclesCloud->size())
260  {
261  //project on XY plane
262  typename pcl::PointCloud<PointT>::Ptr obstaclesCloudProjected;
263  obstaclesCloudProjected = util3d::projectCloudOnXYPlane(*obstaclesCloud);
264  //voxelize to grid cell size
265  obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
266 
267  obstacles = cv::Mat(1, (int)obstaclesCloudProjected->size(), CV_32FC2);
268  for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
269  {
270  cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();
271  ptr[i][0] = obstaclesCloudProjected->at(i).x;
272  ptr[i][1] = obstaclesCloudProjected->at(i).y;
273  }
274  }
275 }
276 
277 template<typename PointT>
278 void occupancy2DFromCloud3D(
279  const typename pcl::PointCloud<PointT>::Ptr & cloud,
280  const pcl::IndicesPtr & indices,
281  cv::Mat & ground,
282  cv::Mat & obstacles,
283  float cellSize,
284  float groundNormalAngle,
285  int minClusterSize,
286  bool segmentFlatObstacles,
287  float maxGroundHeight)
288 {
289  if(cloud->size() == 0)
290  {
291  return;
292  }
293  pcl::IndicesPtr groundIndices, obstaclesIndices;
294 
295  segmentObstaclesFromGround<PointT>(
296  cloud,
297  indices,
298  groundIndices,
299  obstaclesIndices,
300  20,
301  groundNormalAngle,
302  cellSize*2.0f,
303  minClusterSize,
304  segmentFlatObstacles,
305  maxGroundHeight);
306 
307  occupancy2DFromGroundObstacles<PointT>(
308  cloud,
309  groundIndices,
310  obstaclesIndices,
311  ground,
312  obstacles,
313  cellSize);
314 }
315 
316 template<typename PointT>
317 void occupancy2DFromCloud3D(
318  const typename pcl::PointCloud<PointT>::Ptr & cloud,
319  cv::Mat & ground,
320  cv::Mat & obstacles,
321  float cellSize,
322  float groundNormalAngle,
323  int minClusterSize,
324  bool segmentFlatObstacles,
325  float maxGroundHeight)
326 {
327  pcl::IndicesPtr indices(new std::vector<int>);
328  occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize, segmentFlatObstacles, maxGroundHeight);
329 }
330 
331 }
332 }
333 
334 #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:1518
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:507
rtabmap::util3d::getMinMax3D
void RTABMAP_EXP getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2593
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:1646
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:278
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:3060
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:407
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:203
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:1351

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:41