i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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_ */

11
corelib/src/CMakeLists.txt
View File

@@ -25,7 +25,18 @@ SET(SRC_FILES
Features2d.cpp
Transform.cpp
util2d.cpp
util3d.cpp
util3d_filtering.cpp
util3d_mapping.cpp
util3d_transforms.cpp
util3d_registration.cpp
util3d_surface.cpp
util3d_features.cpp
util3d_correspondences.cpp
util3d_conversions.cpp
SensorData.cpp
Graph.cpp
Compression.cpp

37
corelib/src/Memory.cpp
View File

@@ -40,9 +40,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/EpipolarGeometry.h>
#include "VisualWord.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d.h"
#include "DBDriverSqlite3.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_conversions.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/Statistics.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/Graph.h"
@@ -1954,7 +1959,7 @@ Transform Memory::computeVisualTransform(
UDEBUG("Forcing 2D...");
float x,y,z,r,p,yaw;
transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
transform = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
transform = Transform(x,y,0, 0, 0, yaw);
}
}
else
@@ -2022,7 +2027,7 @@ Transform Memory::computeVisualTransform(
UDEBUG("Forcing 2D...");
float x,y,z,r,p,yaw;
transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
transform = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
transform = Transform(x,y,0, 0, 0, yaw);
}
}
else if(inliersCount < _bowMinInliers)
@@ -2236,8 +2241,8 @@ Transform Memory::computeIcpTransform(
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ, _icpPointToPlaneNormalNeighbors);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
oldCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(oldCloud);
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud);
if(newCloud->size() && oldCloud->size())
{
@@ -2335,7 +2340,7 @@ Transform Memory::computeIcpTransform(
// We are 2D here, make sure the guess has only YAW rotation
float x,y,z,r,p,yaw;
guess.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
guess = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
guess = Transform(x,y,0, 0, 0, yaw);
if(r!=0 || p!=0)
{
UINFO("2D ICP: Dropping z (%f), roll (%f) and pitch (%f) rotation!", z, r, p);
@@ -2350,8 +2355,8 @@ Transform Memory::computeIcpTransform(
//voxelize
if(_icp2VoxelSize > _laserScanVoxelSize)
{
oldCloud = util3d::voxelize<pcl::PointXYZ>(oldCloud, _icp2VoxelSize);
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
oldCloud = util3d::voxelize(oldCloud, _icp2VoxelSize);
newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
}
if(newCloud->size() && oldCloud->size())
@@ -2517,7 +2522,7 @@ Transform Memory::computeScanMatchingTransform(
//voxelize
if(assembledOldClouds->size() && _icp2VoxelSize > 0.0f)
{
assembledOldClouds = util3d::voxelize<pcl::PointXYZ>(assembledOldClouds, _icp2VoxelSize);
assembledOldClouds = util3d::voxelize(assembledOldClouds, _icp2VoxelSize);
}
// get the new cloud
@@ -2530,7 +2535,7 @@ Transform Memory::computeScanMatchingTransform(
//voxelize
if(newCloud->size() && _icp2VoxelSize > _laserScanVoxelSize)
{
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
}
Transform transform;
@@ -3664,7 +3669,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
}
//generate a disparity map
disparity = util3d::disparityFromStereoImages(
disparity = util2d::disparityFromStereoImages(
imageMono,
data.rightImage(),
leftCorners,
@@ -3834,7 +3839,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
//generate a disparity map
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
cv::Mat disparity = util3d::disparityFromStereoImages(
cv::Mat disparity = util2d::disparityFromStereoImages(
imageMono,
data.rightImage(),
leftCorners,
@@ -3983,8 +3988,8 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
// apply decimation?
if((this->isBinDataKept() || this->isRawDataKept()) && _imageDecimation > 1)
{
image = util3d::decimate(image, _imageDecimation);
depthOrRightImage = util3d::decimate(depthOrRightImage, _imageDecimation);
image = util2d::decimate(image, _imageDecimation);
depthOrRightImage = util2d::decimate(depthOrRightImage, _imageDecimation);
cx/=float(_imageDecimation);
cy/=float(_imageDecimation);
fx/=float(_imageDecimation);
@@ -3998,7 +4003,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
cv::Mat laserScan = data.laserScan();
if(!laserScan.empty() && _laserScanVoxelSize > 0.0f)
{
laserScan = util3d::laserScanFromPointCloud(*util3d::voxelize<pcl::PointXYZ>(util3d::laserScanToPointCloud(laserScan), _laserScanVoxelSize));
laserScan = util3d::laserScanFromPointCloud(*util3d::voxelize(util3d::laserScanToPointCloud(laserScan), _laserScanVoxelSize));
}
Signature * s;

4
corelib/src/OdometryBOW.cpp
View File

@@ -29,7 +29,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"

6
corelib/src/OdometryICP.cpp
View File

@@ -26,7 +26,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
@@ -95,7 +97,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
if(newCloudXYZ->size() != newCloud->size())
{
UWARN("removed nan normals...");

3
corelib/src/OdometryMono.cpp
View File

@@ -29,7 +29,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
@@ -38,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
#include <pcl/common/centroid.h>
namespace rtabmap {

4
corelib/src/OdometryOpticalFlow.cpp
View File

@@ -28,13 +28,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/calib3d/calib3d.hpp>
namespace rtabmap {

2
corelib/src/Rtabmap.cpp
View File

@@ -49,8 +49,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/filters/crop_box.h>
#include <pcl/io/pcd_io.h>
#include "rtabmap/core/util3d.h"
#include <stdlib.h>
#include <set>

491
corelib/src/util2d.cpp Normal file
View File

@@ -0,0 +1,491 @@
/*
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.
*/
#include "rtabmap/core/util2d.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
namespace rtabmap
{
namespace util2d
{
cv::Mat disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1 &&
leftImage.cols == rightImage.cols &&
leftImage.rows == rightImage.rows);
cv::Mat leftMono;
if(leftImage.channels() == 3)
{
cv::cvtColor(leftImage, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = leftImage;
}
cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
stereo.state->SADWindowSize = 15;
stereo.state->minDisparity = 0;
stereo.state->numberOfDisparities = 64;
stereo.state->preFilterSize = 9;
stereo.state->preFilterCap = 31;
stereo.state->uniquenessRatio = 15;
stereo.state->textureThreshold = 10;
stereo.state->speckleWindowSize = 100;
stereo.state->speckleRange = 4;
cv::Mat disparity;
stereo(leftMono, rightImage, disparity, CV_16SC1);
return disparity;
}
cv::Mat disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps,
float maxCorrespondencesSlope)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.cols == rightImage.cols &&
leftImage.rows == rightImage.rows);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> rightCorners;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
leftImage,
rightImage,
leftCorners,
rightCorners,
status,
err,
cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope);
}
cv::Mat depthFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
float fx,
float baseline,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.cols == rightImage.cols &&
leftImage.rows == rightImage.rows);
UASSERT(fx > 0.0f && baseline > 0.0f);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> rightCorners;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
leftImage,
rightImage,
leftCorners,
rightCorners,
status,
err,
cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
return depthFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, fx, baseline);
}
cv::Mat 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)
{
UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
cv::Mat disparity = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
for(unsigned int i=0; i<leftCorners.size(); ++i)
{
if(mask.size() == 0 || mask[i])
{
float d = leftCorners[i].x - rightCorners[i].x;
float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (leftCorners[i].x - rightCorners[i].x));
if(d > 0.0f && slope < maxSlope)
{
disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
}
}
}
return disparity;
}
cv::Mat 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)
{
UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
cv::Mat depth = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
for(unsigned int i=0; i<leftCorners.size(); ++i)
{
if(mask.size() == 0 || mask[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f)
{
float d = baseline * fx / disparity;
depth.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
}
}
}
return depth;
}
float getDepth(
const cv::Mat & depthImage,
float x, float y,
bool smoothing,
float maxZError)
{
UASSERT(!depthImage.empty());
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
int u = int(x+0.5f);
int v = int(y+0.5f);
if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
{
UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
x,u,y,v,depthImage.cols, depthImage.rows);
return 0;
}
bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
// Inspired from RGBDFrame::getGaussianMixtureDistribution() method from
// https://github.com/ccny-ros-pkg/rgbdtools/blob/master/src/rgbd_frame.cpp
// Window weights:
// | 1 | 2 | 1 |
// | 2 | 4 | 2 |
// | 1 | 2 | 1 |
int u_start = std::max(u-1, 0);
int v_start = std::max(v-1, 0);
int u_end = std::min(u+1, depthImage.cols-1);
int v_end = std::min(v+1, depthImage.rows-1);
float depth = isInMM?(float)depthImage.at<uint16_t>(v,u)*0.001f:depthImage.at<float>(v,u);
if(depth!=0.0f && uIsFinite(depth))
{
if(smoothing)
{
float sumWeights = 0.0f;
float sumDepths = 0.0f;
for(int uu = u_start; uu <= u_end; ++uu)
{
for(int vv = v_start; vv <= v_end; ++vv)
{
if(!(uu == u && vv == v))
{
float d = isInMM?(float)depthImage.at<uint16_t>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
// ignore if not valid or depth difference is too high
if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
{
if(uu == u || vv == v)
{
sumWeights+=2.0f;
d*=2.0f;
}
else
{
sumWeights+=1.0f;
}
sumDepths += d;
}
}
}
}
// set window weight to center point
depth *= 4.0f;
sumWeights += 4.0f;
// mean
depth = (depth+sumDepths)/sumWeights;
}
}
else
{
depth = 0;
}
return depth;
}
cv::Mat decimate(const cv::Mat & image, int decimation)
{
UASSERT(decimation >= 1);
cv::Mat out;
if(!image.empty())
{
if(decimation > 1)
{
if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
{
UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0, "Decimation of depth images should be exact!");
out = cv::Mat(image.rows/decimation, image.cols/decimation, image.type());
if(image.type() == CV_32FC1)
{
for(int j=0; j<out.rows; ++j)
{
for(int i=0; i<out.cols; ++i)
{
out.at<float>(j, i) = image.at<float>(j*decimation, i*decimation);
}
}
}
else // CV_16UC1
{
for(int j=0; j<out.rows; ++j)
{
for(int i=0; i<out.cols; ++i)
{
out.at<unsigned short>(j, i) = image.at<unsigned short>(j*decimation, i*decimation);
}
}
}
}
else
{
cv::resize(image, out, cv::Size(), 1.0f/float(decimation), 1.0f/float(decimation), cv::INTER_AREA);
}
}
else
{
out = image;
}
}
return out;
}
// Registration Depth to RGB
cv::Mat registerDepth(
const cv::Mat & depth,
const cv::Mat & depthK,
const cv::Mat & colorK,
const rtabmap::Transform & transform)
{
UASSERT(!transform.isNull());
UASSERT(!depth.empty());
UASSERT(depth.type() == CV_16UC1); // mm
UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
float fx = depthK.at<double>(0,0);
float fy = depthK.at<double>(1,1);
float cx = depthK.at<double>(0,2);
float cy = depthK.at<double>(1,2);
float rfx = colorK.at<double>(0,0);
float rfy = colorK.at<double>(1,1);
float rcx = colorK.at<double>(0,2);
float rcy = colorK.at<double>(1,2);
Eigen::Affine3f proj = transform.toEigen3f();
Eigen::Vector4f P4,P3;
P4[3] = 1;
cv::Mat registered = cv::Mat::zeros(depth.rows, depth.cols, depth.type());
for(int y=0; y<depth.rows; ++y)
{
for(int x=0; x<depth.cols; ++x)
{
//filtering
float dz = float(depth.at<unsigned short>(y,x))*0.001f; // put in meter for projection
if(dz>=0.0f)
{
// Project to 3D
P4[0] = (x - cx) * dz / fx; // Optimization: we could have (x-cx)/fx in a lookup table
P4[1] = (y - cy) * dz / fy; // Optimization: we could have (y-cy)/fy in a lookup table
P4[2] = dz;
P3 = proj * P4;
float z = P3[2];
float invZ = 1.0f/z;
int dx = (rfx*P3[0])*invZ + rcx;
int dy = (rfy*P3[1])*invZ + rcy;
if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
{
unsigned short z16 = z * 1000; //mm
unsigned short &zReg = registered.at<unsigned short>(dy, dx);
if(zReg == 0 || z16 < zReg)
{
zReg = z16;
}
}
}
}
}
return registered;
}
void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool horizontal, bool fillDoubleHoles)
{
UASSERT(registeredDepth.type() == CV_16UC1);
int margin = fillDoubleHoles?2:1;
for(int x=1; x<registeredDepth.cols-margin; ++x)
{
for(int y=1; y<registeredDepth.rows-margin; ++y)
{
unsigned short & b = registeredDepth.at<unsigned short>(y, x);
bool set = false;
if(vertical)
{
const unsigned short & a = registeredDepth.at<unsigned short>(y-1, x);
unsigned short & c = registeredDepth.at<unsigned short>(y+1, x);
if(a && c)
{
unsigned short error = 0.01*((a+c)/2);
if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
(a>c?a-c<=error:c-a<=error))
{
b = (a+c)/2;
set = true;
if(!horizontal)
{
++y;
}
}
}
if(!set && fillDoubleHoles)
{
const unsigned short & d = registeredDepth.at<unsigned short>(y+2, x);
if(a && d && (b==0 || c==0))
{
unsigned short error = 0.01*((a+d)/2);
if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
((c == 0 && a && d) || (c > a+error && c > d+error)) &&
(a>d?a-d<=error:d-a<=error))
{
if(a>d)
{
unsigned short tmp = (a-d)/4;
b = d + tmp;
c = d + 3*tmp;
}
else
{
unsigned short tmp = (d-a)/4;
b = a + tmp;
c = a + 3*tmp;
}
set = true;
if(!horizontal)
{
y+=2;
}
}
}
}
}
if(!set && horizontal)
{
const unsigned short & a = registeredDepth.at<unsigned short>(y, x-1);
unsigned short & c = registeredDepth.at<unsigned short>(y, x+1);
if(a && c)
{
unsigned short error = 0.01*((a+c)/2);
if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
(a>c?a-c<=error:c-a<=error))
{
b = (a+c)/2;
set = true;
}
}
if(!set && fillDoubleHoles)
{
const unsigned short & d = registeredDepth.at<unsigned short>(y, x+2);
if(a && d && (b==0 || c==0))
{
unsigned short error = 0.01*((a+d)/2);
if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
((c == 0 && a && d) || (c > a+error && c > d+error)) &&
(a>d?a-d<=error:d-a<=error))
{
if(a>d)
{
unsigned short tmp = (a-d)/4;
b = d + tmp;
c = d + 3*tmp;
}
else
{
unsigned short tmp = (d-a)/4;
b = a + tmp;
c = a + 3*tmp;
}
}
}
}
}
}
}
}
}
}

