Functions
rtabmap::util3d Namespace Reference

Functions

LaserScan RTABMAP_EXP adjustNormalsToViewPoint (const LaserScan &scan, const Eigen::Vector3f &viewpoint, bool forceGroundNormalsUp)
 
void RTABMAP_EXP adjustNormalsToViewPoint (pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0), bool forceGroundNormalsUp=false)
 
void RTABMAP_EXP adjustNormalsToViewPoint (pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0), bool forceGroundNormalsUp=false)
 
void RTABMAP_EXP adjustNormalsToViewPoints (const std::map< int, Transform > &poses, const pcl::PointCloud< pcl::PointXYZ >::Ptr &rawCloud, const std::vector< int > &rawCameraIndices, pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
 
void RTABMAP_EXP adjustNormalsToViewPoints (const std::map< int, Transform > &poses, const pcl::PointCloud< pcl::PointXYZ >::Ptr &rawCloud, const std::vector< int > &rawCameraIndices, pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud)
 
std::multimap< int, cv::KeyPoint > RTABMAP_EXP aggregate (const std::list< int > &wordIds, const std::vector< cv::KeyPoint > &keypoints)
 
void RTABMAP_EXP appendMesh (pcl::PointCloud< pcl::PointXYZRGBNormal > &cloudA, std::vector< pcl::Vertices > &polygonsA, const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloudB, const std::vector< pcl::Vertices > &polygonsB)
 
void RTABMAP_EXP appendMesh (pcl::PointCloud< pcl::PointXYZRGB > &cloudA, std::vector< pcl::Vertices > &polygonsA, const pcl::PointCloud< pcl::PointXYZRGB > &cloudB, const std::vector< pcl::Vertices > &polygonsB)
 
pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh (const cv::Mat &cloudMat, const std::vector< std::vector< unsigned int > > &polygons)
 
pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh (const cv::Mat &cloudMat, const std::vector< std::vector< std::vector< unsigned int > > > &polygons, const std::vector< std::vector< Eigen::Vector2f > > &texCoords, cv::Mat &textures, bool mergeTextures=false)
 
cv::Mat bgrFromCloud (const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, bool bgrOrder)
 
void RTABMAP_EXP cleanTextureMesh (pcl::TextureMesh &textureMesh, int minClusterSize)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP cloudFromDepth (const cv::Mat &imageDepth, const CameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr cloudFromDepth (const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation, float maxDepth, float minDepth, std::vector< int > *validIndices)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP 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)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloudFromDepthRGB (const cv::Mat &imageRgb, const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation, float maxDepth, float minDepth, std::vector< int > *validIndices)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP 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_EXP 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_EXP 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_EXP 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_EXP 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::list< std::list< int > > RTABMAP_EXP clusterPolygons (const std::vector< std::set< int > > &neighborPolygons, int minClusterSize=0)
 
LaserScan RTABMAP_EXP commonFiltering (const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, bool forceGroundNormalsUp=false)
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeFastOrganizedNormals (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float maxDepthChangeFactor=0.02f, float normalSmoothingSize=10.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeFastOrganizedNormals (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, float maxDepthChangeFactor=0.02f, float normalSmoothingSize=10.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeFastOrganizedNormals2D (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeFastOrganizedNormals2D (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
template<typename PointT >
pcl::PointCloud< pcl::Normal >::Ptr computeFastOrganizedNormals2DImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, int searchK, float searchRadius, const Eigen::Vector3f &viewPoint)
 
cv::Mat RTABMAP_EXP computeNormals (const cv::Mat &laserScan, int searchK, float searchRadius)
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=20, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, int searchK=20, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, int searchK=20, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, int searchK=20, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, int searchK=20, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const pcl::IndicesPtr &indices, int searchK=20, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
LaserScan computeNormals (const LaserScan &laserScan, int searchK, float searchRadius)
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals2D (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals2D (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
 
template<typename PointT >
pcl::PointCloud< pcl::Normal >::Ptr computeNormals2DImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, int searchK, float searchRadius, const Eigen::Vector3f &viewPoint)
 
float RTABMAP_EXP computeNormalsComplexity (const LaserScan &scan, cv::Mat *pcaEigenVectors=0, cv::Mat *pcaEigenValues=0)
 
float RTABMAP_EXP computeNormalsComplexity (const pcl::PointCloud< pcl::Normal > &normals, bool is2d=false, cv::Mat *pcaEigenVectors=0, cv::Mat *pcaEigenValues=0)
 
float RTABMAP_EXP computeNormalsComplexity (const pcl::PointCloud< pcl::PointNormal > &cloud, bool is2d=false, cv::Mat *pcaEigenVectors=0, cv::Mat *pcaEigenValues=0)
 
float RTABMAP_EXP computeNormalsComplexity (const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, bool is2d=false, cv::Mat *pcaEigenVectors=0, cv::Mat *pcaEigenValues=0)
 
template<typename PointT >
pcl::PointCloud< pcl::Normal >::Ptr computeNormalsImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, int searchK, float searchRadius, const Eigen::Vector3f &viewPoint)
 
std::vector< float > computeReprojErrors (std::vector< cv::Point3f > opoints, std::vector< cv::Point2f > ipoints, const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const cv::Mat &rvec, const cv::Mat &tvec, float reprojErrorThreshold, std::vector< int > &inliers)
 
void RTABMAP_EXP computeVarianceAndCorrespondences (const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloudA, const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloudB, double maxCorrespondenceDistance, double maxCorrespondenceAngle, double &variance, int &correspondencesOut)
 
void RTABMAP_EXP computeVarianceAndCorrespondences (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloudA, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloudB, double maxCorrespondenceDistance, double &variance, int &correspondencesOut)
 
pcl::IndicesPtr RTABMAP_EXP concatenate (const std::vector< pcl::IndicesPtr > &indices)
 Concatenate a vector of indices to a single vector. More...
 
pcl::IndicesPtr RTABMAP_EXP concatenate (const pcl::IndicesPtr &indicesA, const pcl::IndicesPtr &indicesB)
 Concatenate two vector of indices to a single vector. More...
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP concatenateClouds (const std::list< pcl::PointCloud< pcl::PointXYZ >::Ptr > &clouds)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP concatenateClouds (const std::list< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > &clouds)
 
void RTABMAP_EXP concatenateTextureMaterials (pcl::TextureMesh &mesh, const cv::Size &imageSize, int textureSize, int maxTextures, float &scale, std::vector< bool > *materialsKept=0)
 
pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes (const std::list< pcl::TextureMesh::Ptr > &meshes)
 
cv::Mat RTABMAP_EXP convertImage8U2Map (const cv::Mat &map8U, bool pgmFormat=false)
 
cv::Mat RTABMAP_EXP convertMap2Image8U (const cv::Mat &map8S, bool pgmFormat=false)
 
std::vector< std::vector< unsigned int > > RTABMAP_EXP convertPolygonsFromPCL (const std::vector< pcl::Vertices > &polygons)
 
std::vector< std::vector< std::vector< unsigned int > > > RTABMAP_EXP convertPolygonsFromPCL (const std::vector< std::vector< pcl::Vertices > > &polygons)
 
std::vector< pcl::Vertices > RTABMAP_EXP convertPolygonsToPCL (const std::vector< std::vector< unsigned int > > &polygons)
 
std::vector< std::vector< pcl::Vertices > > RTABMAP_EXP convertPolygonsToPCL (const std::vector< std::vector< std::vector< unsigned int > > > &tex_polygons)
 
int RTABMAP_EXP countUniquePairs (const std::multimap< int, pcl::PointXYZ > &wordsA, const std::multimap< int, pcl::PointXYZ > &wordsB)
 
cv::Mat RTABMAP_EXP create2DMap (const std::map< int, Transform > &poses, const std::map< int, std::pair< cv::Mat, cv::Mat > > &scans, const std::map< int, cv::Point3f > &viewpoints, float cellSize, bool unknownSpaceFilled, float &xMin, float &yMin, float minMapSize=0.0f, float scanMaxRange=0.0f)
 
cv::Mat 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, float scanMaxRange)
 
cv::Mat create2DMap (const std::map< int, Transform > &poses, const std::map< int, pcl::PointCloud< pcl::PointXYZ >::Ptr > &scans, const std::map< int, cv::Point3f > &viewpoints, float cellSize, bool unknownSpaceFilled, float &xMin, float &yMin, float minMapSize, float scanMaxRange)
 
cv::Mat RTABMAP_EXP 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)
 
pcl::PolygonMesh::Ptr RTABMAP_EXP 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=true)
 
void RTABMAP_EXP createPolygonIndexes (const std::vector< pcl::Vertices > &polygons, int cloudSize, std::vector< std::set< int > > &neighborPolygons, std::vector< std::set< int > > &vertexPolygons)
 Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons. More...
 
pcl::texture_mapping::CameraVector createTextureCameras (const std::map< int, Transform > &poses, const std::map< int, std::vector< CameraModel > > &cameraModels, const std::map< int, cv::Mat > &cameraDepths, const std::vector< float > &roiRatios)
 
pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh (const pcl::PolygonMesh::Ptr &mesh, const std::map< int, Transform > &poses, const std::map< int, CameraModel > &cameraModels, const std::map< int, cv::Mat > &cameraDepths, float maxDistance=0.0f, float maxDepthError=0.0f, float maxAngle=0.0f, int minClusterSize=50, const std::vector< float > &roiRatios=std::vector< float >(), const ProgressState *state=0, std::vector< std::map< int, pcl::PointXY > > *vertexToPixels=0)
 
pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh (const pcl::PolygonMesh::Ptr &mesh, const std::map< int, Transform > &poses, const std::map< int, std::vector< CameraModel > > &cameraModels, const std::map< int, cv::Mat > &cameraDepths, float maxDistance=0.0f, float maxDepthError=0.0f, float maxAngle=0.0f, int minClusterSize=50, const std::vector< float > &roiRatios=std::vector< float >(), const ProgressState *state=0, std::vector< std::map< int, pcl::PointXY > > *vertexToPixels=0)
 
pcl::IndicesPtr RTABMAP_EXP cropBox (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
 
pcl::IndicesPtr RTABMAP_EXP cropBox (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
 
pcl::IndicesPtr RTABMAP_EXP cropBox (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
 
pcl::IndicesPtr RTABMAP_EXP cropBox (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP cropBox (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP cropBox (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cropBox (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP cropBox (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
 
template<typename PointT >
pcl::IndicesPtr cropBoxImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform, bool negative)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr cropBoxImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform, bool negative)
 
template<typename pointRGBT >
void denseMeshPostProcessing (pcl::PolygonMeshPtr &mesh, float meshDecimationFactor, int maximumPolygons, const typename pcl::PointCloud< pointRGBT >::Ptr &cloud, float transferColorRadius, bool coloredOutput, bool cleanMesh, int minClusterSize, ProgressState *progressState)
 
cv::Mat RTABMAP_EXP depthFromCloud (const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, float &fx, float &fy, bool depth16U=true)
 
LaserScan RTABMAP_EXP downsample (const LaserScan &cloud, int step)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP downsample (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int step)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP downsample (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, int step)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP downsample (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, int step)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP downsample (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, int step)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr downsampleImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, int step)
 
cv::Mat RTABMAP_EXP erodeMap (const cv::Mat &map)
 
Transform RTABMAP_EXP estimateMotion3DTo2D (const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::KeyPoint > &words2B, const CameraModel &cameraModel, int minInliers=10, int iterations=100, double reprojError=5., int flagsPnP=0, int pnpRefineIterations=1, const Transform &guess=Transform::getIdentity(), const std::map< int, cv::Point3f > &words3B=std::map< int, cv::Point3f >(), cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0)
 
Transform RTABMAP_EXP estimateMotion3DTo3D (const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::Point3f > &words3B, int minInliers=10, double inliersDistance=0.1, int iterations=100, int refineIterations=5, cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0)
 
std::vector< pcl::IndicesPtr > RTABMAP_EXP extractClusters (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)
 
std::vector< pcl::IndicesPtr > RTABMAP_EXP extractClusters (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)
 
std::vector< pcl::IndicesPtr > RTABMAP_EXP extractClusters (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)
 Wrapper of the pcl::EuclideanClusterExtraction class. More...
 
std::vector< pcl::IndicesPtr > RTABMAP_EXP extractClusters (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)
 
std::vector< pcl::IndicesPtr > RTABMAP_EXP extractClusters (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)
 
std::vector< pcl::IndicesPtr > RTABMAP_EXP extractClusters (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)
 
template<typename PointT >
std::vector< pcl::IndicesPtr > extractClustersImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float clusterTolerance, int minClusterSize, int maxClusterSize, int *biggestClusterIndex)
 
pcl::IndicesPtr RTABMAP_EXP extractIndices (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
 
pcl::IndicesPtr RTABMAP_EXP extractIndices (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
 
pcl::IndicesPtr RTABMAP_EXP extractIndices (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
 
pcl::IndicesPtr RTABMAP_EXP extractIndices (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP extractIndices (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative, bool keepOrganized)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP extractIndices (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative, bool keepOrganized)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP extractIndices (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative, bool keepOrganized)
 
template<typename PointT >
pcl::IndicesPtr extractIndicesImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr extractIndicesImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative, bool keepOrganized)
 
pcl::IndicesPtr extractPlane (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float distanceThreshold, int maxIterations=100, pcl::ModelCoefficients *coefficientsOut=0)
 
pcl::IndicesPtr extractPlane (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float distanceThreshold, int maxIterations=100, pcl::ModelCoefficients *coefficientsOut=0)
 
void RTABMAP_EXP 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 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 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 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 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 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 fillProjectedCloudHoles (cv::Mat &depthRegistered, bool verticalDirection, bool fillToBorder)
 
std::vector< pcl::Vertices > RTABMAP_EXP filterCloseVerticesFromMesh (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr cloud, const std::vector< pcl::Vertices > &polygons, float radius, float angle, bool keepLatestInRadius)
 
std::vector< pcl::Vertices > RTABMAP_EXP filterInvalidPolygons (const std::vector< pcl::Vertices > &polygons)
 
void RTABMAP_EXP filterMaxDepth (pcl::PointCloud< pcl::PointXYZ > &inliers1, pcl::PointCloud< pcl::PointXYZ > &inliers2, float maxDepth, char depthAxis, bool removeDuplicates)
 
std::vector< int > RTABMAP_EXP filterNaNPointsFromMesh (const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const std::vector< pcl::Vertices > &polygons, pcl::PointCloud< pcl::PointXYZRGB > &outputCloud, std::vector< pcl::Vertices > &outputPolygons)
 
std::vector< int > RTABMAP_EXP filterNotUsedVerticesFromMesh (const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const std::vector< pcl::Vertices > &polygons, pcl::PointCloud< pcl::PointXYZRGBNormal > &outputCloud, std::vector< pcl::Vertices > &outputPolygons)
 
std::vector< int > RTABMAP_EXP filterNotUsedVerticesFromMesh (const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const std::vector< pcl::Vertices > &polygons, pcl::PointCloud< pcl::PointXYZRGB > &outputCloud, std::vector< pcl::Vertices > &outputPolygons)
 
void RTABMAP_EXP 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 findCorrespondences (const std::multimap< int, cv::Point3f > &words1, const std::multimap< int, cv::Point3f > &words2, std::vector< cv::Point3f > &inliers1, std::vector< cv::Point3f > &inliers2, float maxDepth, std::vector< int > *uniqueCorrespondences=0)
 
void RTABMAP_EXP findCorrespondences (const std::map< int, cv::Point3f > &words1, const std::map< int, cv::Point3f > &words2, std::vector< cv::Point3f > &inliers1, std::vector< cv::Point3f > &inliers2, float maxDepth, std::vector< int > *correspondences=0)
 
void RTABMAP_EXP fixTextureMeshForVisualization (pcl::TextureMesh &textureMesh)
 
pcl::IndicesPtr RTABMAP_EXP frustumFiltering (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative=false)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP frustumFiltering (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative=false)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP frustumFiltering (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const Transform &cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative=false)
 
template<typename PointT >
pcl::IndicesPtr frustumFilteringImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr frustumFilteringImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const Transform &cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative)
 
int gcd (int a, int b)
 
std::vector< cv::Point3f > RTABMAP_EXP generateKeypoints3DDepth (const std::vector< cv::KeyPoint > &keypoints, const cv::Mat &depth, const CameraModel &cameraModel, float minDepth=0, float maxDepth=0)
 
std::vector< cv::Point3f > RTABMAP_EXP generateKeypoints3DDepth (const std::vector< cv::KeyPoint > &keypoints, const cv::Mat &depth, const std::vector< CameraModel > &cameraModels, float minDepth=0, float maxDepth=0)
 
std::vector< cv::Point3f > RTABMAP_EXP generateKeypoints3DDisparity (const std::vector< cv::KeyPoint > &keypoints, const cv::Mat &disparity, const StereoCameraModel &stereoCameraModel, float minDepth=0, float maxDepth=0)
 
std::vector< cv::Point3f > RTABMAP_EXP generateKeypoints3DStereo (const std::vector< cv::Point2f > &leftCorners, const std::vector< cv::Point2f > &rightCorners, const StereoCameraModel &model, const std::vector< unsigned char > &mask=std::vector< unsigned char >(), float minDepth=0, float maxDepth=0)
 
std::map< int, cv::Point3f > RTABMAP_EXP generateWords3DMono (const std::map< int, cv::KeyPoint > &kpts, const std::map< int, cv::KeyPoint > &previousKpts, const CameraModel &cameraModel, Transform &cameraTransform, int pnpIterations=100, float pnpReprojError=8.0f, int pnpFlags=0, int pnpRefineIterations=1, float ransacParam1=3.0f, float ransacParam2=0.99f, const std::map< int, cv::Point3f > &refGuess3D=std::map< int, cv::Point3f >(), double *variance=0)
 
int RTABMAP_EXP getCorrespondencesCount (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_target, float maxDistance)
 
void RTABMAP_EXP getMinMax3D (const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
 
void RTABMAP_EXP getMinMax3D (const cv::Mat &laserScan, pcl::PointXYZ &min, pcl::PointXYZ &max)
 
Transform RTABMAP_EXP icp (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool &hasConverged, pcl::PointCloud< pcl::PointXYZ > &cloud_source_registered, float epsilon=0.0f, bool icp2D=false)
 
Transform RTABMAP_EXP icpPointToPlane (const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool &hasConverged, pcl::PointCloud< pcl::PointNormal > &cloud_source_registered, float epsilon=0.0f, bool icp2D=false)
 
bool RTABMAP_EXP isFinite (const cv::Point3f &pt)
 
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointNormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZINormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
 
pcl::PointCloud< pcl::PointXYZ > RTABMAP_EXP 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_EXP laserScanFromDepthImages (const cv::Mat &depthImages, const std::vector< CameraModel > &cameraModels, float maxDepth, float minDepth)
 
LaserScan RTABMAP_EXP laserScanFromPointCloud (const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointNormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointNormal > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZ > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZI > &cloud, const pcl::PointCloud< pcl::Normal > &normals, const Transform &transform=Transform(), bool filterNaNs=true)
 
cv::Mat RTABMAP_EXP laserScanFromPointCloud (const pcl::PointCloud< pcl::PointXYZINormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
 
pcl::PointXYZ RTABMAP_EXP laserScanToPoint (const LaserScan &laserScan, int index)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud (const LaserScan &laserScan, const Transform &transform=Transform())
 
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2 (const LaserScan &laserScan, const Transform &transform=Transform())
 
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP laserScanToPointCloudI (const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
 
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP laserScanToPointCloudINormal (const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal (const LaserScan &laserScan, const Transform &transform=Transform())
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB (const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal (const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
 
pcl::PointXYZI RTABMAP_EXP laserScanToPointI (const LaserScan &laserScan, int index, float intensity)
 
pcl::PointXYZINormal RTABMAP_EXP laserScanToPointINormal (const LaserScan &laserScan, int index, float intensity)
 
pcl::PointNormal RTABMAP_EXP laserScanToPointNormal (const LaserScan &laserScan, int index)
 
pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB (const LaserScan &laserScan, int index, unsigned char r=255, unsigned char g=255, unsigned char b=255)
 
pcl::PointXYZRGBNormal RTABMAP_EXP laserScanToPointRGBNormal (const LaserScan &laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP loadBINCloud (const std::string &fileName)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr loadBINCloud (const std::string &fileName, int dim)
 
cv::Mat RTABMAP_EXP loadBINScan (const std::string &fileName)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr loadCloud (const std::string &path, const Transform &transform, int downsampleStep, float voxelSize)
 
LaserScan RTABMAP_EXP loadScan (const std::string &path)
 
cv::Mat RTABMAP_EXP mergeTextures (pcl::TextureMesh &mesh, const std::map< int, cv::Mat > &images, const std::map< int, CameraModel > &calibrations, const Memory *memory=0, const DBDriver *dbDriver=0, int textureSize=4096, int textureCount=1, const std::vector< std::map< int, pcl::PointXY > > &vertexToPixels=std::vector< std::map< int, pcl::PointXY > >(), bool gainCompensation=true, float gainBeta=10.0f, bool gainRGB=true, bool blending=true, int blendingDecimation=0, int brightnessContrastRatioLow=0, int brightnessContrastRatioHigh=0, bool exposureFusion=false, const ProgressState *state=0)
 
cv::Mat RTABMAP_EXP mergeTextures (pcl::TextureMesh &mesh, const std::map< int, cv::Mat > &images, const std::map< int, std::vector< CameraModel > > &calibrations, const Memory *memory=0, const DBDriver *dbDriver=0, int textureSize=4096, int textureCount=1, const std::vector< std::map< int, pcl::PointXY > > &vertexToPixels=std::vector< std::map< int, pcl::PointXY > >(), bool gainCompensation=true, float gainBeta=10.0f, bool gainRGB=true, bool blending=true, int blendingDecimation=0, int brightnessContrastRatioLow=0, int brightnessContrastRatioHigh=0, bool exposureFusion=false, const ProgressState *state=0)
 
pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation (const pcl::PolygonMesh::Ptr &mesh, float factor)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP mls (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float searchRadius=0.0f, int polygonialOrder=2, int upsamplingMethod=0, float upsamplingRadius=0.0f, float upsamplingStep=0.0f, int pointDensity=0, float dilationVoxelSize=1.0f, int dilationIterations=0)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP mls (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, float searchRadius=0.0f, int polygonialOrder=2, int upsamplingMethod=0, float upsamplingRadius=0.0f, float upsamplingStep=0.0f, int pointDensity=0, float dilationVoxelSize=1.0f, int dilationIterations=0)
 
pcl::IndicesPtr RTABMAP_EXP normalFiltering (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
 
pcl::IndicesPtr RTABMAP_EXP normalFiltering (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
 
pcl::IndicesPtr RTABMAP_EXP normalFiltering (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
 Given a normal and a maximum angle error, keep all points of the cloud respecting this normal. More...
 
pcl::IndicesPtr RTABMAP_EXP normalFiltering (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
 
pcl::IndicesPtr RTABMAP_EXP normalFiltering (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
 
pcl::IndicesPtr RTABMAP_EXP normalFiltering (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
 
template<typename PointT >
pcl::IndicesPtr normalFilteringImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
 
template<typename PointT >
pcl::IndicesPtr normalFilteringImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float angleMax, const Eigen::Vector4f &normal)
 
template<typename pointT >
std::vector< pcl::Vertices > normalizePolygonsSide (const typename pcl::PointCloud< pointT > &cloud, const std::vector< pcl::Vertices > &polygons, const pcl::PointXYZ &viewPoint)
 
template<typename pointT >
std::vector< pcl::Vertices > normalizePolygonsSide (const pcl::PointCloud< pointT > &cloud, const std::vector< pcl::Vertices > &polygons, const pcl::PointXYZ &viewPoint=pcl::PointXYZ(0, 0, 0))
 
template<typename PointT >
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)
 
template<typename PointT >
void occupancy2DFromCloud3D (const typename pcl::PointCloud< PointT >::Ptr &cloud, cv::Mat &ground, cv::Mat &obstacles, float cellSize, float groundNormalAngle, int minClusterSize, bool segmentFlatObstacles, float maxGroundHeight)
 
template<typename PointT >
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)
 
template<typename PointT >
void occupancy2DFromGroundObstacles (const typename pcl::PointCloud< PointT >::Ptr &groundCloud, const typename pcl::PointCloud< PointT >::Ptr &obstaclesCloud, cv::Mat &ground, cv::Mat &obstacles, float cellSize)
 
void occupancy2DFromLaserScan (const cv::Mat &scan, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled, float scanMaxRange)
 
void RTABMAP_EXP occupancy2DFromLaserScan (const cv::Mat &scanHit, const cv::Mat &scanNoHit, const cv::Point3f &viewpoint, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f)
 
void occupancy2DFromLaserScan (const cv::Mat &scan, const cv::Point3f &viewpoint, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled, float scanMaxRange)
 
std::vector< pcl::Vertices > RTABMAP_EXP organizedFastMesh (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double angleTolerance, bool quad, int trianglePixelSize, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
 
std::vector< pcl::Vertices > RTABMAP_EXP organizedFastMesh (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, double angleTolerance=M_PI/16, bool quad=true, int trianglePixelSize=2, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
 
std::vector< pcl::Vertices > RTABMAP_EXP organizedFastMesh (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, double angleTolerance=M_PI/16, bool quad=true, int trianglePixelSize=2, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
 
pcl::IndicesPtr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
 
pcl::IndicesPtr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
 
pcl::IndicesPtr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
 
pcl::IndicesPtr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
 
pcl::IndicesPtr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
 
pcl::IndicesPtr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const std::string &axis, float min, float max, bool negative=false)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const std::string &axis, float min, float max, bool negative=false)
 
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const std::string &axis, float min, float max, bool negative=false)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const std::string &axis, float min, float max, bool negative=false)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const std::string &axis, float min, float max, bool negative=false)
 
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP passThrough (const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &cloud, const std::string &axis, float min, float max, bool negative=false)
 
template<typename PointT >
pcl::IndicesPtr passThroughImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr passThroughImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const std::string &axis, float min, float max, bool negative)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr projectCloudOnXYPlane (const typename pcl::PointCloud< PointT > &cloud)
 
cv::Mat RTABMAP_EXP projectCloudToCamera (const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const cv::Mat &laserScan, const rtabmap::Transform &cameraTransform)
 
cv::Mat RTABMAP_EXP projectCloudToCamera (const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const pcl::PointCloud< pcl::PointXYZ >::Ptr laserScan, const rtabmap::Transform &cameraTransform)
 
cv::Mat RTABMAP_EXP projectCloudToCamera (const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const pcl::PCLPointCloud2::Ptr laserScan, const rtabmap::Transform &cameraTransform)
 
pcl::PointXYZ RTABMAP_EXP 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_EXP projectDepthTo3DRay (const cv::Size &imageSize, float x, float y, float cx, float cy, float fx, float fy)
 
cv::Point3f RTABMAP_EXP projectDisparityTo3D (const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
 
cv::Point3f RTABMAP_EXP projectDisparityTo3D (const cv::Point2f &pt, const cv::Mat &disparity, const StereoCameraModel &model)
 
pcl::IndicesPtr RTABMAP_EXP radiusFiltering (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
 
pcl::IndicesPtr RTABMAP_EXP radiusFiltering (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
 
pcl::IndicesPtr RTABMAP_EXP radiusFiltering (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
 
pcl::IndicesPtr RTABMAP_EXP radiusFiltering (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
 
pcl::IndicesPtr RTABMAP_EXP radiusFiltering (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float radiusSearch, int minNeighborsInRadius)
 Wrapper of the pcl::RadiusOutlierRemoval class. More...
 
pcl::IndicesPtr RTABMAP_EXP radiusFiltering (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float radiusSearch, int minNeighborsInRadius)
 
pcl::IndicesPtr RTABMAP_EXP radiusFiltering (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, float radiusSearch, int minNeighborsInRadius)
 
pcl::IndicesPtr RTABMAP_EXP radiusFiltering (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float radiusSearch, int minNeighborsInRadius)
 
template<typename PointT >
pcl::IndicesPtr radiusFilteringImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float radiusSearch, int minNeighborsInRadius)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP randomSampling (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int samples)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP randomSampling (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, int samples)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr randomSamplingImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, int samples)
 
LaserScan RTABMAP_EXP rangeFiltering (const LaserScan &scan, float rangeMin, float rangeMax)
 
void RTABMAP_EXP rayTrace (const cv::Point2i &start, const cv::Point2i &end, cv::Mat &grid, bool stopOnObstacle)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP removeNaNFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr removeNaNFromPointCloudImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr removeNaNNormalsFromPointCloudImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud)
 
void RTABMAP_EXP rgbdFromCloud (const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, cv::Mat &rgb, cv::Mat &depth, float &fx, float &fy, bool bgrOrder=true, bool depth16U=true)
 
cv::Mat RTABMAP_EXP rgbFromCloud (const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, bool bgrOrder=true)
 
 RTABMAP_DEPRECATED (void RTABMAP_EXP occupancy2DFromLaserScan(const cv::Mat &scan, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f),"Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base.")
 
 RTABMAP_DEPRECATED (void RTABMAP_EXP occupancy2DFromLaserScan(const cv::Mat &scan, const cv::Point3f &viewpoint, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f),"Use interface with scanHit/scanNoHit parameters: scanNoHit set to null matrix has the same functionality than this method.")
 
 RTABMAP_DEPRECATED (cv::Mat RTABMAP_EXP 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),"Use interface with \"viewpoints\" parameter to make sure the ray tracing origin is from the sensor and not the base.")
 
 RTABMAP_DEPRECATED (pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP 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),"Use cloudFromDepth with CameraModel interface.")
 
 RTABMAP_DEPRECATED (cv::Mat RTABMAP_EXP create2DMap(const std::map< int, Transform > &poses, const std::map< int, pcl::PointCloud< pcl::PointXYZ >::Ptr > &scans, const std::map< int, cv::Point3f > &viewpoints, float cellSize, bool unknownSpaceFilled, float &xMin, float &yMin, float minMapSize=0.0f, float scanMaxRange=0.0f),"Use interface with cv::Mat scans.")
 
 RTABMAP_DEPRECATED (pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP 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),"Use cloudFromDepthRGB with CameraModel interface.")
 
 RTABMAP_DEPRECATED (pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP loadBINCloud(const std::string &fileName, int dim),"Use interface without dim argument.")
 
 RTABMAP_DEPRECATED (pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP loadCloud(const std::string &path, const Transform &transform=Transform::getIdentity(), int downsampleStep=1, float voxelSize=0.0f),"Use loadScan() instead.")
 
void RTABMAP_EXP savePCDWords (const std::string &fileName, const std::multimap< int, pcl::PointXYZ > &words, const Transform &transform=Transform::getIdentity())
 
void RTABMAP_EXP savePCDWords (const std::string &fileName, const std::multimap< int, cv::Point3f > &words, const Transform &transform=Transform::getIdentity())
 
template<typename PointT >
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)
 
template<typename PointT >
void segmentObstaclesFromGround (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, pcl::IndicesPtr &ground, pcl::IndicesPtr &obstacles, int normalKSearch, float groundNormalAngle, float clusterRadius, int minClusterSize, bool segmentFlatObstacles=false, float maxGroundHeight=0.0f, pcl::IndicesPtr *flatObstacles=0, const Eigen::Vector4f &viewPoint=Eigen::Vector4f(0, 0, 100, 0))
 
template<typename PointT >
void segmentObstaclesFromGround (const typename pcl::PointCloud< PointT >::Ptr &cloud, 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)
 
void RTABMAP_EXP solvePnPRansac (const std::vector< cv::Point3f > &objectPoints, const std::vector< cv::Point2f > &imagePoints, const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, cv::Mat &rvec, cv::Mat &tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, int minInliersCount, std::vector< int > &inliers, int flags, int refineIterations=1, float refineSigma=3.0f)
 
double sqr (uchar v)
 
pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearchRatio=0.01, int minNeighborsInRadius=1, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
 
pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearchRatio=0.01, float maxAngle=M_PI/4.0f, int minNeighborsInRadius=1, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP subtractFiltering (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, float radiusSearch, int minNeighborsInRadius=1)
 
pcl::IndicesPtr RTABMAP_EXP subtractFiltering (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearch, int minNeighborsInRadius=1)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP subtractFiltering (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::PointCloud< pcl::PointNormal >::Ptr &substractCloud, float radiusSearch, float maxAngle=M_PI/4.0f, int minNeighborsInRadius=1)
 
pcl::IndicesPtr RTABMAP_EXP subtractFiltering (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const pcl::PointCloud< pcl::PointNormal >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearch, float maxAngle=M_PI/4.0f, int minNeighborsInRadius=1)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP subtractFiltering (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &substractCloud, float radiusSearch, float maxAngle=M_PI/4.0f, int minNeighborsInRadius=1)
 
pcl::IndicesPtr RTABMAP_EXP subtractFiltering (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearch, float maxAngle=M_PI/4.0f, int minNeighborsInRadius=1)
 
template<typename PointT >
pcl::IndicesPtr subtractFilteringImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const typename pcl::PointCloud< PointT >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearch, int minNeighborsInRadius)
 
template<typename PointT >
pcl::IndicesPtr subtractFilteringImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const typename pcl::PointCloud< PointT >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearch, float maxAngle, int minNeighborsInRadius)
 
Transform RTABMAP_EXP transformFromXYZCorrespondences (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud1, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud2, double inlierThreshold=0.02, int iterations=100, int refineModelIterations=10, double refineModelSigma=3.0, std::vector< int > *inliers=0, cv::Mat *variance=0)
 
Transform RTABMAP_EXP transformFromXYZCorrespondencesSVD (const pcl::PointCloud< pcl::PointXYZ > &cloud1, const pcl::PointCloud< pcl::PointXYZ > &cloud2)
 
LaserScan RTABMAP_EXP transformLaserScan (const LaserScan &laserScan, const Transform &transform)
 
cv::Point3f RTABMAP_EXP transformPoint (const cv::Point3f &pt, const Transform &transform)
 
pcl::PointXYZ RTABMAP_EXP transformPoint (const pcl::PointXYZ &pt, const Transform &transform)
 
pcl::PointXYZI RTABMAP_EXP transformPoint (const pcl::PointXYZI &pt, const Transform &transform)
 
pcl::PointXYZRGB RTABMAP_EXP transformPoint (const pcl::PointXYZRGB &pt, const Transform &transform)
 
pcl::PointNormal RTABMAP_EXP transformPoint (const pcl::PointNormal &point, const Transform &transform)
 
pcl::PointXYZRGBNormal RTABMAP_EXP transformPoint (const pcl::PointXYZRGBNormal &point, const Transform &transform)
 
pcl::PointXYZINormal RTABMAP_EXP transformPoint (const pcl::PointXYZINormal &point, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const Transform &transform)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &cloud, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &transform)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP transformPointCloud (const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &transform)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr uniformSampling (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr uniformSampling (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr uniformSampling (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float voxelSize)
 
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloud, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, float voxelSize)
 
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP voxelize (const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &cloud, float voxelSize)
 
template<typename PointT >
pcl::PointCloud< PointT >::Ptr voxelizeImpl (const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
 

Function Documentation

LaserScan rtabmap::util3d::adjustNormalsToViewPoint ( const LaserScan scan,
const Eigen::Vector3f &  viewpoint,
bool  forceGroundNormalsUp 
)

Definition at line 2827 of file util3d_surface.cpp.

void rtabmap::util3d::adjustNormalsToViewPoint ( pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const Eigen::Vector3f &  viewpoint = Eigen::Vector3f(0,0,0),
bool  forceGroundNormalsUp = false 
)

Definition at line 2862 of file util3d_surface.cpp.

void rtabmap::util3d::adjustNormalsToViewPoint ( pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const Eigen::Vector3f &  viewpoint = Eigen::Vector3f(0,0,0),
bool  forceGroundNormalsUp = false 
)

Definition at line 2888 of file util3d_surface.cpp.

void rtabmap::util3d::adjustNormalsToViewPoints ( const std::map< int, Transform > &  poses,
const pcl::PointCloud< pcl::PointXYZ >::Ptr &  rawCloud,
const std::vector< int > &  rawCameraIndices,
pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud 
)

Definition at line 2914 of file util3d_surface.cpp.

void rtabmap::util3d::adjustNormalsToViewPoints ( const std::map< int, Transform > &  poses,
const pcl::PointCloud< pcl::PointXYZ >::Ptr &  rawCloud,
const std::vector< int > &  rawCameraIndices,
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud 
)

Definition at line 2960 of file util3d_surface.cpp.

std::multimap< int, cv::KeyPoint > rtabmap::util3d::aggregate ( const std::list< int > &  wordIds,
const std::vector< cv::KeyPoint > &  keypoints 
)

Definition at line 495 of file util3d_features.cpp.

void rtabmap::util3d::appendMesh ( pcl::PointCloud< pcl::PointXYZRGBNormal > &  cloudA,
std::vector< pcl::Vertices > &  polygonsA,
const pcl::PointCloud< pcl::PointXYZRGBNormal > &  cloudB,
const std::vector< pcl::Vertices > &  polygonsB 
)

Definition at line 268 of file util3d_surface.cpp.

void rtabmap::util3d::appendMesh ( pcl::PointCloud< pcl::PointXYZRGB > &  cloudA,
std::vector< pcl::Vertices > &  polygonsA,
const pcl::PointCloud< pcl::PointXYZRGB > &  cloudB,
const std::vector< pcl::Vertices > &  polygonsB 
)

Definition at line 294 of file util3d_surface.cpp.

pcl::PolygonMesh::Ptr rtabmap::util3d::assemblePolygonMesh ( const cv::Mat &  cloudMat,
const std::vector< std::vector< unsigned int > > &  polygons 
)

Definition at line 1369 of file util3d_surface.cpp.

pcl::TextureMesh::Ptr rtabmap::util3d::assembleTextureMesh ( const cv::Mat &  cloudMat,
const std::vector< std::vector< std::vector< unsigned int > > > &  polygons,
const std::vector< std::vector< Eigen::Vector2f > > &  texCoords,
cv::Mat &  textures,
bool  mergeTextures = false 
)

Definition at line 1246 of file util3d_surface.cpp.

cv::Mat rtabmap::util3d::bgrFromCloud ( const pcl::PointCloud< pcl::PointXYZRGBA > &  cloud,
bool  bgrOrder 
)

Definition at line 50 of file util3d.cpp.

void rtabmap::util3d::cleanTextureMesh ( pcl::TextureMesh &  textureMesh,
int  minClusterSize 
)

Remove not textured polygon clusters. If minClusterSize<0, only the largest cluster is kept.

Definition at line 851 of file util3d_surface.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr 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 
)

Definition at line 277 of file util3d.cpp.

pcl::PointCloud<pcl::PointXYZ>::Ptr rtabmap::util3d::cloudFromDepth ( const cv::Mat &  imageDepth,
float  cx,
float  cy,
float  fx,
float  fy,
int  decimation,
float  maxDepth,
float  minDepth,
std::vector< int > *  validIndices 
)

Definition at line 264 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr 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 
)

Definition at line 428 of file util3d.cpp.

pcl::PointCloud<pcl::PointXYZRGB>::Ptr rtabmap::util3d::cloudFromDepthRGB ( const cv::Mat &  imageRgb,
const cv::Mat &  imageDepth,
float  cx,
float  cy,
float  fx,
float  fy,
int  decimation,
float  maxDepth,
float  minDepth,
std::vector< int > *  validIndices 
)

Definition at line 414 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr 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 
)

Definition at line 610 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr 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 
)

Definition at line 711 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP 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>() 
)

Definition at line 852 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr 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() 
)

Definition at line 812 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP 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>() 
)

Create an RGB cloud from the images contained in SensorData. If there is only one camera, the returned cloud is organized. Otherwise, all NaN points are removed and the cloud will be dense.

Parameters
sensorData,thesensor data.
decimation,imagesare decimated by this factor before projecting points to 3D. The factor should be a factor of the image width and height.
maxDepth,maximumdepth of the projected points (farther points are set to null in case of an organized cloud).
minDepth,minimumdepth of the projected points (closer points are set to null in case of an organized cloud).
validIndices,theindices of valid points in the cloud
roiRatios,[left,right,top,bottom]region of interest (in ratios) of the image projected.
Returns
a RGB cloud.

Definition at line 1031 of file util3d.cpp.

std::list< std::list< int > > rtabmap::util3d::clusterPolygons ( const std::vector< std::set< int > > &  neighborPolygons,
int  minClusterSize = 0 
)

Definition at line 116 of file util3d_surface.cpp.

LaserScan rtabmap::util3d::commonFiltering ( const LaserScan scan,
int  downsamplingStep,
float  rangeMin = 0.0f,
float  rangeMax = 0.0f,
float  voxelSize = 0.0f,
int  normalK = 0,
float  normalRadius = 0.0f,
bool  forceGroundNormalsUp = false 
)

Do some filtering approaches and try to avoid converting between pcl and opencv and to avoid not needed operations like computing normals while the scan has already normals and voxel filtering is not used.

Definition at line 74 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeFastOrganizedNormals ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
float  maxDepthChangeFactor = 0.02f,
float  normalSmoothingSize = 10.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2488 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeFastOrganizedNormals ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  maxDepthChangeFactor = 0.02f,
float  normalSmoothingSize = 10.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2497 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeFastOrganizedNormals2D ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
int  searchK = 5,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2471 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeFastOrganizedNormals2D ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
int  searchK = 5,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2479 of file util3d_surface.cpp.

template<typename PointT >
pcl::PointCloud<pcl::Normal>::Ptr rtabmap::util3d::computeFastOrganizedNormals2DImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
int  searchK,
float  searchRadius,
const Eigen::Vector3f &  viewPoint 
)

Definition at line 2377 of file util3d_surface.cpp.

cv::Mat RTABMAP_EXP rtabmap::util3d::computeNormals ( const cv::Mat &  laserScan,
int  searchK,
float  searchRadius 
)
pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeNormals ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
int  searchK = 20,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2230 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeNormals ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
int  searchK = 20,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2239 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeNormals ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
int  searchK = 20,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2248 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeNormals ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
int  searchK = 20,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2257 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeNormals ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
int  searchK = 20,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2266 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeNormals ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
int  searchK = 20,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2275 of file util3d_surface.cpp.

LaserScan rtabmap::util3d::computeNormals ( const LaserScan laserScan,
int  searchK,
float  searchRadius 
)

Definition at line 2132 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeNormals2D ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
int  searchK = 5,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2359 of file util3d_surface.cpp.

pcl::PointCloud< pcl::Normal >::Ptr rtabmap::util3d::computeNormals2D ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
int  searchK = 5,
float  searchRadius = 0.0f,
const Eigen::Vector3f &  viewPoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 2367 of file util3d_surface.cpp.

template<typename PointT >
pcl::PointCloud<pcl::Normal>::Ptr rtabmap::util3d::computeNormals2DImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
int  searchK,
float  searchRadius,
const Eigen::Vector3f &  viewPoint 
)

Definition at line 2286 of file util3d_surface.cpp.

float rtabmap::util3d::computeNormalsComplexity ( const LaserScan scan,
cv::Mat *  pcaEigenVectors = 0,
cv::Mat *  pcaEigenValues = 0 
)

Definition at line 2536 of file util3d_surface.cpp.

float rtabmap::util3d::computeNormalsComplexity ( const pcl::PointCloud< pcl::Normal > &  normals,
bool  is2d = false,
cv::Mat *  pcaEigenVectors = 0,
cv::Mat *  pcaEigenValues = 0 
)

Definition at line 2648 of file util3d_surface.cpp.

float rtabmap::util3d::computeNormalsComplexity ( const pcl::PointCloud< pcl::PointNormal > &  cloud,
bool  is2d = false,
cv::Mat *  pcaEigenVectors = 0,
cv::Mat *  pcaEigenValues = 0 
)

Definition at line 2605 of file util3d_surface.cpp.

float rtabmap::util3d::computeNormalsComplexity ( const pcl::PointCloud< pcl::PointXYZRGBNormal > &  cloud,
bool  is2d = false,
cv::Mat *  pcaEigenVectors = 0,
cv::Mat *  pcaEigenValues = 0 
)

Definition at line 2691 of file util3d_surface.cpp.

template<typename PointT >
pcl::PointCloud<pcl::Normal>::Ptr rtabmap::util3d::computeNormalsImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
int  searchK,
float  searchRadius,
const Eigen::Vector3f &  viewPoint 
)

Definition at line 2192 of file util3d_surface.cpp.

std::vector<float> rtabmap::util3d::computeReprojErrors ( std::vector< cv::Point3f >  opoints,
std::vector< cv::Point2f >  ipoints,
const cv::Mat &  cameraMatrix,
const cv::Mat &  distCoeffs,
const cv::Mat &  rvec,
const cv::Mat &  tvec,
float  reprojErrorThreshold,
std::vector< int > &  inliers 
)

Definition at line 292 of file util3d_motion_estimation.cpp.

void rtabmap::util3d::computeVarianceAndCorrespondences ( const pcl::PointCloud< pcl::PointNormal >::ConstPtr &  cloudA,
const pcl::PointCloud< pcl::PointNormal >::ConstPtr &  cloudB,
double  maxCorrespondenceDistance,
double  maxCorrespondenceAngle,
double &  variance,
int &  correspondencesOut 
)

Definition at line 240 of file util3d_registration.cpp.

void rtabmap::util3d::computeVarianceAndCorrespondences ( const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &  cloudA,
const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &  cloudB,
double  maxCorrespondenceDistance,
double &  variance,
int &  correspondencesOut 
)

Definition at line 301 of file util3d_registration.cpp.

pcl::IndicesPtr rtabmap::util3d::concatenate ( const std::vector< pcl::IndicesPtr > &  indices)

Concatenate a vector of indices to a single vector.

Parameters
indicesthe vector of indices to concatenate.
Note
This methods doesn't check if indices exist in the two set and doesn't sort the output indices. If we are not sure if the the two set of indices set are disjoint and/or you need sorted indices, the use of mergeIndices().
Returns
the indices concatenated.

Definition at line 3060 of file util3d.cpp.

pcl::IndicesPtr rtabmap::util3d::concatenate ( const pcl::IndicesPtr &  indicesA,
const pcl::IndicesPtr &  indicesB 
)

Concatenate two vector of indices to a single vector.

Parameters
indicesAthe first vector of indices to concatenate.
indicesBthe second vector of indices to concatenate.
Note
This methods doesn't check if indices exist in the two set and doesn't sort the output indices. If we are not sure if the the two set of indices set are disjoint and/or you need sorted indices, the use of mergeIndices().
Returns
the indices concatenated.

Definition at line 3080 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::concatenateClouds ( const std::list< pcl::PointCloud< pcl::PointXYZ >::Ptr > &  clouds)

Definition at line 3040 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::concatenateClouds ( const std::list< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > &  clouds)

Definition at line 3050 of file util3d.cpp.

void rtabmap::util3d::concatenateTextureMaterials ( pcl::TextureMesh &  mesh,
const cv::Size &  imageSize,
int  textureSize,
int  maxTextures,
float &  scale,
std::vector< bool > *  materialsKept = 0 
)

Definition at line 1053 of file util3d_surface.cpp.

pcl::TextureMesh::Ptr rtabmap::util3d::concatenateTextureMeshes ( const std::list< pcl::TextureMesh::Ptr > &  meshes)

Definition at line 984 of file util3d_surface.cpp.

cv::Mat rtabmap::util3d::convertImage8U2Map ( const cv::Mat &  map8U,
bool  pgmFormat = false 
)

Definition at line 930 of file util3d_mapping.cpp.

cv::Mat rtabmap::util3d::convertMap2Image8U ( const cv::Mat &  map8S,
bool  pgmFormat = false 
)

Definition at line 897 of file util3d_mapping.cpp.

std::vector< std::vector< unsigned int > > rtabmap::util3d::convertPolygonsFromPCL ( const std::vector< pcl::Vertices > &  polygons)

Definition at line 1201 of file util3d_surface.cpp.

std::vector< std::vector< std::vector< unsigned int > > > rtabmap::util3d::convertPolygonsFromPCL ( const std::vector< std::vector< pcl::Vertices > > &  polygons)

Definition at line 1210 of file util3d_surface.cpp.

std::vector< pcl::Vertices > rtabmap::util3d::convertPolygonsToPCL ( const std::vector< std::vector< unsigned int > > &  polygons)

Definition at line 1223 of file util3d_surface.cpp.

std::vector< std::vector< pcl::Vertices > > rtabmap::util3d::convertPolygonsToPCL ( const std::vector< std::vector< std::vector< unsigned int > > > &  tex_polygons)

Definition at line 1232 of file util3d_surface.cpp.

int rtabmap::util3d::countUniquePairs ( const std::multimap< int, pcl::PointXYZ > &  wordsA,
const std::multimap< int, pcl::PointXYZ > &  wordsB 
)

Definition at line 208 of file util3d_correspondences.cpp.

cv::Mat rtabmap::util3d::create2DMap ( const std::map< int, Transform > &  poses,
const std::map< int, std::pair< cv::Mat, cv::Mat > > &  scans,
const std::map< int, cv::Point3f > &  viewpoints,
float  cellSize,
bool  unknownSpaceFilled,
float &  xMin,
float &  yMin,
float  minMapSize,
float  scanMaxRange 
)

Create 2d Occupancy grid (CV_8S) -1 = unknown 0 = empty space 100 = obstacle

Parameters
poses
scans
viewpoints
cellSizem
unknownSpaceFilledif false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle)
xMin
yMin
minMapSizeminimum map size in meters
scanMaxRangelaser scan maximum range, would be set if unknownSpaceFilled=true

