VINS初始化原理及代码

Nie_Xun

已于 2024-06-22 17:49:55 修改

阅读量611

点赞数 17

分类专栏： SLAM VINS 文章标签：算法

于 2024-05-08 15:32:37 首次发布

本文链接：https://blog.csdn.net/weixin_41469272/article/details/138536126

版权

SLAM 同时被 2 个专栏收录

45 篇文章 37 订阅

订阅专栏

VINS

3 篇文章 0 订阅

订阅专栏

文章目录

预备

VINS整体流程

在这里插入图片描述

符号定义

$\left(\cdot\right)^w$ 表示世界坐标系下的表示，Z轴与重力方向重合。
$\left(\cdot\right)^b$ 表示体坐标系下的表示，与IMU坐标系绑定。
$\left(\cdot\right)^c$ 表示相机坐标系下的表示。
$q_b^w$ 和 $p_b^w$ 分别表示imu 体坐标系下转换到世界坐标系下对应的旋转和平移。
$q_c^b$ 和 $p_c^b$ 分别表示相机坐标系下转换到体坐标系下对应的旋转和平移。
$\left(\cdot\right)^{b_k}$ 表示第k帧图像对应的体坐标系。
$\left(\cdot\right)^{c_k}$ 表示第k帧图像对应的体坐标系。
$\left(\hat{\cdot}\right)$ 表示带误差的观测值/估计值(带噪声)。
$\bar{p}.$ 表示不带尺度的视觉位移估计量。

代码中常用的容器

feature_buf存放feature_tracker_node.cpp发布的特征点信息
imu_buf存放imu_msg

measurements 中配套存放新图像帧特征点信息及其时间戳对应的IMU预积分数据。

IntegrationBase预积分结构体：
result_delta_q/p/v指中值预积分；
jacobian存放预积分变量的雅可比
covariance协方差矩阵。

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image：帧特征点属性管理map
每个特征点属性包括：(camera_id,[x,y,z,u,v,vx,vy])，索引为feature_id
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);

sfm_f统计各特征点在各帧中的观测。

Matrix3d ric[NUM_OF_CAM];相机到imu体坐标系之间的旋转关系
Vector3d tic[NUM_OF_CAM];相机到imu体坐标系之间的位置关系
Vector3d Ps[(WINDOW_SIZE + 1)]; 保存滑窗内每帧的位置(以首帧为原点坐标系，imu体坐标系表征位姿)
Vector3d Vs[(WINDOW_SIZE + 1)];保存滑窗内每帧的速度(以首帧为原点坐标系，imu体坐标系表征位姿)
Matrix3d Rs[(WINDOW_SIZE + 1)];保存滑窗内每帧的姿态(以首帧为原点坐标系，imu体坐标系表征位姿)
Vector3d Bas[(WINDOW_SIZE + 1)];滑窗内每帧加速度零偏
Vector3d Bgs[(WINDOW_SIZE + 1)];滑窗内每帧陀螺仪零偏

all_image_frame存放所有图像帧数据(包含了特征点及IMU积分数据)。
通过all_image_frame管理所有帧，通过Ps，Rs等管理滑窗关键帧。
frame_count滑窗内的帧个数。

pre_integrations 存放预积分量，与图像关键帧对应。

Estimator 为VIO结构体，包含各类接口及变量。

FeatureManager f_manager存放管理特征点信息及各类特征点相关接口。

FeaturePerId单个特征点的信息包含了vector<feature_per_frame>即观测到该点的所有帧的观测坐标。
FeatureManager::feature存放所有特征点在被观测帧下的观测坐标i，即list<FeaturePerId>。如下图所示：

在这里插入图片描述
图来自参考文献《VINS 论文推导及代码解析崔华坤》

初始化原理及步骤

初始化目的

由于单目视觉的尺度信息缺失，采用IMU的位姿积分数据与通过视觉获得的位姿数据及逆行对比，从而估计出尺度信息，从而进一步得到所有特征点的坐标。
然而，完成IMU数据与相机数据的结合，需要知道相机坐标系与IMU坐标系之间的关系( $q_c^b$ 和 $p_c^b$ )，而此可以通过标定数据或者带IMU相机的工厂参数获得。
通常，相机与IMU之间的角度差异带来的影响较大，而其较小的位置差异对整个里程计的位置估计影响较小，因而，其位置差异 $p_c^b)$ 可以被忽略。

