目录
一、算法原理
强调一下：
- PCL中计算法向量的方法有两种，其中一种就是本文pcl::NormalEstimation中的方法；
- 该方法是基于PCA主成分分析法实现的；
- 最小二乘拟合邻域平面时用的是KD树查找邻域点。
1、法向量估计
基于局部表面拟合的方法进行法向量估计：点云的采样表面处处光滑的情况下，任何点的局部邻域都可以用平面进行很好的拟合；为此，对于点云中的每个扫描点
p
p
p，搜索到与其最近邻的
K
K
K个相邻点，然后计算这些点最小二乘意义上的局部平面
P
P
P，此平面可以表示为：
P
(
n
⃗
,
d
)
=
arg min
(
n
⃗
,
d
)
∑
i
=
1
k
(
n
⃗
⋅
p
i
−
d
)
2
(1)
P(\vec{n},d)=\argmin \limits_{(\vec{n},d)} \sum_{i=1}^k\ (\vec{n}\cdot p_i-d)^2\tag{1}
P(n,d)=(n,d)argmini=1∑k (n⋅pi−d)2(1)
式中，
n
⃗
\vec{n}
n为平面
P
P
P的法向量，
d
d
d为
P
P
P到坐标原点的距离。
可以认为由
K
K
K个最近点拟合出的平面的法向量即当前扫描点的法向量。平面
P
P
P的法向量可以由主成分分析(PCA)得到，由运算知
P
P
P经过其
K
K
K邻域点的质心
P
0
P_0
P0,且法向量
n
⃗
\vec{n}
n满足
∣
∣
n
⃗
∣
∣
=
1
||\vec{n}||=1
∣∣n∣∣=1,先对式（2）中的协方差矩阵
M
M
M进行特征值分解，求得
M
M
M的各特征值，
M
M
M的最小特征值所对应的特征向量即
P
P
P的法向量。
M
=
1
k
∑
i
=
1
k
(
p
i
−
p
0
)
(
p
i
−
p
0
)
T
(2)
M=\frac{1}{k}\sum_{i=1}^k\ ( p_i-p_0)( p_i-p_0)^T\tag{2}
M=k1i=1∑k (pi−p0)(pi−p0)T(2)
2、法向量定向
前面计算出的法向量具有二义性，即只是得到了法向量所在的直线，而没有确定以直线的那个方向为法向量的最终方向。用以下方法对求出的法向量进行重定向：
假设点云足够稠密且采样平面处处光滑，那么相邻两点的法向量会接近于平行。令
n
i
⃗
、
n
j
⃗
\vec{n_i}、\vec{n_j}
ni、nj为相邻两点
x
i
、
x
j
{x_i}、{x_j}
xi、xj的法向量，如果法向量的方向是一致的，那么
n
i
⃗
⋅
n
j
⃗
≈
1
\vec{n_i}\cdot\vec{n_j}\approx1
ni⋅nj≈1若此内积为负，则说明其中某个点的法向量需要被翻转。因此，首先为点云中某个点设定一个法向量朝向，然后遍历其他所有点，若当前点法向量设定为
n
i
⃗
，
n
j
⃗
\vec{n_i}，\vec{n_j}
ni，nj为下一个要遍历的点，如果
n
i
⃗
⋅
n
j
⃗
<
0
\vec{n_i}\cdot\vec{n_j}<0
ni⋅nj<0则将
n
j
⃗
\vec{n_j}
nj翻转，否则保持不变。
3、表面曲率
对式（2）中的协方差矩阵
M
M
M进行特征值分解，求得
M
M
M的各特征值，若特征值满足
λ
0
≤
λ
1
≤
λ
2
\lambda_0\leq\lambda_1\leq\lambda_2
λ0≤λ1≤λ2，则
P
P
P点的表面曲率为：
δ
=
λ
0
λ
0
+
λ
1
+
λ
2
(3)
\delta=\frac{\lambda_0}{\lambda_0+\lambda_1+\lambda_2}\tag{3}
δ=λ0+λ1+λ2λ0(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,B,C
A,B,C表示坐标值，
A
i
,
B
i
,
C
I
A_i,B_i,C_I
Ai,Bi,CI表示向量，以
O
A
→
\overrightarrow{OA}
OA 作为正方向，由于
A
A
A点法向量已经定向，所以
A
A
A点法向量为：
A
i
→
\overrightarrow{A_i}
Ai=(0,0,1)，对
B
B
B点的法向量进行法向量方向的确定：
其中，
B
B
B点的法向量坐标为：
B
i
1
→
=
(
1
2
,
0
,
1
2
)
\overrightarrow{B_{i1}}=(\frac{1}{\sqrt{2}},0,\frac{1}{\sqrt{2}})
Bi1=(21,0,21)，
B
i
2
→
=
(
1
2
,
0
,
−
1
2
)
\overrightarrow{B_{i2}}=(\frac{1}{\sqrt{2}},0,-\frac{1}{\sqrt{2}})
Bi2=(21,0,−21)；（可以看出
B
i
→
\overrightarrow{B_i}
Bi 的方向与
O
A
→
\overrightarrow{OA}
OA 同向，定向的目的就是找出与
O
A
→
\overrightarrow{OA}
OA同向的
B
i
→
\overrightarrow{B_i}
Bi ），其理论依据为：
A
i
→
⋅
B
i
→
>
0
(4)
\overrightarrow{A_i}\cdot \overrightarrow{B_i}>0\tag{4}
Ai⋅Bi>0(4)
所谓的调整方向就是：上式中计算，你带入的
B
i
→
\overrightarrow{B_i}
Bi 值为
B
i
2
→
\overrightarrow{B_{i2}}
Bi2时，对
B
i
2
→
\overrightarrow{B_{i2}}
Bi2进行操作使
B
i
2
→
\overrightarrow{B_{i2}}
Bi2=
B
i
1
→
\overrightarrow{B_{i1}}
Bi1。
实际操作中，以
A
A
A点作为视点，求出：
B
i
→
⋅
(
A
−
B
)
>
0
\overrightarrow{B_i}\cdot \ (A-B)>0
Bi⋅ (A−B)>0时的
B
i
→
\overrightarrow{B_i}
Bi即为
B
B
B点与视点
A
A
A同向的法向量。
综上所述：所谓的定向就是以其中一点
N
i
N_i
Ni的法向量方向作为正方向，其邻域内所有点的法向量
N
j
N_j
Nj满足：
N j ⋅ N i > 0 (5) {N_j}\cdot {N_i}>0\tag{5} Nj⋅Ni>0(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
V_p
Vp，任意一点的坐标为
P
i
P_i
Pi，法向量为
n
i
⃗
\vec{n_i}
ni。对所有法向量
n
i
⃗
\vec{n_i}
ni定向使它们朝向视点方向，需满足如下公式：
n
i
⃗
⋅
(
V
p
−
P
i
)
>
0
\vec{n_i}\cdot(V_p-P_i)>0
ni⋅(Vp−Pi)>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——计算点云的法向量
八、结果展示
174
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