Definition at line 566 of file util3d_mapping.cpp.

cv::Mat 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,
float  scanMaxRange 
)

Create 2d Occupancy grid (CV_8S) -1 = unknown 0 = empty space 100 = obstacle

Parameters
poses
scans
cellSizem
unknownSpaceFilledif false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle)
xMin
yMin
minMapSizeminimum map size in meters
scanMaxRangelaser scan maximum range, would be set if unknownSpaceFilled=true

Definition at line 499 of file util3d_mapping.cpp.

cv::Mat rtabmap::util3d::create2DMap ( const std::map< int, Transform > &  poses,
const std::map< int, pcl::PointCloud< pcl::PointXYZ >::Ptr > &  scans,
const std::map< int, cv::Point3f > &  viewpoints,
float  cellSize,
bool  unknownSpaceFilled,
float &  xMin,
float &  yMin,
float  minMapSize,
float  scanMaxRange 
)

Definition at line 525 of file util3d_mapping.cpp.

cv::Mat rtabmap::util3d::create2DMapFromOccupancyLocalMaps ( const std::map< int, Transform > &  posesIn,
const std::map< int, std::pair< cv::Mat, cv::Mat > > &  occupancy,
float  cellSize,
float &  xMin,
float &  yMin,
float  minMapSize,
bool  erode,
float  footprintRadius 
)

