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

Added some parameters for mesh reconstruction

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1315 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-03 19:28:59 +00:00
parent b875d8f7be
commit 0fc6228b4f
6 changed files with 395 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -247,7 +247,17 @@ Transform RTABMAP_EXP icp2D(
double & fitnessScore);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
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,
@@ -343,7 +353,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
bool fastNonmaxSuppression=true,
float maxDepth = 5.0f);
pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float maxEdgeLength = 0.025, bool smoothing = true);
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,
float gp3NormalConsistency = false);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, Transform> & poses,

122
corelib/src/util3d.cpp
View File

@@ -1247,7 +1247,9 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
return transformFromEigen4f(icp.getFinalTransformation());
}
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
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>);
@@ -1258,7 +1260,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(const pcl::PointCloud<pcl:
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
n.setInputCloud (cloud);
n.setSearchMethod (tree);
n.setKSearch (20);
n.setKSearch (normalKSearch);
n.compute (*normals);
//* normals should not contain the point normals + surface curvatures
@@ -1269,6 +1271,56 @@ pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(const pcl::PointCloud<pcl:
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;
}
// 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,
@@ -1442,67 +1494,39 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
return points;
}
pcl::PolygonMesh::Ptr createMesh(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float maxEdgeLength, bool smoothing)
pcl::PolygonMesh::Ptr createMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
float gp3SearchRadius,
float gp3Mu,
int gp3MaximumNearestNeighbors,
float gp3MaximumSurfaceAngle,
float gp3MinimumAngle,
float gp3MaximumAngle,
float gp3NormalConsistency)
{
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);
if(smoothing)
{
// 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 (true);
mls.setSearchMethod (tree);
mls.setSearchRadius (maxEdgeLength);
// Reconstruct
mls.process (*cloud_with_normals);
}
else
{
// 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 (20);
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*/
}
cloud_with_normals = removeNaNNormalsFromPointCloud(cloud_with_normals);
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 (cloud_with_normals);
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 (maxEdgeLength);
gp3.setSearchRadius (gp3SearchRadius);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (100);
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(false);
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 (cloud_with_normals);
gp3.setInputCloud (cloudWithNormalsNoNaN);
gp3.setSearchMethod (tree2);
gp3.reconstruct (*mesh);