泊松重建dd

目录：
一、代码
1、无序点云快速三角化代码
2、泊松重建代码：
二、法线估计
三、泊松重建

一、代码

1、无序点云快速三角化代码
 // Load input file into a PointCloud<T> with an appropriate type
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCLPointCloud2 cloud_blob;
    pcl::io::loadPCDFile (in, cloud_blob);
    pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
    //* the data should be available in cloud

    // Normal estimation*
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud);
    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::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
    pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
    //* cloud_with_normals = cloud + normals

    // Create search tree*
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
    tree2->setInputCloud (cloud_with_normals);

    // Initialize objects
    pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
    pcl::PolygonMesh triangles;

    // Set the maximum distance between connected points (maximum edge length)
    gp3.setSearchRadius (0.1);

    // 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);

    // Get result
    gp3.setInputCloud (cloud_with_normals);
    gp3.setSearchMethod (tree2);
    gp3.reconstruct (triangles);

    // Additional vertex information
    std::vector<int> parts = gp3.getPartIDs();
    std::vector<int> states = gp3.getPointStates();

    //    pcl::io::savePolygonFile ("E:\\pcdxyzrgb\\object_mesh.vtk", triangles);
    pcl::io::savePLYFile(outs, triangles);
    std::cout<<parts.size()<<std::endl;
    std::cout<<states.size()<<std::endl;

2、泊松重建代码：