此外，IMU数据存在零偏(零偏变化)，加速度计中包含重力加速度。

初始化原理

初始化原理核心：
通过将相机视觉得到的角度信息与IMU角度信息对比，可以估计陀螺仪零偏，以及在线估计IMU与相机之间的角度差异。

通过将相机视觉得到的位置信息于IMU加速度积分的位置信息进行对比，从而估计出尺度信息，以及修正重力加速度。

初始化步骤

 预处理：
（1）特征点提取。IMU预积分(基于IMU的时间戳)
（2） IMU与图像时间戳对齐与关联

 关联：
（3） (若无)则估计外参

 VINS初始化:
（4） (视觉无尺度)SFM求解。
（5）视觉惯性对齐
陀螺仪零偏估计；
尺度，重力加速度，速度计算

初始化详细流程、原理及代码。

 预处理：
（1）特征点提取；IMU预积分。
光流法提取跟踪特征点。IMU预积分(基于IMU的时间戳)，二者分别运行于ROS两个node（feature_tracker_node.cpp和estimator_node.cpp）

光流法提取跟踪特征点：feature_tracker_node.cpp
核心函数：img_callback()：readImage()：
createCLAHE->apply(): 首先对(太亮或太暗EQUALIZE=1)图像进行灰度直方图均衡化处理
calcOpticalFlowPyrLK(): 使用LK光流法跟踪特征点
outliers rejectWithF(): 通过基本矩阵剔除;
setMask(): 设置mask保证提取特征点的均匀分布;
goodFeaturesToTrack(): 查看是否需要提取特征点，并提取；
addPoints(): 将goodFeaturesToTrack检测到的新特征点n_pts添加到forw_pts, 其id先设为-1，而后使用updateID进行id更新(新特征点在上一个基础上++)
undistortedPoints(): 对特征点坐标进行去畸变矫正，转换到归一化坐标系上，并计算每个角点的速度。
IMU预积分：estimator_node.cpp和factor/integration_base.h
imu_callback()：调用predict()发布高频PVQ，与IMU数据频率一致；
核心函数：predict()通过中值积分计算当前时刻的PVQ,
${\bar {\hat a}_i}^\prime = \frac{1}{2}[{\hat q_i}({\hat a_i} - {\hat b_{{a_i}}}) + {\hat q_{i + 1}}({\hat a_{i + 1}} - {\hat b_{{a_i}}})]$ ${\bar {\hat \omega}_i}^\prime = \frac{1}{2}({\hat \omega _i} + {\hat \omega _{i + 1}}) - {\hat b_\omega }$

$\begin{array}{} {\hat R_w^{{b_k}}\hat p_{{b_{k + 1}}}^w = \hat R_w^{{b_k}}\left( {\hat p_{{b_k}}^w + \hat v_{{b_k}}^w{\rm{\Delta }}{t_k} - \frac{1}{2}{g^w}{\rm{\Delta }}t_k^2} \right) + \hat \alpha _{{b_{k + 1}}}^{{b_k}}}\\ {\hat R_w^{{b_k}}\hat v_{{b_{k + 1}}}^w = \hat R_w^{{b_k}}(\hat v_{{b_k}}^w - {g^w}{\rm{\Delta }}{t_k}) + \hat \beta _{{b_{k + 1}}}^{{b_k}}}\\ {\hat q_w^{{b_k}} \otimes \hat q_{{b_{k + 1}}}^w = \hat \gamma _{{b_{k + 1}}}^{{b_k}}} \end{array}$
详细参见https://blog.csdn.net/weixin_41469272/article/details/138505746

步骤（2）及以后都对应estimator_node.cpp的代码，核心函数process()。
imu_callback()和feature_callback()分别打包imu预积分数据及特征点数据。并通过锁机制触发process()中的操作。

estimator_node.cpp: imu_callback()/feature_callback()/process()

process()：VIO线程主函数
|–getMeasurements()： 搜集image及其时间戳附近的imu数据，打包存放到measurement。

|–estimator.processIMU()：线性插值IMU数据，使之对齐图像时间戳。
|–new IntegrationBase
|–IntegrationBase->push_back(): propagate(): midPointIntegration() 中值积分，得到PVQ/协方差/雅可比

|–estimator.setReloFrame()： 重定位函数。

