#include <rtabmap/core/EpipolarGeometry.h>#include <rtabmap/utilite/UMath.h>#include <rtabmap/utilite/UStl.h>#include <rtabmap/utilite/ULogger.h>#include <pcl/registration/transformation_estimation_svd.h>#include <pcl/registration/transformation_estimation_2D.h>#include <pcl/registration/correspondence_rejection_sample_consensus.h>#include <pcl/registration/icp_nl.h>#include <pcl/io/pcd_io.h>#include <pcl/common/distances.h>#include <pcl/surface/gp3.h>#include <pcl/features/normal_3d_omp.h>#include <pcl/surface/mls.h>#include <pcl/ModelCoefficients.h>#include <pcl/segmentation/sac_segmentation.h>#include <opencv2/calib3d/calib3d.hpp>#include <opencv2/video/tracking.hpp>#include <rtabmap/core/VWDictionary.h>#include <cmath>#include <stdio.h>#include "rtabmap/utilite/UConversion.h"#include "rtabmap/utilite/UTimer.h"#include "rtabmap/core/util3d.h"#include "rtabmap/core/Signature.h"#include <pcl/filters/random_sample.h>
Include dependency graph for util3d.cpp:
Go to the source code of this file.
Namespaces | |
| namespace | rtabmap |
| namespace | rtabmap::util3d |
Functions | |
| std::multimap< int, cv::KeyPoint > RTABMAP_EXP | rtabmap::util3d::aggregate (const std::list< int > &wordIds, const std::vector< cv::KeyPoint > &keypoints) |
| cv::Mat | rtabmap::util3d::bgrFromCloud (const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, bool bgrOrder) |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::cloudFromDepth (const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1) |
| pcl::PointCloud < pcl::PointXYZRGB >::Ptr RTABMAP_EXP | rtabmap::util3d::cloudFromDepthRGB (const cv::Mat &imageRgb, const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1) |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::cloudFromDisparity (const cv::Mat &imageDisparity, float cx, float cy, float fx, float baseline, int decimation=1) |
| pcl::PointCloud < pcl::PointXYZRGB >::Ptr RTABMAP_EXP | rtabmap::util3d::cloudFromDisparityRGB (const cv::Mat &imageRgb, const cv::Mat &imageDisparity, float cx, float cy, float fx, float baseline, int decimation=1) |
| pcl::PointCloud < pcl::PointXYZRGB >::Ptr RTABMAP_EXP | rtabmap::util3d::cloudFromStereoImages (const cv::Mat &imageLeft, const cv::Mat &imageRight, float cx, float cy, float fx, float baseline, int decimation=1) |
| pcl::PointCloud < pcl::PointNormal >::Ptr RTABMAP_EXP | rtabmap::util3d::computeNormals (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int normalKSearch=20) |
| pcl::PointCloud < pcl::PointXYZRGBNormal > ::Ptr RTABMAP_EXP | rtabmap::util3d::computeNormals (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, int normalKSearch=20) |
| pcl::PointCloud < pcl::PointXYZRGBNormal > ::Ptr RTABMAP_EXP | rtabmap::util3d::computeNormalsSmoothed (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float smoothingSearchRadius=0.025, bool smoothingPolynomialFit=true) |
| pcl::IndicesPtr RTABMAP_EXP | rtabmap::util3d::concatenate (const std::vector< pcl::IndicesPtr > &indices) |
| Concatenate a vector of indices to a single vector. | |
| pcl::IndicesPtr RTABMAP_EXP | rtabmap::util3d::concatenate (const pcl::IndicesPtr &indicesA, const pcl::IndicesPtr &indicesB) |
| Concatenate two vector of indices to a single vector. | |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::concatenateClouds (const std::list< pcl::PointCloud< pcl::PointXYZ >::Ptr > &clouds) |
| pcl::PointCloud < pcl::PointXYZRGB >::Ptr RTABMAP_EXP | rtabmap::util3d::concatenateClouds (const std::list< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > &clouds) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::convertMap2Image8U (const cv::Mat &map8S) |
| int RTABMAP_EXP | rtabmap::util3d::countUniquePairs (const std::multimap< int, pcl::PointXYZ > &wordsA, const std::multimap< int, pcl::PointXYZ > &wordsB) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::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) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::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) |
| pcl::PolygonMesh::Ptr RTABMAP_EXP | rtabmap::util3d::createMesh (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloudWithNormals, float gp3SearchRadius=0.025, float gp3Mu=2.5, int gp3MaximumNearestNeighbors=100, float gp3MaximumSurfaceAngle=M_PI/4, float gp3MinimumAngle=M_PI/18, float gp3MaximumAngle=2 *M_PI/3, bool gp3NormalConsistency=false) |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::cvMat2Cloud (const cv::Mat &matrix, const Transform &tranform=Transform::getIdentity()) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::cvtDepthFromFloat (const cv::Mat &depth32F) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::cvtDepthToFloat (const cv::Mat &depth16U) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::decimate (const cv::Mat &image, int d) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::depthFromCloud (const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, float &fx, float &fy, bool depth16U=true) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::depthFromDisparity (const cv::Mat &disparity, float fx, float baseline, int type=CV_32FC1) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::depthFromStereoCorrespondences (const cv::Mat &leftImage, const std::vector< cv::Point2f > &leftCorners, const std::vector< cv::Point2f > &rightCorners, const std::vector< unsigned char > &mask, float fx, float baseline) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::depthFromStereoImages (const cv::Mat &leftImage, const cv::Mat &rightImage, const std::vector< cv::Point2f > &leftCorners, float fx, float baseline, int flowWinSize=9, int flowMaxLevel=4, int flowIterations=20, double flowEps=0.02) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::disparityFromStereoCorrespondences (const cv::Mat &leftImage, const std::vector< cv::Point2f > &leftCorners, const std::vector< cv::Point2f > &rightCorners, const std::vector< unsigned char > &mask, float maxSlope=0.1f) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::disparityFromStereoImages (const cv::Mat &leftImage, const cv::Mat &rightImage) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::disparityFromStereoImages (const cv::Mat &leftImage, const cv::Mat &rightImage, const std::vector< cv::Point2f > &leftCorners, int flowWinSize=9, int flowMaxLevel=4, int flowIterations=20, double flowEps=0.02, float maxCorrespondencesSlope=0.1f) |
| void RTABMAP_EXP | rtabmap::util3d::extractXYZCorrespondences (const std::multimap< int, pcl::PointXYZ > &words1, const std::multimap< int, pcl::PointXYZ > &words2, pcl::PointCloud< pcl::PointXYZ > &cloud1, pcl::PointCloud< pcl::PointXYZ > &cloud2) |
| void RTABMAP_EXP | rtabmap::util3d::extractXYZCorrespondences (const std::list< std::pair< cv::Point2f, cv::Point2f > > &correspondences, const cv::Mat &depthImage1, const cv::Mat &depthImage2, float cx, float cy, float fx, float fy, float maxDepth, pcl::PointCloud< pcl::PointXYZ > &cloud1, pcl::PointCloud< pcl::PointXYZ > &cloud2) |
| void RTABMAP_EXP | rtabmap::util3d::extractXYZCorrespondences (const std::list< std::pair< cv::Point2f, cv::Point2f > > &correspondences, const pcl::PointCloud< pcl::PointXYZ > &cloud1, const pcl::PointCloud< pcl::PointXYZ > &cloud2, pcl::PointCloud< pcl::PointXYZ > &inliers1, pcl::PointCloud< pcl::PointXYZ > &inliers2, char depthAxis) |
| void RTABMAP_EXP | rtabmap::util3d::extractXYZCorrespondences (const std::list< std::pair< cv::Point2f, cv::Point2f > > &correspondences, const pcl::PointCloud< pcl::PointXYZRGB > &cloud1, const pcl::PointCloud< pcl::PointXYZRGB > &cloud2, pcl::PointCloud< pcl::PointXYZ > &inliers1, pcl::PointCloud< pcl::PointXYZ > &inliers2, char depthAxis) |
| template<typename PointT > | |
| void | rtabmap::util3d::extractXYZCorrespondencesImpl (const std::list< std::pair< cv::Point2f, cv::Point2f > > &correspondences, const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< pcl::PointXYZ > &inliers1, pcl::PointCloud< pcl::PointXYZ > &inliers2, char depthAxis) |
| void RTABMAP_EXP | rtabmap::util3d::extractXYZCorrespondencesRANSAC (const std::multimap< int, pcl::PointXYZ > &words1, const std::multimap< int, pcl::PointXYZ > &words2, pcl::PointCloud< pcl::PointXYZ > &cloud1, pcl::PointCloud< pcl::PointXYZ > &cloud2) |
| void RTABMAP_EXP | rtabmap::util3d::fillRegisteredDepthHoles (cv::Mat &depth, bool vertical, bool horizontal, bool fillDoubleHoles=false) |
| void RTABMAP_EXP | rtabmap::util3d::filterMaxDepth (pcl::PointCloud< pcl::PointXYZ > &inliers1, pcl::PointCloud< pcl::PointXYZ > &inliers2, float maxDepth, char depthAxis, bool removeDuplicates) |
| void RTABMAP_EXP | rtabmap::util3d::findCorrespondences (const std::multimap< int, cv::KeyPoint > &wordsA, const std::multimap< int, cv::KeyPoint > &wordsB, std::list< std::pair< cv::Point2f, cv::Point2f > > &pairs) |
| void RTABMAP_EXP | rtabmap::util3d::findCorrespondences (const std::multimap< int, pcl::PointXYZ > &words1, const std::multimap< int, pcl::PointXYZ > &words2, pcl::PointCloud< pcl::PointXYZ > &inliers1, pcl::PointCloud< pcl::PointXYZ > &inliers2, float maxDepth, std::set< int > *uniqueCorrespondences=0) |
| std::list< std::pair < cv::Point2f, cv::Point2f > > | rtabmap::util3d::findCorrespondences (const std::multimap< int, cv::KeyPoint > &words1, const std::multimap< int, cv::KeyPoint > &words2) |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::generateKeypoints3DDepth (const std::vector< cv::KeyPoint > &keypoints, const cv::Mat &depth, float fx, float fy, float cx, float cy, const Transform &transform) |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::generateKeypoints3DDisparity (const std::vector< cv::KeyPoint > &keypoints, const cv::Mat &disparity, float fx, float baseline, float cx, float cy, const Transform &transform) |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::generateKeypoints3DStereo (const std::vector< cv::KeyPoint > &keypoints, const cv::Mat &leftImage, const cv::Mat &rightImage, float fx, float baseline, float cx, float cy, const Transform &transform=Transform::getIdentity(), int flowWinSize=9, int flowMaxLevel=4, int flowIterations=20, double flowEps=0.02) |
| std::multimap< int, pcl::PointXYZ > RTABMAP_EXP | rtabmap::util3d::generateWords3DMono (const std::multimap< int, cv::KeyPoint > &kpts, const std::multimap< int, cv::KeyPoint > &previousKpts, float fx, float fy, float cx, float cy, const Transform &localTransform, Transform &cameraTransform, int pnpIterations=100, float pnpReprojError=8.0f, int pnpFlags=cv::ITERATIVE, float ransacParam1=3.0f, float ransacParam2=0.99f, const std::multimap< int, pcl::PointXYZ > &refGuess3D=std::multimap< int, pcl::PointXYZ >(), double *variance=0) |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::get3DFASTKpts (const cv::Mat &image, const cv::Mat &imageDepth, float constant, int fastThreshold=50, bool fastNonmaxSuppression=true, float maxDepth=5.0f) |
| int RTABMAP_EXP | rtabmap::util3d::getCorrespondencesCount (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_target, float maxDistance) |
| float RTABMAP_EXP | rtabmap::util3d::getDepth (const cv::Mat &depthImage, float x, float y, bool smoothing, float maxZError=0.02f) |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::getICPReadyCloud (const cv::Mat &depth, float fx, float fy, float cx, float cy, int decimation, double maxDepth, float voxel, int samples, const Transform &transform=Transform::getIdentity()) |
| Transform RTABMAP_EXP | rtabmap::util3d::icp (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool *hasConverged=0, double *variance=0, int *correspondences=0) |
| Transform RTABMAP_EXP | rtabmap::util3d::icp2D (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool *hasConverged=0, double *variance=0, int *correspondences=0) |
| Transform RTABMAP_EXP | rtabmap::util3d::icpPointToPlane (const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool *hasConverged=0, double *variance=0, int *correspondences=0) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud) |
| pcl::PointCloud< pcl::PointXYZ > ::Ptr RTABMAP_EXP | rtabmap::util3d::laserScanToPointCloud (const cv::Mat &laserScan) |
| void RTABMAP_EXP | rtabmap::util3d::occupancy2DFromLaserScan (const cv::Mat &scan, cv::Mat &ground, cv::Mat &obstacles, float cellSize) |
| pcl::PointXYZ RTABMAP_EXP | rtabmap::util3d::projectDepthTo3D (const cv::Mat &depthImage, float x, float y, float cx, float cy, float fx, float fy, bool smoothing, float maxZError=0.02f) |
| pcl::PointXYZ RTABMAP_EXP | rtabmap::util3d::projectDisparityTo3D (const cv::Point2f &pt, float disparity, float cx, float cy, float fx, float baseline) |
| pcl::PointXYZ RTABMAP_EXP | rtabmap::util3d::projectDisparityTo3D (const cv::Point2f &pt, const cv::Mat &disparity, float cx, float cy, float fx, float baseline) |
| void RTABMAP_EXP | rtabmap::util3d::rayTrace (const cv::Point2i &start, const cv::Point2i &end, cv::Mat &grid, bool stopOnObstacle) |
| cv::Mat RTABMAP_EXP | rtabmap::util3d::registerDepth (const cv::Mat &depth, const cv::Mat &depthK, const cv::Mat &colorK, const rtabmap::Transform &transform) |
| void RTABMAP_EXP | 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_EXP | rtabmap::util3d::savePCDWords (const std::string &fileName, const std::multimap< int, pcl::PointXYZ > &words, const Transform &transform=Transform::getIdentity()) |
| Transform RTABMAP_EXP | rtabmap::util3d::transformFromXYZCorrespondences (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud1, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud2, double inlierThreshold=0.02, int iterations=100, bool refineModel=false, double refineModelSigma=3.0, int refineModelIterations=10, std::vector< int > *inliers=0, double *variance=0) |