Create 2d Occupancy grid (CV_8S) from 2d occupancy -1 = unknown 0 = empty space 100 = obstacle

Parameters
poses
occupancy<empty, occupied>
cellSizem
xMin
yMin
minMapSizeminimum width (m)
erode

Definition at line 160 of file util3d_mapping.cpp.

pcl::PolygonMesh::Ptr 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 = true 
)

Definition at line 540 of file util3d_surface.cpp.

void rtabmap::util3d::createPolygonIndexes ( const std::vector< pcl::Vertices > &  polygons,
int  cloudSize,
std::vector< std::set< int > > &  neighborPolygons,
std::vector< std::set< int > > &  vertexPolygons 
)

Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons.

Parameters
polygonsthe polygons to be indexed.
cloudSizethe size of the cloud of the corresponding mesh to polygons (must be at least as high as the highest vertex value contained in the polygons).
neighborPolygonsreturned index from polygons to neighbor polygons (index size = polygons size).
vertexPolygonsreturned index from vertices to polygons (index size = cloudSize).

Definition at line 79 of file util3d_surface.cpp.

pcl::texture_mapping::CameraVector rtabmap::util3d::createTextureCameras ( const std::map< int, Transform > &  poses,
const std::map< int, std::vector< CameraModel > > &  cameraModels,
const std::map< int, cv::Mat > &  cameraDepths,
const std::vector< float > &  roiRatios 
)