|–estimator.processImage(): 关键帧判定；外参标定；视觉惯性对齐；初始化等
|–f_manager.addFeatureCheckParallax(): 判断是否为关键帧，选择marg帧。
|–f_manager.getCorresponding(): 得到两帧的共视特征点。
|–CalibrationExRotation()：标定相机IMU外参。
|–solveRelativeR(): 通过计算F阵解算R。

|–initialStructure(): 初始化/初始化失败执行滑窗。
|–relativePose(): 寻找滑窗中的枢纽帧。
|–solveRelativeRT(): 调用cv的findFundamentalMat和recoverPose计算枢纽帧和最新帧之间的 $T$ 。
|–sfm.construct()：求解滑窗口中的所有图像帧的位姿和特征点坐标。
|–solveFrameByPnPPnP求解帧间位姿。
|–triangulateTwoFrames()/triangulatePoint(): 三角化特征点。
|–ceres::Problem...: 滑动优化关键帧位姿及地图点坐标。
|–visualInitialAlign()初始化核心函数，通过联合视觉IMU数据，求解尺度，重力，等等。
|–VisualIMUAlignment: 计算陀螺仪偏置，尺度，重力加速度和速度。
|–solveGyroscopeBias(): 陀螺仪零偏估计。
|–LinearAlignment(): 尺度，重力加速度，速度计算。
|–RefineGravity:固定重力幅值，进一步求解重力加速度及尺度等。

|–slideWindow(): 滑窗计算。

|–update(): 获取滑动窗口当前图像帧的imu更新项[ $P,Q,V,b_a,b_\omega,a,g$ ]; 更新imu_buf中的PVQ

|process() start-------------------------------------------------------------:

（2）IMU与图像时间戳对齐与关联

打包特征点数据及其前后发布的IMU积分数据
核心函数getMeasurements()搜集image及其时间戳附近的imu数据，可出现以下两种情况:

对IMU数据进行线性插值，对齐图像帧。
将新图像帧特征点信息及其时间戳对应的IMU预积分数据配套存放在measurements 中
measurements 中配套存放新图像帧特征点信息及其时间戳对应的IMU预积分数据。
IMU积分对齐图像时间戳(时间插值)
核心函数estimator.processIMU() 计算imu数据与图像数据之间的时间差，利用中值积分及插值估算与图像帧时间戳对应的IMU积分值。
processIMU()：new IntegrationBase对象，并通过IntegrationBase->push_back->propagate->midPointIntegration()完成与图像帧对应的中值预积分(result_delta_q/p/v)，以及预积分的雅可比(jacobian)及协方差矩阵(covariance)。
创建特征点特征管理器（map）
每个特征点属性包括：(camera_id,[x,y,z,u,v,vx,vy])s，索引为feature_id

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);

以下功能均在processImage()的函数内容：关键帧判定；外参标定；初始化；VIO等。processImage()是视觉核心函数，处理特征点信息，完成初始化，滑窗操作。
输入为当前帧特征点特征(7维) map, 代码中命名为image，以及msg header，用于输入本帧的时间戳。

all_image_frame存放所有图像帧数据(包含了特征点及IMU积分数据)
f_manager中存放管理特征点信息，包含各类特征点相关接口

|processImage() start-------------------------------------------------------------:

先判断关键帧-》再判断是否进行外参在线计算：-》再判断是否需要初始化
f_manager.addFeatureCheckParallax()检测视差并选择添加关键帧：为新特征点创建FeaturePerId结构并添加其至feature，计算各特征点的视差，若视差大，则认为该帧为关键帧

再判断是否进行外参在线计算：

如果需要，则每帧图像来之后都需要进行以下操作，直到（frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25（即信息量足够大）
getCorresponding 得到最新帧与次新帧之间的共视关键点（即同一个关键点在该两帧下的图像坐标）
CalibrationExRotation(): frame_count++，通过F矩阵解算该两帧之间的R，并使用SVD方法进行外参解算
这两步在每帧来时均会被执行，在外参标定好后，则不再执行之后frame_count永远= WINDOW_SIZE

通过getCorresponding()获取最新两帧的共视点，输送给CalibrationExRotation()，再结合之前的帧共同估算相机与IMU外参。

 关联：

若需要估计外参，则估计外参 $q_c^b$ ; 首先通过f_manager.getCorresponding()得到两帧之间的共视点。
执行核心函数CalibrationExRotation()：构建 $A x = 0$ 问题，使用SVD方法进行求解。

