CN110411476B

CN110411476B - 视觉惯性里程计标定适配及评价方法和系统

Info

Publication number: CN110411476B
Application number: CN201910687659.1A
Authority: CN
Inventors: 刘万凯; 顾升宇; 刘力; 张小军
Original assignee: Shichen Information Technology Shanghai Co ltd
Current assignee: Shichen Information Technology Shanghai Co ltd
Priority date: 2019-07-29
Filing date: 2019-07-29
Publication date: 2021-03-23
Anticipated expiration: 2039-07-29
Also published as: CN110411476A

Abstract

本发明实施例提供了一种视觉惯性里程计标定适配及评价方法和系统，其中所述方法包括在视觉惯性里程计中对获取到的相机与IMU的联合参数进行优化，当所述相机与IMU的联合参数趋于稳定后停止优化，得到所述相机与IMU的联合参数的优化值，并将所述相机与IMU的联合参数优化值设为所述视觉惯性里程计中的初值；对优化后的视觉惯性里程计进行测试评价，如果通过所述测试，则完成对所述视觉惯性里程计的标定。本发明实施例提供的方法和系统能完整地对视觉惯性里程计使用的参数进行适配以及进行误差评价反馈优化。

Description

视觉惯性里程计标定适配及评价方法和系统

技术领域

本发明涉及计算机视觉领域，特别是涉及一种视觉惯性里程计标定适配及评价方法和系统。

背景技术

在机器人学和计算机视觉等技术领域内，定位与地图构建(SLAM)是研究的关键，目前SLAM系统广泛应用在增强现实(AR)、虚拟现实(VR)、导航、移动机器人、无人机、无人驾驶等领域。视觉里程计(Visual Odometry，VO)作为SLAM系统的前端，负责接收图像序列，通过特征匹配等方法估计相机在相邻帧间的运动，从而获得累积的里程信息。视觉惯性里程计是在视觉里程计的相机基础上加上惯性测量单元(IMU)，能在SLAM中更好地发挥实时追踪定位的功能。目前相机和惯性测量单元(IMU)已配备在大多数移动端设备上，采用视觉和惯性融合的里程计定位算法受到越来越多的关注。在机器人、无人机导航、运动捕捉以及增强现实等领域中，经常将相机与惯性测量单元绑定在一个运动物体上，通过对两者信息的融合，确定出一个较精确的位姿。

但是在视觉惯性里程的工作过程中，相机的内部参数、惯性测量单元的内部参数、相机与惯性测量单元的相对位姿以及相机与惯性测量单元的延时误差等均是关键的参数，未经校准的相机和惯性测量单元可能存在漂移误差或偏置，且相机与惯性测量单元的相对位姿和延迟时差对视觉惯性里程计的输出结果影响较大，因此需要对相机的内部参数、惯性测量单元的内部参数、相机与惯性测量单元的相对位姿以及相机与惯性测量单元的延时误差等参数进行标定适配，以期提高视觉惯性里程计的计算精度。

目前现有的视觉惯性里程计的标定方法仅适用于单目或双目相机，不具有兼容性，本发明实施例提供的视觉惯性里程计的标定适配及评价方法是通用的流程，不限于单目、双目或多目相机。

发明内容

本发明提供一种视觉惯性里程计标定适配及评价方法和系统，可用于多目相机的多参数标定适配及评价优化，提高了视觉惯性里程计中参数标定的准确性。

本发明提供一种视觉惯性里程计标定适配及评价方法，所述方法包括：

在视觉惯性里程计中对获取到的相机与惯性测量单元(IMU)的联合参数进行优化；当所述相机与IMU的联合参数趋于稳定后停止优化，得到所述相机与IMU的联合参数的优化值，并将所述相机与IMU的联合参数优化值设为所述视觉惯性里程计中的初值；

对优化后的视觉惯性里程计进行测试评价，如果通过所述测试，则完成对所述视觉惯性里程计的标定；

所述测试评价包括离线评价、在线原点漂移评价、在线视觉尺度评价中的至少一种，其中：

所述离线评价是将所述视觉惯性里程计输出的结果与设备移动轨迹的真值进行比较，统计所述设备移动轨迹的误差，所述误差小于设定值时则通过所述离线评价；

所述在线原点漂移评价是在所述设备开始运动的时刻记录第一帧坐标X₀，令所述设备按闭合路线移动并回到开始时刻的设备位置，所述移动过程中记录每一帧坐标X_n，计算移动路径总长度

相对漂移率

所述相对漂移率小于设定值时则通过所述在线原点漂移评价；

