＜VINS-MONO＞代码阅读笔记

最新推荐文章于 2022-09-05 20:52:55 发布

BRAND-NEO

最新推荐文章于 2022-09-05 20:52:55 发布

阅读量839

点赞数 1

分类专栏： SLAM 文章标签： slam

本文链接：https://blog.csdn.net/Neo11111/article/details/120271222

版权

SLAM 专栏收录该内容

3 篇文章 0 订阅

订阅专栏

FeatureTracker 视觉前端

视觉回调函数
- 发布频率控制
- 图像消息解码
- 调用readImage函数
- 更新特征点ID
- 按照频率发布特征追踪结果
  - 时间戳
  - 点坐标
  - 点ID
  - 2D图像上角点两个方向上的运动速度
readImage函数 前端2D-2D追踪
- 判断是否进行自适应直方图均衡化
- 当上一帧有特征点时
  - 对上一帧的特征点做光流追踪
  - 光流外点剔除
  - 对光流内点，记录追踪成功次数+1
- 当该帧需要“发布”时（发布频率控制）
  - 对极约束剔除外点
  - 均匀化剔除聚集点
    - setMask 函数实现
    - 按追踪成功次数排序
  - 检查角点数是否不足，若不足则补充检测
    - 利用上一步Mask
    - SHI-Tomasi 角点
    - 加入新检测到的点
  - 计算当前帧的特征点的3D去畸变坐标并记录
  - 计算当前帧特征点坐标对比上一帧的二维速度并记录
  - 前后帧变量交换记录

Estimator IMU数据处理和后端优化

特征回调函数
- 往feature_buf里存来自视觉前段的跟踪结果
IMU回调函数
- 往imu_buf里存储来自IMU的测量数据
后端线程 （process函数）
- 特征&IMU数据抓取与同步（getMeasurements函数）
  - 保证图像时间戳之前有IMU数据且IMU数据完整
  - 把该图像和该图像时间戳之前（+下一帧IMU）的IMU数据记录并返回
