i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Increased version to 0.9.0. Refactoring: Split util3d.h into multiple files util3d_****.h to reduce compilation time. Also removed all PCL templates to reduce memory used while compiling.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-13 19:54:23 -04:00
parent b54ff8547e
commit 9fde57843f
39 changed files with 4819 additions and 3644 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/CameraModel.h
View File

@@ -138,5 +138,6 @@ private:
cv::Mat F_;
};
} /* namespace rtabmap */
#endif /* CAMERAMODEL_H_ */

510
corelib/include/rtabmap/core/impl/util3d.hpp
View File

@@ -1,510 +0,0 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_HPP_
#define UTIL3D_HPP_
#include <rtabmap/utilite/ULogger.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/random_sample.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/extract_clusters.h>
namespace rtabmap{
namespace util3d{
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr voxelize(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
float voxelSize)
{
typedef typename pcl::PointCloud<PointT> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(voxelSize > 0.0f);
PointCloudPtr output(new PointCloud);
pcl::VoxelGrid<PointT> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr sampling(
const typename pcl::PointCloud<PointT>::Ptr & cloud, int samples)
{
typedef typename pcl::PointCloud<PointT> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(samples > 0);
PointCloudPtr output(new PointCloud);
pcl::RandomSample<PointT> filter;
filter.setSample(samples);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr passThrough(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const std::string & axis,
float min,
float max)
{
typedef typename pcl::PointCloud<PointT> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(max > min);
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
PointCloudPtr output(new PointCloud);
pcl::PassThrough<PointT> filter;
filter.setFilterFieldName(axis);
filter.setFilterLimits(min, max);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud)
{
typedef typename pcl::PointCloud<PointT> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
PointCloudPtr output(new PointCloud);
std::vector<int> indices;
pcl::removeNaNFromPointCloud<PointT>(*cloud, *output, indices);
return output;
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud)
{
typedef typename pcl::PointCloud<PointT> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
PointCloudPtr output(new PointCloud);
std::vector<int> indices;
pcl::removeNaNNormalsFromPointCloud<PointT>(*cloud, *output, indices);
return output;
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr transformPointCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const Transform & transform)
{
typedef typename pcl::PointCloud<PointT> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
PointCloudPtr output(new PointCloud);
pcl::transformPointCloud<PointT>(*cloud, *output, transform.toEigen4f());
return output;
}
template<typename PointT>
PointT transformPoint(
const PointT & pt,
const Transform & transform)
{
return pcl::transformPoint(pt, transform.toEigen3f());
}
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize,
bool segmentFlatObstacles)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
if(cloud->size())
{
// Find the ground
pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
cloud,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalRadiusSearch*2.0f,
Eigen::Vector4f(0,0,100,0));
if(segmentFlatObstacles)
{
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
cloud,
flatSurfaces,
normalRadiusSearch*2.0f,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
if((int)i!=biggestFlatSurfaceIndex)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
}
}
}
else
{
ground = flatSurfaces;
}
if(ground->size() != cloud->size())
{
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);
//Cluster remaining stuff (obstacles)
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
cloud,
otherStuffIndices,
normalRadiusSearch*2.0f,
minClusterSize);
// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
}
}
}
template<typename PointT>
void projectCloudOnXYPlane(
typename pcl::PointCloud<PointT>::Ptr & cloud)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
cloud->at(i).z = 0;
}
}
template<typename PointT>
pcl::IndicesPtr radiusFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering<PointT>(cloud, indices, radiusSearch, minNeighborsInRadius);
}
template<typename PointT>
pcl::IndicesPtr radiusFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree (new KdTree(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud, indices);
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
output->resize(oi);
return output;
}
}
template<typename PointT>
pcl::IndicesPtr normalFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return normalFiltering<PointT>(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
}
template<typename PointT>
pcl::IndicesPtr normalFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
pcl::IndicesPtr output(new std::vector<int>());
if(cloud->size())
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
ne.setInputCloud (cloud);
if(indices->size())
{
ne.setIndices(indices);
}
KdTreePtr tree (new KdTree(false));
if(indices->size())
{
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud(cloud);
}
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (radiusSearch);
if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
{
ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
}
ne.compute (*cloud_normals);
output->resize(cloud_normals->size());
int oi = 0; // output iterator
Eigen::Vector3f n(normal[0], normal[1], normal[2]);
for(unsigned int i=0; i<cloud_normals->size(); ++i)
{
Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
float angle = pcl::getAngle3D(normal, v);
if(angle < angleMax)
{
output->at(oi++) = indices->size()!=0?indices->at(i):i;
}
}
output->resize(oi);
}
return output;
}
template<typename PointT>
std::vector<pcl::IndicesPtr> extractClusters(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
pcl::IndicesPtr indices(new std::vector<int>);
return extractClusters<PointT>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
}
template<typename PointT>
std::vector<pcl::IndicesPtr> extractClusters(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree(new KdTree);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (clusterTolerance);
ec.setMinClusterSize (minClusterSize);
ec.setMaxClusterSize (maxClusterSize);
ec.setInputCloud (cloud);
if(indices->size())
{
ec.setIndices(indices);
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud(cloud);
}
ec.setSearchMethod (tree);
std::vector<pcl::PointIndices> cluster_indices;
ec.extract (cluster_indices);
int maxIndex=-1;
unsigned int maxSize = 0;
std::vector<pcl::IndicesPtr> output(cluster_indices.size());
for(unsigned int i=0; i<cluster_indices.size(); ++i)
{
output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
if(maxSize < cluster_indices[i].indices.size())
{
maxSize = (unsigned int)cluster_indices[i].indices.size();
maxIndex = i;
}
}
if(biggestClusterIndex)
{
*biggestClusterIndex = maxIndex;
}
return output;
}
template<typename PointT>
pcl::IndicesPtr extractNegativeIndices(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices)
{
pcl::IndicesPtr output(new std::vector<int>);
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(true);
extract.filter(*output);
return output;
}
template<typename PointT>
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize,
float groundNormalAngle,
int minClusterSize)
{
if(cloud->size() == 0)
{
return;
}
pcl::IndicesPtr groundIndices, obstaclesIndices;
segmentObstaclesFromGround<PointT>(cloud,
groundIndices,
obstaclesIndices,
cellSize,
groundNormalAngle,
minClusterSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
//project on XY plane
util3d::projectCloudOnXYPlane<pcl::PointXYZ>(groundCloud);
//voxelize to grid cell size
groundCloud = util3d::voxelize<pcl::PointXYZ>(groundCloud, cellSize);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
//project on XY plane
util3d::projectCloudOnXYPlane<pcl::PointXYZ>(obstaclesCloud);
//voxelize to grid cell size
obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
}
ground = cv::Mat();
if(groundCloud->size())
{
ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
for(unsigned int i=0;i<groundCloud->size(); ++i)
{
ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
}
}
obstacles = cv::Mat();
if(obstaclesCloud->size())
{
obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
{
obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
}
}
}
} // util3d
} // rtabmap
#endif //UTIL3D_HPP_