所述在线视觉尺度评价是在所述设备开始运动的时刻记录第一帧相机坐标X₀，令所述设备沿直线移动至终点，记录最后一帧相机坐标X_n，计算X_n相对X₀的位移长度真值path_gt，并计算尺度误差率

所述尺度误差率小于设定值时则通过所述在线视觉尺度评价；

对于未通过所述测试评价的视觉惯性里程计，直接在所述视觉惯性里程计中适配所述视觉惯性里程计中所使用的所述相机和所述IMU的初始参数，从而在所述视觉惯性里程计中进行优化。

优选地，所述在视觉惯性里程计中对获取到的所述相机与IMU的联合参数进行优化，包括：

在所述视觉惯性里程计中，通过计算所述相机与所述IMU的位姿关系，迭代优化所述相机与IMU的联合参数。

优选地，所述视觉惯性里程计是基于卡尔曼滤波器(EKF)或基于误差函数对所述相机与IMU的联合参数进行优化的。

优选地，所述相机与IMU的联合参数包括所述相机与IMU延时误差参数t_d以及相机和IMU的相对位姿关系变换矩阵Tcb。

优选地，所述方法还包括：

将标定完成的所述相机的内部参数发送给所述IMU；

利用所述IMU采集到的运动信息，标定IMU的内部参数；

根据所述相机的内部参数以及所述IMU的内部参数，标定所述相机与IMU的联合参数；

将所述相机的内部参数，所述IMU的内部参数以及所述相机与IMU的联合参数发送给所述视觉惯性里程计。

优选地，对于未通过所述测试评价的视觉惯性里程计，重新标定所述相机的内部参数和IMU的内部参数和/或重新标定所述相机与IMU的联合参数，并将所述重新标定的IMU的内部参数和/或所述重新标定的相机与IMU的联合参数发送给所述视觉惯性里程计进行优化。

本发明还提供一种视觉惯性里程计标定适配及评价系统，包括存储器和处理器，其中：

所述存储器，用于储存代码和相关数据；

所述处理器，用于执行所述存储器中的代码用以实施前述任一项所述的方法步骤。

本发明还提供一种视觉惯性里程计标定适配及评价系统，包括视觉惯性里程计和测试评价模块，其中：

所述视觉惯性里程计模块，用于对获取到的相机与惯性测量单元(IMU)的联合参数进行优化，当所述相机与IMU的联合参数趋于稳定后停止优化，得到所述相机与IMU的联合参数的优化值，并将所述相机与IMU的联合参数优化值设为所述视觉惯性里程计中的初值；对于未通过所述测试评价的视觉惯性里程计，直接在所述视觉惯性里程计中适配所述视觉惯性里程计中所使用的所述相机和所述IMU的初始参数，从而在所述视觉惯性里程计中进行优化；

所述测试评价模块，用于对所述优化后的视觉惯性里程计进行测试评价，如果通过所述测试，则完成对所述视觉惯性里程计的标定；

所述测试评价包括离线评价、在线原点漂移评价、在线视觉尺度评价中的至少一种；

所述离线评价是将所述视觉惯性里程计输出的结果与安装有视觉惯性里程计的设备的移动轨迹的真值进行比较，统计所述设备的移动轨迹误差，所述误差小于设定值时则通过所述离线评价；

所述在线原点漂移评价是在所述设备开始运动的时刻记录第一帧标X₀，令所述设备按闭合路线移动并回到开始时刻的设备位置，所述移动过程中记录每一帧坐标X_n，计算移动路径总长度

相对漂移率

所述尺度误差率小于设定值时则通过所述在线视觉尺度评价。

优选地，所述视觉惯性里程计模块还用于通过计算所述相机与所述IMU的位姿关系，迭代优化所述相机与IMU的联合参数。

优选地，所述相机与IMU的联合参数包括所述相机与所述IMU延时误差参数t_d以及所述相机和所述IMU的相对位姿关系变换矩阵Tcb。

优选地，所述系统还包括所述相机和所述IMU，其中：

所述相机，用于将标定后的所述相机的内部参数发送给所述IMU；

所述IMU，用于利用所述IMU采集到的运动信息，标定IMU的内部参数；根据所述相机的内部参数以及所述IMU的内部参数，标定所述相机与IMU的联合参数；并将所述相机的内部参数、IMU的内部参数以及所述相机与IMU的联合参数发送给所述视觉惯性里程计。

优选地，所述相机，还用于对未通过所述测试评价的视觉惯性里程计的情形下，重新标定所述相机的内部参数；

所述IMU，还用于重新标定所述IMU的内部参数，并根据重新标定的所述相机的内部参数以及所述IMU的内部参数，重新标定所述相机与IMU的联合参数，并将重新标定的IMU内部参数以及所述相机与IMU的联合参数发送给所述视觉惯性里程计进行优化。