- 若未抓取到合适的数据，直接返回进行下一轮
- 开始数据处理
  - IMU数据处理：预积分
    - 解析IMU数据
    - 调用estimator的processIMU函数
      - 传入单帧IMU数据
      - 若对于某图像帧的一段IMU数据来说是第一帧IMU，新建IntegrationBase
      - 调用IntegrationBase的push_back函数，预积分+雅克比递推+协方差递推
        传入IMU测量数据和dt
        存入buff，以备repropagate
        调用propagate和midPointIntegration函数
        预积分
        雅可比递推
        协方差递推
      - 用IMU测量进行PVQ状态递推，作为初值
  - 用relo_msg的队头设置重定位帧 ※
  - 视觉数据处理：
    - 将特征点导入FeatureManager
      - 数据结构：list<FeaturePerID>
        FeaturePerID记录特征点的ID及其起始帧
        另外用成员vector<FeaturePerFrame>记录从起始帧开始的一连串跟踪帧上的特征信息
        FeaturePerFrame记录点的三维坐标/二维坐标/速度/深度等信息
    - 判断是否成为关键帧，关键帧条件如下：
      - 是第一帧
      - 该帧上跟踪成功的点数过少
      - 与上一帧无共视
      - 平均视差大于给定阈值
    - 若相机和IMU外参未标定，对其进行标定
      - 利用两图像帧之间的对极约束结算的旋转R和IMU预积分的R’构造最小二乘方程组，SVD分解求解
      - 滑窗内有2帧及以上时，利用相邻帧视觉解算R和IMU预积分的R’构造方程，向方程组里增加约束，精化标定结果
      - 每次新一帧进入滑窗，向方程增加约束时，利用上一次标定的结果给各帧间约束方程定权，越偏离上次结果权重越小
    - 初始化
      - 滑窗满了后进行(num: WINDOW_SIZE+1)，且需要外参已知或标定完成
      - 调用initialStructure函数进行初始化
        先判断是否有足够的运动激励
        计算线加速度均值和标准差
        
        全局SfM
        2D-2D Epipolar
        一帧是最新帧（frame_r）
        另一帧找和最新帧有充分共视而视差又足够的滑窗中的帧（frame_l）
        对极几何计算该两帧之间的R和T
        
        调用GlobalSFM类的construct函数，传入滑窗内特征/两帧的ID/R/T
        STEP 1：用l和r两帧三角化地图点
        STEP 2：用上述地图点PnP求解第l+1帧-第r-1帧的位姿，注意把上一次求解结果作为初值带入下一次求解
        STEP 3：用l和上一步解得位姿的各帧分别和l帧做三角化，进一步得到更多地图点
        STEP 4：用上述地图点PnP求解第l-1帧-最初帧的位姿，同样注意上一次求解结果作为下一次求解的初值
        STEP 5：三角化其余特征点，用其观测帧中视差最大的两帧
        STEP 6：full BA ※ 并用优化结果更新各帧位姿
        
        对于非关键帧，通过用SfM的地图点PnP求解位姿，用其临近的关键帧的位姿作为初值
        调用visualInitialAlign函数
        调用VisualIMUAlignment函数
        标定陀螺零偏
        * 不止用关键帧，也用到了上一步确定了位姿的所有非关键帧
        * 视觉R和IMU DeltaQ对齐
        * Jacobian : dR/dbg
        * 线性最小二乘问题，直接求伪逆求解
        处理速度、重力、尺度
        a) IMU预积分值
        b) 视觉计算得到的各帧位姿+尺度(未知)+各帧在其b系下的速度(未知)+cam0系下重力(未知) 可以估计预积分
        c) 用a和b构造残差，同样转化为线性最小二乘问题
        重力 Refine
        * 结合当地重力值
        * 取上一步优化得到的重力方向，模设置为当地重力值
        * 将重力参数化为当前重力方向切平面上的基向量坐标
        * 用上一步的约束并在其优化结果基础上进一步精化重力结果
        
        用SfM结果更新滑窗帧位姿
        FeatureManager中各特征点三角化
        用标定的陀螺零偏重新传播预积分
        3D点深度*尺度优化结果
        通过重力标定结果把相机坐标系与理想坐标系对齐，并把航偏角旋转为0
      - 若初始化未成功
        标志位仍保持为INITIAL
        窗口滑动 slideWindow函数
        当最新帧为关键帧
        marge最老帧 ： 帧扔掉，特征深度转移
        从最老帧开始，P/V/Q/Ba/Bg/Header与后一帧依次交换
        最新帧P/V/Q/Ba/Bg/Header赋值为次新帧的值
        删除最新帧预积分，新建IntegrationBase，清空最新帧的预积分测量值buffer
        如果系统正在初始化
        遍历特征点，若其起始帧不是被marge掉的帧，则将其起始帧ID自减1
        若起始帧是被marge掉的最老帧，直接把特征从滑窗feature_manager中丢掉不要
        
        如果系统已完成初始化
        遍历特征点，若其起始帧不是被marge掉的帧，则将其起始帧ID自减1
        对于该特征的观测，删去marge帧，起始帧变为次老帧
        若该特征点观测帧只剩下一个，即次老帧，从feature_manager中删掉这个特征
        若起始帧是被marge掉的最老帧，则将用最老帧上的深度记录计算次老帧上的深度，转移深度估计结果
        
        当最新帧为非关键帧
        marge次新帧，移花接木
        把最新帧的IMU测量数据从buffer中提取，累加到次新帧继续预积分
        用最新帧的Headers/P/V/Q/Ba/Bg设置次新帧的数据
        删除最新帧预积分，新建IntegrationBase，清空最新帧的预积分测量值buffer
        遍历特征点，如果该特征点是从最新帧被跟踪到的，由于次新帧被marge掉，故其start_frame-1
        如果有特征点一直被跟踪到次新帧，需要在其观测中去掉次新帧信息
        如果有特征点从次新帧被跟踪到最新帧，删除它
      - 若初始化成功
        标志位设置为NON_LINEAR
        滑窗优化 solveOdometry
        三角化 f_manager.triangulate
        遍历滑窗中所有特征点
        当某特征点的观测数不足2帧/起始帧是倒数第三帧及之后的帧，跳过，不进行三角化
        若已有深度，跳过
        用所有观测构造超定方程，得到该特征点三角化结果，记录深度
        
        优化 optimization()
        核函数为柯西核函数
        传入优化变量地址和大小
        Pose（11x7）
        V/Ba/Bg（11x9）
        ExPose（1x7）
        Td（1x1）
        
        添加上一次边缘化所引入的先验约束
        MarginalizationFactor实现
        关联部分状态量：上次边缘化操作后为一部分留下的状态量引入了先验约束
        上述先验约束以linearized_residuals和linearized_jacobians的形式被保存在MarginalizationFactor中
        MarginalizationFactor中还保存了上次边缘化后态量的值
        传入新的参数块值后，MarginalizationFactor的Evaluate函数负责根据上两步提到的值和新参数值来计算残差和Jacobian
        
        添加IMU约束
        IMUFactor实现
        关联状态：滑窗相邻两帧的位姿和速度
        用预计分和传入的参数块计算残差和雅克比
        
        添加视觉约束
        关联状态：特征点在起始帧上的逆深度、起始帧和另一帧位姿、外参
        
        处理重定位 ※
        设置求解器参数，求解，用优化结果更新各状态量
        边缘化（最老帧）
        若上次边缘化后保留的块中有与最老帧有关的参数，标注drop，构造残差添加到marginalization_info中
        向marginalization_info中添加与最老帧相连的IMU残差，标注最老帧的PVQ需要被marg
        传入参数地址，传入计算残差和雅克比的factor，下同
        向marginalization_info中添加与最老帧上特征点有关的视觉残差，标注最老帧位姿和逆深度需要被marg
        为各个约束重新计算残差和Jacobian
        构造矩阵块 MM MR RM RR，并记录好各个参数块所在的位置，把要marg掉的参数放左上角
        多线程同步构造Ax=b中的A( $J^TJ$ )矩阵和b( $J^Tf$ )矩阵，利用了 $J^TJ$ 的对称矩阵性质
        构造完成后，求舒尔补
        从求解好的A’和b’中恢复出线性的先验残差和雅克比矩阵，保存在marginalization_info中
        记录边缘化后海瑟矩阵中保留下来的块的信息，留待下次优化使用
        
        边缘化（次新帧）
        如果上次边缘化后保留的块与次新帧无关，跳过
        取上次边缘化后保留的块中与次新帧有关的参数，标注drop，构造残差添加到marginalization_info中
        其余同上
      - 若初始化未成功
        窗口滑动
    - 初始化完成后
      - 滑窗优化
      - 失败检测
        跟踪成功点数太少
        Bias估计值太大
        较大的位移/旋转
      - 窗口滑动
      - 去掉优化失败的特征点

BRAND-NEO

关注

1
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
＜VINS-MONO＞代码阅读笔记

FeatureTracker 视觉前端视觉回调函数发布频率控制图像消息解码调用readImage函数更新特征点ID发布特征追踪结果点坐标点ID2D图像上角点两个方向上的运动速度readImage函数前端2D-2D追踪判断是否进行自适应直方图均衡化当上一帧有特征点时光流追踪光流外点剔除追踪成功次数计数当该帧需要“发布”时（发布频率控制）对极约束剔除外点均匀化剔除聚集点setMask 函数实现按追踪成功次数排序检查角点数是否不足，若不..
复制链接

扫一扫