[发明专利]一种多线激光雷达稀疏点云的表面法向量估计方法

权利要求书

1.一种多线激光雷达稀疏点云的表面法向量估计方法，其特征在于，所述方法包括：

S1.获取多线激光雷达的一帧点云数据，将数据按要求排序；

S2.对于点云数据中每一个有效测量点p_l,i，计算其所属扫描线在该点处的切线向量；

S3.在相邻扫描线上查找2个满足条件的p_l,i近邻点，通过近邻点估计p_l,i的2条副切线向量；

S4.将上一步得到的2条副切线向量与扫描线切线向量进行叉乘计算，得到当前点表面法向量的2个初始估计值；

S5.对法向量初始估计值方向进行检测，若其与雷达的激光发射方向夹角小于90度，则取反方向；

S6.通过法向量的2个估计值综合估计该点的表面法向量。

2.根据权利要求1所述的多线激光雷达稀疏点云的表面法向量估计方法，所述步骤S1中，获取多线激光雷达的一帧点云数据，将数据按要求排序，其特征还包括：

将数据按按扫描线进行分组，对于同一条扫描线的数据，根据其扫描时间的先后顺序进行排序；对于不同组的数据，按所在扫描线的高低角度，由低到高进行排序，排序完成的数据如下：

p_1,1,p_1,2,…,p_1,n,p_2,1,p_2,2,…,p_2,n,…,p_m,1,p_m,2,…,p_m,n

其中，m为多线激光雷达的线数，n为一帧中每条扫描线的点数，p_l,i为第l扫描线上的第i个点，对没有反射的点，存储为无效值NaN。

3.根据权利要求1所述的多线激光雷达稀疏点云的表面法向量估计方法，所述步骤S2中，对于点云数据中每一个有效测量点p_l,i，计算其所属扫描线在该点处的切线向量，其特征还包括：

S2.1以待计算点p_l,i为中心，分别沿扫描线向后向前查找点其中，应满足如下条件：1)与p_l,i的空间距离大于阈值d_threshold；2)与p_l,i的下标距离|i-i_prev/next|小于等于i_threshold，或与p_l,i的空间距离小于等于5d_threshold，若无法所搜到满足条件1和条件2的点，则或记为NaN；

S2.2根据S2.1的返回结果，分别计算p_l,i与和构成的切线向量和其中，Normalize(·)为归一化函数，若或为NaN，则对应的v₁或v₂为NaN；

S2.3通过v₁和v₂计算扫描线l在点p_l,i处的切线向量(1)若v₁和v₂均为NaN，则p_l,i的切向量无法计算，结束该点的表面法向量计算，返回S2继续计算下一个点的法向量；(2)若v₁有效且v₂为NaN，则(3)若v₂有效且v₁为NaN，则(4)若v₁和v₂都不为NaN，首先计算向量v₁和v₂的夹角θ＝Acos(v₁·v₂),如果θ小于阈值θ_threshold，则否则，