2584
corelib/src/util3d.cpp
View File

File diff suppressed because it is too large Load Diff

142
corelib/src/util3d_conversions.cpp Normal file
View File

@@ -0,0 +1,142 @@
/*
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.
*/
#include "rtabmap/core/util3d_conversions.h"
#include "rtabmap/utilite/ULogger.h"
#include <pcl/common/transforms.h>
namespace rtabmap
{
namespace util3d
{
cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
{
UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
cv::Mat depth16U;
if(!depth32F.empty())
{
depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
for(int i=0; i<depth32F.rows; ++i)
{
for(int j=0; j<depth32F.cols; ++j)
{
float depth = (depth32F.at<float>(i,j)*1000.0f);
unsigned short depthMM = 0;
if(depth <= (float)USHRT_MAX)
{
depthMM = (unsigned short)depth;
}
depth16U.at<unsigned short>(i, j) = depthMM;
}
}
}
return depth16U;
}
cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
{
UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
cv::Mat depth32F;
if(!depth16U.empty())
{
depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
for(int i=0; i<depth16U.rows; ++i)
{
for(int j=0; j<depth16U.cols; ++j)
{
float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
depth32F.at<float>(i, j) = depth;
}
}
}
return depth32F;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
for(unsigned int i=0; i<cloud.size(); ++i)
{
laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
}
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(laserScan.cols);
for(int i=0; i<laserScan.cols; ++i)
{
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
}
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
const cv::Mat & matrix,
const Transform & tranform)
{
UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
UASSERT(matrix.rows == 1);
Eigen::Affine3f t = tranform.toEigen3f();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(matrix.cols);
if(matrix.channels() == 2)
{
for(int i=0; i<matrix.cols; ++i)
{
cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
cloud->at(i).z = 0.0f;
cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
}
}
else // channels=3
{
for(int i=0; i<matrix.cols; ++i)
{
cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
}
}
return cloud;
}
}
}

