VINS的初始化

最新推荐文章于 2024-09-11 11:00:10 发布

changshen_xu

最新推荐文章于 2024-09-11 11:00:10 发布

阅读量829

点赞数 1

分类专栏： # ----------VIO原理系列文章标签： VINS

本文链接：https://blog.csdn.net/CSDN_XCS/article/details/99706110

版权

首先，明确在系统初始化阶段可以利用的已知量，以及待求解的未知量。

已知：1.各帧相对于第1帧的位姿 $q_{c_1c_i},p_{c_1c_i}$ ；（根据SfM求得）
2.各相邻帧之间的位姿变化 $q_{b_kb_{k+1}},p_{b_kb_{k+1}}$ ；（根据IMU的预积分求得）

待求解：1.相机和IMU之间的外参 $q_{bc},p_{bc}$ ；
2.传感器的偏置 $b$ ；
3.初始速度 $v$ ，尺度 $s$ ，重力矢量 $g^{c_1}$ ；
4.计算初始时刻IMU系相对于世界坐标系的位姿 $q_{wb_1}$ ，。

1.相机和IMU之间的外参

几何约束：
对于任意相邻两个时刻， $t_{k+1}$ 时刻的相机坐标系在 $t_k$ 时刻的IMU系下的表示，都有两种表示路径（如图蓝线和绿线）即：
$q_{b_kc_{k+1}}=q_{b_kc_{k}}\otimes q_{c_{k}c_{k+1}}=q_{b_kb_{k+1}}\otimes q_{b_{k+1}c_{k+1}}$

【注意】相机和IMU之间的外参 $q_{bc}$ 可以认为是不变化的！有：
$\begin{aligned} q_{bc}\otimes q_{c_{k}c_{k+1}} &= q_{b_kb_{k+1}}\otimes q_{bc} \\ [ q_{c_{k}c_{k+1}}]_R q_{bc} &= [q_{b_kb_{k+1}}]_L q_{bc} \\ ([ q_{c_{k}c_{k+1}}]_R - [q_{b_kb_{k+1}}]_L) q_{bc} &= 0 \\ Q^k_{k+1} q_{bc} &= 0 \\ \end{aligned}$