MSCKF

Published: 2020-01-03 | Lastmod: 2020-03-15

前端跟踪 #

1. 根据IMU给出的旋转量，假设相机发生了纯旋转，估算下一帧特征点的位置；
2. 使用LK光流法，进一步优化特征点的位置；
3. 判断是否为纯旋转。若不是纯旋转，则使用RANSAC方法，随机选2个点，估算出最优的平移量；
4. 将图像分成网格，每个网格最多4个特征点，保证特征点均匀分布。

QR分解 #

\[ A_{2m \times 3} = Q_{2m \times 2m}R_{2m \times 3} = \begin{bmatrix} B_{2m \times 3} &C_{2m \times (2m - 3)} \end{bmatrix} \begin{bmatrix} D_{3 \times 3} \\ 0_{(2m - 3) \times 3} \end{bmatrix} \]

其中，\(Q\) 为正交矩阵，每一列代表一个基向量，与除了自身以外的其他基向量的点积都为0。Where \(B\) and \(C\) are unitary matrices whose columns form bases for the range and nullspace of \(A\).

\[ {C^T}_{(2m-3) * 2m} A_{2m \times 3} = \begin{bmatrix} 0_{(2m-3) \times 3} &I_{(2m-3) \times (2m - 3)} \end{bmatrix} \begin{bmatrix} D_{3 \times 3} \\ 0_{(2m - 3) \times 3} \end{bmatrix} = 0_{(2m-3) \times 3} \]

\(C\)称为\(A\)的左零空间，当左乘\(C^T\)以后，整个矩阵都变为0。

\[ {B^T}_{3 * 2m} A_{2m \times 3} = \begin{bmatrix} I_{3 \times 3} &0_{3 \times (2m - 3)} \end{bmatrix} \begin{bmatrix} D_{3 \times 3} \\ 0_{(2m - 3) \times 3} \end{bmatrix} = D_{3 \times 3} \]

当左乘\(B^T\)以后，仅重要信息被保留下来为\(D\)

状态量 #

当前IMU状态\(15\)维，加上滑窗内\(N\)帧图像，每个相机位姿6维，共计\(15+6N\)

\[ {\hat{X}}_k = \begin{bmatrix} ^I_G\bar{q} \\ {b_g} \\ {^Gv_I} \\ {b_a} \\ {^Gp_I} \\ {^{C_1}_G\hat{\bar{q}}} \\ ^G\hat{p}_{C_1} \\ \vdots \\ {^{C_N}_G\hat{\bar{q}}} \\ ^G\hat{p}_{C_N} \end{bmatrix} \]

状态增广 #

当增加第\(N+1\)个新的相机位姿时，状态向量需要增广。

\[ \begin{bmatrix} ^I_G\bar{q}^T \\ {b_g}^T \\ {^Gv_I}^T \\ {b_a}^T \\ {^Gp_I}^T \\ {^{C_1}_G\hat{\bar{q}}}^T \\ ^G\hat{p}_{C_1}^T \\ \vdots \\ {^{C_N}_G\hat{\bar{q}}}^T \\ ^G\hat{p}_{C_N}^T \\ {^{C_{N+1}}_G\hat{\bar{q}}}^T \\ ^G\hat{p}_{C_{N+1}}^T \end{bmatrix} = \underbrace{\begin{bmatrix} I &0 &0 &0 &0 &0 &0 &\cdots &0 &0\\ 0 &I &0 &0 &0 &0 &0 &\cdots &0 &0\\ 0 &0 &I &0 &0 &0 &0 &\cdots &0 &0\\ 0 &0 &0 &I &0 &0 &0 &\cdots &0 &0\\ 0 &0 &0 &0 &I &0 &0 &\cdots &0 &0\\ 0 &0 &0 &0 &0 &I &0 &\cdots &0 &0\\ 0 &0 &0 &0 &0 &0 &I &\cdots &0 &0\\ & & & &\vdots \\ 0 &0 &0 &0 &0 &0 &0 &\cdots &I &0\\ 0 &0 &0 &0 &0 &0 &0 &\cdots &0 &I\\ C(^C_I\bar{q}) & 0 & 0 & 0 & 0 & 0 & 0 &\cdots &0 &0 \\ \lfloor C_{\hat{q}}^T p_C \times \rfloor &0 &0 & 0 &I &0 &0 &\cdots &0 &0 \end{bmatrix}}_A * \begin{bmatrix} ^I_G\bar{q}^T \\ {b_g}^T \\ {^Gv_I}^T \\ {b_a}^T \\ {^Gp_I}^T \\ {^{C_1}_G\hat{\bar{q}}}^T \\ ^G\hat{p}_{C_1}^T \\ \vdots \\ {^{C_N}_G\hat{\bar{q}}}^T \\ ^G\hat{p}_{C_N}^T \end{bmatrix} \]

对应的协方差矩阵也需要进行传播。 \( P_{k|k} = A P_{k|k} A^T \)

多余的话
关于协方差增广，可以与 slam for dummies 进行对比学习。在该文中，新的特征点与当前的机器人位姿、激光观测量都有关系，其协方差为：

\[ P^{N+1 \ N+1} = J_{xr} P J_{xr}^T + J_z R J_z^T \]