392
corelib/src/util3d_correspondences.cpp Normal file
View File

@@ -0,0 +1,392 @@
/*
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.
*/
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d.h"
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/core/EpipolarGeometry.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <pcl/search/kdtree.h>
namespace rtabmap
{
namespace util3d
{
void 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)
{
const std::list<int> & ids = uUniqueKeys(words1);
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(words1.count(*i) == 1 && words2.count(*i) == 1)
{
const pcl::PointXYZ & pt1 = words1.find(*i)->second;
const pcl::PointXYZ & pt2 = words2.find(*i)->second;
if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
{
cloud1.push_back(pt1);
cloud2.push_back(pt2);
}
}
}
}
void 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)
{
std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> > pairs;
const std::list<int> & ids = uUniqueKeys(words1);
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(words1.count(*i) == 1 && words2.count(*i) == 1)
{
const pcl::PointXYZ & pt1 = words1.find(*i)->second;
const pcl::PointXYZ & pt2 = words2.find(*i)->second;
if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
{
pairs.push_back(std::pair<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2));
}
}
}
if(pairs.size() > 7)
{
// Remove outliers using fundamental matrix RANSAC
std::vector<uchar> status(pairs.size(), 0);
//Convert Keypoints to a structure that OpenCV understands
//3 dimensions (Homogeneous vectors)
cv::Mat points1(1, (int)pairs.size(), CV_32FC2);
cv::Mat points2(1, (int)pairs.size(), CV_32FC2);
float * points1data = points1.ptr<float>(0);
float * points2data = points2.ptr<float>(0);
// Fill the points here ...
int i=0;
for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::const_iterator iter = pairs.begin();
iter != pairs.end();
++iter )
{
points1data[i*2] = (*iter).first.x;
points1data[i*2+1] = (*iter).first.y;
points2data[i*2] = (*iter).second.x;
points2data[i*2+1] = (*iter).second.y;
++i;
}
// Find the fundamental matrix
cv::Mat fundamentalMatrix = cv::findFundamentalMat(
points1,
points2,
status,
cv::FM_RANSAC,
3.0,
0.99);
if(!fundamentalMatrix.empty())
{
int i = 0;
for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
{
if(status[i])
{
cloud1.push_back(iter->first);
cloud2.push_back(iter->second);
}
++i;
}
}
}
}
void 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)
{
cloud1.resize(correspondences.size());
cloud2.resize(correspondences.size());
int oi=0;
for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
iter!=correspondences.end();
++iter)
{
pcl::PointXYZ pt1 = projectDepthTo3D(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
pcl::PointXYZ pt2 = projectDepthTo3D(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
if(pcl::isFinite(pt1) && pcl::isFinite(pt2) &&
(maxDepth <= 0 || (pt1.z <= maxDepth && pt2.z<=maxDepth)))
{
cloud1[oi] = pt1;
cloud2[oi] = pt2;
++oi;
}
}
cloud1.resize(oi);
cloud2.resize(oi);
}
template<typename PointT>
inline 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)
{
for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
iter!=correspondences.end();
++iter)
{
PointT pt1 = cloud1.at(int(iter->first.y+0.5f) * cloud1.width + int(iter->first.x+0.5f));
PointT pt2 = cloud2.at(int(iter->second.y+0.5f) * cloud2.width + int(iter->second.x+0.5f));
if(pcl::isFinite(pt1) &&
pcl::isFinite(pt2))
{
inliers1.push_back(pcl::PointXYZ(pt1.x, pt1.y, pt1.z));
inliers2.push_back(pcl::PointXYZ(pt2.x, pt2.y, pt2.z));
}
}
}
void 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)
{
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
}
void 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)
{
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
}
int countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
const std::multimap<int, pcl::PointXYZ> & wordsB)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
int pairs = 0;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
std::list<pcl::PointXYZ> ptsA = uValues(wordsA, *i);
std::list<pcl::PointXYZ> ptsB = uValues(wordsB, *i);
if(ptsA.size() == 1 && ptsB.size() == 1)
{
++pairs;
}
}
return pairs;
}
void filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
char depthAxis,
bool removeDuplicates)
{
std::list<pcl::PointXYZ> addedPts;
if(maxDepth > 0.0f && inliers1.size() && inliers1.size() == inliers2.size())
{
pcl::PointCloud<pcl::PointXYZ> tmpInliers1;
pcl::PointCloud<pcl::PointXYZ> tmpInliers2;
for(unsigned int i=0; i<inliers1.size(); ++i)
{
if((depthAxis == 'x' && inliers1.at(i).x < maxDepth && inliers2.at(i).x < maxDepth) ||
(depthAxis == 'y' && inliers1.at(i).y < maxDepth && inliers2.at(i).y < maxDepth) ||
(depthAxis == 'z' && inliers1.at(i).z < maxDepth && inliers2.at(i).z < maxDepth))
{
bool dup = false;
if(removeDuplicates)
{
for(std::list<pcl::PointXYZ>::iterator iter = addedPts.begin(); iter!=addedPts.end(); ++iter)
{
if(iter->x == inliers1.at(i).x &&
iter->y == inliers1.at(i).y &&
iter->z == inliers1.at(i).z)
{
dup = true;
}
}
if(!dup)
{
addedPts.push_back(inliers1.at(i));
}
}
if(!dup)
{
tmpInliers1.push_back(inliers1.at(i));
tmpInliers2.push_back(inliers2.at(i));
}
}
}
inliers1 = tmpInliers1;
inliers2 = tmpInliers2;
}
}
// a kdtree is constructed with cloud_target, then nearest neighbor
// is computed for each cloud_source points.
int getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
float maxDistance)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud_target);
int count = 0;
float sqrdMaxDistance = maxDistance * maxDistance;
for(unsigned int i=0; i<cloud_source->size(); ++i)
{
std::vector<int> ind(1);
std::vector<float> dist(1);
if(kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist) && dist[0] < sqrdMaxDistance)
{
++count;
}
}
return count;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
void findCorrespondences(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
pairs.clear();
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
if(ptsA.size() == 1 && ptsB.size() == 1)
{
pairs.push_back(std::pair<cv::Point2f, cv::Point2f>(ptsA.front().pt, ptsB.front().pt));
}
}
}
std::list<std::pair<cv::Point2f, cv::Point2f> > findCorrespondences(
const std::multimap<int, cv::KeyPoint> & words1,
const std::multimap<int, cv::KeyPoint> & words2)
{
std::list<std::pair<cv::Point2f, cv::Point2f> > correspondences;
// Find pairs
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
rtabmap::EpipolarGeometry::findPairsUnique(words1, words2, pairs);
if(pairs.size() > 7) // 8 min?
{
// Find fundamental matrix
std::vector<uchar> status;
cv::Mat fundamentalMatrix = rtabmap::EpipolarGeometry::findFFromWords(pairs, status);
//ROS_INFO("inliers = %d/%d", uSum(status), pairs.size());
if(!fundamentalMatrix.empty())
{
int i = 0;
//int goodCount = 0;
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
{
if(status[i])
{
correspondences.push_back(std::pair<cv::Point2f, cv::Point2f>(iter->second.first.pt, iter->second.second.pt));
//ROS_INFO("inliers kpts %f %f vs %f %f", iter->second.first.pt.x, iter->second.first.pt.y, iter->second.second.pt.x, iter->second.second.pt.y);
}
++i;
}
}
}
return correspondences;
}
void 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)
{
std::list<int> ids = uUniqueKeys(words1);
// Find pairs
inliers1.resize(ids.size());
inliers2.resize(ids.size());
int oi=0;
for(std::list<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
if(words1.count(*iter) == 1 && words2.count(*iter) == 1)
{
inliers1[oi] = words1.find(*iter)->second;
inliers2[oi] = words2.find(*iter)->second;
if(pcl::isFinite(inliers1[oi]) &&
pcl::isFinite(inliers2[oi]) &&
(inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
(inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
(maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
{
++oi;
if(uniqueCorrespondences)
{
uniqueCorrespondences->insert(*iter);
}
}
}
}
inliers1.resize(oi);
inliers2.resize(oi);
}
}
}

508
corelib/src/util3d_features.cpp Normal file
View File

@@ -0,0 +1,508 @@
/*
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.
*/
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <opencv2/video/tracking.hpp>
namespace rtabmap
{
namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
const Transform & transform)
{
UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
if(!depth.empty())
{
keypoints3d->resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(
depth,
keypoints[i].pt.x,
keypoints[i].pt.y,
cx,
cy,
fx,
fy,
true);
if(!transform.isNull() && !transform.isIdentity())
{
pt = util3d::transformPoint(pt, transform);
}
keypoints3d->at(i) = pt;
}
}
return keypoints3d;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
pcl::PointXYZ pt = util3d::projectDisparityTo3D(
keypoints[i].pt,
disparity,
cx,
cy,
fx,
baseline);
if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
{
pt = util3d::transformPoint(pt, transform);
}
keypoints3d->at(i) = pt;
}
return keypoints3d;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr 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,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> rightCorners;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
leftImage,
rightImage,
leftCorners,
rightCorners,
status,
err,
cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(keypoints.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(status.size() == keypoints.size());
for(unsigned int i=0; i<status.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
if(status[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f)
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
cx, cy, fx, baseline);
if(pcl::isFinite(tmpPt))
{
pt = tmpPt;
if(!transform.isNull() && !transform.isIdentity())
{
pt = util3d::transformPoint(pt, transform);
}
}
}
}
keypoints3d->at(i) = pt;
}
return keypoints3d;
}
// cameraTransform, from ref to next
// return 3D points in ref referential
// If cameraTransform is not null, it will be used for triangulation instead of the camera transform computed by epipolar geometry
// when refGuess3D is passed and cameraTransform is null, scale will be estimated, returning scaled cloud and camera transform
std::multimap<int, pcl::PointXYZ> generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & refWords,
const std::multimap<int, cv::KeyPoint> & nextWords,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
Transform & cameraTransform,
int pnpIterations,
float pnpReprojError,
int pnpFlags,
float ransacParam1,
float ransacParam2,
const std::multimap<int, pcl::PointXYZ> & refGuess3D,
double * varianceOut)
{
std::multimap<int, pcl::PointXYZ> words3D;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
{
std::vector<unsigned char> status;
cv::Mat F = EpipolarGeometry::findFFromWords(pairs, status, ransacParam1, ransacParam2);
if(!F.empty())
{
//get inliers
//normalize coordinates
int oi = 0;
UASSERT(status.size() == pairs.size());
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
std::vector<cv::Point2f> refCorners(status.size());
std::vector<cv::Point2f> newCorners(status.size());
std::vector<int> indexes(status.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i])
{
refCorners[oi] = iter->second.first.pt;
newCorners[oi] = iter->second.second.pt;
indexes[oi] = iter->first;
++oi;
}
++iter;
}
refCorners.resize(oi);
newCorners.resize(oi);
indexes.resize(oi);
UDEBUG("inliers=%d/%d", oi, pairs.size());
if(oi > 3)
{
std::vector<cv::Point2f> refCornersRefined;
std::vector<cv::Point2f> newCornersRefined;
cv::correctMatches(F, refCorners, newCorners, refCornersRefined, newCornersRefined);
refCorners = refCornersRefined;
newCorners = newCornersRefined;
cv::Mat x(3, (int)refCorners.size(), CV_64FC1);
cv::Mat xp(3, (int)refCorners.size(), CV_64FC1);
for(unsigned int i=0; i<refCorners.size(); ++i)
{
x.at<double>(0, i) = refCorners[i].x;
x.at<double>(1, i) = refCorners[i].y;
x.at<double>(2, i) = 1;
xp.at<double>(0, i) = newCorners[i].x;
xp.at<double>(1, i) = newCorners[i].y;
xp.at<double>(2, i) = 1;
}
cv::Mat K = (cv::Mat_<double>(3,3) <<
fx, 0, cx,
0, fy, cy,
0, 0, 1);
cv::Mat Kinv = K.inv();
cv::Mat E = K.t()*F*K;
cv::Mat x_norm = Kinv * x;
cv::Mat xp_norm = Kinv * xp;
x_norm = x_norm.rowRange(0,2);
xp_norm = xp_norm.rowRange(0,2);
cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
if(!P.empty())
{
cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
P0.at<double>(0,0) = 1;
P0.at<double>(1,1) = 1;
P0.at<double>(2,2) = 1;
bool useCameraTransformGuess = !cameraTransform.isNull();
//if camera transform is set, use it instead of the computed one from epipolar geometry
if(useCameraTransformGuess)
{
Transform t = (localTransform.inverse()*cameraTransform*localTransform).inverse();
P = (cv::Mat_<double>(3,4) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
(double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
(double)t.r31(), (double)t.r32(), (double)t.r33(), (double)t.z());
}
// triangulate the points
//std::vector<double> reprojErrors;
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
//EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
cv::Mat pts4D;
cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
for(unsigned int i=0; i<indexes.size(); ++i)
{
//if(cloud->at(i).z > 0)
//{
// words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(cloud->at(i), localTransform)));
//}
pts4D.col(i) /= pts4D.at<double>(3,i);
if(pts4D.at<double>(2,i) > 0)
{
words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), localTransform)));
}
}
if(!useCameraTransformGuess)
{
cv::Mat R, T;
EpipolarGeometry::findRTFromP(P, R, T);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
cameraTransform = (localTransform * t).inverse() * localTransform;
}
if(refGuess3D.size())
{
// scale estimation
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRef(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRefGuess(new pcl::PointCloud<pcl::PointXYZ>);
util3d::findCorrespondences(
words3D,
refGuess3D,
*inliersRef,
*inliersRefGuess,
0);
if(inliersRef->size())
{
// estimate the scale
float scale = 1.0f;
float variance = 1.0f;
if(!useCameraTransformGuess)
{
std::multimap<float, float> scales; // <variance, scale>
for(unsigned int i=0; i<inliersRef->size(); ++i)
{
// using x as depth, assuming we are in global referential
float s = inliersRefGuess->at(i).x/inliersRef->at(i).x;
std::vector<float> errorSqrdDists(inliersRef->size());
for(unsigned int j=0; j<inliersRef->size(); ++j)
{
pcl::PointXYZ refPt = inliersRef->at(j);
refPt.x *= s;
refPt.y *= s;
refPt.z *= s;
const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
float var = 2.1981 * median_error_sqr;
//UDEBUG("scale %d = %f variance = %f", (int)i, s, variance);
scales.insert(std::make_pair(var, s));
}
scale = scales.begin()->second;
variance = scales.begin()->first;;
}
else
{
//compute variance at scale=1
std::vector<float> errorSqrdDists(inliersRef->size());
for(unsigned int j=0; j<inliersRef->size(); ++j)
{
const pcl::PointXYZ & refPt = inliersRef->at(j);
const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
variance = 2.1981 * median_error_sqr;
}
UDEBUG("scale used = %f (variance=%f)", scale, variance);
if(varianceOut)
{
*varianceOut = variance;
}
if(!useCameraTransformGuess)
{
std::vector<cv::Point3f> objectPoints(indexes.size());
std::vector<cv::Point2f> imagePoints(indexes.size());
int oi=0;
for(unsigned int i=0; i<indexes.size(); ++i)
{
std::multimap<int, pcl::PointXYZ>::iterator iter = words3D.find(indexes[i]);
if(pcl::isFinite(iter->second))
{
iter->second.x *= scale;
iter->second.y *= scale;
iter->second.z *= scale;
objectPoints[oi].x = iter->second.x;
objectPoints[oi].y = iter->second.y;
objectPoints[oi].z = iter->second.z;
imagePoints[oi] = newCorners[i];
++oi;
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
//PnPRansac
Transform guess = localTransform.inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<int> inliersV;
cv::solvePnPRansac(
objectPoints,
imagePoints,
K,
cv::Mat(),
rvec,
tvec,
true,
pnpIterations,
pnpReprojError,
0,
inliersV,
pnpFlags);
UDEBUG("PnP inliers = %d / %d", (int)inliersV.size(), (int)objectPoints.size());
if(inliersV.size())
{
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
cameraTransform = (localTransform * pnp).inverse();
}
else
{
UWARN("No inliers after PnP!");
cameraTransform = Transform();
}
}
}
else
{
UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess");
}
}
}
}
}
}
UDEBUG("wordsSet=%d / %d", (int)words3D.size(), (int)refWords.size());
return words3D;
}
std::multimap<int, cv::KeyPoint> aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints)
{
std::multimap<int, cv::KeyPoint> words;
std::vector<cv::KeyPoint>::const_iterator kpIter = keypoints.begin();
for(std::list<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
{
words.insert(std::pair<int, cv::KeyPoint >(*iter, *kpIter));
++kpIter;
}
return words;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
const cv::Mat & image,
const cv::Mat & imageDepth,
float constant,
int fastThreshold,
bool fastNonmaxSuppression,
float maxDepth)
{
// Extract words
cv::FastFeatureDetector detector(fastThreshold, fastNonmaxSuppression);
std::vector<cv::KeyPoint> kpts;
detector.detect(image, kpts);
pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
for(unsigned int i=0; i<kpts.size(); ++i)
{
pcl::PointXYZ pt = projectDepthTo3D(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
if(uIsFinite(pt.z) && (maxDepth <= 0 || pt.z <= maxDepth))
{
points->push_back(pt);
}
}
UDEBUG("points %d -> %d", (int)kpts.size(), (int)points->size());
return points;
}
}
}