原理：联合视觉帧间角度估计(solveRelativeR()通过F矩阵计算得到)与IMU角度积分值(对应图像帧)得到。
$q_{b_{k+1}}^{b_k}\otimes q_c^b=q_c^b\otimes q_{c_{k+1}}^{c_k}$
上式可写为
$\left(\left[q_{b_{k+1}}^{b_k}\right]_L-\left[q_{c_{k+1}}^{c_k}\right]_R\right)q_c^b=Q_{k+1}^k\cdot q_c^b=0$
从而滑窗中的多组帧间约束，并利用鲁棒核函数构建问题：
$\left[ {\begin{array}{} {w_1^0 \cdot Q_1^0}\\ {w_2^1 \cdot Q_2^1}\\ \vdots \\ {w_N^{N - 1} \cdot Q_N^{N - 1}} \end{array}} \right]q_c^b = {Q_N} \cdot q_c^b = 0$
其中鲁棒核函数权重：
$w_{k + 1}^k = \left\{ {\begin{array}{} {1,}&{r_{k + 1}^k < \delta }\\ {\frac{\delta }{{r_{k + 1}^k}},}&{otherwise} \end{array}} \right.$
从而以上为 $A X = 0$ 的经典问题，可使用SVD方法进行求解，原理参考：https://blog.csdn.net/weixin_41469272/article/details/123696963

|initialStructure() start---------------------------------------------------------:
 VINS初始化initialStructure()

判断是否初始化完成:

若需要初始化，则进行初始化操作initialStructure()
初始化成功后直接进行一次滑窗优化。
若不需要初始化，则进行滑窗。

判断是否满足启动条件，找到枢纽帧，对滑窗中的相机位姿进行PNP求解，共视点进行三角化()
（1）SFM求解

通过计算加速度的标准差，判断是否存在足够的偏移运动，从而使得三角化更加可信；
统计各特征点在各帧中的观测，存放在sfm_f中。
寻找滑窗中的最后一帧(最新帧)有足够视差的且足够多共视点的帧作为枢纽帧，与最后一帧完成三角化及PnP。核心函数relativePose():
五角星指选出的枢纽帧，遍历滑窗中的帧，计算其与滑窗最后一帧之间的视差和 $R, t$ ，当视差足够，且计算的 $R, t$ 对应的内点数足够时(solveRelativeRT=true)时，则认定该帧为枢纽帧。
relativePose():solveRelativeRT():cv::findFundamentalMat/cv::recoverPose来得到枢纽帧与最新帧之间的变换关系 $R, t$ 。以枢纽帧为参考帧（零点坐标系），下一步也时计算滑窗中各帧相对于枢纽帧的变换关系。
通过上一步得到的特征点进一步使用SFM计算出滑窗中所有帧的位姿及特征点。
核心函数sfm.construct()：|–解滑窗口中的所有图像帧的位姿和特征点坐标。

首先设立枢纽帧的位姿为单位阵(以枢纽帧为原点进行三角化及PNP)，叠加relativePose()的结果得到滑窗最后一帧的位姿；

遍历滑窗内的帧(先从枢纽帧向后遍历，再向前遍历)，分别三角化其与滑窗中最后一帧的共视特征点(triangulateTwoFrames())，及PNP求解该帧位姿(solveFrameByPnP())。同时也三角化一下滑窗中相邻帧的之间的共视特征点(triangulatePoint())。
solveFrameByPnP调用cv::solvePnP函数实现；
triangulateTwoFrames()及triangulatePoint()采用构造 $A X = 0$ ，而后使用SVD方法实现三角化，原理见https://blog.csdn.net/weixin_41469272/article/details/123696963

进行滑动窗口优化(不带尺度)，对地图点及滑窗内各相机的位姿进行优化计算。尺度任意设置，并设置滑窗内第一帧为本次滑窗优化的参考帧。

之后取出滑窗中的位姿，并基于为非滑窗帧设立初始值，根据滑窗中得到的3d点PNP得到非关键帧的位姿(最后转换到imu体坐标系下)。

ceres::CostFunction* cost_function = ReprojectionError3D::Create( sfm_f[i].observation[j].second.x(), sfm_f[i].observation[j].second.y());
// 约束了这一帧位姿和3d地图点
problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l], sfm_f[i].position);

sfm.construct() -------------------------------------------------------------------end|

遍历所有帧(all_image_frame包括非滑窗帧)，调用cv::solvePnP函数实现得到各帧的位姿。