165
corelib/include/rtabmap/core/impl/util3d_mapping.hpp Normal file
View File

@@ -0,0 +1,165 @@
/*
* util3d_mapping.hpp
*
* Created on: 2015-05-13
* Author: mathieu
*/
#ifndef UTIL3D_MAPPING_HPP_
#define UTIL3D_MAPPING_HPP_
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/io.h>
namespace rtabmap{
namespace util3d{
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize,
bool segmentFlatObstacles)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
if(cloud->size())
{
// Find the ground
pcl::IndicesPtr flatSurfaces = normalFiltering(
cloud,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalRadiusSearch*2.0f,
Eigen::Vector4f(0,0,100,0));
if(segmentFlatObstacles)
{
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
cloud,
flatSurfaces,
normalRadiusSearch*2.0f,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
if((int)i!=biggestFlatSurfaceIndex)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
}
}
}
else
{
ground = flatSurfaces;
}
if(ground->size() != cloud->size())
{
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices(cloud, ground);
//Cluster remaining stuff (obstacles)
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
cloud,
otherStuffIndices,
normalRadiusSearch*2.0f,
minClusterSize);
// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
}
}
}
template<typename PointT>
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize,
float groundNormalAngle,
int minClusterSize)
{
if(cloud->size() == 0)
{
return;
}
pcl::IndicesPtr groundIndices, obstaclesIndices;
segmentObstaclesFromGround<PointT>(cloud,
groundIndices,
obstaclesIndices,
cellSize,
groundNormalAngle,
minClusterSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
//project on XY plane
util3d::projectCloudOnXYPlane(groundCloud);
//voxelize to grid cell size
groundCloud = util3d::voxelize(groundCloud, cellSize);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
//project on XY plane
util3d::projectCloudOnXYPlane(obstaclesCloud);
//voxelize to grid cell size
obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
}
ground = cv::Mat();
if(groundCloud->size())
{
ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
for(unsigned int i=0;i<groundCloud->size(); ++i)
{
ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
}
}
obstacles = cv::Mat();
if(obstaclesCloud->size())
{
obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
{
obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
}
}
}
}
}
#endif /* UTIL3D_MAPPING_HPP_ */