600
corelib/src/util3d_filtering.cpp Normal file
View File

@@ -0,0 +1,600 @@
/*
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.
*/
#include "rtabmap/core/util3d_filtering.h"
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/random_sample.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/segmentation/extract_clusters.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap
{
namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize)
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(voxelSize > 0.0f);
PointCloudPtr output(new PointCloud);
pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize)
{
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(voxelSize > 0.0f);
PointCloudPtr output(new PointCloud);
pcl::VoxelGrid<pcl::PointXYZRGB> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(samples > 0);
PointCloudPtr output(new PointCloud);
pcl::RandomSample<pcl::PointXYZ> filter;
filter.setSample(samples);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
{
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(samples > 0);
PointCloudPtr output(new PointCloud);
pcl::RandomSample<pcl::PointXYZRGB> filter;
filter.setSample(samples);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const std::string & axis,
float min,
float max)
{
typedef pcl::PointCloud<pcl::PointXYZ> 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<pcl::PointXYZ> filter;
filter.setFilterFieldName(axis);
filter.setFilterLimits(min, max);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::string & axis,
float min,
float max)
{
typedef pcl::PointCloud<pcl::PointXYZRGB> 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<pcl::PointXYZRGB> filter;
filter.setFilterFieldName(axis);
filter.setFilterLimits(min, max);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr removeNaNFromPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud, *output, indices);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr removeNaNFromPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud, *output, indices);
return output;
}
pcl::PointCloud<pcl::PointNormal>::Ptr removeNaNNormalsFromPointCloud(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
{
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
std::vector<int> indices;
pcl::removeNaNNormalsFromPointCloud(*cloud, *output, indices);
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr removeNaNNormalsFromPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector<int> indices;
pcl::removeNaNNormalsFromPointCloud(*cloud, *output, indices);
return output;
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
typedef pcl::search::KdTree<pcl::PointXYZ> 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;
}
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
typedef pcl::search::KdTree<pcl::PointXYZRGB> 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;
}
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
}
pcl::IndicesPtr 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 output(new std::vector<int>());
if(cloud->size())
{
typedef pcl::search::KdTree<pcl::PointXYZ> KdTree;
typedef KdTree::Ptr KdTreePtr;
pcl::NormalEstimation<pcl::PointXYZ, 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;
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::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 pcl::search::KdTree<pcl::PointXYZRGB> KdTree;
typedef KdTree::Ptr KdTreePtr;
pcl::NormalEstimation<pcl::PointXYZRGB, 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;
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
pcl::IndicesPtr indices(new std::vector<int>);
return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
pcl::IndicesPtr indices(new std::vector<int>);
return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
typedef pcl::search::KdTree<pcl::PointXYZ> KdTree;
typedef KdTree::Ptr KdTreePtr;
KdTreePtr tree(new KdTree);
pcl::EuclideanClusterExtraction<pcl::PointXYZ> 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;
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
typedef pcl::search::KdTree<pcl::PointXYZRGB> KdTree;
typedef KdTree::Ptr KdTreePtr;
KdTreePtr tree(new KdTree);
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> 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;
}
pcl::IndicesPtr extractNegativeIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices)
{
pcl::IndicesPtr output(new std::vector<int>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(true);
extract.filter(*output);
return output;
}
pcl::IndicesPtr extractNegativeIndices(
const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices)
{
pcl::IndicesPtr output(new std::vector<int>);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(true);
extract.filter(*output);
return output;
}
}
}

