实操 ----Mocap采集VIO数据集

目的

本文介绍采用Mocap采集VIO数据集的流程以及相关标定处理过程。
参考论文：The TUM VI Benchmark for Evaluating Visual-Inertial Odometry
参考代码：https://gitlab.com/VladyslavUsenko/basalt

简介

VIO数据集包含轨迹真值，可以通过轨迹对齐的方式，求解绝对误差ATE，相对误差APE来评估VIO系统的误差。

轨迹对齐有两种方式：相似变换，手眼标定。

相似变换

通常公开数据集里面的真值已经做了坐标系转换，VIO数据集中的真值轨迹，已经是IMU坐标系的轨迹真值。因此，可以直接采用相似变换进行轨迹对齐。
刚体变换（scale = 1的相似变换）估计的是真值轨迹和VIO轨迹的两个世界坐标系之间的相对位姿$T_{w_2}^{w_1}=[R,t]$, 如下图：
相似变换求解原理（通过两条轨迹的3D位移点来对齐）：
$$p_{align}=s\ast R\ast p+t$$
其中， $p_{align}$ 与 $p$ 分别为两条轨迹的匹配3D点（ 匹配关系通过找对应时刻的3D点）。

轨迹对齐之后，可以计算两条轨迹的绝对误差ATE和相对误差APE.

手眼标定

Mocap 采集红外小球构成的刚体（Marker）轨迹，直接和VIO轨迹对齐时，涉及到两个世界坐标系之间的变换$T_{w_2}^{w_1}$以及Marker与IMU之间的外参$T_{marker}^{imu}$。手眼标定通过轨迹的相对姿态，可以求解得到： $T_{imu}^{marker}$.
如下图所示，可以构建求解等式:
$$AX=XB$$
其中, X 为待求解变量 $T_{imu}^{marker}$ , A 为VIO轨迹两个点之间的相对pose $T_{i_2}^{i_1}$, B 为真值轨迹两个点之间的相对pose $T_{m_1}^{m_2}$.

求解得到$T_{imu}^{marker}$之后，可以将真值轨迹从Marker坐标系转到IMU坐标系下，即：
$$T_{imu}^{w_2}=T_{marker}^{w_2}\ast T_{imu}^{marker}$$

然后，得到的真值轨迹，再通过刚体变换，可以实现轨迹对齐，从而评估VIO轨迹。

注意：

• 这里两个轨迹已经认为做过时间同步，相应的匹配关系，通过搜索最近时间点来获取。
• 由于计算$T_{imu}^{marker}$时，用到了VIO的轨迹，如果VIO轨迹有偏差，则估计的$T_{imu}^{marker}$就不准确，从而会把这个误差引入到对齐误差中。因此，通常情况下，当我们采集到Marker的真值轨迹时，会通过额外提前标定好$T_{imu}^{marker}$来处理轨迹，得到IMU的真值轨迹。后续，直接采用轨迹对齐，即可实现轨迹评估。

标定

论文 The TUM VI Benchmark for Evaluating Visual-Inertial Odometry 介绍了采集VIO数据集时，涉及到的相关标定。包含cam-IMU之间的外参、timeshift， 以及Mocap-IMU之间外参、timeshift， 以及IMU内参标定等。 basalt 提供了相应的标定代码。

下面介绍采集Mocap数据集的主要流程：

• 双目静态标定内参以及双目外参
• Allan方差标定IMU噪声
• AprilgTag采集动态轨迹，标定cam-IMU之间的外参、timeshift， 以及Mocap-IMU之间外参、timeshift，以及IMU的轴偏和尺度因子。
• 采集Mocap轨迹和cam-imu数据
• 处理Mocap轨迹到IMU坐标系。
• 时间同步Mocap轨迹到IMU时间系统。

Mocap-IMU-cam标定

basalt代码的 basalt_calibrate_imu 模块可以标定 cam-IMU 之间的外参、timeshift， 以及 Mocap-IMU 之间外参、timeshift，以及IMU的轴偏和尺度因子。

cam-imu标定

cam-imu的标定原理可以参考kailbr.
相机对着AprilTag板子，采集动态数据，包含cam图像和IMU数据。
标定变量：cam-imu的外参，timeshift，IMU的轴偏和尺度因子。

