util3d.cpp File Reference

#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>

Include dependency graph for util3d.cpp:

Go to the source code of this file.

Classes
class	rtabmap::util3d::ProjectionInfo

Namespaces
	rtabmap
	rtabmap::util3d

Functions
cv::Mat	rtabmap::util3d::bgrFromCloud (const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, bool bgrOrder)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::cloudFromDepth (const cv::Mat &imageDepth, const CameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::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)
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::cloudFromDepthRGB (const cv::Mat &imageRgb, const cv::Mat &imageDepth, const CameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::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)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::cloudFromDisparity (const cv::Mat &imageDisparity, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::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)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::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 >())
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::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())
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::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 >())
std::vector< pcl::PointCloud< pcl::PointXYZ >::Ptr > RTABMAP_CORE_EXPORT	rtabmap::util3d::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 >())
std::vector< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > RTABMAP_CORE_EXPORT	rtabmap::util3d::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 >())
pcl::IndicesPtr RTABMAP_CORE_EXPORT	rtabmap::util3d::concatenate (const pcl::IndicesPtr &indicesA, const pcl::IndicesPtr &indicesB)
	Concatenate two vector of indices to a single vector. More...
pcl::IndicesPtr RTABMAP_CORE_EXPORT	rtabmap::util3d::concatenate (const std::vector< pcl::IndicesPtr > &indices)
	Concatenate a vector of indices to a single vector. More...
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::concatenateClouds (const std::list< pcl::PointCloud< pcl::PointXYZ >::Ptr > &clouds)
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::concatenateClouds (const std::list< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > &clouds)
cv::Mat RTABMAP_CORE_EXPORT	rtabmap::util3d::depthFromCloud (const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, float &fx, float &fy, bool depth16U=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::deskew (const LaserScan &input, double inputStamp, const rtabmap::Transform &velocity)
	Lidar deskewing. More...
void RTABMAP_CORE_EXPORT	rtabmap::util3d::fillProjectedCloudHoles (cv::Mat &depthRegistered, bool verticalDirection, bool fillToBorder)
void RTABMAP_CORE_EXPORT	rtabmap::util3d::getMinMax3D (const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
void RTABMAP_CORE_EXPORT	rtabmap::util3d::getMinMax3D (const cv::Mat &laserScan, pcl::PointXYZ &min, pcl::PointXYZ &max)
bool RTABMAP_CORE_EXPORT	rtabmap::util3d::isFinite (const cv::Point3f &pt)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointNormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZINormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
pcl::PointCloud< pcl::PointXYZ > RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromDepthImage (const cv::Mat &depthImage, float fx, float fy, float cx, float cy, float maxDepth=0, float minDepth=0, const Transform &localTransform=Transform::getIdentity())
pcl::PointCloud< pcl::PointXYZ > RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromDepthImages (const cv::Mat &depthImages, const std::vector< CameraModel > &cameraModels, float maxDepth, float minDepth)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointNormal > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointNormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZINormal > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZINormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
pcl::PointXYZ RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPoint (const LaserScan &laserScan, int index)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointCloud (const LaserScan &laserScan, const Transform &transform=Transform())
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointCloud2 (const LaserScan &laserScan, const Transform &transform=Transform())
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointCloudI (const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointCloudINormal (const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointCloudNormal (const LaserScan &laserScan, const Transform &transform=Transform())
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointCloudRGB (const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointCloudRGBNormal (const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
pcl::PointXYZI RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointI (const LaserScan &laserScan, int index, float intensity)
pcl::PointXYZINormal RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointINormal (const LaserScan &laserScan, int index, float intensity)
pcl::PointNormal RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointNormal (const LaserScan &laserScan, int index)
pcl::PointXYZRGB RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointRGB (const LaserScan &laserScan, int index, unsigned char r=100, unsigned char g=100, unsigned char b=100)
pcl::PointXYZRGBNormal RTABMAP_CORE_EXPORT	rtabmap::util3d::laserScanToPointRGBNormal (const LaserScan &laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::loadBINCloud (const std::string &fileName)
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::loadBINCloud (const std::string &fileName, int dim)
cv::Mat RTABMAP_CORE_EXPORT	rtabmap::util3d::loadBINScan (const std::string &fileName)
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT	rtabmap::util3d::loadCloud (const std::string &path, const Transform &transform=Transform::getIdentity(), int downsampleStep=1, float voxelSize=0.0f)
LaserScan RTABMAP_CORE_EXPORT	rtabmap::util3d::loadScan (const std::string &path)
cv::Mat RTABMAP_CORE_EXPORT	rtabmap::util3d::projectCloudToCamera (const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const cv::Mat &laserScan, const rtabmap::Transform &cameraTransform)
cv::Mat RTABMAP_CORE_EXPORT	rtabmap::util3d::projectCloudToCamera (const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const pcl::PCLPointCloud2::Ptr laserScan, const rtabmap::Transform &cameraTransform)
cv::Mat RTABMAP_CORE_EXPORT	rtabmap::util3d::projectCloudToCamera (const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const pcl::PointCloud< pcl::PointXYZ >::Ptr laserScan, const rtabmap::Transform &cameraTransform)
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > >	rtabmap::util3d::projectCloudToCameras (const typename pcl::PointCloud< pcl::PointXYZINormal > &cloud, const std::map< int, Transform > &cameraPoses, const std::map< int, std::vector< CameraModel > > &cameraModels, float maxDistance, float maxAngle, const std::vector< float > &roiRatios, const cv::Mat &projMask, bool distanceToCamPolicy, const ProgressState *state)
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > >	rtabmap::util3d::projectCloudToCameras (const typename pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const std::map< int, Transform > &cameraPoses, const std::map< int, std::vector< CameraModel > > &cameraModels, float maxDistance, float maxAngle, const std::vector< float > &roiRatios, const cv::Mat &projMask, bool distanceToCamPolicy, const ProgressState *state)
template<class PointT >
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > >	rtabmap::util3d::projectCloudToCamerasImpl (const typename pcl::PointCloud< PointT > &cloud, const std::map< int, Transform > &cameraPoses, const std::map< int, std::vector< CameraModel > > &cameraModels, float maxDistance, float maxAngle, const std::vector< float > &roiRatios, const cv::Mat &projMask, bool distanceToCamPolicy, const ProgressState *state)
pcl::PointXYZ RTABMAP_CORE_EXPORT	rtabmap::util3d::projectDepthTo3D (const cv::Mat &depthImage, float x, float y, float cx, float cy, float fx, float fy, bool smoothing, float depthErrorRatio=0.02f)
Eigen::Vector3f RTABMAP_CORE_EXPORT	rtabmap::util3d::projectDepthTo3DRay (const cv::Size &imageSize, float x, float y, float cx, float cy, float fx, float fy)
cv::Point3f RTABMAP_CORE_EXPORT	rtabmap::util3d::projectDisparityTo3D (const cv::Point2f &pt, const cv::Mat &disparity, const StereoCameraModel &model)
cv::Point3f RTABMAP_CORE_EXPORT	rtabmap::util3d::projectDisparityTo3D (const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
void RTABMAP_CORE_EXPORT	rtabmap::util3d::rgbdFromCloud (const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, cv::Mat &rgb, cv::Mat &depth, float &fx, float &fy, bool bgrOrder=true, bool depth16U=true)
void RTABMAP_CORE_EXPORT	rtabmap::util3d::savePCDWords (const std::string &fileName, const std::multimap< int, cv::Point3f > &words, const Transform &transform=Transform::getIdentity())
void RTABMAP_CORE_EXPORT	rtabmap::util3d::savePCDWords (const std::string &fileName, const std::multimap< int, pcl::PointXYZ > &words, const Transform &transform=Transform::getIdentity())