105
corelib/include/rtabmap/core/util2d.h Normal file
View File

@@ -0,0 +1,105 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL2D_H_
#define UTIL2D_H_
#include <rtabmap/core/RtabmapExp.h>
#include <opencv2/core/core.hpp>
#include <rtabmap/core/Transform.h>
namespace rtabmap
{
namespace util2d
{
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage);
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02,
float maxCorrespondencesSlope = 0.1f);
cv::Mat RTABMAP_EXP depthFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
float fx,
float baseline,
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02);
cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(
const cv::Mat & leftImage,
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const std::vector<unsigned char> & mask,
float maxSlope = 0.1f);
cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
const cv::Mat & leftImage,
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const std::vector<unsigned char> & mask,
float fx, float baseline);
float RTABMAP_EXP getDepth(
const cv::Mat & depthImage,
float x, float y,
bool smoothing,
float maxZError = 0.02f);
cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
// Registration Depth to RGB
cv::Mat RTABMAP_EXP registerDepth(
const cv::Mat & depth,
const cv::Mat & depthK,
const cv::Mat & colorK,
const rtabmap::Transform & transform);
void RTABMAP_EXP fillRegisteredDepthHoles(
cv::Mat & depth,
bool vertical,
bool horizontal,
bool fillDoubleHoles = false);
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL2D_H_ */

478
corelib/include/rtabmap/core/util3d.h
View File

