PCL 计算点云法向量并显示

一、算法原理

强调一下：

• PCL中计算法向量的方法有两种，其中一种就是本文pcl::NormalEstimation中的方法；
• 该方法是基于PCA主成分分析法实现的；
• 最小二乘拟合邻域平面时用的是KD树查找邻域点。

1、法向量估计

  基于局部表面拟合的方法进行法向量估计：点云的采样表面处处光滑的情况下，任何点的局部邻域都可以用平面进行很好的拟合；为此，对于点云中的每个扫描点$p$，搜索到与其最近邻的$K$个相邻点，然后计算这些点最小二乘意义上的局部平面$P$，此平面可以表示为：
$$P(\vec{n},d)=\underset{(\vec{n},d)}{\mathrm{argmin}}\sum _{i=1}^k(\vec{n}\cdot p_i-d{)}^2\text{\hspace{1em}(1)}$$
  式中，$\vec{n}$为平面$P$的法向量，$d$为$P$到坐标原点的距离。
  可以认为由$K$个最近点拟合出的平面的法向量即当前扫描点的法向量。平面$P$的法向量可以由主成分分析(PCA)得到，由运算知$P$经过其$K$邻域点的质心$P_0$,且法向量$\vec{n}$满足$||\vec{n}||=1$,先对式（2）中的协方差矩阵$M$进行特征值分解，求得$M$的各特征值，$M$的最小特征值所对应的特征向量即$P$的法向量。
$$M=\frac{1}{k}\sum _{i=1}^k(p_i-p_0)(p_i-p_0{)}^T\text{\hspace{1em}(2)}$$

2、法向量定向

  前面计算出的法向量具有二义性，即只是得到了法向量所在的直线，而没有确定以直线的那个方向为法向量的最终方向。用以下方法对求出的法向量进行重定向：
  假设点云足够稠密且采样平面处处光滑，那么相邻两点的法向量会接近于平行。令$\overrightarrow{n_i}、\overrightarrow{n_j}$为相邻两点$x_i、x_j$的法向量，如果法向量的方向是一致的，那么$\overrightarrow{n_i}\cdot \overrightarrow{n_j}\approx 1$若此内积为负，则说明其中某个点的法向量需要被翻转。因此，首先为点云中某个点设定一个法向量朝向，然后遍历其他所有点，若当前点法向量设定为$\overrightarrow{n_i}，\overrightarrow{n_j}$为下一个要遍历的点，如果$\overrightarrow{n_i}\cdot \overrightarrow{n_j}<0$则将$\overrightarrow{n_j}$翻转，否则保持不变。

3、表面曲率

  对式（2）中的协方差矩阵$M$进行特征值分解，求得$M$的各特征值，若特征值满足${\lambda }_0\le {\lambda }_1\le {\lambda }_2$，则$P$点的表面曲率为：
$$\delta =\frac{{\lambda }_0}{{\lambda }_0+{\lambda }_1+{\lambda }_2}\text{\hspace{1em}(3)}$$
   $\delta$越小表明邻域越平坦， $\delta$越大则表明邻域的起伏变化越大。

4、参考文献

[1]王飞,刘如飞,任红伟,柴永宁.利用道路目标特征的多期车载激光点云配准[J].测绘科学技术学报,2020,37(05):496-502.
[2]邢正全,邓喀中,薛继群.基于K-近邻搜索的点云初始配准[J].测绘科学,2013,38(02):93-95.
[3]李新春,闫振宇,林森,贾迪.基于邻域特征点提取和匹配的点云配准[J].光子学报,2020,49(04):255-265.

5、法向量定向的理解

   $A,B,C$表示坐标值，$A_i,B_i,C_I$表示向量，以$\overrightarrow{OA}$ 作为正方向，由于$A$点法向量已经定向，所以$A$点法向量为：$\overrightarrow{A_i}$=(0,0,1)，对$B$点的法向量进行法向量方向的确定：
   其中，$B$点的法向量坐标为：$\overrightarrow{B_{i1}}=(\frac{1}{\sqrt{2}},0,\frac{1}{\sqrt{2}})$，$\overrightarrow{B_{i2}}=(\frac{1}{\sqrt{2}},0,-\frac{1}{\sqrt{2}})$；（可以看出$\overrightarrow{B_i}$ 的方向与$\overrightarrow{OA}$ 同向，定向的目的就是找出与$\overrightarrow{OA}$同向的$\overrightarrow{B_i}$ ），其理论依据为：
