一种基于统一残差模型的空间多元特征配准优化方法及装置与流程

技术特征：

1.一种基于统一残差模型的空间多元特征配准优化方法，其特征在于，包括以下步骤：

对单帧图像中的点、线、面特征进行统一参数化描述；

根据当前帧在全局坐标系下的初始位姿x，将当前帧中的局部特征要素m转换到全局坐标系，得到局部特征要素m的全局参数m'；所述的全局坐标系为起始帧相机坐标系；

将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用kdtree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系；

定义统一的残差模型，根据匹配关系，计算当前帧的局部特征要素与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿；

利用局部地图中各个帧在全局坐标系下的最优位姿对局部地图进行图优化。

2.根据权利要求1所述的方法，其特征在于，所述的对单帧图像中的点、线、面特征进行统一参数化描述，包括：

定义点p、线l、面α特征的三维中心点p'、方向矢量3*3姿态矩阵r、3*3形态矩阵ω；

其中点p、线l、面α特征的三维中心点p'、方向矢量如下所示：

点p、线l、面α特征的3*3姿态矩阵r如下所示：

点p、线l、面α特征的3*3形态矩阵ω如下所示：

特征类型形态矩阵ω点pdiag(1,1,1)线ldiag(0,1,1)面αdiag(1,0,0)

基于上述定义，将任意点p、线l、面α特征参数化为：m:{p'm,rm,ωm}。

3.根据权利要求2所述的方法，其特征在于，所述的，根据当前帧在全局坐标系下的初始位姿x，将当前帧中的局部特征要素m转换到全局坐标系，得到局部特征要素m的全局参数m'，包括：

获取当前帧在全局坐标系下的位姿初值x；定义x＝{rx|tx}∈se3，其中se3为三维特殊欧式群，rx代表当前帧在全局坐标系下对应的3*3维姿态旋转矩阵,tx代表当前帧在全局坐标系下对应的3*1维位姿平移矩阵；

根据公式

将当前帧中所包含的局部特征要素m转换到全局坐标系，得到全局参数m'。

4.根据权利要求1所述的方法，其特征在于，所述的将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用kdtree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系，包括：

根据全局路标的特征类型，将全局路标参数化为空间节点并存储为二叉树结构；

将上一轮跟踪成功或局部地图匹配中匹配量最大的帧作为参考帧，将当前帧中的局部特征要素与参考帧中的全局路标进行匹配；

以当前帧为基准更新局部地图，并将当前帧中的局部特征要素与局部地图中存储的全局路标进行匹配；

若当前帧中的全局特征要素与参考帧中的路标的匹配以及当前帧中的局部特征要素与局部地图中存储的路标的匹配均失败时，进行全局搜索，将当前帧与全局地图中的全局路标进行匹配。

5.根据权利要求4所述的方法，其特征在于，所述的将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用kdtree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系，还包括：如果当前帧与参考帧、局部地图、全局地图的特征匹配均未成功，则将局部特征要素m的全局参数m'作为新的全局路标加入全局地图。

6.根据权利要求1所述的方法，其特征在于，所述的统一的残差模型如下定义：

定义第i帧的第j个局部特征要素mij:{p'ij,rij,ωij}与全局路标之间的7维残差模型：

其中，为第i帧位姿矩阵，se3为三维特殊欧式群，代表第i帧在全局坐标系下的3*3维姿态旋转矩阵,ti代表第i帧在全局坐标系下对应的3*1维位姿平移矩阵；为全局路标的三维中心点，为全局路标的方向矢量。

7.根据权利要求1或6所述的方法，其特征在于，所述的根据匹配关系，计算当前帧的局部特征要素m与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿，包括：

步骤301，根据外源传感器信息进行当前帧位姿插值或根据匀速运动模型基于历史帧预测当前帧的位姿

步骤302，按照残差模型，根据当前帧的位姿计算当前帧的每一局部特征要素的匹配残差；定义第k个局部特征要素的匹配残差为ek，按照下式计算当前帧的所有局部特征要素的匹配残差

步骤303，判断是否相对于上一轮迭代更新后已不再减小或迭代次数是否达到最大，若是则迭代结束并取当前的作为当前帧的最优位姿，否则执行步骤304；

步骤304，遍历当前帧每一局部特征要素的匹配残差，定义第k项残差相对状态量的雅克比矩阵为jk，按照下式计算各要素残差相对状态量的雅克比矩阵；

步骤305，分别计算中间变量矩阵：h矩阵和b矩阵，计算公式如下所示：

其中，ωk为残差信息矩阵；

步骤306，计算位姿迭代更新的扰动量δx：δx←-h^-1b；

步骤307，通过下式，用扰动量δx更新当前帧预测位姿并跳转至步骤302；

其中，rx()、ry()、rz()分别代表绕局部坐标系x、y、z轴旋转一定角度的rodriguz变换，δαx，δαy，δαz为绕局部坐标系x、y、z轴的相对旋转角。

8.一种基于统一残差模型的空间多元特征配准优化装置，其特征在于，包括：

参数描述模块，用于对单帧图像中的点、线、面特征进行统一参数化描述；

匹配模块，用于根据当前帧在全局坐标系下的初始位姿x，将当前帧中的局部特征要素m转换到全局坐标系，得到局部特征要素m的全局参数m'；用于将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用kdtree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系；

配准模块，用于定义统一的残差模型，用于根据匹配关系，计算当前帧的局部特征要素与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿；

优化模块，用于利用局部地图中各个帧在全局坐标系下的最优位姿对局部地图进行图优化。

9.一种电子设备，其特征在于，包括：

存储器，用于存储计算机软件程序；

处理器，用于读取并执行所述存储器内存储的计算机软件程序，进而实现权利要求1-7任一项所述的一种基于统一残差模型的空间多元特征配准优化方法。

10.一种非暂态计算机可读存储介质，其特征在于，所述存储介质中存储有用于实现权利要求1-7任一项所述的一种基于统一残差模型的空间多元特征配准优化方法的计算机软件程序。

技术总结
本发明涉及一种基于统一残差模型的空间多元特征配准优化方法及装置，该方法提出一种对点、线、面三种不同类型的特征点云构建统一的参数化表达模型，在前端匹配跟踪环节根据不同类型的特征匹配关系设计统一的残差模型进行融合配准。在后端局部优化环节根据统一的残差模型定义局部优化方程，实现统一的优化方程下对局部地图中的点、线、面三种类型的特征要素的联合优化。该方法将不同类型参数同一参数化表达，并定义同一的残差模型，保证了地图优化时的稳定性、效率以及精度。

技术研发人员：王晨宇;吴凯;贾腾龙;刘奋
受保护的技术使用者：武汉中海庭数据技术有限公司
技术研发日：2020.06.12
技术公布日：2020.10.23