@@ -29,20 +29,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define UTIL3D_H_
#include "rtabmap/core/RtabmapExp.h"
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include <string>
#include <set>
#include <rtabmap/core/Link.h>
#include <rtabmap/utilite/UThread.h>
#include <pcl/common/eigen.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
#include <rtabmap/core/Transform.h>
#include <opencv2/core/core.hpp>
#include <list>
namespace rtabmap
{
@@ -50,12 +43,16 @@ namespace rtabmap
namespace util3d
{
cv::Mat RTABMAP_EXP rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder = true);
cv::Mat RTABMAP_EXP rgbFromCloud(
const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
bool bgrOrder = true);
cv::Mat RTABMAP_EXP depthFromCloud(
const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
float & fx,
float & fy,
bool depth16U = true);
void RTABMAP_EXP rgbdFromCloud(
const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
cv::Mat & rgb,
@@ -65,68 +62,6 @@ void RTABMAP_EXP rgbdFromCloud(
bool bgrOrder = true,
bool depth16U = true);
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform = Transform::getIdentity(),
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02);
std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & kpts,
const std::multimap<int, cv::KeyPoint> & previousKpts,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
Transform & cameraTransform,
int pnpIterations = 100,
float pnpReprojError = 8.0f,
int pnpFlags = cv::ITERATIVE,
float ransacParam1 = 3.0f,
float ransacParam2 = 0.99f,
const std::multimap<int, pcl::PointXYZ> & refGuess3D = std::multimap<int, pcl::PointXYZ>(),
double * variance = 0);
std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints);
float RTABMAP_EXP getDepth(
const cv::Mat & depthImage,
float x, float y,
bool smoothing,
float maxZError = 0.02f);
pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
const cv::Mat & depthImage,
float x, float y,
@@ -168,45 +103,6 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
float fx, float baseline,
int decimation = 1);
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage);
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02,
float maxCorrespondencesSlope = 0.1f);
cv::Mat RTABMAP_EXP depthFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
float fx,
float baseline,
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02);
cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(
const cv::Mat & leftImage,
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const std::vector<unsigned char> & mask,
float maxSlope = 0.1f);
cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
const cv::Mat & leftImage,
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const std::vector<unsigned char> & mask,
float fx, float baseline);
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,
@@ -221,192 +117,10 @@ cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
float fx, float baseline,
int type = CV_32FC1);
cv::Mat RTABMAP_EXP registerDepth(
const cv::Mat & depth,
const cv::Mat & depthK,
const cv::Mat & colorK,
const rtabmap::Transform & transform);
void RTABMAP_EXP fillRegisteredDepthHoles(cv::Mat & depth, bool vertical, bool horizontal, bool fillDoubleHoles = false);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
// remove depth by z axis
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 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 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);
int RTABMAP_EXP countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
const std::multimap<int, pcl::PointXYZ> & wordsB);
void RTABMAP_EXP filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
char depthAxis,
bool removeDuplicates);
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,
bool refineModel = false,
double refineModelSigma = 3.0,
int refineModelIterations = 10,
std::vector<int> * inliers = 0,
double * variance = 0);
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 = 0,
double * variance = 0,
int * correspondences = 0);
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 = 0,
double * variance = 0,
int * correspondences = 0);
Transform RTABMAP_EXP icp2D(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool * hasConverged = 0,
double * variance = 0,
int * correspondences = 0);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormalsSmoothed(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float smoothingSearchRadius = 0.025,
bool smoothingPolynomialFit = true);
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 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, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
std::set<int> * uniqueCorrespondences = 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
const cv::Mat & matrix,
const Transform & tranform = Transform::getIdentity());
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
int decimation,
double maxDepth,
float voxel,
int samples,
const Transform & transform = Transform::getIdentity());
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);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
const cv::Mat & image,
const cv::Mat & imageDepth,
float constant,
int fastThreshold=50,
bool fastNonmaxSuppression=true,
float maxDepth = 5.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 = false);
void RTABMAP_EXP occupancy2DFromLaserScan(
const cv::Mat & scan,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize);
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);
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);
void RTABMAP_EXP rayTrace(const cv::Point2i & start,
const cv::Point2i & end,
cv::Mat & grid,
bool stopOnObstacle);
cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
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);
/**
* @brief Concatenate a vector of indices to a single vector.
@@ -434,178 +148,12 @@ pcl::IndicesPtr RTABMAP_EXP concatenate(
const pcl::IndicesPtr & indicesA,
const pcl::IndicesPtr & indicesB);
cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
void RTABMAP_EXP savePCDWords(
const std::string & fileName,
const std::multimap<int, pcl::PointXYZ> & words,
const Transform & transform = Transform::getIdentity());
///////////////////
// Templated PCL methods
///////////////////
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr voxelize(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
float voxelSize);
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr sampling(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
int samples);
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr passThrough(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const std::string & axis,
float min,
float max);
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud);
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud);
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr transformPointCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const Transform & transform);
template<typename PointT>
PointT transformPoint(
const PointT & pt,
const Transform & transform);
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize,
bool segmentFlatObstacles = false);
template<typename PointT>
void projectCloudOnXYPlane(
typename pcl::PointCloud<PointT>::Ptr & cloud);
/**
* For convenience.
*/
template<typename PointT>
pcl::IndicesPtr radiusFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius);
/**
* @brief Wrapper of the pcl::RadiusOutlierRemoval class.
*
* Points in the cloud which have less than a minimum of neighbors in the
* specified radius are filtered.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
* @param radiusSearch the radius in meter.
* @param minNeighborsInRadius the minimum of neighbors to keep the point.
* @return the indices of the points satisfying the parameters.
*/
template<typename PointT>
pcl::IndicesPtr radiusFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius);
/**
* For convenience.
*/
template<typename PointT>
pcl::IndicesPtr normalFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint);
/**
* @brief 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.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
* @param angleMax the maximum angle.
* @param normal the normal to which each point's normal is compared.
* @param radiusSearch radius parameter used for normal estimation (see pcl::NormalEstimation).
* @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
* @return the indices of the points which respect the normal constraint.
*/
template<typename PointT>
pcl::IndicesPtr normalFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint);
/**
* For convenience.
*/
template<typename PointT>
std::vector<pcl::IndicesPtr> extractClusters(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
float clusterTolerance,
int minClusterSize,
int maxClusterSize = std::numeric_limits<int>::max(),
int * biggestClusterIndex = 0);
/**
* @brief Wrapper of the pcl::EuclideanClusterExtraction class.
*
* Extract all clusters from a point cloud given a maximum cluster distance tolerance.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
* @param clusterTolerance the cluster distance tolerance (see pcl::EuclideanClusterExtraction).
* @param minClusterSize minimum size of the clusters to return (see pcl::EuclideanClusterExtraction).
* @param maxClusterSize maximum size of the clusters to return (see pcl::EuclideanClusterExtraction).
* @param biggestClusterIndex the index of the biggest cluster, if the clusters are empty, a negative index is set.
* @return the indices of each cluster found.
*/
template<typename PointT>
std::vector<pcl::IndicesPtr> extractClusters(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize = std::numeric_limits<int>::max(),
int * biggestClusterIndex = 0);
template<typename PointT>
pcl::IndicesPtr extractNegativeIndices(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices);
template<typename PointT>
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize = 0.05f,
float groundNormalAngle = M_PI_4,
int minClusterSize = 20);
} // namespace util3d
} // namespace rtabmap
#include "rtabmap/core/impl/util3d.hpp"
#endif /* UTIL3D_H_ */