//typedef pcl::PointCloud  PointCloud;
void MainWindow::poisson_reconstruction(std::string in,std::string outs)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PCLPointCloud2 cloud_blob;
    pcl::io::loadPCDFile (in, cloud_blob);
    pcl::fromPCLPointCloud2 (cloud_blob, *object_cloud);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    pcl::copyPointCloud(*object_cloud, *cloud);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZRGB>());

    pcl::PassThrough<pcl::PointXYZRGB> filter;
    filter.setInputCloud(cloud);
    filter.filter(*filtered);
    cout << "passthrough filter complete" << endl;

    cout << "begin normal estimation" << endl;
    pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> ne;
    //    NormalEstimationOMP<pcl::PointXYZRGB, Normal> ne;//计算点云法向
    ne.setNumberOfThreads(20);//设定临近点
    ne.setInputCloud(filtered);
    ne.setRadiusSearch(0.1);//设定搜索半径setSearchRadius (0.1);
    Eigen::Vector4f centroid;
    compute3DCentroid(*filtered, centroid);//计算点云中心
    ne.setViewPoint(centroid[0], centroid[1], centroid[2]);//将向量计算原点置于点云中心

    PointCloud<Normal>::Ptr cloud_normals (new PointCloud<Normal>());
    ne.compute(*cloud_normals);
    cout << "normal estimation complete" << endl;
    cout << "reverse normals' direction" << endl;

    //将法向量反向
    for(size_t i = 0; i < cloud_normals->size(); ++i)
    {
        cloud_normals->points[i].normal_x *= -1;
        cloud_normals->points[i].normal_y *= -1;
        cloud_normals->points[i].normal_z *= -1;
    }

    //融合RGB点云和法向
    cout << "combine points and normals" << endl;
    PointCloud<PointXYZRGBNormal>::Ptr cloud_smoothed_normals(new PointCloud<PointXYZRGBNormal>());
    concatenateFields(*filtered, *cloud_normals, *cloud_smoothed_normals);

    //泊松重建
    cout << "begin poisson reconstruction" << endl;
    Poisson<PointXYZRGBNormal> poisson;
    //poisson.setDegree(2);
    poisson.setDepth(8);
    poisson.setSolverDivide (6);
    poisson.setIsoDivide (6);

    poisson.setConfidence(false);
    poisson.setManifold(false);
    poisson.setOutputPolygons(false);

    poisson.setInputCloud(cloud_smoothed_normals);
    PolygonMesh mesh;
    poisson.reconstruct(mesh);

    cout << "finish poisson reconstruction" << endl;

    //给mesh染色
    pcl::PointCloud<pcl::PointXYZRGB> cloud_color_mesh;
    pcl::fromPCLPointCloud2(mesh.cloud, cloud_color_mesh);

    pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
    kdtree.setInputCloud (cloud);
    // K nearest neighbor search
    int K = 5;
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);
    for(int i=0;i<cloud_color_mesh.points.size();++i)
    {
        uint8_t r = 0;
        uint8_t g = 0;
        uint8_t b = 0;
        float dist = 0.0;
        int red = 0;
        int green = 0;
        int blue = 0;
        uint32_t rgb;

        if ( kdtree.nearestKSearch (cloud_color_mesh.points[i], K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
        {
            for (int j = 0; j < pointIdxNKNSearch.size (); ++j)
            {

                r = cloud->points[ pointIdxNKNSearch[j] ].r;
                g = cloud->points[ pointIdxNKNSearch[j] ].g;
                b = cloud->points[ pointIdxNKNSearch[j] ].b;

                red += int(r);
                green += int(g);
                blue += int(b);
                dist += 1.0/pointNKNSquaredDistance[j];

                std::cout<<"red: "<<int(r)<<std::endl;
                std::cout<<"green: "<<int(g)<<std::endl;
                std::cout<<"blue: "<<int(b)<<std::endl;
                cout<<"dis:"<<dist<<endl;
            }
        }

        cloud_color_mesh.points[i].r = int(red/pointIdxNKNSearch.size ()+0.5);
        cloud_color_mesh.points[i].g = int(green/pointIdxNKNSearch.size ()+0.5);
        cloud_color_mesh.points[i].b = int(blue/pointIdxNKNSearch.size ()+0.5);


    }
    toPCLPointCloud2(cloud_color_mesh, mesh.cloud);


    pcl::io::savePLYFile(outs, mesh);
}

二、法线估计
法线估计是一个很重要的特征，常常在被使用在很多计算机视觉的应用里面，比如可以用来推出光源的位置，通过阴影与其他视觉影响。

给一个几何表面，去推断给定点的法线方向，即垂直向量的方向往往是不容易的。然而，在我们获取物体表面的点云数据后，有两大选择:

1.从已获取的点云数据集中得到潜在表面，并用表面网格化技术，来计算网格的表面法线。

2.使用近似值来推断点云数据集的表面法线。

尽管有很多法线估计的方法存在，但是我们这次将会讲的是最简单的方法。表面法线的问题可以近似化解为切面的问题，这个切面的问题又会变成最小二乘法拟合平面的问题。

解决表面法线估计的问题可以最终化简为对一个协方差矩阵的特征向量和特征值的分析(或者也叫PCA-Principal Component Analysis 主成分分析)，这个协方差矩阵是由查询点的最近邻产生的。对于每个点Pi,我们假设协方差矩阵C如下:

这里K指的是离点的最近的K个点，是最近邻的中心，是第j个特征值，是第j个特征向量。

下面是一段用来估计协方差矩阵的代码

// Placeholder for the 3x3 covariance matrix at each surface patch
Eigen::Matrix3f covariance_matrix;
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;

// Estimate the XYZ centroid
compute3DCentroid (cloud, xyz_centroid);

// Compute the 3x3 covariance matrix
computeCovarianceMatrix (cloud, xyz_centroid, covariance_matrix);

总的来说，因为数学上没有方法解决法线的符号，比如一个球面，法线可以指向球心，也可以背向球心。下面的两幅图像是描述厨房的点云图，右边的图像是通过高斯扩展图(EGI Extended Gaussian Image)，也常常叫做法线球。法线球是一个描述点云里面所有法线方向的一种方式。因为数据集是2.5D的，何为2.5D，你可以把上下，左右，前后看成一个D，然后现实生活里面往往不可能每个方向都兼顾，比如摄像机只能拍到前面的，所有是1(上下)+1(左右)+0.5(前)叫2.5D，当然这是建立在摄像机为单一视角的情况下，即摄像机不会动，一直是固定的。所以理论上，EGI图，即高斯球也应该是2.5D的，因为你摄像机是向前拍摄的，所以物体的法线也是向前的，然而因为这个算法的原因。主成分分析这个算法，不能解决法线的符号，所以导致了法线指向可能往前，也可能往后，导致整个球里面各个方向都可能存在着法线。

解决上面的法线方向不定的问题，我们得知道视角的向量，这就很简单了，只要法线和 视角与点的连线，这两条线的夹角是锐角，即这两个向量的点积大于0即可。

我们经过上述的处理后，图片就变成了这样

可以看到左边的那副图，法线的指向全都变成一个方向了，同时高斯扩展图只有前半个球面是有法线的，即我们的方法是有效的。

我们可以使用下面的方法去改变法线的方向

flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Vector4f &normal);
上面的这个方法就像我们刚才说的，只适用于单一视角的情况下。