Definition at line 583 of file util3d_surface.cpp.

pcl::TextureMesh::Ptr rtabmap::util3d::createTextureMesh ( const pcl::PolygonMesh::Ptr &  mesh,
const std::map< int, Transform > &  poses,
const std::map< int, CameraModel > &  cameraModels,
const std::map< int, cv::Mat > &  cameraDepths,
float  maxDistance = 0.0f,
float  maxDepthError = 0.0f,
float  maxAngle = 0.0f,
int  minClusterSize = 50,
const std::vector< float > &  roiRatios = std::vector<float>(),
const ProgressState state = 0,
std::vector< std::map< int, pcl::PointXY > > *  vertexToPixels = 0 
)

Definition at line 656 of file util3d_surface.cpp.

pcl::TextureMesh::Ptr rtabmap::util3d::createTextureMesh ( const pcl::PolygonMesh::Ptr &  mesh,
const std::map< int, Transform > &  poses,
const std::map< int, std::vector< CameraModel > > &  cameraModels,
const std::map< int, cv::Mat > &  cameraDepths,
float  maxDistance = 0.0f,
float  maxDepthError = 0.0f,
float  maxAngle = 0.0f,
int  minClusterSize = 50,
const std::vector< float > &  roiRatios = std::vector<float>(),
const ProgressState state = 0,
std::vector< std::map< int, pcl::PointXY > > *  vertexToPixels = 0 
)

