vins estimator ProcessImage

xiaoma_bk

已于 2022-06-14 18:11:50 修改

阅读量220

点赞数

分类专栏： # vins 文章标签：计算机视觉自动驾驶人工智能

于 2022-06-06 13:50:03 首次发布

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/125145248

版权

文章目录

ProcessImage

ProcessImage

brief：处理图像特征数据
description：
- addFeatureCheckParallax 添加特征到feature中，并计算出跟踪的次数及视差，评判出是否为关键帧
- 评判是否有外参，无外参进行旋转标定
- 进行视觉惯性联合初始化或基于滑动窗口非线性优化的紧耦合VIO
- 返回：bool true：次新帧是关键帧;false：非关键帧
Param：
- image 某帧所有特征点构成的mapcamera_id,[x,y,z,u,v,vx,vy]，索引为feature_id
- header某帧图像的头信息，frame_id + time
流程：
- 添加特征，并评判是否为关键帧，并设置标记addFeatureCheckParallax
  - 添加之前检测到的特征点到feature容器中，计算每一个点跟踪的次数，以及它的视差，
  - 通过检测两帧之间的视差决定次新帧是否作为关键帧
  - 若为关键帧，边缘化标记为老帧否则为新帧
- 图像帧加入滑窗中
  - 构造ImageFrame image+time+临时预积分值
  - 将其添加到所有图像帧中 all_image_frame
  - 更新临时预积分器
- 若相机到IMU外参未进行标定，则标定其外参，并更新标记
  - 得到当前帧和前一帧归一化的特征点 corres，一一对应
  - 进行标定：CalibrationExRotation
    - 两帧间 imu预积分值即临时预积分所得
    - 两帧间特征点对应关系从特征点管理得到
  - 若标定成功，则更新标定数据和标记
- 若系统状态solver_flag == 未进行初始化
  - 将滑窗内帧数缓存到与窗口尺寸值
    - 此处操作只有：frame_count++
    - 但 Imu预积分帧率高，检测到frame_count变化时，采用中值积更新窗口数据
  - 有外参且当前帧时间戳大于初始化时间戳0.1秒，就进行初始化操作
    - 视觉惯导联合初始化initialStructure
    - 更新初始化时间戳
  - 若初始化成功，则进行：
    - 系统状态变为非线性优化
    - 进行滑窗优化，并移除跟踪错误的特征
      - solveOdometry
      - slideWindow
    - 更新 last_R,last_P,last_R0,last_p0
  - 若初始化失败，则直接滑动窗口slideWindow
- 若系统状态非初始化，则紧耦合的非线性优化
  - solveOdometry
  - 故障检测与恢复,一旦检测到故障，系统将切换回初始化阶段
    - 清除状态，并重置参数
  - slideWindow
  - 并移除跟踪错误的特征点
  - 更新 last_R,last_P,last_R0,last_p0

initialStructure

brief：视觉结构的初始化
description：确保IMU有充分运动激励
- relativePose()找到具有足够视差的两帧,由F矩阵恢复R、t作为初始值
- sfm.construct() 全局纯视觉SFM 恢复滑动窗口帧的位姿
- visualInitialAlign()视觉惯性联合初始化
流程：
- 通过加速度标准差判断IMU是否有充分运动以初始化
  - 计算帧间平均加速度的值
    - 遍历所有的图像帧，计算帧间预积分的总的加速度
      - 取帧间的总时间 sum_dt，取帧间的速度变化 delta_v
      - 得出此帧间加速度，并叠加得到总的加速度
    - 总的加速度/帧个数得到平均加速度
  - 计算帧间平均加速度的标准差
    - 遍历所有图像帧，得到所有帧间总的协方差
      - 计算每帧间加速度，跟上一样
      - 得到此帧间协方差： ${(tmp\_g-aver\_g).transpose * (tmp\_g-aver\_g)}$
      - 相加得到所有帧间总的协方差
    - 总的协方差 /帧个数然后开根号得到标准差
  - 若加速度标准差小于0.25时，imu运动不充分，返回false
- 将f_manager中的所有feature保存到存有SFMFeature对象的sfm_f中
  - 定义每帧的Pose：旋转(Q[frame_size+1])+平移(T(frame_size+1))
  - 遍历滑窗中的特征，将特征信息存入 vector<SFMFeature>中
    - 定义SFMFeature特征，并将每帧(frame)观察到该特征数据push进去
- 保证具有足够的视差,由F矩阵恢复 $R, t$ relativePose
  参考帧 f 到当前帧(最后一帧) 的转换关系
  - 第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，会作为参考帧到下面的全局sfm使用
  - 此处的relative_R，relative_T为当前帧到参考帧（第l帧）的坐标系变换Rt
  - 若求取失败，
    - 边缘化最老的一帧
    - 则返回false，即初始化失败
- 对窗口中每个图像帧求解sfm问题 GlobalSFM sfm.construct
  先利用帧l到当前帧三角话出对应特征点的深度，然后Pnp求解 l+1帧，再三角化其特征深度
  - 得到所有图像帧相对于参考帧的姿态四元数Q、平移向量T和特征点坐标sfm_tracked_points
- 对all_image_frame图像帧赋值初值。若在滑窗内，直接赋初值，若不在滑窗内，提供初始的R,t估计，然后solvePnP进行优化,得到每一帧的姿态
  - 遍历all_image_frame，若已在滑窗内，则赋值 Rt，continue
  - 若不在滑窗内：
    - 即：frame_it 时间大于 headers[i]时，i++
    - 取R/T_inital = Q/T[i].inverse
    - 利用德里格斯公式将旋转矩阵转换成旋转向量
    - 在frame_it跟踪点中找到 pts_3和pts_2
      - 遍历特征点，并将其存起来
    - 保证特征点大于5，否则返回false，即初始化失败
    - 调用cv::solvePnP 得到旋转
    - 赋值当前帧 R,t
- 进行视觉惯性联合初始化 visualInitialAlign
- 若联合