$$\overrightarrow{A_i}\cdot \overrightarrow{B_i}>0\text{\hspace{1em}(4)}$$
  所谓的调整方向就是：上式中计算，你带入的$\overrightarrow{B_i}$ 值为$\overrightarrow{B_{i2}}$时，对 $\overrightarrow{B_{i2}}$进行操作使$\overrightarrow{B_{i2}}$=$\overrightarrow{B_{i1}}$。
实际操作中，以$A$点作为视点，求出：$\overrightarrow{B_i}\cdot (A-B)>0$时的$\overrightarrow{B_i}$即为$B$点与视点$A$同向的法向量。
   综上所述：所谓的定向就是以其中一点$N_i$的法向量方向作为正方向，其邻域内所有点的法向量$N_j$满足：

$$N_j\cdot N_i>0\text{\hspace{1em}(5)}$$

6、CloudCompare

CloudCompare软件中也有计算法向量的功能，这里给出具体操作。

Normals > Compute：计算所选实体的法线
Normals > Invert：反转所选实体的法线
Normals > Orient Normals > With Minimum Spanning Tree：用同样的方法重新定位点云的全部法线（最小生成树）
Normals > Orient Normals > With Fast Marching：用同样的方法重新定位点云的全部法线（快速行进法）
Normals > Convert to > HSV：将云的法线转换到 HSV 颜色字段
Normals > Convert to > Dip and Dip direction SFs：转换点云的法线到两个标量域
Normals > Clear：为选定的实体移除法线

二、pcl::Normal的定义

compute(*normal)里计算出来的结果是：法向量的x,y,z坐标和表面曲率curvature。其内部结构为：

 /*brief A point structure representing normal coordinates and the surface curvature estimate. (SSE friendly)ingroup common*/

struct Normal : public _Normal
{
    inline Normal (const _Normal &p)
    {
     normal_x = p.normal_x; 
     normal_y = p.normal_y; 
     normal_z = p.normal_z;
     data_n[3] = 0.0f;
     curvature = p.curvature;
    }

    inline Normal ()
    {
     normal_x = normal_y = normal_z = data_n[3] = 0.0f;
     curvature = 0;
    }

    inline Normal (float n_x, float n_y, float n_z)
    {
     normal_x = n_x; normal_y = n_y; normal_z = n_z;
     curvature = 0;
     data_n[3] = 0.0f;
    }

    friend std::ostream& operator << (std::ostream& os, const Normal& p);
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

三、pcl::Normal的几种输出方式

          cout<<normals->points[0]<<endl;
     cout<<"["<<normals->points[0].normal_x<<" "
              <<normals->points[0].normal_y<<" "
              <<normals->points[0].normal_z<<" "
              <<normals->points[0].curvature<<"]"<<endl;

     cout<<"["<<normals->points[0].normal[0]<<" "
              <<normals->points[0].normal[1]<<" "
              <<normals->points[0].normal[2]<<" "
              <<normals->points[0].curvature<<"]"<<endl;

     cout<<"["<<normals->points[0].data_n[0]<<" "
              <<normals->points[0].data_n[1]<<" "
              <<normals->points[0].data_n[2]<<" "
              <<normals->points[0].curvature<<"]"<<endl;

四、计算法线并显示

1、计算输入点云的所有点的法线

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>//使用OMP需要添加的头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using namespace std;
int main()
{
	//------------------加载点云数据-------------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("车载点云.pcd", *cloud) == -1)
	{
		PCL_ERROR("Could not read file\n");
	}

	//------------------计算法线----------------------
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;//OMP加速
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	//建立kdtree来进行近邻点集搜索
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	n.setNumberOfThreads(10);//设置openMP的线程数
	//n.setViewPoint(0,0,0);//设置视点，默认为（0，0，0）
	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	n.setKSearch(10);//点云法向计算时，需要所搜的近邻点大小
	//n.setRadiusSearch(0.03);//半径搜素
	n.compute(*normals);//开始进行法向计
	
	//----------------可视化--------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Normal viewer"));
	//viewer->initCameraParameters();//设置照相机参数，使用户从默认的角度和方向观察点云
	//设置背景颜色
	viewer->setBackgroundColor(0.3, 0.3, 0.3);
	viewer->addText("faxian", 10, 10, "text");
	//设置点云颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 225, 0);
	//添加坐标系
	viewer->addCoordinateSystem(0.1);
	viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud");
	
 
    //添加需要显示的点云法向。cloud为原始点云模型，normal为法向信息，20表示需要显示法向的点云间隔，即每20个点显示一次法向，0.02表示法向长度。
	viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, normals, 20, 0.02, "normals");
	//设置点云大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return 0;
}