标定原理简述：

构建 $T_i^w$ 位姿样条，通过三个残差优化样条系数和待求解变量。

• $e=[u,v{]}^T-\pi (T_i^w\ast T_c^i\ast p)$ , 其中$p$ 为AprilTag点。$T_i^w$由样条获得。$T_c^i$ 为待求解的cam-imu的外参。
• $e=w-(w_m-b_g)$, 旋转样条微分得到角速度$w$，与IMU测量角速度$w_m$构建残差，优化样条系数和$b_g$。
• $e=R^{-1}(a+g)-(a_m-b_a)$, 位移样条两次微分得到加速度$a$，与IMU的加速度测量$a_m$构建残差，优化样条系数，重力加速度 $g$, 和 $b_a$。

Mocap-IMU-cam标定

basalt标定模块可以同时标定cam-imu和Mocap-imu的相关参数。

仅需要增加Mocap测量的一个残差：$e=log(T_{mocap}^{marker}\ast (T_w^{mocap}\ast T_i^w\ast T_{marker}^i))$ ，优化$T_w^{mocap}$ 和 $T_{marker}^i$ 以及样条系数. $T_{mocap}^{marker}$为Mocap系统的测量值。

优化时，需要提供一个timeshift和$T_{marker}^{imu}$的优化初始值。

初始化timeshift

$T_{marker}^{imu}$ 可以人为给出一个大致初值，我们在贴小球时，可以尽量使得Marker坐标系与IMU坐标系的某个轴对齐，因此可以大致估计$R_{imu}^{marker}$ ， $t_{imu}^{marker}$也可以通过大致测量小球质心和IMU坐标系中心来给出。

根据提供的初值$T_{marker}^{imu}$ ， 可以将$T_{mocap}^{marker}$ 的计算的旋转角速度经过$R_{imu}^{marker}$ 可以转换为IMU的旋转角速度，从而能够得到角速度曲线。与IMU原始测量的角速度曲线进行相关性计算，即可以得到timeshift的初值。如下图，左图为不同timeshift对应的相关性误差，其最小误差就对应timeshift的大致初值，basalt的time_alignment代码中还增加了抛物线拟合，来细化timeshift， 标定过程中可以不进行细化过程。最右图为角速度曲线。

初始化 $T_{marker}^{imu}$

得到了timeshift的初始值，就可以得到同一时间下的两个角速度，一个是根据$T_{mocap}^{marker}$测量值， 经过$R_{imu}^{marker}$ 转换得到的角速度，一个是IMU陀螺仪测量的角速度。且两个角速度有如下等式：
$$w_{imu}=w_{marker}\ast R_{imu}^{marker}$$
多个测量值构建的多个等式，联合求解，即可以得到$R_{imu}^{marker}$.

$t_{imu}^{marker}$ 人为给出，或者直接设置为0.

数据后处理

设备贴好小球且标定完成之后，可以进行数据采集。Mocap系统给出Marker的轨迹真值，设备自己记录cam图像和IMU数据。

采集完原始数据之后，需要进行后处理：

• 应用$T_{imu}^{marker}$，将Marker真值轨迹转为IMU系的真值轨迹。
• 时间同步

时间同步

通常MoCap和VIO数据采集设备的时间系统不一致，Opti-Track系统每次记录轨迹时，其时间从0开始计数。因此，标定过程中得到的timeshift不能应用在这种情况。这种情况下，每次采集完一个数据时，需要对齐Marker和IMU的时间。

对齐方式和上文中《初始化timeshift》的原理一致，由于已经标定出$T_{imu}^{marker}$， 可以直接得到IMU的轨迹真值，从而可以估计出MoCap和IMU时间系统的 timeshift。注意，在标定过程中计算timeshift的初始值时，可以不进行细化，但是这里，需要抛物线拟合来细化timeshift, 从而提高timeshift的精度。

抛物线拟合细化： 取GridSearch得到的大致timeshift前后的相关性误差数据，进行抛物线拟合($f=a{x}^2+bx+c$)，根据拟合得到的抛物线参数，计算抛物线最小值（$\frac{-b}{2a}$）。

basalt 的 basalt_time_alignment 模块提供了时间同步的代码。