[发明专利]一种基于法向约束的点云精匹配方法

权利要求书

1.一种基于法向约束的点云精匹配方法，其特征在于，包括：

步骤S1：采用点对特征匹配实现模型点云和场景点云的点云粗匹配，获取点云粗匹配下的位姿，并将获取的位姿作为初始值；

步骤S2：选取模型点云中的一模型点，在场景点云中采用KD树搜索方法搜索若干个场景点，所述搜索的若干个场景点为选取的模型点距离近的场景点，将所述模型点与若干个场景点进行配对；

步骤S3：计算配对后的若干个场景点和一模型点的点距序列和法向夹角序列，并删除所有大于点距指定值的点距和所有大于法向夹角指定值的法向夹角，未被删除的配对点作为候选匹配点对；

步骤S4：对于每个模型点的候选匹配点对，根据点距序列和法向夹角序列计算场景点是模型点的配对点的概率；

步骤S5：对于每个模型点的候选匹配点对，将点距匹配概率和对应的法向夹角匹配概率相乘，再除以全体候选匹配点对的概率乘积之和，作为同时考虑点距和法向夹角情况下，每个场景点和模型点匹配的联合概率；

步骤S6：基于模型点云与场景点云的全体候选匹配点对，建立ICP算法所用的坐标变换方程组，其中每个方程使用一对候选匹配点对，并在方程两端同时乘以该场景点和模型点匹配对应的联合概率，作为该方程的权重系数，求解此方程组，可获得当前迭代步中，模型点云到场景点云的最优变换阵与平移量；

步骤S7：若在当前迭代步执行完成后获得的模型点云坐标，与当前迭代步未执行时的模型点云坐标的初始值相比，不满足当前迭代步执行前和迭代步执行后的模型点云的平均偏差小于给定的阈值的情况，则重复步骤S2至步骤S7，将当前迭代步执行后的模型点云坐标，作为迭代后的下一次模型点云坐标的初始值进行迭代；若在当前迭代步执行完成后获得的模型点云坐标，与当前迭代步未执行时的模型点云坐标的初始值相比，当前迭代步执行前和迭代步执行后的模型点云的平均偏差小于给定的阈值，则结束迭代。

2.根据权利要求1所述的一种基于法向约束的点云精匹配方法，其特征在于，所述步骤S1中的位姿是由模型点云坐标变换到场景中对齐区域的场景点云坐标，所需的坐标变换旋转阵的三个旋转角以及三维平移向量构成的六维向量。

3.根据权利要求1所述的一种基于法向约束的点云精匹配方法，其特征在于，所述步骤S3中的法向夹角为模型点的法向与邻近的场景点的法向之间的夹角。

4.根据权利要求1所述的一种基于法向约束的点云精匹配方法，其特征在于，所述步骤S4中点距序列中场景点是模型点的配对点的概率计算方法，步骤包括：

步骤1：先计算若干个场景点分别与所选取的模型点的点距，并将计算获得的若干个点距进行求和，得到全体点距之和；

步骤2：将各个场景点和模型点的点距分别与步骤1中的全体点距之和进行相比，得到若干个点距比值；

步骤3：用1分别减去步骤2中的若干个点距比值，获得仅考虑点距大小的情况下，各个点对匹配的概率。

5.根据权利要求1所述的一种基于法向约束的点云精匹配方法，其特征在于，所述步骤S4中法向夹角序列中场景点是模型点的配对点的概率计算方法，步骤包括：

步骤A：先计算若干个场景点分别与所选取的模型点的法向夹角，并将计算获得的若干个法向夹角进行求和，得到全体法向夹角之和；

步骤B：将各个场景点和模型点的法向夹角分别与步骤1中的全体法向夹角之和进行相比，得到若干个法向夹角比值；

步骤C：用1分别减去步骤2中的若干个法向夹角比值，获得仅考虑法向夹角大小的情况下，各个点对匹配的概率。

6.根据权利要求1所述的一种基于法向约束的点云精匹配方法，其特征在于，所述步骤S6中的模型点云到场景点云的最优变换阵与平移量方法，包括：

步骤a：基于模型点云与场景点云的全体候选匹配点对，获取经过一模型点的一平面，所述平面与一模型点的法向垂直，建立模型点到平面的ICP优化方程组；

步骤b：在步骤a中建立的ICP优化方程组的两端同时乘以联合概率；

步骤c：求解步骤b中乘以联合概率后的ICP优化方程组，进而获得模型点云到场景点云的坐标变换。