57
corelib/include/rtabmap/core/util3d_conversions.h Normal file
View File

@@ -0,0 +1,57 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_CONVERSIONS_H_
#define UTIL3D_CONVERSIONS_H_
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/core/core.hpp>
#include <rtabmap/core/Transform.h>
namespace rtabmap
{
namespace util3d
{
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
const cv::Mat & matrix,
const Transform & tranform = Transform::getIdentity());
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_CONVERSIONS_H_ */

105
corelib/include/rtabmap/core/util3d_correspondences.h Normal file
View File

@@ -0,0 +1,105 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_CORRESPONDENCES_H_
#define UTIL3D_CORRESPONDENCES_H_
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/features2d/features2d.hpp>
#include <set>
#include <map>
#include <list>
namespace rtabmap
{
namespace util3d
{
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, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
std::set<int> * uniqueCorrespondences = 0);
// remove depth by z axis
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 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 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);
int RTABMAP_EXP countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
const std::multimap<int, pcl::PointXYZ> & wordsB);
void RTABMAP_EXP filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
char depthAxis,
bool removeDuplicates);
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_CORRESPONDENCES_H_ */

110
corelib/include/rtabmap/core/util3d_features.h Normal file
View File

@@ -0,0 +1,110 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_FEATURES_H_
#define UTIL3D_FEATURES_H_
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <rtabmap/core/Transform.h>
#include <list>
namespace rtabmap
{
namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform = Transform::getIdentity(),
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02);
std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & kpts,
const std::multimap<int, cv::KeyPoint> & previousKpts,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
Transform & cameraTransform,
int pnpIterations = 100,
float pnpReprojError = 8.0f,
int pnpFlags = cv::ITERATIVE,
float ransacParam1 = 3.0f,
float ransacParam2 = 0.99f,
const std::multimap<int, pcl::PointXYZ> & refGuess3D = std::multimap<int, pcl::PointXYZ>(),
double * variance = 0);
std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
const cv::Mat & image,
const cv::Mat & imageDepth,
float constant,
int fastThreshold=50,
bool fastNonmaxSuppression=true,
float maxDepth = 5.0f);
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_FEATURES_H_ */

220
corelib/include/rtabmap/core/util3d_filtering.h Normal file
View File