本发明实施例所提供的视觉惯性里程计标定适配及评价方法和系统，可以实现的有益效果是：

本发明提供一套完整的相机和IMU相对位姿适配及误差评价反馈的优化流程，利用设备所搭载的相机和IMU，计算设备在当前环境中的位置和姿态信息，显著提高视觉惯性里程计在不同环境中、不同设备上的计算精度和计算效率。本发明实施例所提供的方法及系统应用面广泛，可适用于不限单目、双目和多目的相机，不限于单IMU和多IMU的设备；参数标定适配的结果不断由视觉惯性里程计进行优化，可以加快初始化速率；可通过实时相机与IMU的相对位姿关系优化，设备和场景针对性强。

附图说明

图1是本发明实施例视觉惯性里程计标定适配及评价方法的第一流程示意图；

图2是本发明实施例视觉惯性里程计标定适配及评价方法的第二流程示意图；

图3是本发明实施例视觉惯性里程计标定适配及评价方法的第三流程示意图；

图4是本发明实施例视觉惯性里程计标定适配及评价方法的第四流程示意图；

图5是本发明实施例视觉惯性里程计标定适配及评价方法的第五流程示意图；

图6是本发明实施例视觉惯性里程计标定适配及评价方法的第六流程示意图；

图7是本发明实施例视觉惯性里程计标定适配及评价方法的第七流程示意图；

图8是本发明实施例视觉惯性里程计标定适配和评价系统的第一结构框图；

图9是本发明实施例视觉惯性里程计标定适配和评价系统的第二结构框图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一：

如图1、图7所示，本发明实施例提供了一种视觉惯性里程计标定适配及评价方法，包括以下步骤：

100、将标定完成的相机内部参数发送给惯性测量单元(IMU)；其中，相机的内部参数包括相机的内参矩阵K和畸变参数矩阵D；所述相机的内参矩阵K是一个3*3的矩阵，包括但不限于相机的主点、焦距、中心点偏移等信息。畸变参数矩阵D是一个一维的矩阵，包括但不限于径向畸变参数、切向畸变参数或鱼眼畸变参数；

200、利用所述IMU采集到的运动信息，标定IMU的内部参数；其中，IMU的内部参数包括IMU在xyz三个方向上的偏置以及IMU在xyz三个方向上的噪声方差，以及IMU在xyz三个方向上的随机游走。

300、根据所述相机的内部参数以及所述IMU的内部参数，标定相机与IMU的联合参数，并将所述相机的内部参数、所述IMU的内部参数，以及所述相机与IMU的联合参数发送给所述视觉惯性里程计；其中，所述相机和IMU的联合参数包括相机与IMU延时误差参数t_d(t_imu＝t_cam+t_d)以及相机和IMU的相对位姿关系变换矩阵Tcb([R|t])。所述[R|t]中t是1*3的矩阵，R是3*3的矩阵。

400、在视觉惯性里程计中，分别计算所述相机与所述IMU的运动轨迹，不断交叉验证所述相机与所述IMU的运动轨迹，迭代优化所述IMU的内部参数；通过所述视觉惯性里程计对所述相机与IMU的联合参数进行不断优化，在所述相机与IMU的联合参数趋于稳定后结束优化，得到所述相机与IMU的联合参数的优化值，并将其设为所述视觉惯性里程计中的初值之一。所述视觉惯性里程计中的初值可以包括步骤100中所述相机内部参数、步骤200中所述IMU内部参数和步骤300中所述的相机与IMU的联合参数。

进一步的，在视觉惯性里程计中，迭代优化所述相机与IMU的联合参数的同时，也对所述IMU的内部参数进行优化，得到所述IMU的内部参数的优化值，并将其设为所述视觉惯性里程计中的初值。

在本发明实施例中，所述视觉惯性里程计是基于卡尔曼滤波器或基于误差函数对所述相机与IMU的联合参数进行优化的。

500、对所述优化后的视觉惯性里程计进行测试评价，如果通过测试，则完成对所述视觉惯性里程计的标定。

在本发明实施例中，对于未通过所述评价测试的，重新标定所述相机的内部参数和IMU的内部参数和/或重新标定所述相机和IMU的联合参数，继续发送给视觉惯性里程计进行优化，或者，还可以直接在视觉惯性里程计中适配所述相机与所述IMU的初始参数，并继续进行优化。

在本发明实施例中，所述测试评价包括离线评价、在线原点漂移评价、在线视觉尺度评价中的至少一种。

实施例二：

