PCL：点云数据基于法线的边界提取(从最初的法线估计理论推导到最终的边界提取)

该边界提取采用PCL库里边的方法，基于法线估计来实现的边界检测与提取：

首先从原始点云上计算出法线，再由法线结合数据估计出边界。（这样解释还是特别抽像吧）

------------法线求解：（平面的法线是垂直于它的单位向量。在点云的表面的法线被定义为垂直于与点云表面相切的平面的向量。法线提供了关于曲面的曲率信息）

对点云数据集的每个点的法线估计，可以看作是对表面法线的近似推断。（因此该表面的判断就是你寻找的周围几个点或者半径内几个点组成的平面，就是下述代码中reforn这个参数，该参数的设置一般设置为分辨率的10倍时，效果较好，主要是对于法线估计。邻域半径选择太小了，噪声较大，估计的法线就容易出错，而搜索邻域半径设置的太大估计速度就比较慢。）

----------求解原理

确定表面一点法线的问题近似于估计表面的一个相切面法线的问题，因此转换过来以后就变成一个最小二乘法平面拟合估计问题。

平面方程用法线式表示为: $\cos\alpha \cdot x+\cos\beta \cdot y+\cos\gamma \cdot z+p=0$

$\cos\alpha ,\cos\beta ,\cos\gamma$ 为平面上点 $(x,y,z)$ 处法向量的方向余弦，|p|为原点到平面的距离。

$ax+by+cz=d(d\geqslant 0),\qquad a^{2}+b^{2}+c^{2}=1$ (此处a,b,c能够构成该平面的一个法向量n，后边的约束条件三方和为1只是控制了法向量大小，并没有改变方向，是为了下边利用点到平面的距离公式时，控制分母为1简化计算)

$a,b,c,d$ 求平面方程即转化为求四个参数。

将上式整理为： $Ax=\lambda x$ (由该式子联系求特征值特征向量可以想到， $\lambda$ 为特征值， $x$ 即 $(a,b,c)^{T}$ 为 $\lambda$ 对应的特征向量)

即转化到了求解矩阵A的特征值与特征向量的问题，矩阵A即为n个点的协方差矩阵。 $(a,b,c)^{T}$ 即为该矩阵的一个特征向量。

//PCL中的NormalEstimation
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);... read, pass in or create a point cloud ...// Create the normal estimation class, and pass the input dataset to itpcl::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 datasetspcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);// Use all neighbors in a sphere of radius 3cmne.setRadiusSearch (0.03);// Compute the featuresne.compute (*cloud_normals);// cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
}

OpenMP加速法线估计
PCL提供了表面法线估计的加速实现，基于OpenMP使用多核/多线程来加速计算。该类的名称是pcl :: NormalEstimationOMP，其API与单线程pcl :: NormalEstimation 100％兼容。在具有8个内核的系统上，一般计算时间可以加快6-8倍。

include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h>{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);... read, pass in or create a point cloud ...// Create the normal estimation class, and pass the input dataset to itpcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;ne.setNumberOfThreads(12);  // 手动设置线程数，否则提示错误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 datasetspcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);// Use all neighbors in a sphere of radius 3cmne.setRadiusSearch (0.03);// Compute the featuresne.compute (*cloud_normals);// cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
}

上述完整的介绍了法线的估计过程，自认为只要有稍微的高数基础应该都能看懂的哈哈

下边接着开始，怎么根据求的法线来找边界呢？？?

基于法线完成的边界估计主要是利用各个法线方向之间的夹角来做的判断（所以有个设置角度的阈值参数）。（此处还是没有太明白怎么根据法线夹角确定哪些点是边界点，如有特别明白的可以下边评论留言。）

对于边界的估计就是这个函数boundEst.setRadiusSearch(re)，参数re也设置为分辨率（此处的分辨率指的是点云的密度）的10倍，太小则内部的很多点就都当成边界点了。最后一个参数是边界判断时的角度阈值，默认值为PI/2，此处设置为PI/4，用户也可以根据需要进行更改。