647
corelib/src/util3d_mapping.cpp Normal file
View File

@@ -0,0 +1,647 @@
/*
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.
*/
#include "rtabmap/core/util3d_mapping.h"
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/io.h>
namespace rtabmap
{
namespace util3d
{
void occupancy2DFromLaserScan(
const cv::Mat & scan,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize)
{
if(scan.empty())
{
return;
}
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud = util3d::laserScanToPointCloud(scan);
//obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> scans;
scans.insert(std::make_pair(1, obstaclesCloud));
float xMin, yMin;
cv::Mat map8S = create2DMap(poses, scans, cellSize, false, xMin, yMin);
// find ground cells
std::list<int> groundIndices;
for(unsigned int i=0; i< map8S.total(); ++i)
{
if(map8S.data[i] == 0)
{
groundIndices.push_back(i);
}
}
// Convert to position matrices, get points to each center of the cells
ground = cv::Mat();
if(groundIndices.size())
{
ground = cv::Mat((int)groundIndices.size(), 1, CV_32FC2);
int i=0;
for(std::list<int>::iterator iter=groundIndices.begin();iter!=groundIndices.end(); ++iter)
{
int x = *iter / map8S.cols;
int y = *iter - x*map8S.cols;
ground.at<cv::Vec2f>(i)[0] = (float(y)+0.5)*cellSize + xMin;
ground.at<cv::Vec2f>(i)[1] = (float(x)+0.5)*cellSize + yMin;
++i;
}
}
// copy directly obstacles precise positions
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;
}
}
}
/**
* Create 2d Occupancy grid (CV_8S) from 2d occupancy
* -1 = unknown
* 0 = empty space
* 100 = obstacle
* @param poses
* @param occupancy <empty, occupied>
* @param cellSize m
* @param xMin
* @param yMin
* @param minMapSize minimum width (m)
* @param erode
*/
cv::Mat 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,
bool erode)
{
UASSERT(minMapSize >= 0.0f);
UDEBUG("cellSize=%f m, minMapSize=%f m, erode=%d", cellSize, minMapSize, erode?1:0);
UTimer timer;
std::map<int, cv::Mat> emptyLocalMaps;
std::map<int, cv::Mat> occupiedLocalMaps;
float minX=-minMapSize/2.0, minY=-minMapSize/2.0, maxX=minMapSize/2.0, maxY=minMapSize/2.0;
bool undefinedSize = minMapSize == 0.0f;
float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f,cosT=0.0f,sinT=0.0f;
cv::Mat affineTransform(2,3,CV_32FC1);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
if(undefinedSize)
{
minX = maxX = x;
minY = maxY = y;
undefinedSize = false;
}
else
{
if(minX > x)
minX = x;
else if(maxX < x)
maxX = x;
if(minY > y)
minY = y;
else if(maxY < y)
maxY = y;
}
if(uContains(occupancy, iter->first))
{
const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
cosT = cos(yaw);
sinT = sin(yaw);
affineTransform.at<float>(0,0) = cosT;
affineTransform.at<float>(0,1) = -sinT;
affineTransform.at<float>(1,0) = sinT;
affineTransform.at<float>(1,1) = cosT;
affineTransform.at<float>(0,2) = x;
affineTransform.at<float>(1,2) = y;
//ground
if(pair.first.rows)
{
UASSERT(pair.first.type() == CV_32FC2);
cv::Mat ground(pair.first.rows, pair.first.cols, pair.first.type());
cv::transform(pair.first, ground, affineTransform);
for(int i=0; i<ground.rows; ++i)
{
if(minX > ground.at<float>(i,0))
minX = ground.at<float>(i,0);
else if(maxX < ground.at<float>(i,0))
maxX = ground.at<float>(i,0);
if(minY > ground.at<float>(i,1))
minY = ground.at<float>(i,1);
else if(maxY < ground.at<float>(i,1))
maxY = ground.at<float>(i,1);
}
emptyLocalMaps.insert(std::make_pair(iter->first, ground));
}
//obstacles
if(pair.second.rows)
{
UASSERT(pair.second.type() == CV_32FC2);
cv::Mat obstacles(pair.second.rows, pair.second.cols, pair.second.type());
cv::transform(pair.second, obstacles, affineTransform);
for(int i=0; i<obstacles.rows; ++i)
{
if(minX > obstacles.at<float>(i,0))
minX = obstacles.at<float>(i,0);
else if(maxX < obstacles.at<float>(i,0))
maxX = obstacles.at<float>(i,0);
if(minY > obstacles.at<float>(i,1))
minY = obstacles.at<float>(i,1);
else if(maxY < obstacles.at<float>(i,1))
maxY = obstacles.at<float>(i,1);
}
occupiedLocalMaps.insert(std::make_pair(iter->first, obstacles));
}
}
}
UDEBUG("timer=%fs", timer.ticks());
cv::Mat map;
if(minX != maxX && minY != maxY)
{
//Get map size
float margin = cellSize*10.0f;
xMin = minX-margin;
yMin = minY-margin;
float xMax = maxX+margin;
float yMax = maxY+margin;
if(fabs((yMax - yMin) / cellSize) > 99999 ||
fabs((xMax - xMin) / cellSize) > 99999)
{
UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
"There's maybe an error with the poses provided! The map will not be created!",
xMin, yMin, xMax, yMax);
}
else
{
UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
for(std::map<int, Transform>::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
{
std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
if(iter!=emptyLocalMaps.end())
{
for(int i=0; i<iter->second.rows; ++i)
{
cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
map.at<char>(pt.y, pt.x) = 0; // free space
}
}
if(jter!=occupiedLocalMaps.end())
{
for(int i=0; i<jter->second.rows; ++i)
{
cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
map.at<char>(pt.y, pt.x) = 100; // obstacles
}
}
//UDEBUG("empty=%d occupied=%d", empty, occupied);
}
// fill holes and remove empty from obstacle borders
cv::Mat updatedMap = map.clone();
std::list<std::pair<int, int> > obstacleIndices;
for(int i=2; i<map.rows-2; ++i)
{
for(int j=2; j<map.cols-2; ++j)
{
if(map.at<char>(i, j) == -1 &&
map.at<char>(i+1, j) != -1 &&
map.at<char>(i-1, j) != -1 &&
map.at<char>(i, j+1) != -1 &&
map.at<char>(i, j-1) != -1)
{
updatedMap.at<char>(i, j) = 0;
}
else if(map.at<char>(i, j) == 100)
{
// obstacle/empty/unknown -> remove empty
// unknown/empty/obstacle -> remove empty
if(map.at<char>(i-1, j) == 0 &&
map.at<char>(i-2, j) == -1)
{
updatedMap.at<char>(i-1, j) = -1;
}
else if(map.at<char>(i+1, j) == 0 &&
map.at<char>(i+2, j) == -1)
{
updatedMap.at<char>(i+1, j) = -1;
}
if(map.at<char>(i, j-1) == 0 &&
map.at<char>(i, j-2) == -1)
{
updatedMap.at<char>(i, j-1) = -1;
}
else if(map.at<char>(i, j+1) == 0 &&
map.at<char>(i, j+2) == -1)
{
updatedMap.at<char>(i, j+1) = -1;
}
if(erode)
{
obstacleIndices.push_back(std::make_pair(i, j));
}
}
else if(map.at<char>(i, j) == 0)
{
// obstacle/empty/obstacle -> remove empty
if(map.at<char>(i-1, j) == 100 &&
map.at<char>(i+1, j) == 100)
{
updatedMap.at<char>(i, j) = -1;
}
else if(map.at<char>(i, j-1) == 100 &&
map.at<char>(i, j+1) == 100)
{
updatedMap.at<char>(i, j) = -1;
}
}
}
}
map = updatedMap;
if(erode)
{
// remove obstacles which touch to empty cells but not unknown cells
cv::Mat erodedMap = map.clone();
for(std::list<std::pair<int,int> >::iterator iter = obstacleIndices.begin();
iter!= obstacleIndices.end();
++iter)
{
int i = iter->first;
int j = iter->second;
bool touchEmpty = map.at<char>(i+1, j) == 0 ||
map.at<char>(i-1, j) == 0 ||
map.at<char>(i, j+1) == 0 ||
map.at<char>(i, j-1) == 0;
if(touchEmpty && map.at<char>(i+1, j) != -1 &&
map.at<char>(i-1, j) != -1 &&
map.at<char>(i, j+1) != -1 &&
map.at<char>(i, j-1) != -1)
{
erodedMap.at<char>(i, j) = 0; // empty
}
}
map = erodedMap;
}
}
}
UDEBUG("timer=%fs", timer.ticks());
return map;
}
/**
* Create 2d Occupancy grid (CV_8S)
* -1 = unknown
* 0 = empty space
* 100 = obstacle
* @param poses
* @param scans
* @param cellSize m
* @param unknownSpaceFilled if false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle)
* @param xMin
* @param yMin
*/
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)
{
UDEBUG("poses=%d, scans = %d", poses.size(), scans.size());
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
pcl::PointCloud<pcl::PointXYZ> minMax;
if(minMapSize > 0.0f)
{
minMax.push_back(pcl::PointXYZ(-minMapSize/2.0, -minMapSize/2.0, 0));
minMax.push_back(pcl::PointXYZ(minMapSize/2.0, minMapSize/2.0, 0));
}
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(uContains(scans, iter->first) && scans.at(iter->first)->size())
{
UASSERT(!iter->second.isNull());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(scans.at(iter->first), iter->second);
pcl::PointXYZ min, max;
pcl::getMinMax3D(*cloud, min, max);
minMax.push_back(min);
minMax.push_back(max);
minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
localScans.insert(std::make_pair(iter->first, cloud));
}
}
cv::Mat map;
if(minMax.size())
{
//Get map size
pcl::PointXYZ min, max;
pcl::getMinMax3D(minMax, min, max);
// Added X2 to make sure that all points are inside the map (when rounded to integer)
float marging = cellSize*10.0f;
xMin = min.x-marging;
yMin = min.y-marging;
float xMax = max.x+marging;
float yMax = max.y+marging;
UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
UTimer timer;
map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
std::vector<float> maxSquaredLength(localScans.size(), 0.0f);
int j=0;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
const Transform & pose = poses.at(iter->first);
cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
for(unsigned int i=0; i<iter->second->size(); ++i)
{
cv::Point2i end((iter->second->points[i].x-xMin)/cellSize + 0.5f, (iter->second->points[i].y-yMin)/cellSize + 0.5f);
map.at<char>(end.y, end.x) = 100; // obstacle
rayTrace(start, end, map, true); // trace free space
if(unknownSpaceFilled)
{
float dx = iter->second->points[i].x - pose.x();
float dy = iter->second->points[i].y - pose.y();
float l = dx*dx + dy*dy;
if(l > maxSquaredLength[j])
{
maxSquaredLength[j] = l;
}
}
}
++j;
}
UDEBUG("Ray trace known space=%fs", timer.ticks());
// now fill unknown spaces
if(unknownSpaceFilled)
{
j=0;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
if(iter->second->size() > 1 && maxSquaredLength[j] > 0.0f)
{
float maxLength = sqrt(maxSquaredLength[j]);
if(maxLength > cellSize)
{
// compute angle
float a = (CV_PI/2.0f) / (maxLength / cellSize);
//UWARN("a=%f PI/256=%f", a, CV_PI/256.0f);
UASSERT_MSG(a >= 0 && a < 5.0f*CV_PI/8.0f, uFormat("a=%f length=%f cell=%f", a, maxLength, cellSize).c_str());
const Transform & pose = poses.at(iter->first);
cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
//UWARN("maxLength = %f", maxLength);
//rotate counterclockwise from the first point until we pass the last point
cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a),
sin(a), cos(a));
cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
origin.at<float>(0) = pose.x();
origin.at<float>(1) = pose.y();
endFirst.at<float>(0) = iter->second->points[0].x;
endFirst.at<float>(1) = iter->second->points[0].y;
endLast.at<float>(0) = iter->second->points[iter->second->points.size()-1].x;
endLast.at<float>(1) = iter->second->points[iter->second->points.size()-1].y;
//UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1));
//UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1));
//UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1));
cv::Mat tmp = (endFirst - origin);
cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*maxLength) + origin;
cv::Mat endLastVector(3,1,CV_32F), endRotatedVector(3,1,CV_32F);
endLastVector.at<float>(0) = endLast.at<float>(0) - origin.at<float>(0);
endLastVector.at<float>(1) = endLast.at<float>(1) - origin.at<float>(1);
endLastVector.at<float>(2) = 0.0f;
endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
endRotatedVector.at<float>(2) = 0.0f;
//UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1));
while(endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
{
cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize + 0.5f, (endRotated.at<float>(1)-yMin)/cellSize + 0.5f);
//end must be inside the grid
end.x = end.x < 0?0:end.x;
end.x = end.x >= map.cols?map.cols-1:end.x;
end.y = end.y < 0?0:end.y;
end.y = end.y >= map.rows?map.rows-1:end.y;
rayTrace(start, end, map, true); // trace free space
// next point
endRotated = rotation*(endRotated - origin) + origin;
endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
//UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1));
}
}
}
++j;
}
UDEBUG("Fill empty space=%fs", timer.ticks());
}
}
return map;
}
void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle)
{
UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
cv::Point2i ptA, ptB;
ptA = start;
ptB = end;
float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
bool swapped = false;
if(slope<-1.0f || slope>1.0f)
{
// swap x and y
slope = 1.0f/slope;
int tmp = ptA.x;
ptA.x = ptA.y;
ptA.y = tmp;
tmp = ptB.x;
ptB.x = ptB.y;
ptB.y = tmp;
swapped = true;
}
float b = ptA.y - slope*ptA.x;
for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
{
int upperbound = float(x)*slope + b;
int lowerbound = upperbound;
if(x != ptA.x)
{
lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
}
if(lowerbound > upperbound)
{
int tmp = upperbound;
upperbound = lowerbound;
lowerbound = tmp;
}
if(!swapped)
{
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
}
else
{
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.cols, x, slope, b, x).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.cols, x+1, slope, b, x).c_str());
}
for(int y = lowerbound; y<=(int)upperbound; ++y)
{
char * v;
if(swapped)
{
v = &grid.at<char>(x, y);
}
else
{
v = &grid.at<char>(y, x);
}
if(*v == 100 && stopOnObstacle)
{
return;
}
else
{
*v = 0; // free space
}
}
}
}
//convert to gray scaled map
cv::Mat convertMap2Image8U(const cv::Mat & map8S)
{
UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
for (int i = 0; i < map8S.rows; ++i)
{
for (int j = 0; j < map8S.cols; ++j)
{
char v = map8S.at<char>(i, j);
unsigned char gray;
if(v == 0)
{
gray = 178;
}
else if(v == 100)
{
gray = 0;
}
else // -1
{
gray = 89;
}
map8U.at<unsigned char>(i, j) = gray;
}
}
return map8U;
}
void projectCloudOnXYPlane(
pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
cloud->at(i).z = 0;
}
}
}
}