Definition at line 691 of file util3d_surface.cpp.

pcl::IndicesPtr rtabmap::util3d::cropBox ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform = Transform::getIdentity(),
bool  negative = false 
)

Definition at line 603 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::cropBox ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform = Transform::getIdentity(),
bool  negative = false 
)

Definition at line 607 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::cropBox ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform = Transform::getIdentity(),
bool  negative = false 
)

Definition at line 611 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::cropBox ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform = Transform::getIdentity(),
bool  negative = false 
)

Definition at line 615 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::cropBox ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform = Transform::getIdentity(),
bool  negative = false 
)

Definition at line 643 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::cropBox ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform = Transform::getIdentity(),
bool  negative = false 
)

Definition at line 647 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::cropBox ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform = Transform::getIdentity(),
bool  negative = false 
)

Definition at line 651 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::cropBox ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform = Transform::getIdentity(),
bool  negative = false 
)

Definition at line 655 of file util3d_filtering.cpp.

template<typename PointT >
pcl::IndicesPtr rtabmap::util3d::cropBoxImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform,
bool  negative 
)

Definition at line 578 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud<PointT>::Ptr rtabmap::util3d::cropBoxImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const Eigen::Vector4f &  min,
const Eigen::Vector4f &  max,
const Transform transform,
bool  negative 
)

Definition at line 621 of file util3d_filtering.cpp.

template<typename pointRGBT >
void rtabmap::util3d::denseMeshPostProcessing ( pcl::PolygonMeshPtr &  mesh,
float  meshDecimationFactor,
int  maximumPolygons,
const typename pcl::PointCloud< pointRGBT >::Ptr &  cloud,
float  transferColorRadius,
bool  coloredOutput,
bool  cleanMesh,
int  minClusterSize,
ProgressState progressState 
)

Definition at line 49 of file util3d_surface.hpp.

cv::Mat rtabmap::util3d::depthFromCloud ( const pcl::PointCloud< pcl::PointXYZRGBA > &  cloud,
float &  fx,
float &  fy,
bool  depth16U = true 
)

Definition at line 76 of file util3d.cpp.

LaserScan rtabmap::util3d::downsample ( const LaserScan cloud,
int  step 
)

Definition at line 317 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::downsample ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
int  step 
)

Definition at line 364 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::downsample ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
int  step 
)

Definition at line 368 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::downsample ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
int  step 
)

Definition at line 372 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::downsample ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
int  step 
)

Definition at line 376 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud<PointT>::Ptr rtabmap::util3d::downsampleImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
int  step 
)

Definition at line 341 of file util3d_filtering.cpp.

cv::Mat rtabmap::util3d::erodeMap ( const cv::Mat &  map)

Definition at line 981 of file util3d_mapping.cpp.

Transform rtabmap::util3d::estimateMotion3DTo2D ( const std::map< int, cv::Point3f > &  words3A,
const std::map< int, cv::KeyPoint > &  words2B,
const CameraModel cameraModel,
int  minInliers = 10,
int  iterations = 100,
double  reprojError = 5.,
int  flagsPnP = 0,
int  pnpRefineIterations = 1,
const Transform guess = Transform::getIdentity(),
const std::map< int, cv::Point3f > &  words3B = std::map<int, cv::Point3f>(),
cv::Mat *  covariance = 0,
std::vector< int > *  matchesOut = 0,
std::vector< int > *  inliersOut = 0 
)

Definition at line 48 of file util3d_motion_estimation.cpp.

Transform rtabmap::util3d::estimateMotion3DTo3D ( const std::map< int, cv::Point3f > &  words3A,
const std::map< int, cv::Point3f > &  words3B,
int  minInliers = 10,
double  inliersDistance = 0.1,
int  iterations = 100,
int  refineIterations = 5,
cv::Mat *  covariance = 0,
std::vector< int > *  matchesOut = 0,
std::vector< int > *  inliersOut = 0 
)

Definition at line 212 of file util3d_motion_estimation.cpp.

std::vector< pcl::IndicesPtr > rtabmap::util3d::extractClusters ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
float  clusterTolerance,
int  minClusterSize,
int  maxClusterSize = std::numeric_limits<int>::max(),
int *  biggestClusterIndex = 0 
)

For convenience.

Definition at line 1518 of file util3d_filtering.cpp.

std::vector< pcl::IndicesPtr > rtabmap::util3d::extractClusters ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
float  clusterTolerance,
int  minClusterSize,
int  maxClusterSize = std::numeric_limits<int>::max(),
int *  biggestClusterIndex = 0 
)

Definition at line 1528 of file util3d_filtering.cpp.

std::vector< pcl::IndicesPtr > rtabmap::util3d::extractClusters ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  clusterTolerance,
int  minClusterSize,
int  maxClusterSize = std::numeric_limits<int>::max(),
int *  biggestClusterIndex = 0 
)

Wrapper of the pcl::EuclideanClusterExtraction class.

Extract all clusters from a point cloud given a maximum cluster distance tolerance.

Parameters
cloudthe input cloud.
indicesthe input indices of the cloud to process, if empty, all points in the cloud are processed.
clusterTolerancethe cluster distance tolerance (see pcl::EuclideanClusterExtraction).
minClusterSizeminimum size of the clusters to return (see pcl::EuclideanClusterExtraction).
maxClusterSizemaximum size of the clusters to return (see pcl::EuclideanClusterExtraction).
biggestClusterIndexthe index of the biggest cluster, if the clusters are empty, a negative index is set.
Returns
the indices of each cluster found.

Definition at line 1590 of file util3d_filtering.cpp.

std::vector< pcl::IndicesPtr > rtabmap::util3d::extractClusters ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  clusterTolerance,
int  minClusterSize,
int  maxClusterSize = std::numeric_limits<int>::max(),
int *  biggestClusterIndex = 0 
)

Definition at line 1600 of file util3d_filtering.cpp.

std::vector< pcl::IndicesPtr > rtabmap::util3d::extractClusters ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  clusterTolerance,
int  minClusterSize,
int  maxClusterSize = std::numeric_limits<int>::max(),
int *  biggestClusterIndex = 0 
)

Definition at line 1610 of file util3d_filtering.cpp.

std::vector< pcl::IndicesPtr > rtabmap::util3d::extractClusters ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  clusterTolerance,
int  minClusterSize,
int  maxClusterSize = std::numeric_limits<int>::max(),
int *  biggestClusterIndex = 0 
)

Definition at line 1620 of file util3d_filtering.cpp.

template<typename PointT >
std::vector<pcl::IndicesPtr> rtabmap::util3d::extractClustersImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  clusterTolerance,
int  minClusterSize,
int  maxClusterSize,
int *  biggestClusterIndex 
)

Definition at line 1540 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::extractIndices ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
bool  negative 
)

Definition at line 1646 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::extractIndices ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
bool  negative 
)

Definition at line 1650 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::extractIndices ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
bool  negative 
)

Definition at line 1654 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::extractIndices ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
bool  negative 
)

Definition at line 1658 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::extractIndices ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
bool  negative,
bool  keepOrganized 
)

Definition at line 1679 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::extractIndices ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
bool  negative,
bool  keepOrganized 
)

Definition at line 1683 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::extractIndices ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
bool  negative,
bool  keepOrganized 
)

Definition at line 1692 of file util3d_filtering.cpp.

template<typename PointT >
pcl::IndicesPtr rtabmap::util3d::extractIndicesImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
bool  negative 
)

Definition at line 1632 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud<PointT>::Ptr rtabmap::util3d::extractIndicesImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
bool  negative,
bool  keepOrganized 
)

Definition at line 1664 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::extractPlane ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
float  distanceThreshold,
int  maxIterations = 100,
pcl::ModelCoefficients *  coefficientsOut = 0 
)

Definition at line 1697 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::extractPlane ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  distanceThreshold,
int  maxIterations = 100,
pcl::ModelCoefficients *  coefficientsOut = 0 
)

Definition at line 1707 of file util3d_filtering.cpp.

void 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 
)

Definition at line 43 of file util3d_correspondences.cpp.

void 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 
)

Definition at line 136 of file util3d_correspondences.cpp.

void 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 
)

Definition at line 189 of file util3d_correspondences.cpp.

void 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 
)

Definition at line 198 of file util3d_correspondences.cpp.

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 
)
inline

Definition at line 167 of file util3d_correspondences.cpp.

void 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 
)

Definition at line 64 of file util3d_correspondences.cpp.

void rtabmap::util3d::fillProjectedCloudHoles ( cv::Mat &  depthRegistered,
bool  verticalDirection,
bool  fillToBorder 
)

Definition at line 2952 of file util3d.cpp.

std::vector< pcl::Vertices > rtabmap::util3d::filterCloseVerticesFromMesh ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr  cloud,
const std::vector< pcl::Vertices > &  polygons,
float  radius,
float  angle,
bool  keepLatestInRadius 
)

Definition at line 443 of file util3d_surface.cpp.

std::vector< pcl::Vertices > rtabmap::util3d::filterInvalidPolygons ( const std::vector< pcl::Vertices > &  polygons)

Definition at line 516 of file util3d_surface.cpp.

void rtabmap::util3d::filterMaxDepth ( pcl::PointCloud< pcl::PointXYZ > &  inliers1,
pcl::PointCloud< pcl::PointXYZ > &  inliers2,
float  maxDepth,
char  depthAxis,
bool  removeDuplicates 
)

Definition at line 225 of file util3d_correspondences.cpp.

std::vector< int > rtabmap::util3d::filterNaNPointsFromMesh ( const pcl::PointCloud< pcl::PointXYZRGB > &  cloud,
const std::vector< pcl::Vertices > &  polygons,
pcl::PointCloud< pcl::PointXYZRGB > &  outputCloud,
std::vector< pcl::Vertices > &  outputPolygons 
)

Definition at line 400 of file util3d_surface.cpp.

std::vector< int > rtabmap::util3d::filterNotUsedVerticesFromMesh ( const pcl::PointCloud< pcl::PointXYZRGBNormal > &  cloud,
const std::vector< pcl::Vertices > &  polygons,
pcl::PointCloud< pcl::PointXYZRGBNormal > &  outputCloud,
std::vector< pcl::Vertices > &  outputPolygons 
)

Definition at line 320 of file util3d_surface.cpp.

std::vector< int > rtabmap::util3d::filterNotUsedVerticesFromMesh ( const pcl::PointCloud< pcl::PointXYZRGB > &  cloud,
const std::vector< pcl::Vertices > &  polygons,
pcl::PointCloud< pcl::PointXYZRGB > &  outputCloud,
std::vector< pcl::Vertices > &  outputPolygons 
)

Definition at line 360 of file util3d_surface.cpp.

void 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 
)

if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)] realPairsCount = 5

Definition at line 299 of file util3d_correspondences.cpp.

void rtabmap::util3d::findCorrespondences ( const std::multimap< int, cv::Point3f > &  words1,
const std::multimap< int, cv::Point3f > &  words2,
std::vector< cv::Point3f > &  inliers1,
std::vector< cv::Point3f > &  inliers2,
float  maxDepth,
std::vector< int > *  uniqueCorrespondences = 0 
)

