通过点云计算点云法线，通过法线建立网格地图

//
// Created by gaoxiang on 19-4-25.
//

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/surfel_smoothing.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/impl/mls.hpp>

// typedefs
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud< PointT> PointCloud;
typedef pcl::PointCloud< PointT>::Ptr PointCloudPtr;
typedef pcl::PointXYZRGBNormal SurfelT;
typedef pcl::PointCloud< SurfelT> SurfelCloud;
typedef pcl::PointCloud< SurfelT>::Ptr SurfelCloudPtr;

SurfelCloudPtr reconstructSurface(
const PointCloudPtr &input, float radius, int polynomial_order) 根据点云重建表面纹理
{
pcl::MovingLeastSquares<PointT, SurfelT> mls;
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
mls.setSearchMethod(tree);
mls.setSearchRadius(radius);
mls.setComputeNormals(true);
mls.setSqrGaussParam(radius * radius);
mls.setPolynomialFit(polynomial_order > 1);
mls.setPolynomialOrder(polynomial_order);
mls.setInputCloud(input);设置输入点云
SurfelCloudPtr output(new SurfelCloud);
mls.process(*output);process之后的纹理地图存入output
return (output);
}

pcl::PolygonMeshPtr triangulateMesh(const SurfelCloudPtr &surfels) {
// Create search tree*
pcl::search::KdTree< SurfelT>::Ptr tree(new pcl::search::KdTree< SurfelT>);
tree->setInputCloud(surfels);
、、
// Initialize objects
pcl::GreedyProjectionTriangulation gp3;
pcl::PolygonMeshPtr triangles(new pcl::PolygonMesh);
、、
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius(0.05);
、、
// 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(true);
、、
// Get result
gp3.setInputCloud(surfels);
gp3.setSearchMethod(tree);
gp3.reconstruct(*triangles);
、、
return triangles;
}

int main(int argc, char **argv) {
、、
// Load the points
PointCloudPtr cloud(new PointCloud);
if (argc == 0 || pcl::io::loadPCDFile(argv[1], *cloud)加载点云文件) {
cout << “failed to load point cloud!”;
return 1;
}
cout << "point cloud loaded, points: " << cloud->points.size() << endl;
、、
// Compute surface elements
cout << "computing normals … " << endl;
double mls_radius = 0.05, polynomial_order = 2;
auto surfels = reconstructSurface(cloud, mls_radius, polynomial_order);返回纹理地图surfels
、、
// Compute a greedy surface triangulation
cout << "computing mesh … " << endl;
pcl::PolygonMeshPtr mesh = triangulateMesh(surfels);返回三角化网格地图mesh
、、
cout << "display mesh … " << endl;
pcl::visualization::PCLVisualizer vis;创建PCL可视化变量vis
vis.addPolylineFromPolygonMesh(*mesh, “mesh frame”);
vis.addPolygonMesh(*mesh, “mesh”);
vis.resetCamera();
vis.spin();
}