util3d.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_H_
29 #define UTIL3D_H_
30 
31 #include "rtabmap/core/rtabmap_core_export.h"
32 
33 #include <pcl/point_cloud.h>
34 #include <pcl/point_types.h>
35 #include <pcl/pcl_base.h>
36 #include <pcl/TextureMesh.h>
37 #include <rtabmap/core/Transform.h>
38 #include <rtabmap/core/SensorData.h>
39 #include <rtabmap/core/Parameters.h>
40 #include <opencv2/core/core.hpp>
41 #include <rtabmap/core/ProgressState.h>
42 #include <map>
43 #include <list>
44 
45 namespace rtabmap
46 {
47 
48 namespace util3d
49 {
50 
51 cv::Mat RTABMAP_CORE_EXPORT rgbFromCloud(
52  const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
53  bool bgrOrder = true);
54 
55 cv::Mat RTABMAP_CORE_EXPORT depthFromCloud(
56  const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
57  float & fx,
58  float & fy,
59  bool depth16U = true);
60 
61 void RTABMAP_CORE_EXPORT rgbdFromCloud(
62  const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
63  cv::Mat & rgb,
64  cv::Mat & depth,
65  float & fx,
66  float & fy,
67  bool bgrOrder = true,
68  bool depth16U = true);
69 
70 pcl::PointXYZ RTABMAP_CORE_EXPORT projectDepthTo3D(
71  const cv::Mat & depthImage,
72  float x, float y,
73  float cx, float cy,
74  float fx, float fy,
75  bool smoothing,
76  float depthErrorRatio = 0.02f);
77 
78 Eigen::Vector3f RTABMAP_CORE_EXPORT projectDepthTo3DRay(
79  const cv::Size & imageSize,
80  float x, float y,
81  float cx, float cy,
82  float fx, float fy);
83 
84 // Use cloudFromDepth with CameraModel interface.
85 RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(
86  const cv::Mat & imageDepth,
87  float cx, float cy,
88  float fx, float fy,
89  int decimation = 1,
90  float maxDepth = 0.0f,
91  float minDepth = 0.0f,
92  std::vector<int> * validIndices = 0);
93 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(
94  const cv::Mat & imageDepth,
95  const CameraModel & model,
96  int decimation = 1,
97  float maxDepth = 0.0f,
98  float minDepth = 0.0f,
99  std::vector<int> * validIndices = 0);
100 
101 // Use cloudFromDepthRGB with CameraModel interface.
102 RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
103  const cv::Mat & imageRgb,
104  const cv::Mat & imageDepth,
105  float cx, float cy,
106  float fx, float fy,
107  int decimation = 1,
108  float maxDepth = 0.0f,
109  float minDepth = 0.0f,
110  std::vector<int> * validIndices = 0);
111 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
112  const cv::Mat & imageRgb,
113  const cv::Mat & imageDepth,
114  const CameraModel & model,
115  int decimation = 1,
116  float maxDepth = 0.0f,
117  float minDepth = 0.0f,
118  std::vector<int> * validIndices = 0);
119 
120 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDisparity(
121  const cv::Mat & imageDisparity,
122  const StereoCameraModel & model,
123  int decimation = 1,
124  float maxDepth = 0.0f,
125  float minDepth = 0.0f,
126  std::vector<int> * validIndices = 0);
127 
128 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDisparityRGB(
129  const cv::Mat & imageRgb,
130  const cv::Mat & imageDisparity,
131  const StereoCameraModel & model,
132  int decimation = 1,
133  float maxDepth = 0.0f,
134  float minDepth = 0.0f,
135  std::vector<int> * validIndices = 0);
136 
137 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages(
138  const cv::Mat & imageLeft,
139  const cv::Mat & imageRight,
140  const StereoCameraModel & model,
141  int decimation = 1,
142  float maxDepth = 0.0f,
143  float minDepth = 0.0f,
144  std::vector<int> * validIndices = 0,
145  const ParametersMap & parameters = ParametersMap());
146 
160 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RTABMAP_CORE_EXPORT cloudsFromSensorData(
161  const SensorData & sensorData,
162  int decimation = 1,
163  float maxDepth = 0.0f,
164  float minDepth = 0.0f,
165  std::vector<pcl::IndicesPtr> * validIndices = 0,
166  const ParametersMap & stereoParameters = ParametersMap(),
167  const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
168 
184 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
185  const SensorData & sensorData,
186  int decimation = 1,
187  float maxDepth = 0.0f,
188  float minDepth = 0.0f,
189  std::vector<int> * validIndices = 0,
190  const ParametersMap & stereoParameters = ParametersMap(),
191  const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
192 
206 std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(
207  const SensorData & sensorData,
208  int decimation = 1,
209  float maxDepth = 0.0f,
210  float minDepth = 0.0f,
211  std::vector<pcl::IndicesPtr > * validIndices = 0,
212  const ParametersMap & stereoParameters = ParametersMap(),
213  const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
214 
230 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(
231  const SensorData & sensorData,
232  int decimation = 1,
233  float maxDepth = 0.0f,
234  float minDepth = 0.0f,
235  std::vector<int> * validIndices = 0,
236  const ParametersMap & stereoParameters = ParametersMap(),
237  const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
238 
242 pcl::PointCloud<pcl::PointXYZ> RTABMAP_CORE_EXPORT laserScanFromDepthImage(
243  const cv::Mat & depthImage,
244  float fx,
245  float fy,
246  float cx,
247  float cy,
248  float maxDepth = 0,
249  float minDepth = 0,
250  const Transform & localTransform = Transform::getIdentity());
256 pcl::PointCloud<pcl::PointXYZ> RTABMAP_CORE_EXPORT laserScanFromDepthImages(
257  const cv::Mat & depthImages,
258  const std::vector<CameraModel> & cameraModels,
259  float maxDepth,
260  float minDepth);
261 
262 template<typename PointCloud2T>
263 LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs = true, bool is2D = false, const Transform & transform = Transform());
264 // return CV_32FC3 (x,y,z)
265 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
266 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
267 // return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z)
268 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
269 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
270 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
271 // return CV_32FC4 (x,y,z,rgb)
272 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
273 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
274 // return CV_32FC4 (x,y,z,I)
275 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
276 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
277 // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
278 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
279 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
280 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
281 // return CV_32FC7 (x,y,z,I,normal_x,normal_y,normal_z)
282 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
283 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
284 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
285 // return CV_32FC2 (x,y)
286 LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
287 // return CV_32FC3 (x,y,I)
288 LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
289 // return CV_32FC5 (x,y,normal_x, normal_y, normal_z)
290 LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
291 LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
292 // return CV_32FC6 (x,y,I,normal_x, normal_y, normal_z)
293 LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
294 LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
295 
296 pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform = Transform());
297 // For 2d laserScan, z is set to null.
298 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform());
299 // For laserScan without normals, normals are set to null.
300 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform = Transform());
301 // For laserScan without rgb, rgb is set to default r,g,b parameters.
302 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100);
303 // For laserScan without intensity, intensity is set to intensity parameter.
304 pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);
305 // For laserScan without rgb, rgb is set to default r,g,b parameters.
306 // For laserScan without normals, normals are set to null.
307 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100);
308 // For laserScan without intensity, intensity is set to default intensity parameter.
309 // For laserScan without normals, normals are set to null.
310 pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);
311 
312 // For 2d laserScan, z is set to null.
313 pcl::PointXYZ RTABMAP_CORE_EXPORT laserScanToPoint(const LaserScan & laserScan, int index);
314 // For laserScan without normals, normals are set to null.
315 pcl::PointNormal RTABMAP_CORE_EXPORT laserScanToPointNormal(const LaserScan & laserScan, int index);
316 // For laserScan without rgb, rgb is set to default r,g,b parameters.
317 pcl::PointXYZRGB RTABMAP_CORE_EXPORT laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r = 100, unsigned char g = 100, unsigned char b = 100);
318 // For laserScan without intensity, intensity is set to intensity parameter.
319 pcl::PointXYZI RTABMAP_CORE_EXPORT laserScanToPointI(const LaserScan & laserScan, int index, float intensity);
320 // For laserScan without rgb, rgb is set to default r,g,b parameters.
321 // For laserScan without normals, normals are set to null.
322 pcl::PointXYZRGBNormal RTABMAP_CORE_EXPORT laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b);
323 // For laserScan without intensity, intensity is set to default intensity parameter.
324 // For laserScan without normals, normals are set to null.
325 pcl::PointXYZINormal RTABMAP_CORE_EXPORT laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity);
326 
327 void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max);
328 void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max);
329 
330 cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D(
331  const cv::Point2f & pt,
332  float disparity,
333  const StereoCameraModel & model);
334 
335 cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D(
336  const cv::Point2f & pt,
337  const cv::Mat & disparity,
338  const StereoCameraModel & model);
339 
340 // Register point cloud to camera (return registered depth image 32FC1)
341 cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(
342  const cv::Size & imageSize,
343  const cv::Mat & cameraMatrixK,
344  const cv::Mat & laserScan, // assuming points are already in /base_link coordinate
345  const rtabmap::Transform & cameraTransform); // /base_link -> /camera_link
346 
347 // Register point cloud to camera (return registered depth image 32FC1)
348 cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(
349  const cv::Size & imageSize,
350  const cv::Mat & cameraMatrixK,
351  const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
352  const rtabmap::Transform & cameraTransform); // /base_link -> /camera_link
353 
354 // Register point cloud to camera (return registered depth image 32FC1)
355 cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(
356  const cv::Size & imageSize,
357  const cv::Mat & cameraMatrixK,
358  const pcl::PCLPointCloud2::Ptr laserScan, // assuming points are already in /base_link coordinate
359  const rtabmap::Transform & cameraTransform); // /base_link -> /camera_link
360 
361 // Direction vertical (>=0), horizontal (<0)
362 void RTABMAP_CORE_EXPORT fillProjectedCloudHoles(
363  cv::Mat & depthRegistered,
364  bool verticalDirection,
365  bool fillToBorder);
366 
371 std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT projectCloudToCameras (
372  const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
373  const std::map<int, Transform> & cameraPoses,
374  const std::map<int, std::vector<CameraModel> > & cameraModels,
375  float maxDistance = 0.0f,
376  float maxAngle = 0.0f,
377  const std::vector<float> & roiRatios = std::vector<float>(),
378  const cv::Mat & projMask = cv::Mat(),
379  bool distanceToCamPolicy = false,
380  const ProgressState * state = 0);
385 std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT projectCloudToCameras (
386  const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
387  const std::map<int, Transform> & cameraPoses,
388  const std::map<int, std::vector<CameraModel> > & cameraModels,
389  float maxDistance = 0.0f,
390  float maxAngle = 0.0f,
391  const std::vector<float> & roiRatios = std::vector<float>(),
392  const cv::Mat & projMask = cv::Mat(),
393  bool distanceToCamPolicy = false,
394  const ProgressState * state = 0);
395 
396 bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f & pt);
397 
398 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT concatenateClouds(
399  const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
400 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT concatenateClouds(
401  const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
402 
412 pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate(
413  const std::vector<pcl::IndicesPtr> & indices);
414 
425 pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate(
426  const pcl::IndicesPtr & indicesA,
427  const pcl::IndicesPtr & indicesB);
428 
429 void RTABMAP_CORE_EXPORT savePCDWords(
430  const std::string & fileName,
431  const std::multimap<int, pcl::PointXYZ> & words,
432  const Transform & transform = Transform::getIdentity());
433 
434 void RTABMAP_CORE_EXPORT savePCDWords(
435  const std::string & fileName,
436  const std::multimap<int, cv::Point3f> & words,
437  const Transform & transform = Transform::getIdentity());
438 
443 cv::Mat RTABMAP_CORE_EXPORT loadBINScan(const std::string & fileName);
444 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT loadBINCloud(const std::string & fileName);
445 // Use interface without dim argument.
446 RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT loadBINCloud(const std::string & fileName, int dim);
447 
448 // Load *.pcd, *.ply or *.bin (KITTI format).
449 LaserScan RTABMAP_CORE_EXPORT loadScan(const std::string & path);
450 
451 // Use loadScan() instead.
452 RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT loadCloud(
453  const std::string & path,
454  const Transform & transform = Transform::getIdentity(),
455  int downsampleStep = 1,
456  float voxelSize = 0.0f);
457 
466 LaserScan RTABMAP_CORE_EXPORT deskew(
467  const LaserScan & input,
468  double inputStamp,
469  const rtabmap::Transform & velocity);
470 
471 } // namespace util3d
472 } // namespace rtabmap
473 
474 #include "rtabmap/core/impl/util3d.hpp"
475 
476 #endif /* UTIL3D_H_ */
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2402
rtabmap::util3d::cloudFromDepthRGB
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(const cv::Mat &imageRgb, const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:416
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2230
rtabmap::util3d::laserScanToPointI
pcl::PointXYZI RTABMAP_CORE_EXPORT laserScanToPointI(const LaserScan &laserScan, int index, float intensity)
Definition: util3d.cpp:2505
rtabmap::util3d::rgbdFromCloud
void RTABMAP_CORE_EXPORT rgbdFromCloud(const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, cv::Mat &rgb, cv::Mat &depth, float &fx, float &fy, bool bgrOrder=true, bool depth16U=true)
Definition: util3d.cpp:138
rtabmap::util3d::cloudFromStereoImages
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages(const cv::Mat &imageLeft, const cv::Mat &imageRight, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &parameters=ParametersMap())
Definition: util3d.cpp:814
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::util3d::fillProjectedCloudHoles
void RTABMAP_CORE_EXPORT fillProjectedCloudHoles(cv::Mat &depthRegistered, bool verticalDirection, bool fillToBorder)
Definition: util3d.cpp:2927
rtabmap::RTABMAP_DEPRECATED
RTABMAP_DEPRECATED(typedef SensorData Image, "rtabmap::Image class is renamed to rtabmap::SensorData, use the last one instead.")
SensorData.h
rtabmap::util3d::projectCloudToCameras
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > > RTABMAP_CORE_EXPORT projectCloudToCameras(const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const std::map< int, Transform > &cameraPoses, const std::map< int, std::vector< CameraModel > > &cameraModels, float maxDistance=0.0f, float maxAngle=0.0f, const std::vector< float > &roiRatios=std::vector< float >(), const cv::Mat &projMask=cv::Mat(), bool distanceToCamPolicy=false, const ProgressState *state=0)
Transform.h
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
ProgressState.h
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
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
rtabmap::util3d::projectDepthTo3DRay
Eigen::Vector3f RTABMAP_CORE_EXPORT projectDepthTo3DRay(const cv::Size &imageSize, float x, float y, float cx, float cy, float fx, float fy)
Definition: util3d.cpp:246
rtabmap::util3d::loadScan
LaserScan RTABMAP_CORE_EXPORT loadScan(const std::string &path)
Definition: util3d.cpp:3454
Parameters.h
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1268
rtabmap::util3d::projectCloudToCamera
cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const cv::Mat &laserScan, const rtabmap::Transform &cameraTransform)
Definition: util3d.cpp:2693
rtabmap::util3d::laserScanToPoint
pcl::PointXYZ RTABMAP_CORE_EXPORT laserScanToPoint(const LaserScan &laserScan, int index)
Definition: util3d.cpp:2428
rtabmap::util3d::depthFromCloud
cv::Mat RTABMAP_CORE_EXPORT depthFromCloud(const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, float &fx, float &fy, bool depth16U=true)
Definition: util3d.cpp:78
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2322
rtabmap::util3d::deskew
LaserScan RTABMAP_CORE_EXPORT deskew(const LaserScan &input, double inputStamp, const rtabmap::Transform &velocity)
Lidar deskewing.
Definition: util3d.cpp:3553
rtabmap::util3d::isFinite
bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3327
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:1992
rtabmap::util3d::loadBINCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT loadBINCloud(const std::string &fileName)
Definition: util3d.cpp:3444
rtabmap::util3d::concatenateClouds
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT concatenateClouds(const std::list< pcl::PointCloud< pcl::PointXYZ >::Ptr > &clouds)
Definition: util3d.cpp:3332
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2349
rtabmap::util3d::laserScanToPointRGBNormal
pcl::PointXYZRGBNormal RTABMAP_CORE_EXPORT laserScanToPointRGBNormal(const LaserScan &laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
Definition: util3d.cpp:2531
rtabmap::util3d::projectDepthTo3D
pcl::PointXYZ RTABMAP_CORE_EXPORT projectDepthTo3D(const cv::Mat &depthImage, float x, float y, float cx, float cy, float fx, float fy, bool smoothing, float depthErrorRatio=0.02f)
Definition: util3d.cpp:215
rtabmap::util3d::cloudsRGBFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1093
rtabmap::util3d::laserScanToPointNormal
pcl::PointNormal RTABMAP_CORE_EXPORT laserScanToPointNormal(const LaserScan &laserScan, int index)
Definition: util3d.cpp:2443
rtabmap::util3d::laserScanToPointRGB
pcl::PointXYZRGB RTABMAP_CORE_EXPORT laserScanToPointRGB(const LaserScan &laserScan, int index, unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2465
util3d.hpp
rtabmap::Transform
Definition: Transform.h:41
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1040
rtabmap::util3d::rgbFromCloud
cv::Mat RTABMAP_CORE_EXPORT rgbFromCloud(const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, bool bgrOrder=true)
rtabmap::util3d::laserScanToPointINormal
pcl::PointXYZINormal RTABMAP_CORE_EXPORT laserScanToPointINormal(const LaserScan &laserScan, int index, float intensity)
Definition: util3d.cpp:2579
rtabmap::util3d::cloudFromDisparityRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromDisparityRGB(const cv::Mat &imageRgb, const cv::Mat &imageDisparity, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:713
rtabmap::util3d::savePCDWords
void RTABMAP_CORE_EXPORT savePCDWords(const std::string &fileName, const std::multimap< int, pcl::PointXYZ > &words, const Transform &transform=Transform::getIdentity())
Definition: util3d.cpp:3384
rtabmap::util3d::projectDisparityTo3D
cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D(const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
Definition: util3d.cpp:2653
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2269
rtabmap::util3d::cloudFromDepth
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:266
rtabmap::util3d::getMinMax3D
void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2613
rtabmap::util3d::loadBINScan
cv::Mat RTABMAP_CORE_EXPORT loadBINScan(const std::string &fileName)
Definition: util3d.cpp:3421
rtabmap::util3d::loadCloud
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT loadCloud(const std::string &path, const Transform &transform=Transform::getIdentity(), int downsampleStep=1, float voxelSize=0.0f)
Definition: util3d.cpp:3512
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2376
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2296
trace.model
model
Definition: trace.py:4
rtabmap::util3d::laserScanFromDepthImages
pcl::PointCloud< pcl::PointXYZ > RTABMAP_CORE_EXPORT laserScanFromDepthImages(const cv::Mat &depthImages, const std::vector< CameraModel > &cameraModels, float maxDepth, float minDepth)
Definition: util3d.cpp:1357
rtabmap::util3d::laserScanFromDepthImage
pcl::PointCloud< pcl::PointXYZ > RTABMAP_CORE_EXPORT laserScanFromDepthImage(const cv::Mat &depthImage, float fx, float fy, float cx, float cy, float maxDepth=0, float minDepth=0, const Transform &localTransform=Transform::getIdentity())
Definition: util3d.cpp:1321
rtabmap::util3d::cloudsFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZ >::Ptr > RTABMAP_CORE_EXPORT cloudsFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:854
rtabmap::util3d::cloudFromDisparity
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromDisparity(const cv::Mat &imageDisparity, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:612

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