实操 ----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 ] T_{w_2}^{w_1} = [R, t] Tw2w1=[R,t], 如下图:
在这里插入图片描述相似变换求解原理(通过两条轨迹的3D位移点来对齐):
p a l i g n = s ∗ R ∗ p + t p_{align} = s * R * p + t palign=sRp+t
其中, p a l i g n p_{align} palign p p p 分别为两条轨迹的匹配3D点( 匹配关系通过找对应时刻的3D点)。
在这里插入图片描述
轨迹对齐之后,可以计算两条轨迹的绝对误差ATE和相对误差APE.
在这里插入图片描述

手眼标定

Mocap 采集红外小球构成的刚体(Marker)轨迹,直接和VIO轨迹对齐时,涉及到两个世界坐标系之间的变换 T w 2 w 1 T_{w_2}^{w_1} Tw2w1以及Marker与IMU之间的外参 T m a r k e r i m u T_{marker}^{imu} Tmarkerimu在这里插入图片描述手眼标定通过轨迹的相对姿态,可以求解得到: T i m u m a r k e r T^{marker}_{imu} Timumarker.
如下图所示,可以构建求解等式:
A X = X B AX = XB AX=XB
其中, X 为待求解变量 T i m u m a r k e r T^{marker}_{imu} Timumarker , A 为VIO轨迹两个点之间的相对pose T i 2 i 1 T_{i_2}^{i_1} Ti2i1, B 为真值轨迹两个点之间的相对pose T m 1 m 2 T_{m_1}^{m_2} Tm1m2.
在这里插入图片描述
求解得到 T i m u m a r k e r T^{marker}_{imu} Timumarker之后,可以将真值轨迹从Marker坐标系转到IMU坐标系下,即:
T i m u w 2 = T m a r k e r w 2 ∗ T i m u m a r k e r T_{imu}^{w_2} = T_{marker}^{w_2} * T_{imu}^{marker} Timuw2=Tmarkerw2Timumarker

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

注意

  • 这里两个轨迹已经认为做过时间同步,相应的匹配关系,通过搜索最近时间点来获取。
  • 由于计算 T i m u m a r k e r T_{imu}^{marker} Timumarker时,用到了VIO的轨迹,如果VIO轨迹有偏差,则估计的 T i m u m a r k e r T_{imu}^{marker} Timumarker就不准确,从而会把这个误差引入到对齐误差中。因此,通常情况下,当我们采集到Marker的真值轨迹时,会通过额外提前标定好 T i m u m a r k e r T_{imu}^{marker} Timumarker来处理轨迹,得到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 T^w_i Tiw 位姿样条,通过三个残差优化样条系数和待求解变量。

  • e = [ u , v ] T − π ( T i w ∗ T c i ∗ p ) e = [u,v]^T - \pi(T^w_i * T^i_c * p) e=[u,v]Tπ(TiwTcip) , 其中 p p p 为AprilTag点。 T i w T_i^w Tiw由样条获得。 T c i T_c^i Tci 为待求解的cam-imu的外参。
  • e = w − ( w m − b g ) e = w - (w_m - b_g) e=w(wmbg), 旋转样条微分得到角速度 w w w,与IMU测量角速度 w m w_m wm构建残差,优化样条系数和 b g b_g bg
  • e = R − 1 ( a + g ) − ( a m − b a ) e = R^{-1} (a+g) - (a_m - b_a) e=R1(a+g)(amba), 位移样条两次微分得到加速度 a a a,与IMU的加速度测量 a m a_m am构建残差,优化样条系数,重力加速度 g g g, 和 b a b_a ba

Mocap-IMU-cam标定

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

仅需要增加Mocap测量的一个残差: e = l o g ( T m o c a p m a r k e r ∗ ( T w m o c a p ∗ T i w ∗ T m a r k e r i ) ) e = log(T^{marker}_{mocap}*(T_{w}^{mocap}*T_i^w*T_{marker}^{i})) e=log(Tmocapmarker(TwmocapTiwTmarkeri)) ,优化 T w m o c a p T_{w}^{mocap} Twmocap T m a r k e r i T_{marker}^{i} Tmarkeri 以及样条系数. T m o c a p m a r k e r T^{marker}_{mocap} Tmocapmarker为Mocap系统的测量值。

优化时,需要提供一个timeshift和 T m a r k e r i m u T_{marker}^{imu} Tmarkerimu的优化初始值。

初始化timeshift

T m a r k e r i m u T_{marker}^{imu} Tmarkerimu 可以人为给出一个大致初值,我们在贴小球时,可以尽量使得Marker坐标系与IMU坐标系的某个轴对齐,因此可以大致估计 R i m u m a r k e r R^{marker}_{imu} Rimumarker t i m u m a r k e r t^{marker}_{imu} timumarker也可以通过大致测量小球质心和IMU坐标系中心来给出。

根据提供的初值 T m a r k e r i m u T_{marker}^{imu} Tmarkerimu , 可以将 T m o c a p m a r k e r T^{marker}_{mocap} Tmocapmarker 的计算的旋转角速度经过 R i m u m a r k e r R^{marker}_{imu} Rimumarker 可以转换为IMU的旋转角速度,从而能够得到角速度曲线。与IMU原始测量的角速度曲线进行相关性计算,即可以得到timeshift的初值。如下图,左图为不同timeshift对应的相关性误差,其最小误差就对应timeshift的大致初值,basalt的time_alignment代码中还增加了抛物线拟合,来细化timeshift, 标定过程中可以不进行细化过程。最右图为角速度曲线。
在这里插入图片描述

初始化 T m a r k e r i m u T_{marker}^{imu} Tmarkerimu

得到了timeshift的初始值,就可以得到同一时间下的两个角速度,一个是根据 T m o c a p m a r k e r T^{marker}_{mocap} Tmocapmarker测量值, 经过 R i m u m a r k e r R^{marker}_{imu} Rimumarker 转换得到的角速度,一个是IMU陀螺仪测量的角速度。且两个角速度有如下等式:
w i m u = w m a r k e r ∗ R i m u m a r k e r w_{imu} = w_{marker} * R_{imu}^{marker} wimu=wmarkerRimumarker
多个测量值构建的多个等式,联合求解,即可以得到 R i m u m a r k e r R_{imu}^{marker} Rimumarker.

t i m u m a r k e r t_{imu}^{marker} timumarker 人为给出,或者直接设置为0.

数据后处理

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

采集完原始数据之后,需要进行后处理:

  • 应用 T i m u m a r k e r T_{imu}^{marker} Timumarker,将Marker真值轨迹转为IMU系的真值轨迹。
  • 时间同步

时间同步

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

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

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

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

  • 2
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值