Definition at line 317 of file util3d_correspondences.cpp.

void rtabmap::util3d::findCorrespondences ( const std::map< int, cv::Point3f > &  words1,
const std::map< int, cv::Point3f > &  words2,
std::vector< cv::Point3f > &  inliers1,
std::vector< cv::Point3f > &  inliers2,
float  maxDepth,
std::vector< int > *  correspondences = 0 
)

Definition at line 363 of file util3d_correspondences.cpp.

void rtabmap::util3d::fixTextureMeshForVisualization ( pcl::TextureMesh &  textureMesh)

Definition at line 2093 of file util3d_surface.cpp.

pcl::IndicesPtr rtabmap::util3d::frustumFiltering ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Transform cameraPose,
float  horizontalFOV,
float  verticalFOV,
float  nearClipPlaneDistance,
float  farClipPlaneDistance,
bool  negative = false 
)

Definition at line 693 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::frustumFiltering ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const Transform cameraPose,
float  horizontalFOV,
float  verticalFOV,
float  nearClipPlaneDistance,
float  farClipPlaneDistance,
bool  negative = false 
)

Definition at line 726 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::frustumFiltering ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const Transform cameraPose,
float  horizontalFOV,
float  verticalFOV,
float  nearClipPlaneDistance,
float  farClipPlaneDistance,
bool  negative = false 
)

Definition at line 730 of file util3d_filtering.cpp.

template<typename PointT >
pcl::IndicesPtr rtabmap::util3d::frustumFilteringImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Transform cameraPose,
float  horizontalFOV,
float  verticalFOV,
float  nearClipPlaneDistance,
float  farClipPlaneDistance,
bool  negative 
)

Definition at line 661 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud<PointT>::Ptr rtabmap::util3d::frustumFilteringImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const Transform cameraPose,
float  horizontalFOV,
float  verticalFOV,
float  nearClipPlaneDistance,
float  farClipPlaneDistance,
bool  negative 
)

Definition at line 699 of file util3d_filtering.cpp.

int rtabmap::util3d::gcd ( int  a,
int  b 
)

Definition at line 1049 of file util3d_surface.cpp.

std::vector< cv::Point3f > rtabmap::util3d::generateKeypoints3DDepth ( const std::vector< cv::KeyPoint > &  keypoints,
const cv::Mat &  depth,
const CameraModel cameraModel,
float  minDepth = 0,
float  maxDepth = 0 
)

Definition at line 51 of file util3d_features.cpp.

std::vector< cv::Point3f > rtabmap::util3d::generateKeypoints3DDepth ( const std::vector< cv::KeyPoint > &  keypoints,
const cv::Mat &  depth,
const std::vector< CameraModel > &  cameraModels,
float  minDepth = 0,
float  maxDepth = 0 
)

Definition at line 64 of file util3d_features.cpp.

std::vector< cv::Point3f > rtabmap::util3d::generateKeypoints3DDisparity ( const std::vector< cv::KeyPoint > &  keypoints,
const cv::Mat &  disparity,
const StereoCameraModel stereoCameraModel,
float  minDepth = 0,
float  maxDepth = 0 
)

Definition at line 119 of file util3d_features.cpp.

std::vector< cv::Point3f > rtabmap::util3d::generateKeypoints3DStereo ( const std::vector< cv::Point2f > &  leftCorners,
const std::vector< cv::Point2f > &  rightCorners,
const StereoCameraModel model,
const std::vector< unsigned char > &  mask = std::vector<unsigned char>(),
float  minDepth = 0,
float  maxDepth = 0 
)

Definition at line 155 of file util3d_features.cpp.

std::map< int, cv::Point3f > rtabmap::util3d::generateWords3DMono ( const std::map< int, cv::KeyPoint > &  kpts,
const std::map< int, cv::KeyPoint > &  previousKpts,
const CameraModel cameraModel,
Transform cameraTransform,
int  pnpIterations = 100,
float  pnpReprojError = 8.0f,
int  pnpFlags = 0,
int  pnpRefineIterations = 1,
float  ransacParam1 = 3.0f,
float  ransacParam2 = 0.99f,
const std::map< int, cv::Point3f > &  refGuess3D = std::map<int, cv::Point3f>(),
double *  variance = 0 
)

Definition at line 206 of file util3d_features.cpp.

int rtabmap::util3d::getCorrespondencesCount ( const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &  cloud_source,
const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &  cloud_target,
float  maxDistance 
)

Definition at line 275 of file util3d_correspondences.cpp.

void rtabmap::util3d::getMinMax3D ( const cv::Mat &  laserScan,
cv::Point3f &  min,
cv::Point3f &  max 
)

Definition at line 2593 of file util3d.cpp.

void rtabmap::util3d::getMinMax3D ( const cv::Mat &  laserScan,
pcl::PointXYZ &  min,
pcl::PointXYZ &  max 
)

Definition at line 2620 of file util3d.cpp.

Transform 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,
pcl::PointCloud< pcl::PointXYZ > &  cloud_source_registered,
float  epsilon = 0.0f,
bool  icp2D = false 
)

Definition at line 335 of file util3d_registration.cpp.

Transform 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,
pcl::PointCloud< pcl::PointNormal > &  cloud_source_registered,
float  epsilon = 0.0f,
bool  icp2D = false 
)

Definition at line 373 of file util3d_registration.cpp.

bool rtabmap::util3d::isFinite ( const cv::Point3f &  pt)

Definition at line 3035 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScan2dFromPointCloud ( const pcl::PointCloud< pcl::PointXYZ > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 2051 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScan2dFromPointCloud ( const pcl::PointCloud< pcl::PointXYZI > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 2083 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScan2dFromPointCloud ( const pcl::PointCloud< pcl::PointNormal > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 2117 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScan2dFromPointCloud ( const pcl::PointCloud< pcl::PointXYZ > &  cloud,
const pcl::PointCloud< pcl::Normal > &  normals,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 2157 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScan2dFromPointCloud ( const pcl::PointCloud< pcl::PointXYZINormal > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 2201 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScan2dFromPointCloud ( const pcl::PointCloud< pcl::PointXYZI > &  cloud,
const pcl::PointCloud< pcl::Normal > &  normals,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 2243 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZ > 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() 
)

Simulate a laser scan rotating counterclockwise, using middle line of the depth image.

Definition at line 1197 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZ > rtabmap::util3d::laserScanFromDepthImages ( const cv::Mat &  depthImages,
const std::vector< CameraModel > &  cameraModels,
float  maxDepth,
float  minDepth 
)

Simulate a laser scan rotating counterclockwise, using middle line of the depth images. The last value of the scan is the most left value of the first depth image. The first value of the scan is the most right value of the last depth image.

Definition at line 1233 of file util3d.cpp.

LaserScan rtabmap::util3d::laserScanFromPointCloud ( const pcl::PCLPointCloud2 &  cloud,
bool  filterNaNs = true 
)

Definition at line 1266 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZ > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1493 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZ > &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1497 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointNormal > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1559 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointNormal > &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1563 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZ > &  cloud,
const pcl::PointCloud< pcl::Normal > &  normals,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1642 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZRGB > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1688 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZRGB > &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1693 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZI > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1759 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZI > &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1764 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZRGB > &  cloud,
const pcl::PointCloud< pcl::Normal > &  normals,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1828 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZRGBNormal > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1876 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZRGBNormal > &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1880 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZI > &  cloud,
const pcl::PointCloud< pcl::Normal > &  normals,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 1963 of file util3d.cpp.

cv::Mat rtabmap::util3d::laserScanFromPointCloud ( const pcl::PointCloud< pcl::PointXYZINormal > &  cloud,
const Transform transform = Transform(),
bool  filterNaNs = true 
)

Definition at line 2009 of file util3d.cpp.

pcl::PointXYZ rtabmap::util3d::laserScanToPoint ( const LaserScan laserScan,
int  index 
)

Definition at line 2433 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::laserScanToPointCloud ( const LaserScan laserScan,
const Transform transform = Transform() 
)

Definition at line 2328 of file util3d.cpp.

pcl::PCLPointCloud2::Ptr rtabmap::util3d::laserScanToPointCloud2 ( const LaserScan laserScan,
const Transform transform = Transform() 
)

Definition at line 2289 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZI >::Ptr rtabmap::util3d::laserScanToPointCloudI ( const LaserScan laserScan,
const Transform transform = Transform(),
float  intensity = 0.0f 
)

Definition at line 2381 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZINormal >::Ptr rtabmap::util3d::laserScanToPointCloudINormal ( const LaserScan laserScan,
const Transform transform = Transform(),
float  intensity = 0.0f 
)

Definition at line 2416 of file util3d.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::laserScanToPointCloudNormal ( const LaserScan laserScan,
const Transform transform = Transform() 
)

Definition at line 2346 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::laserScanToPointCloudRGB ( const LaserScan laserScan,
const Transform transform = Transform(),
unsigned char  r = 255,
unsigned char  g = 255,
unsigned char  b = 255 
)

Definition at line 2363 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::laserScanToPointCloudRGBNormal ( const LaserScan laserScan,
const Transform transform = Transform(),
unsigned char  r = 255,
unsigned char  g = 255,
unsigned char  b = 255 
)

Definition at line 2399 of file util3d.cpp.

pcl::PointXYZI rtabmap::util3d::laserScanToPointI ( const LaserScan laserScan,
int  index,
float  intensity 
)

Definition at line 2497 of file util3d.cpp.

pcl::PointXYZINormal rtabmap::util3d::laserScanToPointINormal ( const LaserScan laserScan,
int  index,
float  intensity 
)

Definition at line 2560 of file util3d.cpp.

pcl::PointNormal rtabmap::util3d::laserScanToPointNormal ( const LaserScan laserScan,
int  index 
)

Definition at line 2447 of file util3d.cpp.

pcl::PointXYZRGB rtabmap::util3d::laserScanToPointRGB ( const LaserScan laserScan,
int  index,
unsigned char  r = 255,
unsigned char  g = 255,
unsigned char  b = 255 
)

Definition at line 2468 of file util3d.cpp.

pcl::PointXYZRGBNormal rtabmap::util3d::laserScanToPointRGBNormal ( const LaserScan laserScan,
int  index,
unsigned char  r,
unsigned char  g,
unsigned char  b 
)

Definition at line 2522 of file util3d.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::loadBINCloud ( const std::string &  fileName)

Definition at line 3152 of file util3d.cpp.

pcl::PointCloud<pcl::PointXYZ>::Ptr rtabmap::util3d::loadBINCloud ( const std::string &  fileName,
int  dim 
)

Definition at line 3157 of file util3d.cpp.

cv::Mat rtabmap::util3d::loadBINScan ( const std::string &  fileName)

Assume KITTI velodyne format Return scan 4 channels (format=XYZI).

Definition at line 3129 of file util3d.cpp.

pcl::PointCloud<pcl::PointXYZ>::Ptr rtabmap::util3d::loadCloud ( const std::string &  path,
const Transform transform,
int  downsampleStep,
float  voxelSize 
)

Definition at line 3184 of file util3d.cpp.

LaserScan rtabmap::util3d::loadScan ( const std::string &  path)

Definition at line 3162 of file util3d.cpp.

cv::Mat rtabmap::util3d::mergeTextures ( pcl::TextureMesh &  mesh,
const std::map< int, cv::Mat > &  images,
const std::map< int, CameraModel > &  calibrations,
const Memory memory = 0,
const DBDriver dbDriver = 0,
int  textureSize = 4096,
int  textureCount = 1,
const std::vector< std::map< int, pcl::PointXY > > &  vertexToPixels = std::vector<std::map<int, pcl::PointXY> >(),
bool  gainCompensation = true,
float  gainBeta = 10.0f,
bool  gainRGB = true,
bool  blending = true,
int  blendingDecimation = 0,
int  brightnessContrastRatioLow = 0,
int  brightnessContrastRatioHigh = 0,
bool  exposureFusion = false,
const ProgressState state = 0 
)

Merge all textures in the mesh into "textureCount" textures of size "textureSize".

Returns
merged textures corresponding to new materials set in TextureMesh (height=textureSize, width=textureSize*materials)

Definition at line 1412 of file util3d_surface.cpp.

cv::Mat rtabmap::util3d::mergeTextures ( pcl::TextureMesh &  mesh,
const std::map< int, cv::Mat > &  images,
const std::map< int, std::vector< CameraModel > > &  calibrations,
const Memory memory = 0,
const DBDriver dbDriver = 0,
int  textureSize = 4096,
int  textureCount = 1,
const std::vector< std::map< int, pcl::PointXY > > &  vertexToPixels = std::vector<std::map<int, pcl::PointXY> >(),
bool  gainCompensation = true,
float  gainBeta = 10.0f,
bool  gainRGB = true,
bool  blending = true,
int  blendingDecimation = 0,
int  brightnessContrastRatioLow = 0,
int  brightnessContrastRatioHigh = 0,
bool  exposureFusion = false,
const ProgressState state = 0 
)

Original code from OpenCV: GainCompensator

Original code from OpenCV: GainCompensator

Definition at line 1456 of file util3d_surface.cpp.

pcl::PolygonMesh::Ptr rtabmap::util3d::meshDecimation ( const pcl::PolygonMesh::Ptr &  mesh,
float  factor 
)

Definition at line 3008 of file util3d_surface.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::mls ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
float  searchRadius = 0.0f,
int  polygonialOrder = 2,
int  upsamplingMethod = 0,
float  upsamplingRadius = 0.0f,
float  upsamplingStep = 0.0f,
int  pointDensity = 0,
float  dilationVoxelSize = 1.0f,
int  dilationIterations = 0 
)

Definition at line 2734 of file util3d_surface.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::mls ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  searchRadius = 0.0f,
int  polygonialOrder = 2,
int  upsamplingMethod = 0,
float  upsamplingRadius = 0.0f,
float  upsamplingStep = 0.0f,
int  pointDensity = 0,
float  dilationVoxelSize = 1.0f,
int  dilationIterations = 0 
)

Definition at line 2758 of file util3d_surface.cpp.

pcl::IndicesPtr rtabmap::util3d::normalFiltering ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
float  angleMax,
const Eigen::Vector4f &  normal,
int  normalKSearch,
const Eigen::Vector4f &  viewpoint 
)

For convenience.

Definition at line 1351 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::normalFiltering ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
float  angleMax,
const Eigen::Vector4f &  normal,
int  normalKSearch,
const Eigen::Vector4f &  viewpoint 
)

Definition at line 1361 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::normalFiltering ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  angleMax,
const Eigen::Vector4f &  normal,
int  normalKSearch,
const Eigen::Vector4f &  viewpoint 
)

