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

Mesh reconstruction update: fixed normals computation (with and without MLS)

Browse Source
This commit is contained in:
Mathieu Labbé
2015-08-11 17:19:12 -04:00
parent 9c653655bd
commit abdd3de773
16 changed files with 580 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -46,6 +46,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
@@ -141,6 +144,32 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
float radiusSearch,
int minNeighborsInRadius = 0);
/**
* For convenience.
*/
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
float radiusSearch,
int minNeighborsInRadius = 0);
/**
* Subtract a cloud from another one using radius filtering.
* @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 cloud the input cloud to subtract.
* @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
* @param radiusSearch the radius in meter.
* @return the indices of the points satisfying the parameters.
*/
pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
int minNeighborsInRadius = 0);
/**
* For convenience.

40
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
@@ -49,7 +48,7 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
float gp3MaximumSurfaceAngle = M_PI/4,
float gp3MinimumAngle = M_PI/18,
float gp3MaximumAngle = 2*M_PI/3,
bool gp3NormalConsistency = false);
bool gp3NormalConsistency = true);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -62,7 +61,42 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormalsSmoothed(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float smoothingSearchRadius = 0.025,
bool smoothingPolynomialFit = true);
bool smoothingPolynomialFit = true,
float voxelSize = 0.0f);
void RTABMAP_EXP adjustNormalsToViewPoints(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud);
template<typename pointT>
std::vector<pcl::Vertices> normalizePolygonsSide(
const pcl::PointCloud<pointT> & cloud,
const std::vector<pcl::Vertices> & polygons,
const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0))
{
std::vector<pcl::Vertices> output(polygons.size());
for(unsigned int i=0; i<polygons.size(); ++i)
{
pcl::Vertices polygon = polygons[i];
Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
Eigen::Vector3f n = (v1.cross(v2)).normalized();
Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();
float result = n.dot(p);
if(result < 0)
{
//reverse vertices order
int tmp = polygon.vertices[0];
polygon.vertices[0] = polygon.vertices[2];
polygon.vertices[2] = tmp;
}
output[i] = polygon;
}
return output;
}
} // namespace util3d
} // namespace rtabmap

3
corelib/include/rtabmap/core/util3d_transforms.h
View File

@@ -46,6 +46,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & transform);
pcl::PointXYZ RTABMAP_EXP transformPoint(
const pcl::PointXYZ & pt,

88
corelib/src/util3d_filtering.cpp
View File

@@ -72,6 +72,18 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize)
{
UASSERT(voxelSize > 0.0f);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
@@ -357,6 +369,82 @@ pcl::IndicesPtr subtractFiltering(
}
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
}
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
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
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
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,

44
corelib/src/util3d_surface.cpp
View File

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/utilite/ULogger.h"
#include <pcl/search/kdtree.h>
#include <pcl/surface/gp3.h>
#include <pcl/features/normal_3d_omp.h>
@@ -68,12 +69,16 @@ pcl::PolygonMesh::Ptr createMesh(
gp3.setMinimumAngle(gp3MinimumAngle); // 10 degrees
gp3.setMaximumAngle(gp3MaximumAngle); // 120 degrees
gp3.setNormalConsistency(gp3NormalConsistency);
gp3.setConsistentVertexOrdering(gp3NormalConsistency);
// Get result
gp3.setInputCloud (cloudWithNormalsNoNaN);
gp3.setSearchMethod (tree2);
gp3.reconstruct (*mesh);
//UASSERT(mesh->cloud.data.size()/mesh->cloud.point_step == cloudWithNormalsNoNaN->size());
//mesh->polygons = normalizePolygonsSide(*cloudWithNormalsNoNaN, mesh->polygons);
return mesh;
}
@@ -128,7 +133,8 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float smoothingSearchRadius,
bool smoothingPolynomialFit)
bool smoothingPolynomialFit,
float voxelSize)
{
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>);
@@ -144,6 +150,11 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
mls.setPolynomialFit (smoothingPolynomialFit);
mls.setSearchMethod (tree);
mls.setSearchRadius (smoothingSearchRadius);
if(voxelSize > 0.0f)
{
mls.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>::VOXEL_GRID_DILATION);
mls.setDilationVoxelSize(voxelSize);
}
// Reconstruct
mls.process (*cloud_with_normals);
@@ -151,6 +162,37 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
return cloud_with_normals;
}
void adjustNormalsToViewPoints(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud)
{
if(viewpoints->size() && cloud.size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (viewpoints);
for(unsigned int i=0; i<cloud.size(); ++i)
{
std::vector<int> indices;
std::vector<float> dist;
tree->nearestKSearch(pcl::PointXYZ(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z), 1, indices, dist);
UASSERT(indices.size() == 1);
Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud.points[i].getVector3fMap();
Eigen::Vector3f n(cloud.points[i].normal_x, cloud.points[i].normal_y, cloud.points[i].normal_z);
float result = v.dot(n);
if(result < 0)
{
//reverse normal
cloud.points[i].normal_x *= -1.0f;
cloud.points[i].normal_y *= -1.0f;
cloud.points[i].normal_z *= -1.0f;
}
}
}
}
}
}

8
corelib/src/util3d_transforms.cpp
View File

@@ -51,6 +51,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f());
return output;
}
pcl::PointXYZ transformPoint(
const pcl::PointXYZ & pt,