三角化点 #

将相机坐标当作已知值，优化共同观测到的某个特征点坐标

\[ h = \begin{pmatrix} h_0 \\ h_1 \\ h_2 \end{pmatrix} = R \begin{pmatrix} \frac{X}{Z} \\ \frac{Y}{Z} \\ 1 \end{pmatrix} + \frac{1}{Z} t = R \begin{pmatrix} \alpha \\ \beta \\ 1 \end{pmatrix} + \rho t \\ z = \begin{pmatrix} \frac{h_0}{h_2} \\ \frac{h_1}{h_2} \end{pmatrix} \]

由此得到雅可比矩阵：

\[ \begin{aligned} J &= \frac{\partial e_i}{\partial(\alpha, \beta, \rho)} = -\frac{\partial z}{\partial h} \begin{bmatrix} \frac{\partial h}{\partial \alpha} & \frac{\partial h}{\partial \beta} & \frac{\partial h}{\partial \rho} \end{bmatrix} \\ &= -\begin{bmatrix} \frac{1}{h_2} & 0 & -\frac{h_0}{h_2^2} \\ 0 & \frac{1}{h_2} & -\frac{h_1}{h_2^2} \end{bmatrix} \begin{bmatrix} R\begin{pmatrix} 1 \\ 0 \\ 0 \end{pmatrix} & R\begin{pmatrix} 0 \\ 1 \\ 0 \end{pmatrix} & t \end{bmatrix} \\ \end{aligned} \]

观测模型 #

特征点坐标是根据相机坐标计算出来的，而观测量误差同时与特征点和相机的坐标误差相关。

\[ ^{C_i} p_j = \begin{bmatrix} ^{C_i} \hat X_j \\ ^{C_i} \hat Y_j \\ ^{C_i} \hat Z_j \end{bmatrix} =C(^{C_i} _G \hat q)(^G \hat p _{j} - ^G \hat p _{C_i}) \\ \hat z_i^{(j)} = \begin{bmatrix} \frac{^{C_i} \hat X_j}{^{C_i} \hat Z_j} \\ \frac{^{C_i} \hat Y_j}{^{C_i} \hat Z_j} \end{bmatrix} \]

对相机位姿、特征点位置的误差求雅可比：

\[ \begin{aligned} r_i^{(j)} &\approx \frac{\partial z_i^{(j)}}{\partial ^{C_i} p_j} \frac{\partial ^{C_i} p_j}{\partial X_{C_i}} \tilde X + \frac{\partial z_i^{(j)}}{\partial ^{C_i} p_j} \frac{\partial ^{C_i} p_j}{\partial ^Gp_j} {^G}{\tilde{p}_{j}} \\ &= \begin{bmatrix} \frac{1}{^{C_i} \hat Z_j} & 0 & -\frac{^{C_i} \hat X_j}{^{C_i} \hat Z_j} \\ 0 & \frac{1}{^{C_i} \hat Z_j} & -\frac{^{C_i} \hat Y_j}{^{C_i} \hat Z_j} \end{bmatrix} \begin{bmatrix} \lfloor {^{C_i} \hat p _{j}}_\times \rfloor & -C(^{C_i} _G \hat q) \end{bmatrix} \tilde X + \begin{bmatrix} \frac{1}{^{C_i} \hat Z_j} & 0 & -\frac{^{C_i} \hat X_j}{^{C_i} \hat Z_j} \\ 0 & \frac{1}{^{C_i} \hat Z_j} & -\frac{^{C_i} \hat Y_j}{^{C_i} \hat Z_j} \end{bmatrix} \begin{bmatrix} C(^{C_i} _G \hat q) \end{bmatrix} {^G}{\tilde{p}_{j}} \end{aligned} \]

精华：将观测到该特征点的多个相机观测值累积到一起形成大型矩阵，并将观测量误差投影到相机坐标误差雅可比矩阵的左零空间中，消去特征点坐标误差。由此，观测量误差仅与相机坐标误差相关！

\[ \begin{aligned} r^{(j)} &\simeq H_X^{(j)} \tilde{X} + H_f^{(j)} {^G}{\tilde{p}_{f_j}} + n^{(j)} \\ r_o^{(j)} = A^T r^{(j)} &\simeq A^T H_X^{(j)} \tilde{X} + A^Tn^{(j)} = H_o^{(j)} \tilde{X} ^{(j)} + n_o^{(j)} \end{aligned} \]

滤波更新 #

使用QR分解，仅保留观测值中的有效信息。

\[ \begin{aligned} r_o &= \begin{bmatrix} Q_1 &Q_2 \end{bmatrix} \begin{bmatrix} T_H \\ 0 \end{bmatrix} \tilde{X} + n_o \\[2mm] r_n &= Q_1^Tr_o = T_H \tilde{X} + n_n \end{aligned} \]

滑窗的特殊性 #

MSCKF采用的是滤波方法，维护的是协方差，当要边缘化一个变量时，直接将其对应的行和列去掉即可！
这与其他基于图优化的SLAM算法有显著区别。图优化SLAM需要维护H矩阵，即信息矩阵。信息矩阵的某一变量的边际概率，需要进行舒尔补操作！

Next: ORB-SLAM2
Previous: AMCL