2、计算输入点云数据中一个子集的法线

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/normal_3d_omp.h>
using namespace std;
int main()
{
	//---------------------加载点云数据----------------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("车载点云.pcd", *cloud) == -1)
	{
		PCL_ERROR("Could not read file\n");
	}
	//--------------计算云中前10%的点法线-----------------------
	vector<int> point_indices(floor(cloud->points.size() / 10));
	for (size_t i = 0; i < point_indices.size(); ++i) {
		point_indices[i] = i;	
	}
	//-------------------传递索引----------------------------
	pcl::IndicesPtr indices(new vector <int>(point_indices));
	//-------------------计算法线----------------------------
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;//OMP加速
	n.setInputCloud(cloud);
	n.setIndices(indices);
	// 创建一个kd树，方便搜索；并将它传递给上面创建的法线估算类对象
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	n.setSearchMethod(tree);
	n.setRadiusSearch(0.01);
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	//----------------估算特征---------------
	n.compute(*normals);
	//-------------为方便可视化，将前10%点云提出-------------------------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud(*cloud, point_indices, *cloud1);
	//------------------可视化-----------------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Normal viewer"));
	//设置背景颜色
	viewer->setBackgroundColor(0.3, 0.3, 0.3);
	viewer->addText("faxian", 10, 10, "text");
	//设置点云颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud1, 0, 225, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 0, 0);
	//添加坐标系
	//viewer->addCoordinateSystem(0.1);
	viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud");
	viewer->addPointCloud<pcl::PointXYZ>(cloud1, single_color1, "sample cloud1");
	viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud1, normals, 20, 0.02, "normals");
	//设置点云大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud1");
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return 0;
}

3、特殊操作

使用另一个数据集（该数据集比较完整）估计其最近邻近点，估算输入数据集（比较稀疏）中所有点的一组曲面法线。该方法适用于（类似）下采样后的点云。

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d_omp.h>
using namespace std;
int main()

	{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("车载点云.pcd", *cloud) == -1)
	{
		PCL_ERROR("Could not read file\n");
	}
	//------------下采样----------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::VoxelGrid<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.005f, 0.005f, 0.05f);
	sor.filter(*cloud_downsampled);
	//-------------计算法线-------------
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud(cloud_downsampled);
	ne.setSearchSurface(cloud);
	ne.setSearchMethod(tree);
	ne.setRadiusSearch(0.01);
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	ne.compute(*normals);
	//------------------可视化-----------------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Normal viewer"));
	//设置背景颜色
	viewer->setBackgroundColor(0.3, 0.3, 0.3);
	viewer->addText("faxian", 10, 10, "text");
	//设置点云颜色

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud_downsampled, 255, 0, 0);
	//添加坐标系
	//viewer->addCoordinateSystem(0.1);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_downsampled, single_color, "sample cloud");

	viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_downsampled, normals, 10, 0.02, "normals");
	//设置点云大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return 0;
}

五、使用OMP的报错处理

在应用OpenMP，可能会报错“User Error 1001: argument to num_threads clause must be positive”。这是由于设置的线程数必须为正，而程序中可能没有设置，有时候甚至环境变量中设置了，但是依然报错，手动设置如下：

n.setNumberOfThreads(4);  // 手动设置线程数，比源码增加，否则提示错误

六、计算一个点的法线

computePointNormal 函数可以用来计算单个指定索引的点的法线。

void computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
 Eigen::Vector4f &plane_parameters, float &curvature);

• cloud：输入的原始点云
• indices：要估计法线的点的索引
• plane_parameters：法向量
• curvature：表面曲率

七、PCL中的法线定向

  已知视点为$V_p$，任意一点的坐标为$P_i$，法向量为$\overrightarrow{n_i}$。对所有法向量$\overrightarrow{n_i}$定向使它们朝向视点方向，需满足如下公式：
$$\overrightarrow{n_i}\cdot (V_p-P_i)>0$$
  代码中的setViewPoint(0,0,0)就是用来进行视点设置的：

n.setViewPoint(0,0,0);//设置视点，默认为（0，0，0）

  在PCL中对一个已知点的法线进行手动重定向，可以使用如下代码：

flipNormalTowardsViewpoint (const PointT&point,float vp_x,float vp_y,float vp_z,
Eigen::Vector4f&normal)

  如果数据集是从多个捕获视点中配准后集成的，那么上述方法就不再适用。

参考自：《点云库PCL从入门到精通》P209~P210
更多手动定向的方法见：CloudCompare——计算点云的法向量

八、结果展示