如图2至图7所示，本发明实施例提供一种视觉惯性里程计标定适配及评价方法，包括以下步骤：

S1、对相机进行标定，获得相机的内部参数，所述相机的内部参数包括相机的第二内参矩阵K以及畸变矩阵D。所述K是3*3的矩阵，D是1*n的矩阵，其中n是0～10之间的某个值；

具体的，如图3所示，对所述相机的标定过程包括：

步骤101、获取外部场景图对相机进行标定，获得所述相机的第一内参矩阵K，所述K是3*3的矩阵，包含了相机的主点、焦距、中心点偏移等信息；

在对相机进行标定时，可以选择多种标定方式，本发明实施例以张正友标定法为例，通过拍摄预设的模板标定相机来获得相机的内部参数：在场景图中设置平面模板作为待标定图像，使用设备上的相机拍摄一张包含平面模板的场景图，记录并输入所述平面模板的真实物理尺寸；具体的，平面模板可以是由棋盘格、二维码等组成的。

为了二维图像与三维场景的变换，设P＝(X，Y，Z)为场景图中的一点，经过以下几个变换，最终变为二维图像上的像点p＝(μ，v)：将P从世界坐标系通过刚体变换(旋转和平移)变换到相机坐标系；从相机坐标系，通过透视投影变换到相机的成像平面上的像点p＝(x，y)；将像点p从成像坐标系，通过缩放和平移变换到像素坐标系上点p＝(μ，v)。

相机将场景中的三维点变换为图像中的二维点，也就是各个坐标系变换的组合，可将上面的变换过程整理为矩阵相乘的形式：

将矩阵K称为相机的内参矩阵，则

其中，α和α表示图像上单位距离上像素的个数，则f_x＝αf，f_y＝βf，将相机的焦距f变换为在x，y方向上像素度量表示。

另外，为了不失一般性，可以在相机的内参矩阵上添加一个扭曲参数γ，该参数用来表示像素坐标系两个坐标轴的扭曲。则相机的第一内参矩阵K为：

通过一幅标定板的图像可以的得到关于内参数的两个等式，令

矩阵B是一个对称矩阵，其未知量只有6个，将6个未知量写为向量的形式：

b＝[B₁₁，B₁₂，B₂₂，B₁₃，B₂₃，B₃₃]

令h_i为单应矩阵H的第i个行向量，则有：

h_i＝[h_i1，h_i2，h_i3]^T

因此，h_iK^-TK^-1h_j＝h_iBh_j，

拍摄多幅场景图后可以计算求得相机的各个内参数，得到相机的第一内参矩阵K，并输出至步骤102，在获取畸变参数矩阵D和相机的第二内参矩阵K的过程中使用。

为了进一步增加标定结果的可靠性，可以使用最大似然估计(Maximumlikelihood estimation)来优化上面估计得到的结果；这是一个非线性优化问题，也可以使用Levenberg-Marquardt的方法，利用上面得到的解作为初始值，迭代得到最优解。

步骤102、获取外部场景图对相机进行标定，获取相机的内部参数中的畸变参数矩阵D，D是其中畸变参数矩阵D包含了径向畸变等信息，D是1*n的矩阵，其中n是0～10之间的某个值；并利用畸变参数矩阵D优化相机的第一参数矩阵K，获得相机的第二参数矩阵K，K为3*3矩阵：

为了取得好的成像效果，在相机的镜头前添加透镜。在相机成像的过程中，透镜会对光线的传播产生影响，从而影响相机的成像效果，产生畸变。

为了消除畸变，设(μ，v)是理想的无畸变的像素坐标；

是畸变后的像素坐标；(μ₀，v₀)是相机的主点；(x，y)和

理想的无畸变的归一化的图像坐标和畸变后的归一化图像坐标，使用下面的式子表示径向畸变：

畸变参数矩阵D与畸变的关系为：

k＝[k₁k₂]^T＝(D^TD)^-1D^Td

拍摄多幅场景图后计算得到畸变参数矩阵D。

和计算相机的第一内参矩阵K的方法类似，利用最大似然估计取得畸变参数矩阵D的最优解，也可使用Levenberg-Marquardt的方法估计求得最优畸变参数矩阵D。在得到畸变参数矩阵D后，可以利用畸变参数矩阵D对图像进行去畸变处理，获得去畸变后的图像坐标，优化相机的第一参数矩阵K，获得相机的第二参数矩阵K。

由此，获得相机第二内参矩阵K和畸变参数矩阵D，即得到相机的内部参数并输出至IMU单元，在相机与IMU联合标定过程中使用。