（2）视觉惯性对齐visualInitialAlign()
核心函数VisualIMUAlignment()
|VisualIMUAlignment start---------------------------------------------------------：

陀螺仪零偏估计solveGyroscopeBias()
联合视觉帧间角度估计(上文SFM中得到)与IMU角度积分值(对应图像帧)得到。
约束公式如下：

$\gamma _{{b_{k + 1}}}^{{b_k}} \approx \hat \gamma _{{b_{k + 1}}}^{{b_k}} \otimes \left[ {\begin{array}{} 1\\ {\frac{1}{2}{\bf{J}}_{{b_\omega }}^\gamma \delta {{\bf{b}}_\omega }} \end{array}} \right]$
其中:
${q_{b_{k+1}}^{c_0}}^{-1}\otimes q_{b_k}^{c_0}\otimes\gamma_{b_{k+1}}^{b_k}={q_{b_{k+1}}^{c_0}}^{-1}\otimes q_{b_k}^{c_0}\otimes\hat \gamma _{{b_{k + 1}}}^{{b_k}} \otimes \left[ {\begin{array}{} 1\\ {\frac{1}{2}{\bf{J}}_{{b_\omega }}^\gamma \delta {{\bf{b}}_\omega }} \end{array}} \right]=\left[\begin{matrix}1\\0\\\end{matrix}\right]$
$\left[ {\begin{array}{} 1\\ {\frac{1}{2}{\bf{J}}_{{b_\omega }}^\gamma \delta {{\bf{b}}_\omega }} \end{array}} \right]={{\hat{\gamma}}_{b_{k+1}}^{b_k}}^{-1}\otimes{q_{b_k}^{c_0}}^{-1}\otimes q_{b_{k+1}}^{c_0}={{\hat{\gamma}}_{b_{k+1}}^{b_k}}^{-1}\otimes q_{b_{k+1}}^{b_k}$
只考虑虚部，从而得到AX=b的问题。

其中 $J_{b_w}^\gamma为\gamma_{b_{k+1}}^{b_k}$ 对 $\delta b_w$ 的雅可比矩阵，作者从IntegrationBase对象的jacobian中截取。
尺度，重力加速度，速度计算LinearAlignment()
通过对比加速度积分值与相机SFM得到的偏移，计算得到尺度，重力加速度，以及速度信息
待求解变量：

约束公式如下：

通过整理上式，将待求解量提取出来，可将上式化为以下形式：

其中：

从而可构成最小二乘问题：
如此问题变为 $A x = b$ 的问题，使用x = A.ldlt().solve(b)进行求解。

RefineGravity()通过固定重力加速度幅值，从而将重力加速度变为2维参数，再次对重力加速度进行优化。

图中： $\bar{\hat{g}}$ 表示经过上式得到的重力加速度的单位方向向量， $\left|g\right|$ 为指定的重力加速度的幅值，待求解 $w_1,w_2$ 。
此时，待优化变量变为：

同时：

同样的方法进行求解 $A x = b$ 问题。
VisualIMUAlignment--------------------------------------------------end|
得到重力加速度及尺度等信息后，重新三角化及积分
f_manager.triangulate()
pre_integrations[i]->repropagate()
使用尺度信息恢复视觉偏移及特征点深度；
使世界坐标系z轴对齐重力加速度方向~~，并将特征点转换到世界坐标系下~~ 。

…
initialStructure--------------------------------------------------end|
初始化成功后，直接执行一次滑窗优化及边缘化。
…
processImage--------------------------------------------------end|
…
process--------------------------------------------------end|

参考文献

《VINS 论文推导及代码解析崔华坤》
《VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator》
《Quaternion kinematics for the error-state Kalman filter》

Nie_Xun

关注

17
点赞
踩
8

收藏

觉得还不错? 一键收藏
0
评论
VINS初始化原理及代码

⋅w⋅w表示世界坐标系下的表示，Z轴与重力方向重合。⋅b⋅b表示体坐标系下的表示，与IMU坐标系绑定。⋅c⋅c表示相机坐标系下的表示。qbwq_b^wqbw和pbwp_b^wpbw分别表示imu 体坐标系下转换到世界坐标系下对应的旋转和平移。qcbq_c^bqcb和pcbp_c^bpcb分别表示相机坐标系下转换到体坐标系下对应的旋转和平移。⋅bk⋅bk表示第k帧图像对应的体坐标系。⋅ck⋅ck。
复制链接

扫一扫

专栏目录