547
corelib/src/util3d_registration.cpp Normal file
View File

@@ -0,0 +1,547 @@
/*
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.
*/
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d.h"
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/sample_consensus/sac_model_registration.h>
#include <pcl/sample_consensus/ransac.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap
{
namespace util3d
{
// Get transform from cloud2 to cloud1
Transform transformFromXYZCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
double inlierThreshold,
int iterations,
bool refineModel,
double refineModelSigma,
int refineModelIterations,
std::vector<int> * inliersOut,
double * varianceOut)
{
//NOTE: this method is a mix of two methods:
// - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
// - refineModel() in pcl/sample_consensus/sac.h
if(varianceOut)
{
*varianceOut = 1.0;
}
Transform transform;
if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
{
// RANSAC
UDEBUG("iterations=%d inlierThreshold=%f", iterations, inlierThreshold);
std::vector<int> source_indices (cloud2->size());
std::vector<int> target_indices (cloud1->size());
// Copy the query-match indices
for (int i = 0; i < (int)cloud1->size(); ++i)
{
source_indices[i] = i;
target_indices[i] = i;
}
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
pcl::SampleConsensusModelRegistration<pcl::PointXYZ>::Ptr model;
model.reset(new pcl::SampleConsensusModelRegistration<pcl::PointXYZ>(cloud2, source_indices));
// Pass the target_indices
model->setInputTarget (cloud1, target_indices);
// Create a RANSAC model
pcl::RandomSampleConsensus<pcl::PointXYZ> sac (model, inlierThreshold);
sac.setMaxIterations(iterations);
// Compute the set of inliers
if(sac.computeModel())
{
std::vector<int> inliers;
Eigen::VectorXf model_coefficients;
sac.getInliers(inliers);
sac.getModelCoefficients (model_coefficients);
if (refineModel)
{
double inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
double error_threshold = inlierThreshold;
double sigma_sqr = refineModelSigma * refineModelSigma;
int refine_iterations = 0;
bool inlier_changed = false, oscillating = false;
std::vector<int> new_inliers, prev_inliers = inliers;
std::vector<size_t> inliers_sizes;
Eigen::VectorXf new_model_coefficients = model_coefficients;
do
{
// Optimize the model coefficients
model->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
inliers_sizes.push_back (prev_inliers.size ());
// Select the new inliers based on the optimized coefficients and new threshold
model->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
(int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);
if (new_inliers.empty ())
{
++refine_iterations;
if (refine_iterations >= refineModelIterations)
{
break;
}
continue;
}
// Estimate the variance and the new threshold
double variance = model->computeVariance ();
error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f) on iteration %d out of %d.",
error_threshold, variance, refine_iterations, refineModelIterations);
inlier_changed = false;
std::swap (prev_inliers, new_inliers);
// If the number of inliers changed, then we are still optimizing
if (new_inliers.size () != prev_inliers.size ())
{
// Check if the number of inliers is oscillating in between two values
if (inliers_sizes.size () >= 4)
{
if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
{
oscillating = true;
break;
}
}
inlier_changed = true;
continue;
}
// Check the values of the inlier set
for (size_t i = 0; i < prev_inliers.size (); ++i)
{
// If the value of the inliers changed, then we are still optimizing
if (prev_inliers[i] != new_inliers[i])
{
inlier_changed = true;
break;
}
}
}
while (inlier_changed && ++refine_iterations < refineModelIterations);
// If the new set of inliers is empty, we didn't do a good job refining
if (new_inliers.empty ())
{
UWARN ("RANSAC refineModel: Refinement failed: got an empty set of inliers!");
}
if (oscillating)
{
UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
}
std::swap (inliers, new_inliers);
model_coefficients = new_model_coefficients;
}
if (inliers.size() >= 3)
{
if(inliersOut)
{
*inliersOut = inliers;
}
if(varianceOut)
{
*varianceOut = model->computeVariance();
}
// get best transformation
Eigen::Matrix4f bestTransformation;
bestTransformation.row (0) = model_coefficients.segment<4>(0);
bestTransformation.row (1) = model_coefficients.segment<4>(4);
bestTransformation.row (2) = model_coefficients.segment<4>(8);
bestTransformation.row (3) = model_coefficients.segment<4>(12);
transform = Transform::fromEigen4f(bestTransformation);
UDEBUG("RANSAC inliers=%d/%d tf=%s", (int)inliers.size(), (int)cloud1->size(), transform.prettyPrint().c_str());
return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
}
else
{
UDEBUG("RANSAC: Model with inliers < 3");
}
}
else
{
UDEBUG("RANSAC: Failed to find model");
}
}
else
{
UDEBUG("Not enough points to compute the transform");
}
return Transform();
}
// return transform from source to target (All points must be finite!!!)
Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool * hasConvergedOut,
double * variance,
int * correspondencesOut)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
icp.setInputTarget (cloud_target);
icp.setInputSource (cloud_source);
// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (transformationEpsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
icp.align (*cloud_source_registered);
bool hasConverged = icp.hasConverged();
// compute variance
if((correspondencesOut || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
est->setInputTarget(cloud_target);
est->setInputSource(cloud_source_registered);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(variance)
{
if(correspondences.size()>=3)
{
std::vector<double> distances(correspondences.size());
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
}
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
*variance = (2.1981 * median_error_sqr);
}
else
{
hasConverged = false;
*variance = -1.0;
}
}
if(correspondencesOut)
{
*correspondencesOut = (int)correspondences.size();
}
}
else
{
if(correspondencesOut)
{
*correspondencesOut = 0;
}
if(variance)
{
*variance = -1;
}
}
if(hasConvergedOut)
{
*hasConvergedOut = hasConverged;
}
return Transform::fromEigen4f(icp.getFinalTransformation());
}
// return transform from source to target (All points/normals must be finite!!!)
Transform icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool * hasConvergedOut,
double * variance,
int * correspondencesOut)
{
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
// Set the input source and target
icp.setInputTarget (cloud_target);
icp.setInputSource (cloud_source);
pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
icp.setTransformationEstimation(est);
// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (transformationEpsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointNormal>);
icp.align (*cloud_source_registered);
bool hasConverged = icp.hasConverged();
// compute variance
if((correspondencesOut || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
est->setInputTarget(cloud_target);
est->setInputSource(cloud_source_registered);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(variance)
{
if(correspondences.size()>=3)
{
std::vector<double> distances(correspondences.size());
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
}
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
*variance = (2.1981 * median_error_sqr);
}
else
{
hasConverged = false;
*variance = -1.0;
}
}
if(correspondencesOut)
{
*correspondencesOut = (int)correspondences.size();
}
}
else
{
if(correspondencesOut)
{
*correspondencesOut = 0;
}
if(variance)
{
*variance = -1;
}
}
if(hasConvergedOut)
{
*hasConvergedOut = hasConverged;
}
return Transform::fromEigen4f(icp.getFinalTransformation());
}
// return transform from source to target (All points must be finite!!!)
Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool * hasConvergedOut,
double * variance,
int * correspondencesOut)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
icp.setInputTarget (cloud_target);
icp.setInputSource (cloud_source);
pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
icp.setTransformationEstimation(est);
// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (transformationEpsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
icp.align (*cloud_source_registered);
bool hasConverged = icp.hasConverged();
// compute variance
if((correspondencesOut || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
est->setInputTarget(cloud_target);
est->setInputSource(cloud_source_registered);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(variance)
{
if(correspondences.size()>=3)
{
std::vector<double> distances(correspondences.size());
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
}
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
*variance = (2.1981 * median_error_sqr);
}
else
{
hasConverged = false;
*variance = -1.0;
}
}
if(correspondencesOut)
{
*correspondencesOut = (int)correspondences.size();
}
}
else
{
if(correspondencesOut)
{
*correspondencesOut = 0;
}
if(variance)
{
*variance = -1;
}
}
if(hasConvergedOut)
{
*hasConvergedOut = hasConverged;
}
return Transform::fromEigen4f(icp.getFinalTransformation());
}
// If "voxel" > 0, "samples" is ignored
pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
int decimation,
double maxDepth,
float voxel,
int samples,
const Transform & transform)
{
UASSERT(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = cloudFromDepth(
depth,
cx,
cy,
fx,
fy,
decimation);
if(cloud->size())
{
if(maxDepth>0.0)
{
cloud = passThrough(cloud, "z", 0, maxDepth);
}
if(cloud->size())
{
if(voxel>0)
{
cloud = voxelize(cloud, voxel);
}
else if(samples>0 && (int)cloud->size() > samples)
{
cloud = sampling(cloud, samples);
}
if(cloud->size())
{
if(!transform.isNull() && !transform.isIdentity())
{
cloud = transformPointCloud(cloud, transform);
}
}
}
}
return cloud;
}
}
}