选择正确的比例

就像前面说的，预测一个表面法线需要最近邻的方法，那么如何设置最近邻所需要的半径与点的个数呢？

这个问题是非常重要的，是对点的特征自动化评估的重要因素。为了更好的阐述这个问题，下面的图将显示选择一个小比例和大比例的影响。左边的图是比例(r和k)比较小的情况，我们发现它的法线是另人满意的，而右边就不尽人意了，你看那个桌角的位置，有一个法线出轨了，是一个小三，不属于上一个表面也不属于侧面，这就是比例选择太大的弊端，所以小比例往往更注重细节，更适合描述比较复杂的物体。

我们得根据不同的细节来选取比例，简单的说，如果边缘的曲率在杯子的把柄和圆柱体之间的时候，比例需要比较小来获取足够的细节。

下面是官方的一段代码段:

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>

{
pcl::PointCloudpcl::PointXYZ::Ptr cloud (new pcl::PointCloudpcl::PointXYZ);

… read, pass in or create a point cloud …

// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);

// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
pcl::search::KdTreepcl::PointXYZ::Ptr tree (new pcl::search::KdTreepcl::PointXYZ ());
ne.setSearchMethod (tree);

// Output datasets
pcl::PointCloudpcl::Normal::Ptr cloud_normals (new pcl::PointCloudpcl::Normal);

// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);

// Compute the features
ne.compute (*cloud_normals);

// cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
}
NormalEstimation这个类

做了以下3件事

1.得到p的最近邻

2.计算p的表面法线n

3.检查法线的朝向，然后拨乱反正。

默认的视角是(0,0,0)，可以通过下面的方法来更改

setViewPoint (float vpx, float vpy, float vpz);
计算一个点的法线

computePointNormal (const pcl::PointCloud &cloud, const std::vector &indices, Eigen::Vector4f &plane_parameters, float &curvature);
前面两个参数很好理解，plane_parameters包含了4个参数，前面三个是法线的(nx,ny,nz)坐标，加上一个 nc . p_plane (centroid here) + p的坐标，然后最后一个参数是曲率。

接下去是我写的一个代码，先通过从磁盘里面加载一个PCD文件，然后进行降低采样和滤波等操作，最后通过PCLVisulizer显示出来.

#include
#include <pcl/visualization/cloud_viewer.h>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/features/normal_3d.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/filters/statistical_outlier_removal.h>

int main ()
{
pcl::PointCloudpcl::PointXYZ::Ptr cloud_old (new pcl::PointCloudpcl::PointXYZ);
pcl::PointCloudpcl::PointXYZ::Ptr cloud_downsampled (new pcl::PointCloudpcl::PointXYZ);
pcl::PointCloudpcl::PointXYZ::Ptr cloud (new pcl::PointCloudpcl::PointXYZ);
pcl::io::loadPCDFile (“test_pcd.pcd”, *cloud_old);

//Use a voxelSampler to downsample
pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
voxelSampler.setInputCloud(cloud_old);
voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
voxelSampler.filter(*cloud_downsampled);

//Use a filter to reduce noise
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
statFilter.setInputCloud(cloud_downsampled);
statFilter.setMeanK(10);
statFilter.setStddevMulThresh(0.2);
statFilter.filter(*cloud);

// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);

// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);

// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);

// Use all neighbors in a sphere of radius 1cm


ne.setRadiusSearch(0.01);
// Compute the features
ne.compute (*normals);

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);

viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, normals, 10, 0.2, "normals");
viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
while (!viewer->wasStopped())
{
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return 0;

}
因为我的PCD的点云文件里面的点是PointXYZ类型，所以显示得时候，都是白色的，下面上一张效果图。

三、泊松重建

泊松重建是一项结合了全局与局部匹配优点的重建方案。前者大多需要定义径向基函数（RBF），且和所有样本点都会产生联系，需要求解全局非稀疏矩阵，开销很大。后者需要更多经验性的局部近似处理方式，且由于缺乏全局信息，容易产生低频误差。

泊松重建的核心思想是通过将物体表面的离散样本点信息转化到连续（可积，这个是核心原因）表面函数上，从而构造出watertight的隐式表面。

给定一个物体 [公式] ，边界为 [公式] ， [公式] 为满足物体内取值为1，其余为0的指示函数，很显然，得到了整个定义域上每一点 [公式] 的 [公式] ，我们就知道了整个物体表面。