S2、对IMU进行标定，获得IMU的第一内部参数；对相机与IMU进行联合标定，获得相机与IMU的第一联合参数。

如图4所示，相机与IMU联合标定的具体流程包括：

步骤201、对IMU进行标定，获得所述IMU的第一内部参数，所述IMU的第一内部参数包括IMU在xyz三个方向上的偏置、IMU在xyz三个方向上的噪声方差以及IMU在xyz三个方向上的随机游走；

在对IMU进行标定时，可以选择多种标定方式，本发明实施例中仅以其中的一种标定方式为例进行说明：将IMU以不同位姿放置在水平面上，以水平位姿放置的IMU获取的信息构建对应的x轴，以此类推得到根据IMU获取的信息构建的xyz轴，IMU的第一内部参数准确时所构建的xyz轴应当相互正交且各轴尺度一致，不存在畸变。通过重投影误差方法计算获得IMU在xyz三个方向上的偏置、IMU在xyz三个方向上的噪声方差以及IMU在xyz三个方向上的随机游走，使得去除偏置和噪声等后的xyz轴相互正交且各轴尺度一致，记录此时IMU在xyz三个方向上的偏置以及IMU在xyz三个方向上的噪声方差，即获得IMU的第一内部参数并输出至步骤202，在相机与IMU联合标定过程中使用。

步骤202、使用所述相机的内部参数以及所述IMU的第一内部参数对相机与IMU进行联合标定，获得IMU的第二内部参数，以及所述相机与IMU的第一联合参数，其中：相机与IMU的第一联合参数包括相机与IMU的延时误差参数t_d(t_imu＝t_cam+t_d)以及相机和IMU的相对位姿关系变换矩阵Tcb([R|t])，相机和IMU的相对位姿变换关系包括平移和旋转，所述[R|t]中t是1*3的矩阵，R是3*3的矩阵；所述IMU的第二内部参数包括IMU在xyz三个方向上的偏置、IMU在xyz三个方向上的噪声方差以及IMU在xyz三个方向上的随机游走。

具体包括以下步骤：

将搭载有相机和IMU的设备围绕待标定图像进行运动，利用已获取的相机内部参数、IMU的第一内部参数，分别采集并计算相机和惯性测量单元的运动轨迹；对齐相机与IMU采集到的每一帧图像，通过重投影误差算法找到相机与IMU采集到的图像重合度最高时的相机与IMU延时误差参数t_d，使得相机与IMU采集到的图像误差最小；通过梯度下降法，对IMU的第一内部参数进行优化，得到使得相机和惯性测量单元的轨迹重合度最高的IMU的第二内部参数，并读取此时的相机与IMU延时误差参数t_d以及相机与IMU相对位姿关系变换矩阵Tcb，即得到相机与IMU的第一联合参数。记录相机与IMU轨迹误差最小状态下的IMU的第二内部参数、相机与IMU的第一联合参数，并输出至步骤S3，在利用视觉惯性里程计优化参数的过程中使用。在本发明实施例中，所述的设备搭载有本发明实施例所描述的相机和IMU。

S3、利用视觉惯性里程计优化参数，获得IMU的第四内部参数、相机与IMU的第三联合参数。相机与IMU的第三联合参数包括相机与IMU延时误差参数t_d以及相机与IMU相对位姿关系变换矩阵Tcb([R|t])，所述IMU的第四内部参数包括IMU在xyz三个方向上的偏置、IMU在xyz三个方向上的噪声方差以及IMU在xyz三个方向上的随机游走。

如图5所示，使用视觉惯性里程计优化参数的具体步骤包括：

步骤301、使用视觉惯性里程计优化IMU的第二内部参数、相机与IMU的第一联合参数，获得IMU的第三内部参数、相机与IMU的第二联合参数，具体步骤为：

使设备围绕待标定图像进行运动，利用相机的内部参数、IMU的第二内部参数、相机与IMU的第一联合参数，分别获取相机与IMU采集到的图像并计算相机与IMU的运动轨迹，不断使用迭代的方式优化相机和IMU的参数：调整IMU在xyz三个方向上的偏置、IMU在xyz三个方向上的噪声方差、IMU在xyz三个方向上的随机游走、相机与IMU延时误差参数t_d以及相机和惯性测量单元的相对位姿关系变换矩阵Tcb，记录相机与IMU的运动轨迹重合度最高时的对应参数，即获得IMU的第三内部参数、相机与IMU的第二联合参数，并用在步骤302中计算相机与IMU的相对位姿；

