[发明专利]根据匹配线段对计算其所属坐标系间位姿修正值的方法

说明书

本发明涉及一种根据匹配线段对计算其所属坐标系间位姿修正值的方法，包括以下步骤：(1)旋转待匹配线段集，使得旋转后的待匹配线段集和原本的被匹配线段集之间的匹配线段对的夹角最小；(2)计算角度修正值；(3)计算位置修正值；(4)结合角度修正值和位置修正值，得到最终的位姿修正值。本发明提供一种能够得到复合解的线段‑线段匹配方法，利用了匹配线段对中的线段属性，而不是和其他算法一样舍弃了线段、只用其直线属性，充分利用已知信息，在计算旋转角度时本方法没有矩阵操作，计算量比常见方法小得多。

技术领域

本发明属于图像处理技术领域，涉及一种计算位姿修正值的方法，具体涉及一种根据匹配线段对计算其所属坐标系间位姿修正值的方法。

背景技术

关于利用匹配线段对来计算得到位姿修正值这个过程的介绍有很多，其中的方法根据所得位姿修正值的特点不同而大体可以归为两类：第一类得到的位姿修正值是解析解，这类方法利用全部匹配线段对的信息构造一个矩阵，然后根据矩阵运算得到唯一的修正值，这个修正值充分利用了全部的数据；第二类得到的是非解析解(non closed-formsolution)，这类方法把每个匹配线段对都转化为对位姿修正值的一个约束，然后根据全部约束求解位姿修正值，由于不同约束可能直接存在矛盾，最终得到的修正值并不能完全保证同时满足每个约束。

目前，已知的方法都把匹配线段对视为匹配直线对而没有利用线段信息，如PL-ICP、ICL等往往做的是直线-直线匹配，这样一来每条直线上利用的信息只有其方向，这就意味着放弃了线段端点，没有充分利用已知信息，造成了信息的浪费，如公布号为CN110310331 A的发明专利具体公开了一种基于直线特征和点云特征结合的位姿估计方法，其利用的就只有方向，而放弃了线段端点。

发明内容

本发明所要解决的技术问题是克服现有技术的不足，提供一种根据匹配线段对计算其所属坐标系间位姿修正值的方法，一种能够得到复合解的线段-线段匹配方法，在计算旋转角度时本方法没有矩阵操作，计算量比常见方法小得多。

为解决以上技术问题，本发明采取的一种技术方案是：

一种根据匹配线段对计算其所属坐标系间位姿修正值的方法，匹配线段对包括被匹配线段和待匹配线段，所有被匹配线段构成被匹配线段集，记为所有待匹配线段构成待匹配线段集，记为被匹配线段集所在坐标系为被匹配坐标系，待匹配线段集所在坐标系为待匹配坐标系，其中，具有匹配关系的待匹配线段和被匹配线段具有相同的第二下角标，即和相匹配，被匹配坐标系为经过优化的位姿所对应的坐标系；

该方法包括以下步骤：

(1)旋转待匹配线段集，使得旋转后的待匹配线段集和原本的被匹配线段集之间的匹配线段对的夹角最小；

(2)计算角度修正值；

(3)计算位置修正值；

(4)结合角度修正值和位置修正值，得到最终的位姿修正值；

步骤(1)对待匹配线段集的旋转操作通过一个二维旋转矩阵R(θ)来实现，二维旋转矩阵作用在一个待匹配线段矢量之后，这个待匹配线段变成：它和被匹配线段之间的点积是：

对不同的匹配线段对，直接求和：

步骤(2)的具体计算过程为：利用三角公式将步骤(1)中得到的公式转化为以下形式：两参数F_max和满足：

其中，F_cos和F_sin是为了叙述方便而引入的辅助量；通过得到角度修正值的解析解为

优选地，步骤(3)包括以下步骤：

(3.1)按照角度修正值来旋转全部待匹配线段，然后计算每个匹配线段对之间的垂直距离；