回到我们之前的核心思想上，一个朴素的想法是通过直接插值得到 [公式] ，但由于 [公式] 的不连续性，插值得到的介于0和1之间的值没有意义，所以我们采用间接的方式来求取。这里我们先用平滑滤波函数 [公式] 来平滑 [公式] ，通过散度定理可证明，平滑过的指示函数的梯度场，等于平滑过的表面法向量场：

[公式] (1)

其中 [公式] 为卷积符号，此处即为平滑操作，[公式] 为表面处的法向量（指向内侧）。从直觉上看，指示函数的梯度方向显然也应该和法向量重合。关于平滑函数 [公式] ，它不能过大以产生过度平滑误差，也不能过小而使得离样本点较远时插值不可靠。通用的处理方式是高斯滤波，但为了满足最终求解时的稀疏性，即不让 [公式] 受过多样本点的影响，我们需要截断高斯滤波的范围，所以文章中用了3阶盒式滤波器，即连续三次盒式滤波器卷积来近似它。

由于样本点的离散性， [公式] 并不对于表面附近每一点 [公式] 都已知，我们需要对它分段近似：

[公式] (2)

[公式] 是是初始已知样本点集合 [公式] 中的一点，包含了 [公式] 位置和 [公式] 法向量信息， [公式] 是按照空间划分的 [公式] 附近的表面区域，由于做了样本点均匀分布的假设，常数项 [公式] 可略去。由于 [公式] 的范围限制，平滑后的结果是一定范围内的样本信息的线性组合。这样，我们通过离散近似得到了(1)式中等式右边的向量场 [公式] ，即我们最终需要求解的问题为：

[公式]

直接求解 [公式] 需要求积分，但 [公式] 不一定是无旋场，往往不可积，所以我们转而求解其最小二乘近似：

[公式] (3)

其中 [公式] 为拉普拉斯算子， [公式] 为散度算子，(3)式即为泊松方程。

为了得到高质量的重建结果，我们需要尽可能高的分辨率，同时优化掉不必要的采样，以规避立方级的均匀采样的计算开销。文章采用了一种自适应性的八叉树 [公式] ，其自适应性体现于，只在物体表面附近处提高采样分辨率，即节点深度，以满足Marching Cubes操作。八叉树的最大深度 [公式] 使得每个初始样本点都能落入不同的叶子节点中。

对应上述八叉树的每一个节点 [公式] ，我们定义一个基函数 [公式] 。文章里讲得花里胡哨，但其实就是以节点中心为原点的上述平滑函数，并按不同节点深度调节了影响范围：

[公式] ，其中

[公式]

这样，所有的 [公式] 张成了[公式]，我们把他们叠起来可以得到一个 [公式] 维的由基函数组成的向量 [公式] 。

回顾(2)式，我们知道平滑操作是为了让定义域内离散的样本点之外的点 [公式] 处的函数值（此处即为向量场）可以被（线性）插值得到。我们不妨把指示函数 [公式] 也投影到基函数上：

[公式]

这样求解 [公式] 就变成了求解它在基函数上的投影 [公式] ，同时 [公式] 以同样的形式也投影过了（为达到次像素级精度，文章使用了邻近8个八叉树子节点的三线性插值），那么整个问题就可以转化到基函数上了。注意到这个投影的关键之处在于，它将一个函数表示成了多个基函数的线性组合，对于不同的 [公式] ，他的线性组合参数是固定的，只需要在基函数中改变变量即可，这也为最终的矩阵求解提供了可能。

马上就可以开始求解了，可还有一点小问题，就是之前强调过的，由于平滑函数的范围限制，尽管 [公式] 和 [公式] 可以用基函数表示，即他们在 [公式] 内，它们的Laplacian和divergence却不一定可以，所以我们再把 [公式] 和 [公式] 投影到基函数上…即在 [公式] 上最小化下式：

[公式]

这里的 [公式] 和 [公式] 是对连续定义域上的所有 [公式] 积分。现在我们可以求 [公式] 了，也终于可以得到我们的指示函数 [公式] 了。注意得到拉普拉斯系数矩阵是稀疏且对称的，所以可以很方便地用共轭梯度法求解。得到了之后就可以用Marching Cubes提取表面，isovalue根据 [公式] 在所有 [公式] 上的平均值选取。这样，我们就大功告成了。