#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/boundary.h>
#include <math.h>
#include <boost/make_shared.hpp>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/covariance_sampling.h>
#include <pcl/filters/normal_space.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/statistical_outlier_removal.h>
int estimateBorders(pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud, float re, float reforn)
{pcl::PointCloud<pcl::Boundary> boundaries; //保存边界估计结果pcl::BoundaryEstimation<pcl::PointXYZI, pcl::Normal, pcl::Boundary> boundEst; //定义一个进行边界特征估计的对象pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> normEst; //定义一个法线估计的对象pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); //保存法线估计的结果pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_boundary(new pcl::PointCloud<pcl::PointXYZI>);normEst.setInputCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr(cloud));normEst.setRadiusSearch(reforn); //设置法线估计的半径//normEst.setKSearch(10);//表示计算点云法向量时，搜索的点云个数normEst.compute(*normals); //将法线估计结果保存至normals//输出法线的个数std:cout << "reforn: " << reforn << std::endl;std::cerr << "normals: " << normals->size() << std::endl;boundEst.setInputCloud(cloud); //设置输入的点云boundEst.setInputNormals(normals); //设置边界估计的法线，因为边界估计依赖于法线boundEst.setRadiusSearch(re); //设置边界估计所需要的半径,//这里的Threadshold为一个浮点值，可取点云模型密度的10倍boundEst.setAngleThreshold(M_PI / 4); //边界估计时的角度阈值M_PI / 4  并计算k邻域点的法线夹角,若大于阈值则为边界特征点boundEst.setSearchMethod(pcl::search::KdTree<pcl::PointXYZI>::Ptr(new pcl::search::KdTree<pcl::PointXYZI>)); //设置搜索方式KdTreeboundEst.compute(boundaries); //将边界估计结果保存在boundariesstd::cerr << "AngleThreshold: " << M_PI / 4 << std::endl;//输出边界点的个数std::cerr << "boundaries: " << boundaries.points.size() << std::endl;//存储估计为边界的点云数据，将边界结果保存为pcl::PointXYZ类型for (int i = 0; i < cloud->points.size(); i++){if (boundaries[i].boundary_point > 0){cloud_boundary->push_back(cloud->points[i]);}}pcl::PCDWriter writer;std::stringstream ss;ss << "boundary" << ".pcd";writer.write<pcl::PointXYZI>(ss.str(), *cloud_boundary, false);//可视化显示原始点云与边界提取结果boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("边界提取"));int v1(0);MView->createViewPort(0.0, 0.0, 0.5, 1.0, v1);MView->setBackgroundColor(0.3, 0.3, 0.3, v1);MView->addText("Raw point clouds", 10, 10, "v1_text", v1);int v2(0);MView->createViewPort(0.5, 0.0, 1, 1.0, v2);MView->setBackgroundColor(0.5, 0.5, 0.5, v2);MView->addText("Boudary point clouds", 80, 80, "v2_text", v2);MView->addPointCloud<pcl::PointXYZI>(cloud, "sample cloud", v1);MView->addPointCloud<pcl::PointXYZI>(cloud_boundary, "cloud_boundary", v2);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "sample cloud", v1);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "cloud_boundary", v2);MView->addCoordinateSystem(1.0);MView->initCameraParameters();MView->spin();return 0;
}
int
main(int argc, char** argv)
{srand(time(NULL));float re, reforn;re = std::atof(argv[2]);reforn = std::atof(argv[3]);pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZI>);pcl::io::loadPCDFile(argv[1], *cloud_src);//创建滤波器对象pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;sor.setInputCloud(cloud_src);sor.setMeanK(100);//寻找每个点的50个最近邻点sor.setStddevMulThresh(3.0);//一个点的最近邻距离超过全局平均距离的一个标准差以上，就会舍弃sor.filter(*cloud_filtered);std::cout << "cloud_src: " << cloud_src->points.size() << std::endl;std::cout << "cloud_filtered: " << cloud_filtered->points.size() << std::endl;estimateBorders(cloud_src, re, reforn);return 0;
}

右键项目->属性->在命令行输入参数：如下所示：（以上需要三个参数）