156
corelib/src/util3d_surface.cpp Normal file
View File

@@ -0,0 +1,156 @@
/*
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.
*/
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_filtering.h"
#include <pcl/search/kdtree.h>
#include <pcl/surface/gp3.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/surface/mls.h>
namespace rtabmap
{
namespace util3d
{
pcl::PolygonMesh::Ptr createMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
float gp3SearchRadius,
float gp3Mu,
int gp3MaximumNearestNeighbors,
float gp3MaximumSurfaceAngle,
float gp3MinimumAngle,
float gp3MaximumAngle,
bool gp3NormalConsistency)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud(cloudWithNormals);
// Create search tree*
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
tree2->setInputCloud (cloudWithNormalsNoNaN);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (gp3SearchRadius);
// Set typical values for the parameters
gp3.setMu (gp3Mu);
gp3.setMaximumNearestNeighbors (gp3MaximumNearestNeighbors);
gp3.setMaximumSurfaceAngle(gp3MaximumSurfaceAngle); // 45 degrees
gp3.setMinimumAngle(gp3MinimumAngle); // 10 degrees
gp3.setMaximumAngle(gp3MaximumAngle); // 120 degrees
gp3.setNormalConsistency(gp3NormalConsistency);
// Get result
gp3.setInputCloud (cloudWithNormalsNoNaN);
gp3.setSearchMethod (tree2);
gp3.reconstruct (*mesh);
return mesh;
}
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
// Normal estimation*
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
n.setInputCloud (cloud);
n.setSearchMethod (tree);
n.setKSearch (normalKSearch);
n.compute (*normals);
//* normals should not contain the point normals + surface curvatures
// Concatenate the XYZ and normal fields*
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
//* cloud_with_normals = cloud + normals*/
return cloud_with_normals;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud);
// Normal estimation*
pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
n.setInputCloud (cloud);
n.setSearchMethod (tree);
n.setKSearch (normalKSearch);
n.compute (*normals);
//* normals should not contain the point normals + surface curvatures
// Concatenate the XYZ and normal fields*
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
//* cloud_with_normals = cloud + normals*/
return cloud_with_normals;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float smoothingSearchRadius,
bool smoothingPolynomialFit)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud);
// Init object (second point type is for the normals, even if unused)
pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
mls.setComputeNormals (true);
// Set parameters
mls.setInputCloud (cloud);
mls.setPolynomialFit (smoothingPolynomialFit);
mls.setSearchMethod (tree);
mls.setSearchRadius (smoothingSearchRadius);
// Reconstruct
mls.process (*cloud_with_normals);
return cloud_with_normals;
}
}
}

74
corelib/src/util3d_transforms.cpp Normal file
View File

@@ -0,0 +1,74 @@
/*
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.
*/
#include "rtabmap/core/util3d_transforms.h"
#include <pcl/common/transforms.h>
namespace rtabmap
{
namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
const typename pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & transform)
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
PointCloudPtr output(new PointCloud);
pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & transform)
{
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
PointCloudPtr output(new PointCloud);
pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
return output;
}
pcl::PointXYZ transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform)
{
return pcl::transformPoint(pt, transform.toEigen3f());
}
pcl::PointXYZRGB transformPoint(
const pcl::PointXYZRGB & pt,
const Transform & transform)
{
return pcl::transformPoint(pt, transform.toEigen3f());
}
}
}