从点云重建网格## 点云贪心三角化原理
转载
适用于采样点云来自表面连续光滑的曲面，并且点云的密度变化比较均匀的情况,所以使用之前，一般先进行滑动最小二乘滤波以满足条件。

1. 流程
   (1):将点云通过法线投影到某一二维坐标平面。
   (2):然后对投影得到的点云做平面内的三角化，从而得到各点的拓扑连接关系。平面三角化的过程中用到了基于Delaunay三角剖分的空间区域增长算法
   (3):最后根据平面内投影点的拓扑连接关系确定各原始三维点间的拓扑连接，所得三角网格即为重建得到的曲面模型

2. Delaunay三角剖析
   百度百科
   Bowyer-Watson算法：
   （1）：构造一个超级三角形，包含所有散点，放入三角形链表
   （2）：将点集中的散点依次插入，在三角形链表中找出外接圆包含插入点的三角形（称为该点的影响三角形），删除影响三角形的公共边，将插入点同影响三角形的全部顶点连接起来，完成一个点在Delaunay三角形链表中的插入
   （3）：根据优化准则对局部新形成的三角形优化。将形成的三角形放入Delaunay三角形链表
   （4）：循环执行上述第2步，直到所有散点插入完毕。

从点云重建网格

#include <Eigen/Core>
#include <Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/surface/surfel_smoothing.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/impl/mls.hpp>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
using namespace std;
typedef pcl::PointXYZRGB PointT;                    //定义点的类型 XYZ，RGB
typedef pcl::PointCloud<PointT> Pointcloud;         //定义点云
typedef pcl::PointCloud<PointT>::Ptr PointcloudPtr; //定义点云指针
typedef pcl::PointXYZRGBNormal SurfelT;             //网格数据类型 XYZ，RGB，强度曲率
//PointXYZRGBNormal{
//      PCL_ADD_POINT4D;//xyz实际坐标或者xyz1齐次做标
//      PCL_ADD_NORMAL4D;//bgr颜色
//      union
    // {
    //   struct
    //   {
    //     float intensity;//强度
    //     float curvature;//曲率
    //   };
    //   float data_c[4];
    // };
//}
typedef pcl::PointCloud<SurfelT>  Surfelcloud;      //曲率的点云
typedef pcl::PointCloud<SurfelT>::Ptr SurfcloudPtr; //曲率的点云指针
SurfcloudPtr 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>);//Flann kd树查找
    mls.setSearchMethod(tree);//设置查找方法
    mls.setSearchRadius(radius);//设置球面查找半径，半径以内全部为临近点
    mls.setComputeNormals(true);//设置计算法向量
    mls.setSqrGaussParam(radius*radius);//设置高斯函数，用作权重
    mls.setPolynomialFit(polynomial_order>1);//使用多项式拟合曲面和法向量的条件，如果满足条件设为1
    mls.setPolynomialOrder(polynomial_order);//设置拟合多项式的次数
    mls.setInputCloud(input);//输入点云
    SurfcloudPtr output(new Surfelcloud);
    mls.process(*output);//输出结果
    return output;
}
pcl::PolygonMeshPtr TriangularMesh(const SurfcloudPtr &surfels){
    //PolygonMeshPtr
    //PCLHeader header:序号，时间戳，frame id
    //PCLPointCloud2 cloud：
    //  PCLHeader 帧头，
    //  uint32_t 高，
    //  uint32_t 宽，
    //  vector< ::pcl::PCLPointField> field
    //        PCLPointField=(string name,uint32_t offset,pcl::uint8_t datatype;pcl::uint32_t count;),
    //  bool is_bigendian 默认flase为小端模式，通过宏BOOST_BIG_ENDIAN/BOOST_LITTLE_ENDIAN判断
    //  uint32_t point_step，
    //  uint32_t row_step,
    //  vector<pcl::uint8_t> data   
    //  is_dense(false)  是否稠密含有inf nan为flase
    //vector< ::pcl::Vertices> polygons  vector<uint32_t> vertices;
    pcl::search::KdTree<SurfelT>::Ptr tree(new pcl::search::KdTree<SurfelT>);
    tree->setInputCloud(surfels);//输入数据
    
    //初始化目标
    pcl::GreedyProjectionTriangulation<SurfelT> gp3;//3角贪心算法
    pcl::PolygonMeshPtr triangles(new pcl::PolygonMesh);//创建指针
    
    //设置相邻点的最大距离
    gp3.setSearchRadius(0.05);//设置球面半径，在3角化的k最邻点使用
    
    //设置相关参数
    gp3.setMu(2.5);//设置每个点的最终搜索半径的倍数
    gp3.setMaximumNearestNeighbors(100);//设置最邻点的个数
    gp3.setMaximumSurfaceAngle(M_PI/4);//点的法线与查询点法线的偏差大于此值，不进行三角剖分
    gp3.setMaximumAngle(2*M_PI/3);//三角化的最大值
    gp3.setMinimumAngle(M_PI/18);//三角化的最小值
    gp3.setNormalConsistency(true);//如果法线方向一致设为True

    //得到结果
    gp3.setInputCloud(surfels);//输入点
    gp3.setSearchMethod(tree);//输入查找树
    gp3.reconstruct(*triangles);//得到重建后的网格
    return triangles;
}
int main(int argc, const char** argv) {
    PointcloudPtr cloud(new Pointcloud);//加载点云
    pcl::io::loadPCDFile("/home/n1/notes/map/sufel_mapping/map.pcd",*cloud);//加载
    cout<<"点云大小"<<cloud->size()<<endl;
    cout<<"计算 法向量"<<endl;
    double mls_radius=0.05,ploynomial_order=2;
    auto surfels=reconstructSurface(cloud,mls_radius,ploynomial_order);

    cout<<"计算 mesh"<<endl;
    pcl::PolygonMeshPtr mesh=TriangularMesh(surfels);

    pcl::visualization::PCLVisualizer vis;
    vis.addPolylineFromPolygonMesh(*mesh,"mesh frame");
    vis.addPolygonMesh(*mesh,"mesh");
    vis.resetCamera();
    vis.spin();
    return 0;
}