实操 ----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_{}}$