具体的，可以选择多种迭代的方式，例如基于卡尔曼滤波器(EKF)或基于误差函数等进行非线性优化。本发明实施例以基于卡尔曼滤波器(EKF)的视觉惯性里程计为例，通过交叉验证相机与IMU的运动轨迹非线性优化各参数。在卡尔曼滤波器中输入IMU的第三内部参数、相机与IMU的第二联合参数以及其他参数，分别获取相机与IMU采集到的图像并计算相机与IMU的运动轨迹，利用IMU当前帧的位姿预测下一帧的IMU位姿，利用相机当前帧的位姿预测下一帧的相机位姿，并利用相机的预测位姿更新IMU预测的位姿，进行交叉验证，找出设备在下一帧概率最大的位姿，在不停迭代的过程中对各参数进行优化。具体的，卡尔曼滤波器的输入可以包含以下数据：r：IMU坐标系下位置，v：IMU坐标系下速度，q：IMU和世界坐标系变换关系，b：IMU和角速度计噪声偏置，Tcb：IMU和相机坐标系变换关系，μ：视觉特征点位置。卡尔曼滤波器的输入数据越多，最终获取的参数更优，使得相机与IMU的运动轨迹误差更小，对设备的位姿估计精确度更高。

本发明实施例还可以使用基于优化误差函数的视觉惯性里程计，令设备围绕待标定图像进行运动，将相机和惯性测量单元的相对位姿关系作为优化目标量，找到最优的参数使得相机与IMU采集到的图像重合度最高。

步骤302、优化相机与IMU的相对位姿关系，获得IMU的第四内部参数、相机与IMU的第三联合参数，具体步骤为：

优化相机与IMU的相对位姿关系的过程中不断读取视觉惯性里程计当前使用的相机内部参数、IMU的第三内部参数、相机与IMU的第二联合参数，当各参数的读数趋于稳定后记录当前使用的各参数值，即获得IMU的第四内部参数、相机与IMU的第三联合参数并停止优化过程；此时，IMU的第四内部参数是经过优化得到的，可以称为IMU的内部参数的优化值；相机与IMU的第三联合参数也是经过优化得到的，可以称为相机与IMU的联合参数的优化值。在本发明实施例一个优选的方案中，可以将IMU的第四内部参数和/或所述相机与IMU的第三联合参数设置为视觉惯性里程计的初值。

具体的，根据使用情境及工程经验选择参数稳定的阈值，本实施仅举一例：在视觉惯性里程计对各参数进行优化的过程中，记录视觉惯性里程计每次计算过程中所使用的各个参数，以及各参数的变化，比对3秒内所记录下的每次计算过程中使用的相机和惯性测量单元相对位姿关系变换矩阵Tcb，当3秒内的记录不再变化时，停止优化相机与IMU的相对位姿关系，并将当前的相机和惯性测量单元的相对位姿关系变换矩阵Tcb输出至视觉惯性里程计，将优化后的相机和惯性测量单元的相对位姿关系变换矩阵Tcb设为相机与IMU相对位姿关系的初值，使得视觉惯性里程计在下次使用时直接调用预设的相机和惯性测量单元的相对位姿关系变换矩阵Tcb进行计算；采用预设的优化后的相机和惯性测量单元相对位姿关系变换矩阵Tcb，可大幅缩短视觉惯性里程计进入稳定状态的时间，提高优化相机与IMU相对位姿关系的效率。

目前现有的视觉惯性里程计的标定方法仅局限于单个步骤的参数优化，例如仅适用于优化相机与惯性测量单元的相对位姿的标定方法，缺少完整的视觉惯性里程计参数标定流程，单个优化步骤未能和整体标定流程充分融合，也即不能系统地对各参数进行优化。本发明实施例提供的视觉惯性里程计标定适配方法，可以对视觉惯性里程计所使用的各参数进行完整的系统性优化，对于各参数的优化步骤是可交叉验证的、强耦合的，进一步提高了视觉惯性里程计的精度。

S4、评价测试所得的相机的内部参数、IMU的第四内部参数、相机与IMU的第三联合参数。

参考附图6，对所得到的参数进行测试，包括以下步骤：

步骤401、对已优化各参数的设备进行测试评价，如果通过评价测试，则结束标定过程，将步骤302中所确定的各参数输入安装有所述视觉惯性里程计的设备中。评价测试的标准是将测试中设备的输出与各评价测试方式对应的阈值进行比较，低于预设的阈值则通过测试；阈值的设置按具体应用场景的需求进行数值调整。

评价测试可以采用离线评价的方式：

离线评价将所得的视觉惯性里程计输出的结果同设备移动轨迹的真值进行比较，统计轨迹误差，绝对误差函数为