Given a normal and a maximum angle error, keep all points of the cloud respecting this normal.

The normals are computed using the radius search parameter (pcl::NormalEstimation class is used for this), then for each normal, the corresponding point is filtered if the angle (using pcl::getAngle3D()) with the normal specified by the user is larger than the maximum angle specified by the user.

Parameters
cloudthe input cloud.
indicesthe input indices of the cloud to process, if empty, all points in the cloud are processed.
angleMaxthe maximum angle.
normalthe normal to which each point's normal is compared.
normalKSearchnumber of neighbor points used for normal estimation (see pcl::NormalEstimation).
viewpointfrom which viewpoint the normals should be estimated (see pcl::NormalEstimation).
Returns
the indices of the points which respect the normal constraint.

Definition at line 1431 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::normalFiltering ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  angleMax,
const Eigen::Vector4f &  normal,
int  normalKSearch,
const Eigen::Vector4f &  viewpoint 
)

Definition at line 1442 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::normalFiltering ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  angleMax,
const Eigen::Vector4f &  normal,
int  normalKSearch,
const Eigen::Vector4f &  viewpoint 
)

Definition at line 1497 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::normalFiltering ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  angleMax,
const Eigen::Vector4f &  normal,
int  normalKSearch,
const Eigen::Vector4f &  viewpoint 
)

Definition at line 1507 of file util3d_filtering.cpp.

template<typename PointT >
pcl::IndicesPtr rtabmap::util3d::normalFilteringImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  angleMax,
const Eigen::Vector4f &  normal,
int  normalKSearch,
const Eigen::Vector4f &  viewpoint 
)

Definition at line 1374 of file util3d_filtering.cpp.

template<typename PointT >
pcl::IndicesPtr rtabmap::util3d::normalFilteringImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  angleMax,
const Eigen::Vector4f &  normal 
)

Definition at line 1454 of file util3d_filtering.cpp.

template<typename pointT >
std::vector<pcl::Vertices> rtabmap::util3d::normalizePolygonsSide ( const typename pcl::PointCloud< pointT > &  cloud,
const std::vector< pcl::Vertices > &  polygons,
const pcl::PointXYZ &  viewPoint 
)

Definition at line 19 of file util3d_surface.hpp.

template<typename pointT >
std::vector<pcl::Vertices> rtabmap::util3d::normalizePolygonsSide ( const pcl::PointCloud< pointT > &  cloud,
const std::vector< pcl::Vertices > &  polygons,
const pcl::PointXYZ &  viewPoint = pcl::PointXYZ(0, 0, 0) 
)
template<typename PointT >
void rtabmap::util3d::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 at line 278 of file util3d_mapping.hpp.

template<typename PointT >
void rtabmap::util3d::occupancy2DFromCloud3D ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
cv::Mat &  ground,
cv::Mat &  obstacles,
float  cellSize,
float  groundNormalAngle,
int  minClusterSize,
bool  segmentFlatObstacles,
float  maxGroundHeight 
)

Definition at line 317 of file util3d_mapping.hpp.

template<typename PointT >
void rtabmap::util3d::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 at line 203 of file util3d_mapping.hpp.

template<typename PointT >
void rtabmap::util3d::occupancy2DFromGroundObstacles ( const typename pcl::PointCloud< PointT >::Ptr &  groundCloud,
const typename pcl::PointCloud< PointT >::Ptr &  obstaclesCloud,
cv::Mat &  ground,
cv::Mat &  obstacles,
float  cellSize 
)

Definition at line 233 of file util3d_mapping.hpp.

void rtabmap::util3d::occupancy2DFromLaserScan ( const cv::Mat &  scan,
cv::Mat &  empty,
cv::Mat &  occupied,
float  cellSize,
bool  unknownSpaceFilled,
float  scanMaxRange 
)

Definition at line 50 of file util3d_mapping.cpp.

void rtabmap::util3d::occupancy2DFromLaserScan ( const cv::Mat &  scanHit,
const cv::Mat &  scanNoHit,
const cv::Point3f &  viewpoint,
cv::Mat &  empty,
cv::Mat &  occupied,
float  cellSize,
bool  unknownSpaceFilled = false,
float  scanMaxRange = 0.0f 
)

Definition at line 82 of file util3d_mapping.cpp.

void rtabmap::util3d::occupancy2DFromLaserScan ( const cv::Mat &  scan,
const cv::Point3f &  viewpoint,
cv::Mat &  empty,
cv::Mat &  occupied,
float  cellSize,
bool  unknownSpaceFilled,
float  scanMaxRange 
)

Definition at line 70 of file util3d_mapping.cpp.

std::vector< pcl::Vertices > rtabmap::util3d::organizedFastMesh ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
double  angleTolerance,
bool  quad,
int  trianglePixelSize,
const Eigen::Vector3f &  viewpoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 153 of file util3d_surface.cpp.

std::vector< pcl::Vertices > rtabmap::util3d::organizedFastMesh ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
double  angleTolerance = M_PI/16,
bool  quad = true,
int  trianglePixelSize = 2,
const Eigen::Vector3f &  viewpoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 191 of file util3d_surface.cpp.

std::vector< pcl::Vertices > rtabmap::util3d::organizedFastMesh ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
double  angleTolerance = M_PI/16,
bool  quad = true,
int  trianglePixelSize = 2,
const Eigen::Vector3f &  viewpoint = Eigen::Vector3f(0,0,0) 
)

Definition at line 229 of file util3d_surface.cpp.

pcl::IndicesPtr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 507 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 511 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 515 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 519 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 523 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 527 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 552 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 556 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZI >::Ptr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 560 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 564 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 568 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZINormal >::Ptr rtabmap::util3d::passThrough ( const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &  cloud,
const std::string &  axis,
float  min,
float  max,
bool  negative = false 
)

Definition at line 572 of file util3d_filtering.cpp.

template<typename PointT >
pcl::IndicesPtr rtabmap::util3d::passThroughImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const std::string &  axis,
float  min,
float  max,
bool  negative 
)

Definition at line 485 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud<PointT>::Ptr rtabmap::util3d::passThroughImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const std::string &  axis,
float  min,
float  max,
bool  negative 
)

Definition at line 533 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud< PointT >::Ptr rtabmap::util3d::projectCloudOnXYPlane ( const typename pcl::PointCloud< PointT > &  cloud)

Definition at line 41 of file util3d_mapping.hpp.

cv::Mat rtabmap::util3d::projectCloudToCamera ( const cv::Size &  imageSize,
const cv::Mat &  cameraMatrixK,
const cv::Mat &  laserScan,
const rtabmap::Transform cameraTransform 
)

Definition at line 2672 of file util3d.cpp.

cv::Mat rtabmap::util3d::projectCloudToCamera ( const cv::Size &  imageSize,
const cv::Mat &  cameraMatrixK,
const pcl::PointCloud< pcl::PointXYZ >::Ptr  laserScan,
const rtabmap::Transform cameraTransform 
)

Definition at line 2757 of file util3d.cpp.

cv::Mat rtabmap::util3d::projectCloudToCamera ( const cv::Size &  imageSize,
const cv::Mat &  cameraMatrixK,
const pcl::PCLPointCloud2::Ptr  laserScan,
const rtabmap::Transform cameraTransform 
)

Definition at line 2824 of file util3d.cpp.

pcl::PointXYZ 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 
)

Definition at line 213 of file util3d.cpp.

Eigen::Vector3f rtabmap::util3d::projectDepthTo3DRay ( const cv::Size &  imageSize,
float  x,
float  y,
float  cx,
float  cy,
float  fx,
float  fy 
)

Definition at line 244 of file util3d.cpp.

cv::Point3f rtabmap::util3d::projectDisparityTo3D ( const cv::Point2f &  pt,
float  disparity,
const StereoCameraModel model 
)

Definition at line 2633 of file util3d.cpp.

cv::Point3f rtabmap::util3d::projectDisparityTo3D ( const cv::Point2f &  pt,
const cv::Mat &  disparity,
const StereoCameraModel model 
)

Definition at line 2653 of file util3d.cpp.

pcl::IndicesPtr rtabmap::util3d::radiusFiltering ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
float  radiusSearch,
int  minNeighborsInRadius 
)

For convenience.

Definition at line 775 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::radiusFiltering ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
float  radiusSearch,
int  minNeighborsInRadius 
)

Definition at line 780 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::radiusFiltering ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
float  radiusSearch,
int  minNeighborsInRadius 
)

Definition at line 785 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::radiusFiltering ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
float  radiusSearch,
int  minNeighborsInRadius 
)

Definition at line 790 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::radiusFiltering ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  radiusSearch,
int  minNeighborsInRadius 
)

Wrapper of the pcl::RadiusOutlierRemoval class.

Points in the cloud which have less than a minimum of neighbors in the specified radius are filtered.

Parameters
cloudthe input cloud.
indicesthe input indices of the cloud to check, if empty, all points in the cloud are checked.
radiusSearchthe radius in meter.
minNeighborsInRadiusthe minimum of neighbors to keep the point.
Returns
the indices of the points satisfying the parameters.

Definition at line 843 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::radiusFiltering ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  radiusSearch,
int  minNeighborsInRadius 
)

Definition at line 847 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::radiusFiltering ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  radiusSearch,
int  minNeighborsInRadius 
)

Definition at line 851 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::radiusFiltering ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  radiusSearch,
int  minNeighborsInRadius 
)

Definition at line 855 of file util3d_filtering.cpp.

template<typename PointT >
pcl::IndicesPtr rtabmap::util3d::radiusFilteringImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  radiusSearch,
int  minNeighborsInRadius 
)

Definition at line 797 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::randomSampling ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
int  samples 
)

Definition at line 475 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::randomSampling ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
int  samples 
)

Definition at line 479 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud<PointT>::Ptr rtabmap::util3d::randomSamplingImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
int  samples 
)

Definition at line 464 of file util3d_filtering.cpp.

LaserScan rtabmap::util3d::rangeFiltering ( const LaserScan scan,
float  rangeMin,
float  rangeMax 
)

Definition at line 270 of file util3d_filtering.cpp.

void rtabmap::util3d::rayTrace ( const cv::Point2i &  start,
const cv::Point2i &  end,
cv::Mat &  grid,
bool  stopOnObstacle 
)

Definition at line 815 of file util3d_mapping.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::removeNaNFromPointCloud ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud)

Definition at line 745 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::removeNaNFromPointCloud ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud)

Definition at line 749 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud<PointT>::Ptr rtabmap::util3d::removeNaNFromPointCloudImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud)

Definition at line 737 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::removeNaNNormalsFromPointCloud ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud)

Definition at line 763 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::removeNaNNormalsFromPointCloud ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud)

Definition at line 768 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud<PointT>::Ptr rtabmap::util3d::removeNaNNormalsFromPointCloudImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud)

Definition at line 755 of file util3d_filtering.cpp.

void 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 
)

Definition at line 136 of file util3d.cpp.

