28 #ifndef UTIL3D_MAPPING_H_
29 #define UTIL3D_MAPPING_H_
31 #include "rtabmap/core/rtabmap_core_export.h"
33 #include <opencv2/core/core.hpp>
36 #include <pcl/pcl_base.h>
37 #include <pcl/point_cloud.h>
38 #include <pcl/point_types.h>
52 bool unknownSpaceFilled =
false,
53 float scanMaxRange = 0.0f);
58 const cv::Point3f & viewpoint,
62 bool unknownSpaceFilled =
false,
63 float scanMaxRange = 0.0f);
66 const cv::Mat & scanHit,
67 const cv::Mat & scanNoHit,
68 const cv::Point3f & viewpoint,
72 bool unknownSpaceFilled =
false,
73 float scanMaxRange = 0.0f);
76 const std::map<int, Transform> & poses,
77 const std::map<
int, std::pair<cv::Mat, cv::Mat> > & occupancy,
81 float minMapSize = 0.0f,
83 float footprintRadius = 0.0f);
87 const std::map<
int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
89 bool unknownSpaceFilled,
92 float minMapSize = 0.0f,
93 float scanMaxRange = 0.0f);
97 const std::map<
int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
98 const std::map<int, cv::Point3f > & viewpoints,
100 bool unknownSpaceFilled,
103 float minMapSize = 0.0f,
104 float scanMaxRange = 0.0f);
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,
123 const std::map<int, cv::Point3f > & viewpoints,
125 bool unknownSpaceFilled,
128 float minMapSize = 0.0f,
129 float scanMaxRange = 0.0f);
131 void RTABMAP_CORE_EXPORT
rayTrace(
const cv::Point2i & start,
132 const cv::Point2i & end,
134 bool stopOnObstacle);
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);
139 cv::Mat RTABMAP_CORE_EXPORT
erodeMap(
const cv::Mat & map);
141 template<
typename Po
intT>
143 const typename pcl::PointCloud<PointT> & cloud);
146 template<
typename Po
intT>
148 const typename pcl::PointCloud<PointT>::Ptr & cloud,
149 const pcl::IndicesPtr & indices,
150 pcl::IndicesPtr & ground,
151 pcl::IndicesPtr & obstacles,
153 float groundNormalAngle,
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 Po
intT>
163 const typename pcl::PointCloud<PointT>::Ptr & cloud,
164 pcl::IndicesPtr & ground,
165 pcl::IndicesPtr & obstacles,
167 float groundNormalAngle,
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);
176 template<
typename Po
intT>
178 const typename pcl::PointCloud<PointT>::Ptr & cloud,
179 const pcl::IndicesPtr & groundIndices,
180 const pcl::IndicesPtr & obstaclesIndices,
185 template<
typename Po
intT>
187 const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
188 const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
193 template<
typename Po
intT>
195 const typename pcl::PointCloud<PointT>::Ptr & cloud,
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 Po
intT>
205 const typename pcl::PointCloud<PointT>::Ptr & cloud,
206 const pcl::IndicesPtr & indices,
209 float cellSize = 0.05f,
210 float groundNormalAngle = M_PI_4,
211 int minClusterSize = 20,
212 bool segmentFlatObstacles =
false,
213 float maxGroundHeight = 0.0f);