SLAM 将PCL点云转换成网格地图

PCL 3D点云没有表面信息，转换成网格地图之后可以构建出法线、纹理等信息。

先看一下输入的3D点云：

运行程序后会显示生成的网格地图，如下图所示：

看一下放大后的局部对比：

输入点云放大后每个点还是孤立的小方块。

输出的网格地图已经是一个平面了。

输入：map.pcd 3D点云地图

输出：没有保存输出结果

运行：编译后在cmd里才能运行，需要1个参数：输入的3D点云地图

示例：program.exe map.pcd

视频演示：https://www.bilibili.com/video/BV1RMdcBwEUC/

代码：

#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<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); 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); 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<SurfelT> 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); // Compute a greedy surface triangulation cout << "computing mesh ... " << endl; pcl::PolygonMeshPtr mesh = triangulateMesh(surfels); cout << "display mesh ... " << endl; pcl::visualization::PCLVisualizer vis; vis.addPolylineFromPolygonMesh(*mesh, "mesh frame"); vis.addPolygonMesh(*mesh, "mesh"); vis.resetCamera(); vis.spin(); }

参考：高翔《视觉SLAM十四讲》P333页附近