VINS-Mono之外参标定和视觉IMU联合初始化

1.前言

本博客主要介绍VINS-Mono初始化时相机与IMU对齐，主要包括相机到IMU的外参估计、陀螺仪偏置、相机位移和估计空间点的尺度、重力加速度、每帧速度，主要参考了深蓝学院的VIO课程及博客VINS-Mono理论学习——视觉惯性联合初始化与外参标定和崔华坤的《VINS 论文推导及代码解析》。
总体而言，视觉IMU对齐流程主要包括以下几部分：

1. 若旋转外参数$q_c^b$未知，则先估计旋转外参数
2. 利用旋转约束估计陀螺仪偏置：$$q_{b_k}^{c_0}=q_{c_k}^{c_0}\otimes (q_c^b{)}^{-1}$$
3. 利用平移约束估计重力方向，速度，以及尺度初始值$$s{\bar{p}}_{b_k}^{c_0}=s{\bar{p}}_{c_k}^{c_0}-R_{b_k}^{c_0}{p}_c^b$$
4. 对重力向量$g^{c_0}$进行进一步的优化
5. 求解世界坐标系$w$和初始相机坐标系$c_0$之间的旋转矩阵$q_{c_0}^w$，并将轨迹对齐到世界坐标系中

2. 外参标定 (利用旋转约束估计外参数旋转$q_c^b)$

相邻俩时刻$k$,$k+1$之间，$k$时刻的IMU到$k+1$时刻的相机外参旋转矩阵有：$$R_{c_{k+1}}^{b_k}=R_{b_{k+1}}^{b_k}\cdot R_c^b=R_c^b\ast R_{c_{k+1}}^{c_k}$$ 转换为四元素有： $$q_{b_{k+1}}^{b_k}\otimes q_c^b=q_c^b\otimes q_{c_{k+1}}^{c_k}$$ 上式可写为：$$(\mathcal{L}(q_{b_{k+1}}^{b_k})-\mathcal{R}(q_{c_{k+1}}^{c_k}))q_c^b=Q_{k+1}^k\cdot q_c^b=0$$
将多个时刻的上式方程累计起来，并加上鲁棒核权重，可构建超定方程：$$\left[\begin{array}{c}{w}_1^0\cdot Q_1^0\\ w_2^1\cdot Q_2^1\\ \cdots \\ w_N^{N-1}\cdot Q_N^{N-1}\end{array}\right]q_c^b=Q_N\cdot q_c^b=0\text{\hspace{1em}(1)}$$ 其中：
$$w_{k+1}^k=\{\begin{array}{c}1,r_{k+1}^k<threshold\\ \frac{threshold}{r_{k+1}^k},otherwise\end{array}$$ 由旋转和轴角之间的关系$tr(R)=1+2\cos \theta$，能得到角度误差$r$的计算为：$$\begin{gathered} r_{k+1}^k=acos((tr((R_{b_{k+1}}^{b_k}{\hat{R}}_c^b{)}^{-1}{\hat{R}}_c^b{R}_{c_{k+1}}^{c_k})-1)/2) \\ =acos((tr({\hat{R}}_c^{b-1}{R}_{b_{k+1}}^{b_k-1}\widehat{R_c^b}{R}_{c_{k+1}}^{c_k})-1)/2) \end{gathered}$$
公式(1)的求解同样采用SVD分解，即最下奇异值对应的奇异向量即为IMU坐标系到相机坐标系的旋转(四元素表示)，具体代码见initial_ex_rotation.cpp 函数 CalibrationExRotation()

---

对于$tr(R)=1+2\cos \theta$的推导如下：
旋转矩阵和轴角的关系为：$$R=\cos \theta I+(1-\cos \theta )a{a}^T-\sin \theta [a{]}_{\times }$$ 其中$a=[a_1,a_2,a_3{]}^T$为单位旋转轴，$\theta$为绕着旋转轴转动的角度，$[a{]}_{\times }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$
同时，由于$a$为单位旋转轴，有$tr(a{a}^T)=1$，故：
$$\begin{gathered} tr(R)=\cos \theta tr(I)+(1-\cos \theta )tr(a{a}^T)+\sin \theta tr([a{]}_{\times }) \\ =3\cos \theta +(1-\cos \theta )=1+2\cos \theta \end{gathered}$$ 同时也有$Ra=a$，即旋转轴上在旋转后并不发生变化，旋转轴$a$为旋转矩阵$R$的特征值为1对应的特征向量。
关于旋转矩阵和轴角的关系可参考：https://en.wikipedia.org/wiki/Rotation_matrix

---

3. 视觉IMU联合初始化

假如我们已经有一个初始的IMU到相机的外参$T_c^b=[R_c^b|p_c^b]$，同时又有不同时刻帧的位姿$T_{c_k}^{c_0}=[R_{c_k}^{c_0}|p_{c_k}^{c_0}]$ (基于第一帧坐标系下)，那么我们可以知道姿态从相机坐标系到IMU坐标系的关系$T_{bk}^{c_0}=[R_{b_k}^{c_0}|p_{b_k}^{c_0}]$与前俩者的关系：$$T_{c_k}^{c_0}=T_{b_k}^{c_0}{T}_c^b$$ 则有：$$\left[\begin{array}{cc}{R}_{c_k}^{c_0}& s{\bar{p}}_{c_k}^{c_0}\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_{b_k}^{c_0}& s{\bar{p}}_{b_k}^{c_0}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_c^b& p_c^b\\ 0& 1\end{array}\right]$$ 将式子展开有【论文公式(14)】：$$R_{c_k}^{c_0}=R_{b_k}^{c_0}{R}_c^b\to R_{b_k}^{c_0}=R_{c_k}^{c_0}(R_c^b)$$