其中X_n表示第n帧视觉惯性里程计输出的相机坐标值，X_{gt_n}表示第n帧相对应真值，移动路径总长度

相对漂移率

所述绝对误差和/或所述相对漂移率小于设定值则通过所述离线评价。

评价测试还可以采用在线原点漂移评价的方式：

在线原点漂移评价是在装载有相机和IMU的设备开始运动的时刻记录第一帧在世界坐标系下的三维坐标X₀，令设备在目标场景中按闭合路线移动并回到开始时刻的设备位置。在一个优选的实施例中，所述闭合路线可以是任意不规则的，也可以是设定规则的，例如圆形路线、矩形路线等规则图形路线；设备按闭合路线移动的圈数可以是一圈或多圈，闭合路线要求设备最终回到出发点即可。在设备移动过程中记录每一帧在世界坐标系下的三维坐标X_n，同时在移动过程中计算移动路径总长度

相对漂移率

所述相对漂移率小于设定值则通过所述在线原点漂移评价。

评价测试还可以采用在线视觉尺度评价的方式：

在线视觉尺度评价是在装载有相机和IMU的设备开始运动的时刻记录第一帧相机在世界坐标系下的三维坐标X₀，令设备沿直线移动至终点，记录最后一帧相机在世界坐标系下的三维坐标X_n，同时计算X_n相对X₀的位移长度真值path_gt，并计算尺度误差率

所述尺度误差率小于设定值则通过所述在线视觉尺度评价。

在所述评价测试中，所述设定值根据使用场景、计算精度、计算效率等需求进行相应调整。

对于通过评价测试的视觉惯性里程计，可以将得到的优化后的各参数值作为视觉惯性里程计进行后续计算的初始数值。

步骤402、如图7所示，未通过评价测试的，可以根据评价测试的结果，重新设置相机的内部参数、IMU的内部参数以及相机与IMU联合参数，再次进行优化。

重新设置参数的路径有以下选择：相机内参矩阵K和畸变参数矩阵D存在偏差的，返回步骤101重新标定相机的内部参数；IMU的内部参数和/或相机与IMU联合参数存在偏差的，返回步骤201重新标定各参数；相机与IMU的参数适配存在偏差的，适配相机和IMU的初始参数，具体包括：调整视觉惯性里程计中相机的初始参数，例如特征点相邻帧搜索半径、迭代次数、视觉特征点个数等，和/或调整视觉惯性里程计中IMU的初始参数，例如IMU的初始速度等，依据具体应用场景调整改变参数的阈值，并将适配后的相机参数和IMU参数输出至视觉惯性里程计，利用视觉惯性里程计对调整后的各参数重新进行优化，重复步骤301至步骤401。

现有的视觉惯性里程计标定方法通常是在离线状态预先标定相机与惯性测量单元相对位姿参数，目前主流的误差离线评价方法使用姿态捕捉设备校准相机与惯性测量单元的相对位姿，评价误差，但是使用姿态捕捉设备评价进行离线评价时需要进行额外的时钟同步，将会引入额外误差。此外，离线评价的方法难以实时有针对性地对特定场景进行标定。

此外，目前虽然有部分在线标定相机与惯性测量单元相对位姿的方法，但通常在线标定与离线标定的方法相互独立或松耦合，这就导致在线标定的结果无法对离线标定的参数进行优化。本发明实施例提供的视觉惯性里程计标定适配及评价方法将在线评价与离线评价耦合，进一步提高了视觉惯性里程计的可靠性。

在本发明实施例中，还可以使用其他设备实时动态捕捉设备运动轨迹的真值，并在步骤401评价测试的过程中实时与视觉惯性里程计输出的设备当前位姿进行比较，进一步提高视觉惯性里程计的精度。

在本发明实施例中，还可以将相机与IMU的第一联合参数设为所述相机与所述IMU的初值，将步骤302优化相机与IMU的相对位姿关系与步骤202相机与IMU联合标定过程相结合，进一步减少计算量，提高计算效率。

实施例三：

如图8所示，本发明实施例提供了一种视觉惯性里程计标定适配和评价系统，所述系统包括存储器801和处理器802，其中：

所述存储器801，用于存储代码和相关数据；

所述处理器802，用于执行所述存储器801中的代码和相关数据用以实现前述实施例一和实施例二中的方法步骤。

实施例四：

如图9所示，本发明实施例提供了一种视觉惯性里程计标定适配和评价系统，所述系统包括视觉惯性里程计模块901和测试评价模块902，其中：

所述视觉惯性里程计模块901，用于对获取到的相机与IMU的联合参数进行优化，当所述相机与IMU的联合参数趋于稳定后停止优化，得到所述相机与IMU的联合参数的优化值，并将所述相机与IMU的联合参数优化值设为所述视觉惯性里程计中的初值；