cv::Mat RTABMAP_EXP rtabmap::util3d::rgbFromCloud ( const pcl::PointCloud< pcl::PointXYZRGBA > &  cloud,
bool  bgrOrder = true 
)
rtabmap::util3d::RTABMAP_DEPRECATED ( void RTABMAP_EXP   occupancy2DFromLaserScanconst cv::Mat &scan, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f,
"Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base."   
)
rtabmap::util3d::RTABMAP_DEPRECATED ( void RTABMAP_EXP   occupancy2DFromLaserScanconst cv::Mat &scan, const cv::Point3f &viewpoint, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f,
"Use interface with scanHit/scanNoHit parameters: scanNoHit set to null matrix has the same functionality than this method."   
)
rtabmap::util3d::RTABMAP_DEPRECATED ( cv::Mat RTABMAP_EXP   create2DMapconst 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,
"Use interface with \"viewpoints\" parameter to make sure the ray tracing origin is from the sensor and not the base."   
)
rtabmap::util3d::RTABMAP_DEPRECATED ( pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP   cloudFromDepthconst 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,
"Use cloudFromDepth with CameraModel interface."   
)
rtabmap::util3d::RTABMAP_DEPRECATED ( cv::Mat RTABMAP_EXP   create2DMapconst std::map< int, Transform > &poses, const std::map< int, pcl::PointCloud< pcl::PointXYZ >::Ptr > &scans, const std::map< int, cv::Point3f > &viewpoints, float cellSize, bool unknownSpaceFilled, float &xMin, float &yMin, float minMapSize=0.0f, float scanMaxRange=0.0f,
"Use interface with cv::Mat scans."   
)
rtabmap::util3d::RTABMAP_DEPRECATED ( pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP   cloudFromDepthRGBconst 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,
"Use cloudFromDepthRGB with CameraModel interface."   
)
rtabmap::util3d::RTABMAP_DEPRECATED ( pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP   loadBINCloudconst std::string &fileName, int dim,
"Use interface without dim argument."   
)
rtabmap::util3d::RTABMAP_DEPRECATED ( pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP   loadCloudconst std::string &path, const Transform &transform=Transform::getIdentity(), int downsampleStep=1, float voxelSize=0.0f,
"Use loadScan() instead."   
)
void rtabmap::util3d::savePCDWords ( const std::string &  fileName,
const std::multimap< int, pcl::PointXYZ > &  words,
const Transform transform = Transform::getIdentity() 
)

Definition at line 3092 of file util3d.cpp.

void rtabmap::util3d::savePCDWords ( const std::string &  fileName,
const std::multimap< int, cv::Point3f > &  words,
const Transform transform = Transform::getIdentity() 
)

Definition at line 3110 of file util3d.cpp.

template<typename PointT >
void rtabmap::util3d::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 
)

Definition at line 54 of file util3d_mapping.hpp.

template<typename PointT >
void rtabmap::util3d::segmentObstaclesFromGround ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
pcl::IndicesPtr &  ground,
pcl::IndicesPtr &  obstacles,
int  normalKSearch,
float  groundNormalAngle,
float  clusterRadius,
int  minClusterSize,
bool  segmentFlatObstacles = false,
float  maxGroundHeight = 0.0f,
pcl::IndicesPtr *  flatObstacles = 0,
const Eigen::Vector4f &  viewPoint = Eigen::Vector4f(0, 0, 100, 0) 
)
template<typename PointT >
void rtabmap::util3d::segmentObstaclesFromGround ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
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 
)

Definition at line 173 of file util3d_mapping.hpp.

void rtabmap::util3d::solvePnPRansac ( const std::vector< cv::Point3f > &  objectPoints,
const std::vector< cv::Point2f > &  imagePoints,
const cv::Mat &  cameraMatrix,
const cv::Mat &  distCoeffs,
cv::Mat &  rvec,
cv::Mat &  tvec,
bool  useExtrinsicGuess,
int  iterationsCount,
float  reprojectionError,
int  minInliersCount,
std::vector< int > &  inliers,
int  flags,
int  refineIterations = 1,
float  refineSigma = 3.0f 
)

Definition at line 325 of file util3d_motion_estimation.cpp.

double rtabmap::util3d::sqr ( uchar  v)

Definition at line 1407 of file util3d_surface.cpp.

pcl::IndicesPtr rtabmap::util3d::subtractAdaptiveFiltering ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  substractCloud,
const pcl::IndicesPtr &  substractIndices,
float  radiusSearchRatio = 0.01,
int  minNeighborsInRadius = 1,
const Eigen::Vector3f &  viewpoint = Eigen::Vector3f(0,0,0) 
)

Subtract a cloud from another one using radius filtering.

Parameters
cloudthe input cloud.
indicesthe input indices of the cloud to check, if empty, all points in the cloud are checked.
cloudthe input cloud to subtract.
indicesthe input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
radiusSearchRatiothe ratio used to compute the radius at different distances (e.g., a ratio of 0.1 at 4 m results in a radius of 4 cm).
Returns
the indices of the points satisfying the parameters.

Definition at line 1126 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::subtractAdaptiveFiltering ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  substractCloud,
const pcl::IndicesPtr &  substractIndices,
float  radiusSearchRatio = 0.01,
float  maxAngle = M_PI/4.0f,
int  minNeighborsInRadius = 1,
const Eigen::Vector3f &  viewpoint = Eigen::Vector3f(0,0,0) 
)

Subtract a cloud from another one using radius filtering.

Parameters
cloudthe input cloud.
indicesthe input indices of the cloud to check, if empty, all points in the cloud are checked.
cloudthe input cloud to subtract.
indicesthe input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
radiusSearchRatiothe ratio used to compute the radius at different distances (e.g., a ratio of 0.01 at 4 m results in a radius of 4 cm).
Returns
the indices of the points satisfying the parameters.

Definition at line 1205 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::subtractFiltering ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  substractCloud,
float  radiusSearch,
int  minNeighborsInRadius = 1 
)

For convenience.

Definition at line 860 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::subtractFiltering ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  substractCloud,
const pcl::IndicesPtr &  substractIndices,
float  radiusSearch,
int  minNeighborsInRadius = 1 
)

Subtract a cloud from another one using radius filtering.

Parameters
cloudthe input cloud.
indicesthe input indices of the cloud to check, if empty, all points in the cloud are checked.
cloudthe input cloud to subtract.
indicesthe input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
radiusSearchthe radius in meter.
Returns
the indices of the points satisfying the parameters.

Definition at line 936 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::subtractFiltering ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::PointCloud< pcl::PointNormal >::Ptr &  substractCloud,
float  radiusSearch,
float  maxAngle = M_PI/4.0f,
int  minNeighborsInRadius = 1 
)

For convenience.

Definition at line 947 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::subtractFiltering ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const pcl::PointCloud< pcl::PointNormal >::Ptr &  substractCloud,
const pcl::IndicesPtr &  substractIndices,
float  radiusSearch,
float  maxAngle = M_PI/4.0f,
int  minNeighborsInRadius = 1 
)

Subtract a cloud from another one using radius filtering.

Parameters
cloudthe input cloud.
indicesthe input indices of the cloud to check, if empty, all points in the cloud are checked.
cloudthe input cloud to subtract.
indicesthe input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
radiusSearchthe radius in meter.
Returns
the indices of the points satisfying the parameters.

Definition at line 1103 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::subtractFiltering ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  substractCloud,
float  radiusSearch,
float  maxAngle = M_PI/4.0f,
int  minNeighborsInRadius = 1 
)

For convenience.

Definition at line 960 of file util3d_filtering.cpp.

pcl::IndicesPtr rtabmap::util3d::subtractFiltering ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  substractCloud,
const pcl::IndicesPtr &  substractIndices,
float  radiusSearch,
float  maxAngle = M_PI/4.0f,
int  minNeighborsInRadius = 1 
)

Subtract a cloud from another one using radius filtering.

Parameters
cloudthe input cloud.
indicesthe input indices of the cloud to check, if empty, all points in the cloud are checked.
cloudthe input cloud to subtract.
indicesthe input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
radiusSearchthe radius in meter.
Returns
the indices of the points satisfying the parameters.

Definition at line 1114 of file util3d_filtering.cpp.

template<typename PointT >
pcl::IndicesPtr rtabmap::util3d::subtractFilteringImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const typename pcl::PointCloud< PointT >::Ptr &  substractCloud,
const pcl::IndicesPtr &  substractIndices,
float  radiusSearch,
int  minNeighborsInRadius 
)

Definition at line 874 of file util3d_filtering.cpp.

template<typename PointT >
pcl::IndicesPtr rtabmap::util3d::subtractFilteringImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const typename pcl::PointCloud< PointT >::Ptr &  substractCloud,
const pcl::IndicesPtr &  substractIndices,
float  radiusSearch,
float  maxAngle,
int  minNeighborsInRadius 
)

Definition at line 975 of file util3d_filtering.cpp.

Transform rtabmap::util3d::transformFromXYZCorrespondences ( const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &  cloud1,
const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &  cloud2,
double  inlierThreshold = 0.02,
int  iterations = 100,
int  refineModelIterations = 10,
double  refineModelSigma = 3.0,
std::vector< int > *  inliers = 0,
cv::Mat *  variance = 0 
)

Definition at line 63 of file util3d_registration.cpp.

Transform rtabmap::util3d::transformFromXYZCorrespondencesSVD ( const pcl::PointCloud< pcl::PointXYZ > &  cloud1,
const pcl::PointCloud< pcl::PointXYZ > &  cloud2 
)

Definition at line 50 of file util3d_registration.cpp.

LaserScan rtabmap::util3d::transformLaserScan ( const LaserScan laserScan,
const Transform transform 
)

Definition at line 39 of file util3d_transforms.cpp.

cv::Point3f rtabmap::util3d::transformPoint ( const cv::Point3f &  pt,
const Transform transform 
)

Definition at line 193 of file util3d_transforms.cpp.

pcl::PointXYZ rtabmap::util3d::transformPoint ( const pcl::PointXYZ &  pt,
const Transform transform 
)

Definition at line 203 of file util3d_transforms.cpp.

pcl::PointXYZI rtabmap::util3d::transformPoint ( const pcl::PointXYZI &  pt,
const Transform transform 
)

Definition at line 209 of file util3d_transforms.cpp.

pcl::PointXYZRGB rtabmap::util3d::transformPoint ( const pcl::PointXYZRGB &  pt,
const Transform transform 
)

Definition at line 215 of file util3d_transforms.cpp.

pcl::PointNormal rtabmap::util3d::transformPoint ( const pcl::PointNormal &  point,
const Transform transform 
)

Definition at line 223 of file util3d_transforms.cpp.

pcl::PointXYZRGBNormal rtabmap::util3d::transformPoint ( const pcl::PointXYZRGBNormal &  point,
const Transform transform 
)

Definition at line 240 of file util3d_transforms.cpp.

pcl::PointXYZINormal rtabmap::util3d::transformPoint ( const pcl::PointXYZINormal &  point,
const Transform transform 
)

Definition at line 259 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const Transform transform 
)

Definition at line 89 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZI >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
const Transform transform 
)

Definition at line 97 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const Transform transform 
)

Definition at line 105 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const Transform transform 
)

Definition at line 113 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const Transform transform 
)

Definition at line 121 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZINormal >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &  cloud,
const Transform transform 
)

Definition at line 129 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform 
)

Definition at line 138 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZI >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform 
)

Definition at line 147 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform 
)

Definition at line 156 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform 
)

Definition at line 165 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform 
)

Definition at line 174 of file util3d_transforms.cpp.

pcl::PointCloud< pcl::PointXYZINormal >::Ptr rtabmap::util3d::transformPointCloud ( const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
const Transform transform 
)

Definition at line 183 of file util3d_transforms.cpp.

pcl::PointCloud<pcl::PointXYZ>::Ptr rtabmap::util3d::uniformSampling ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
float  voxelSize 
)
inline

Definition at line 125 of file util3d_filtering.h.

pcl::PointCloud<pcl::PointXYZRGB>::Ptr rtabmap::util3d::uniformSampling ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
float  voxelSize 
)
inline

Definition at line 131 of file util3d_filtering.h.

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr rtabmap::util3d::uniformSampling ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
float  voxelSize 
)
inline

Definition at line 137 of file util3d_filtering.h.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  voxelSize 
)

Definition at line 407 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  voxelSize 
)

Definition at line 411 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  voxelSize 
)

Definition at line 415 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  voxelSize 
)

Definition at line 419 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZI >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  voxelSize 
)

Definition at line 423 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZINormal >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  voxelSize 
)

Definition at line 427 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZ >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
float  voxelSize 
)

Definition at line 432 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointNormal >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointNormal >::Ptr &  cloud,
float  voxelSize 
)

Definition at line 437 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGB >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  cloud,
float  voxelSize 
)

Definition at line 442 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &  cloud,
float  voxelSize 
)

Definition at line 447 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZI >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZI >::Ptr &  cloud,
float  voxelSize 
)

Definition at line 452 of file util3d_filtering.cpp.

pcl::PointCloud< pcl::PointXYZINormal >::Ptr rtabmap::util3d::voxelize ( const pcl::PointCloud< pcl::PointXYZINormal >::Ptr &  cloud,
float  voxelSize 
)

Definition at line 457 of file util3d_filtering.cpp.

template<typename PointT >
pcl::PointCloud<PointT>::Ptr rtabmap::util3d::voxelizeImpl ( const typename pcl::PointCloud< PointT >::Ptr &  cloud,
const pcl::IndicesPtr &  indices,
float  voxelSize 
)

Definition at line 382 of file util3d_filtering.cpp.



rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:43