util3d_mapping.h
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_H_
29 #define UTIL3D_MAPPING_H_
30 
31 #include "rtabmap/core/rtabmap_core_export.h"
32 
33 #include <opencv2/core/core.hpp>
34 #include <map>
35 #include <rtabmap/core/Transform.h>
36 #include <pcl/pcl_base.h>
37 #include <pcl/point_cloud.h>
38 #include <pcl/point_types.h>
39 
40 namespace rtabmap
41 {
42 
43 namespace util3d
44 {
45 
46 // Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base.
47 RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan(
48  const cv::Mat & scan, // in /base_link frame
49  cv::Mat & empty,
50  cv::Mat & occupied,
51  float cellSize,
52  bool unknownSpaceFilled = false,
53  float scanMaxRange = 0.0f);
54 
55 // Use interface with scanHit/scanNoHit parameters: scanNoHit set to null matrix has the same functionality than this method.
56 RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan(
57  const cv::Mat & scan, // in /base_link frame
58  const cv::Point3f & viewpoint, // /base_link -> /base_scan
59  cv::Mat & empty,
60  cv::Mat & occupied,
61  float cellSize,
62  bool unknownSpaceFilled = false,
63  float scanMaxRange = 0.0f);
64 
65 void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan(
66  const cv::Mat & scanHit, // in /base_link frame
67  const cv::Mat & scanNoHit, // in /base_link frame
68  const cv::Point3f & viewpoint, // /base_link -> /base_scan
69  cv::Mat & empty,
70  cv::Mat & occupied,
71  float cellSize,
72  bool unknownSpaceFilled = false,
73  float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
74 
75 cv::Mat RTABMAP_CORE_EXPORT create2DMapFromOccupancyLocalMaps(
76  const std::map<int, Transform> & poses,
77  const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
78  float cellSize,
79  float & xMin,
80  float & yMin,
81  float minMapSize = 0.0f,
82  bool erode = false,
83  float footprintRadius = 0.0f);
84 
85 // Use interface with \"viewpoints\" parameter to make sure the ray tracing origin is from the sensor and not the base.
86 RTABMAP_DEPRECATED cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map<int, Transform> & poses,
87  const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans, // in /base_link frame
88  float cellSize,
89  bool unknownSpaceFilled,
90  float & xMin,
91  float & yMin,
92  float minMapSize = 0.0f,
93  float scanMaxRange = 0.0f);
94 
95 // Use interface with cv::Mat scans.
96 RTABMAP_DEPRECATED cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map<int, Transform> & poses,
97  const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans, // in /base_link frame
98  const std::map<int, cv::Point3f > & viewpoints, // /base_link -> /base_scan
99  float cellSize,
100  bool unknownSpaceFilled,
101  float & xMin,
102  float & yMin,
103  float minMapSize = 0.0f,
104  float scanMaxRange = 0.0f);
105 
121 cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map<int, Transform> & poses,
122  const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >, in /base_link frame
123  const std::map<int, cv::Point3f > & viewpoints, // /base_link -> /base_scan
124  float cellSize,
125  bool unknownSpaceFilled,
126  float & xMin,
127  float & yMin,
128  float minMapSize = 0.0f,
129  float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
130 
131 void RTABMAP_CORE_EXPORT rayTrace(const cv::Point2i & start,
132  const cv::Point2i & end,
133  cv::Mat & grid,
134  bool stopOnObstacle);
135 
136 cv::Mat RTABMAP_CORE_EXPORT convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat = false);
137 cv::Mat RTABMAP_CORE_EXPORT convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat = false);
138 
139 cv::Mat RTABMAP_CORE_EXPORT erodeMap(const cv::Mat & map);
140 
141 template<typename PointT>
142 typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
143  const typename pcl::PointCloud<PointT> & cloud);
144 
145 // templated methods
146 template<typename PointT>
148  const typename pcl::PointCloud<PointT>::Ptr & cloud,
149  const pcl::IndicesPtr & indices,
150  pcl::IndicesPtr & ground,
151  pcl::IndicesPtr & obstacles,
152  int normalKSearch,
153  float groundNormalAngle,
154  float clusterRadius,
155  int minClusterSize,
156  bool segmentFlatObstacles = false,
157  float maxGroundHeight = 0.0f,
158  pcl::IndicesPtr * flatObstacles = 0,
159  const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
160  float groundNormalsUp = 0);
161 template<typename PointT>
163  const typename pcl::PointCloud<PointT>::Ptr & cloud,
164  pcl::IndicesPtr & ground,
165  pcl::IndicesPtr & obstacles,
166  int normalKSearch,
167  float groundNormalAngle,
168  float clusterRadius,
169  int minClusterSize,
170  bool segmentFlatObstacles = false,
171  float maxGroundHeight = 0.0f,
172  pcl::IndicesPtr * flatObstacles = 0,
173  const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
174  float groundNormalsUp = 0);
175 
176 template<typename PointT>
178  const typename pcl::PointCloud<PointT>::Ptr & cloud,
179  const pcl::IndicesPtr & groundIndices,
180  const pcl::IndicesPtr & obstaclesIndices,
181  cv::Mat & ground,
182  cv::Mat & obstacles,
183  float cellSize);
184 
185 template<typename PointT>
187  const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
188  const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
189  cv::Mat & ground,
190  cv::Mat & obstacles,
191  float cellSize);
192 
193 template<typename PointT>
195  const typename pcl::PointCloud<PointT>::Ptr & cloud,
196  cv::Mat & ground,
197  cv::Mat & obstacles,
198  float cellSize = 0.05f,
199  float groundNormalAngle = M_PI_4,
200  int minClusterSize = 20,
201  bool segmentFlatObstacles = false,
202  float maxGroundHeight = 0.0f);
203 template<typename PointT>
205  const typename pcl::PointCloud<PointT>::Ptr & cloud,
206  const pcl::IndicesPtr & indices,
207  cv::Mat & ground,
208  cv::Mat & obstacles,
209  float cellSize = 0.05f,
210  float groundNormalAngle = M_PI_4,
211  int minClusterSize = 20,
212  bool segmentFlatObstacles = false,
213  float maxGroundHeight = 0.0f);
214 
215 } // namespace util3d
216 } // namespace rtabmap
217 
219 
220 #endif /* UTIL3D_MAPPING_H_ */
rtabmap::RTABMAP_DEPRECATED
RTABMAP_DEPRECATED(typedef SensorData Image, "rtabmap::Image class is renamed to rtabmap::SensorData, use the last one instead.")
Transform.h
rtabmap::util3d::create2DMap
RTABMAP_DEPRECATED cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map< int, Transform > &poses, const std::map< int, pcl::PointCloud< pcl::PointXYZ >::Ptr > &scans, float cellSize, bool unknownSpaceFilled, float &xMin, float &yMin, float minMapSize=0.0f, float scanMaxRange=0.0f)
Definition: util3d_mapping.cpp:528
rtabmap::util3d::erodeMap
cv::Mat RTABMAP_CORE_EXPORT erodeMap(const cv::Mat &map)
Definition: util3d_mapping.cpp:996
rtabmap::util3d::rayTrace
void RTABMAP_CORE_EXPORT rayTrace(const cv::Point2i &start, const cv::Point2i &end, cv::Mat &grid, bool stopOnObstacle)
Definition: util3d_mapping.cpp:822
rtabmap::util3d::convertImage8U2Map
cv::Mat RTABMAP_CORE_EXPORT convertImage8U2Map(const cv::Mat &map8U, bool pgmFormat=false)
Definition: util3d_mapping.cpp:945
rtabmap::util3d::create2DMapFromOccupancyLocalMaps
cv::Mat RTABMAP_CORE_EXPORT create2DMapFromOccupancyLocalMaps(const std::map< int, Transform > &poses, const std::map< int, std::pair< cv::Mat, cv::Mat > > &occupancy, float cellSize, float &xMin, float &yMin, float minMapSize=0.0f, bool erode=false, float footprintRadius=0.0f)
Definition: util3d_mapping.cpp:191
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
empty
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::occupancy2DFromLaserScan
RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan(const cv::Mat &scan, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f)
Definition: util3d_mapping.cpp:50
util3d_mapping.hpp
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::convertMap2Image8U
cv::Mat RTABMAP_CORE_EXPORT convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
Definition: util3d_mapping.cpp:904
rtabmap
Definition: CameraARCore.cpp:35


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