所述测试评价模块902，用于对所述优化后的视觉惯性里程计进行测试评价，如果通过所述测试，则完成对所述视觉惯性里程计的标定。

在本发明实施例中，所述视觉惯性里程计模块901，还用于对获取到的IMU的内部参数进行优化，得到所述IMU的内部参数的优化值，并将所述IMU的内部参数的优化值设为所述视觉惯性里程计中的初值；所述IMU的内部参数包括IMU在xyz三个方向上的偏置以及IMU在xyz三个方向上的噪声方差。

在本发明实施例中，所述视觉惯性里程计模901块还用于通过计算所述相机与所述IMU的位姿关系，迭代优化所述IMU的内部参数，以及所述相机与IMU的联合参数。相机与IMU的联合参数包括相机与IMU延时误差参数t_d以及相机与IMU相对位姿关系变换矩阵Tcb。

在本发明实施例中，所述测试评价模块902使用离线评价的方式对所述优化后的视觉惯性里程计进行评价，将所述视觉惯性里程计输出的结果与安装有视觉惯性里程计的设备移动轨迹的真值进行比较，统计移动轨迹误差，所述误差小于设定值则通过所述离线评价。

在本发明实施例中，所述测试评价模块902还使用在线原点漂移评价和/或在线视觉尺度评价的方式对所述优化后的视觉惯性里程计进行评价，其中：

所述在线原点漂移评价是在安装有视觉惯性里程计的设备开始运动的时刻记录第一帧的坐标X₀，令所述设备按闭合路线移动并回到开始时刻的设备位置，所述移动过程中记录每一帧坐标X_n，计算移动路径总长度

相对漂移率

所述相对漂移率小于设定值则通过所述在线原点漂移评价；

所述在线视觉尺度评价是在安装有视觉惯性里程计的设备开始运动的时刻记录第一帧的相机坐标X₀，令所述设备沿直线移动至终点，记录最后一帧相机坐标X_n，计算X_n相对X₀的位移长度真值path_gt，并计算尺度误差率

所述尺度误差率小于设定值则通过所述在线视觉尺度评价。

在本发明实施例中，所述视觉惯性里程计模块901还用于对于未通过所述评价测试的视觉惯性里程计，直接在所述视觉惯性里程计中适配所述视觉惯性里程计中所使用的相机和IMU的初始参数，从而在所述视觉惯性里程计中进行优化。所述相机的初始参数包含特征点相邻帧搜索半径、迭代次数、视觉特征点个数等；所述IMU的初始参数包含IMU的初始速度等。依据具体应用场景调整适配相机和IMU的初始参数调整阈值。

在本发明实施例中，所述系统还包括相机903和IMU 904，其中：

所述相机903，用于将标定完成的所述相机的内部参数发送给所述IMU；

所述IMU 904，用于利用所述IMU采集到的运动信息，标定IMU的内部参数；根据所述相机的内部参数以及所述IMU的内部参数，标定所述相机与IMU的联合参数；并将所述IMU的内部参数以及所述相机与IMU的联合参数发送给所述视觉惯性里程计。

在本发明实施例中，所述相机903，还用于对于未通过所述评价测试的视觉惯性里程计，重新标定所述相机的内部参数；

所述IMU 904，还用于重新标定所述IMU的内部参数，并根据重新标定的所述相机的内部参数以及所述IMU的内部参数，重新标定所述相机与IMU的联合参数，并发送给所述视觉惯性里程计进行优化。

为了进一步提高所述系统的精度，所述测试评价模块还可以加入其它用于动态捕捉实际轨迹真值的设备，将视觉惯性里程计模块的轨迹输出实时与动态捕捉所获的真值进行比较。

为了进一步提高所述系统的效率，所述相机与IMU相对位姿优化模块可以放在所述相机与IMU联合标定模块中，进一步减少计算量。

在本发明实施例中，所述视觉惯性里程计模块可以集成在设备上，例如集成在设备的处理器上，设备的处理器可以是CPU、服务器，或者其他中央处理器等。

本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。例如，以上所描述的系统实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所属技术领域的技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于计算机可读取存储介质中，并被通讯设备内部的处理器执行，前述的程序在被执行时处理器可以执行包括上述方法实施例的全部或者部分步骤。其中，所述处理器可以作为一个或多个处理器芯片实施，或者可以为一个或多个专用集成电路(Application Specific Integrated Circuit，ASIC)的一部分；而前述的存储介质可以包括但不限于以下类型的存储介质：闪存(Flash Memory)、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。