@@ -0,0 +1,220 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_FILTERING_H_
#define UTIL3D_FILTERING_H_
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
namespace rtabmap
{
namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int samples);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP sampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int samples);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const std::string & axis,
float min,
float max);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::string & axis,
float min,
float max);
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);
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);
/**
* For convenience.
*/
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::PointXYZRGB>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius);
/**
* @brief Wrapper of the pcl::RadiusOutlierRemoval class.
*
* Points in the cloud which have less than a minimum of neighbors in the
* specified radius are filtered.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
* @param radiusSearch the radius in meter.
* @param minNeighborsInRadius the minimum of neighbors to keep the point.
* @return the indices of the points satisfying the parameters.
*/
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointXYZ>::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);
/**
* For convenience.
*/
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint);
/**
* @brief 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.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
* @param angleMax the maximum angle.
* @param normal the normal to which each point's normal is compared.
* @param radiusSearch radius parameter used for normal estimation (see pcl::NormalEstimation).
* @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
* @return the indices of the points which respect the normal constraint.
*/
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint);
/**
* For convenience.
*/
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);
/**
* @brief Wrapper of the pcl::EuclideanClusterExtraction class.
*
* Extract all clusters from a point cloud given a maximum cluster distance tolerance.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
* @param clusterTolerance the cluster distance tolerance (see pcl::EuclideanClusterExtraction).
* @param minClusterSize minimum size of the clusters to return (see pcl::EuclideanClusterExtraction).
* @param maxClusterSize maximum size of the clusters to return (see pcl::EuclideanClusterExtraction).
* @param biggestClusterIndex the index of the biggest cluster, if the clusters are empty, a negative index is set.
* @return the indices of each cluster found.
*/
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);
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);
pcl::IndicesPtr RTABMAP_EXP extractNegativeIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices);
pcl::IndicesPtr RTABMAP_EXP extractNegativeIndices(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices);
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_FILTERING_H_ */

103
corelib/include/rtabmap/core/util3d_mapping.h Normal file
View File

@@ -0,0 +1,103 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_MAPPING_H_
#define UTIL3D_MAPPING_H_
#include "rtabmap/core/RtabmapExp.h"
#include <opencv2/core/core.hpp>
#include <rtabmap/core/Transform.h>
#include <pcl/pcl_base.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
namespace rtabmap
{
namespace util3d
{
void RTABMAP_EXP occupancy2DFromLaserScan(
const cv::Mat & scan,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize);
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);
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);
void RTABMAP_EXP rayTrace(const cv::Point2i & start,
const cv::Point2i & end,
cv::Mat & grid,
bool stopOnObstacle);
cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
void RTABMAP_EXP projectCloudOnXYPlane(
pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
// templated methods
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize,
bool segmentFlatObstacles = false);
template<typename PointT>
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize = 0.05f,
float groundNormalAngle = M_PI_4,
int minClusterSize = 20);
} // namespace util3d
} // namespace rtabmap
#include "rtabmap/core/impl/util3d_mapping.hpp"
#endif /* UTIL3D_MAPPING_H_ */

101
corelib/include/rtabmap/core/util3d_registration.h Normal file
View File

@@ -0,0 +1,101 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_REGISTRATION_H_
#define UTIL3D_REGISTRATION_H_
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <rtabmap/core/Transform.h>
#include <opencv2/core/core.hpp>
namespace rtabmap
{
namespace util3d
{
int RTABMAP_EXP getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
float maxDistance);
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,
bool refineModel = false,
double refineModelSigma = 3.0,
int refineModelIterations = 10,
std::vector<int> * inliers = 0,
double * variance = 0);
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 = 0,
double * variance = 0,
int * correspondences = 0);
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 = 0,
double * variance = 0,
int * correspondences = 0);
Transform RTABMAP_EXP icp2D(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool * hasConverged = 0,
double * variance = 0,
int * correspondences = 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
int decimation,
double maxDepth,
float voxel,
int samples,
const Transform & transform = Transform::getIdentity());
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_REGISTRATION_H_ */

70
corelib/include/rtabmap/core/util3d_surface.h Normal file
View File

@@ -0,0 +1,70 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_SURFACE_H_
#define UTIL3D_SURFACE_H_
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
namespace rtabmap
{
namespace util3d
{
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 = false);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormalsSmoothed(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float smoothingSearchRadius = 0.025,
bool smoothingPolynomialFit = true);
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_SURFACE_H_ */

60
corelib/include/rtabmap/core/util3d_transforms.h Normal file
View File

@@ -0,0 +1,60 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_TRANSFORMS_H_
#define UTIL3D_TRANSFORMS_H_
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <rtabmap/core/Transform.h>
namespace rtabmap
{
namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & transform);
pcl::PointXYZ RTABMAP_EXP transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform);
pcl::PointXYZRGB RTABMAP_EXP transformPoint(
const pcl::PointXYZRGB & pt,
const Transform & transform);
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_TRANSFORMS_H_ */