本节主要说明当前主流的VIO算法流程。课程来源于B站。
误差项:$$e(x+\Delta x)=e(x)+J(x)\Delta x$$
一次观测多次观测逆深度 + VIO将视觉约束就加到联合优化是紧耦合
将视觉约束后的位姿加入到联合优化是松耦合
具体流程如下所示:
初始化IMU预测视觉跟踪视觉增广视觉更新—选老点或者看不见的点视觉更新—边缘化剔除老帧误差状态向量$$\hat{X_k}^{(15+6N)\times1} = [\hat{X_{IMU_k}} \delta \theta_{C_1} \ce{G}\hat{p}{C_1} …\ce{^{G}\hat{p}{c_N} \ce{G}\hat{p}_{C_N}}]$$
$$\hat{X}_{IMU}_k^{15\times1}=[\delta\theta_I \hat{b_g}\space\ce{G}\hat{v}_I\hat{b}_a\space \ce{G}\hat{p}_I ]^T$$
每得到一个新图像以后需要对协方差矩阵做一个增广
成熟的路标点 表示窗口内各帧都看得到的点
因子图
Tbc表示IMU与相机间的转换矩阵
路标点边缘化完后就可以了
第j个路标点的所有视觉误差为$$r^{2M\times1}\cong H_x^{2M\times(15+6N)\tilde{X}^{(15+N)\times1}}+H_f^{2M\times3}\ce{G}\hat{p}_{f_j}^{3\times1}+n^{2M\times1}$$
将r投影到Hf的左零空间,想党羽对路标点进行边缘化,将边缘化约束来优化共视帧。$$r_0^(2M-3M_L)\times1=A^Tr^2M\times1\cong A^TH_{x}^{2M\times(15+6N)}\tilde{X}^{(15+6N)\times1}+A^Tn^(2M\times1)$$
复杂 不常用
流程
IMU预测视觉更新IEKF相机模型像素坐标校正光度误差QR分解Paych提取及Warp计算路标点质量评价及维护ROVIO使用光度误差
ROVIO将路标点包含到状态向量中。有因将路标点表征在当前帧的坐标系下,有范围限制,因此对其进行归一化得到无约束的状态量—方向向量Bearing Vector
ROVIO的创新点如下所示:
ROVIO中,路标点使用当前帧下的归一化相机系坐标Pc和你深度表示,并作为状态向量进行预测和更新。参数化不同导致预测和更新的细节不同,但总体还是EKF五大公式。又因Pc有边界约束,因此引入了Bearing Vector老了使得Pc平滑可导
状态向量:当前帧的PVQB和路标点( mu,p)
因子图如下
包括滑动窗口内的n个相机状态PVQB、Camera到IMU的外参、m个3D点的逆深度:
$$X=[x_0,x_1,…x_n,x_c^b,\lambda_0,\lambda_1,…\lambda_m]$$
框表示滑动窗口,T中的R、t表示P、Q ;M中的是v和b;lambda表示路标的逆深度;(外参没有考虑)
Tracklocal map:仅优化当前帧
Local Map:优化华创内的所有帧的PVQB和路标点
增量式BA,误差为IMU和视觉,LBA为滑窗优化,GBA为所有KF优化,速度很快。
ICE-BA认为在建立增量方程时,对之前已经算过的且不变的那些状态向量没必要重新线性化(线性化及计算Jacobian),因为即使重新计算Jacobian也没什么变化。另外,也没必要对整体[H|b]矩阵进行消元,而是只对变化的进行更新。最后只计算变化的路标点即可。对于没有变化的状态向量的临时值则一直保存下来,避免重新计算。
总结而言,在ICE-BA中更新Factor有三种情况:
该Factor在本次迭代中没有变化,则不更新;该Factor是新Factor,则在原来基础上+=新Factor。如新观察到一个新的路标点,则对此新路标点的观测约束就是一个新的Factor;该Factor已存在但需要更新,则先减去旧Factor,再架上新Factor。