vins pose_graph

pose_graph

主函数

  • 初始化
    • ros 初始化 ,posegraph中会有发布
    • 读取参数
    • 如果需要闭环时,加载先前的位姿图,用于闭环用
  • 订阅发布话题
    • imu_propagate、vio_odom、image、keyframe_pose、extrinsic(外参)、关键帧点云、重定位位姿
    • match_image、camera_pose_visual、key_odometrys、no_loop_path、match_points
  • 定义两线程
    • 主线程 process
    • 控制线程 command

主线程 process

  • 若不进行闭环处理,直接返回
  • while true 循环
    • 得到具有相同时间戳的pose_msg、image_msg、point_msg
      • image_buf 为基准,m_buf互斥锁
    • 若 得到的数据为空时,sleep 5ms,然后重复循环
    • 1、跳过前10帧,2、每隔SKIP_CNT帧进行一次 不满足continue
    • 构建 关键帧 T,R,不符合下列条件 continue
    • 将距上一关键帧距离(平移向量的模)超过SKIP_DIS的图像创建为关键帧
      • 遍历 特征点,保存3d位姿+图像观测(uv)+id
      • 构建关键帧 KeyFrame
    • 在posegraph中添加关键帧,回环检。 addKeyFrame

KeyFrame 构建

  • 数据的赋值,图片都clone

  • 计算窗口中所有特征点的描述子 compu teWindowBRIEFPoint

    • BriefExtractor 提取,通过Brief模板文件,对图像的关键点计算Brief描述子
    • 遍历窗口内的所有特征,并为其提取brief描述子 window_brief_descriptors
  • 额外检测新特征点并计算所有特征点的描述子,为了回环检测 computeBRIEFPoint

    • 提取 fast角点, cv::Fast

      • *void cv::FAST	(	
            *	InputArray 	image,						关键点所在的灰度图像。
            *	std::vector< KeyPoint > & 	keypoints,	在图像上检测到关键点
            *	int 	threshold,						中心像素的强度与该像素周围圆的像素之间的差异的阈值
            *	bool 	nonmaxSuppression = true 		是否对检测到的角点(关键点)应用非最大抑制
            *)	 
        
    • 计算所有特征点的描述子 存放到brief_descriptors中,BRIEF::compute

      • 若图片为彩色,则转化为 灰度图
      • 高斯线性滤波 cv::GaussianBlur(aux, im, ksize, sigma, sigma)
        • cv::Size ksize(9, 9) , sigma = 2.f
      • 遍历所有特征,对每个特征计算 brief 偏差,并比较得出 brief描述子
      • 返回所有特征的描述子
    • 将特征点进行去畸矫正 m_camera->liftProjective

  • 若 图片不debug时,将其 image release

addKeyFrame

  • 若当前帧的图像序列 与 位姿图中不符合,更新修正数据
    • w_t_vio、w_r_vio 当前位姿到世界位姿的转换 0
    • t_drift、r_drift 累计漂移
  • 更新当前帧的数据
    • 更新当前帧的位姿,主要是转换到世界坐标系
    • 更新当前帧下表, global_index++
  • 根据回环参数确定是否需回环检测,如果,
    • 需回环检测则检测,返回回环候选帧的索引detectLoop
    • 否则,将当前帧的描述子存入字典数据库 addKeyFrameIntoVoc
      • 直接一个 add 添加
  • 若检测到了闭环,则进行下列操作
    • 得到 回环候选帧 getKeyFrame
      • 基于闭环检测的帧id 获取,无则返回 null
    • 当前帧与回环候选帧进行描述子匹配findConnection
      • 关键帧与回环帧进行BRIEF描述子匹配,剔除失败点
        • searchByBRIEFDes
        • 剔除匹配失败的点 reduceVector
      • 若匹配点大于最小回环匹配点
        • 通过PNP检测去除误匹配点 PnPRANSAC,给上标记
        • 基于标记剔除误匹配的点
      • 若匹配点大于最小回环匹配点
        • 计算PnP后得到的相对关系与原vio之间的差异
        • 如果差异角度小于30°且位置小于20个像素,则证明闭环没有问题
        • 发布匹配后的点
    • 如果描述子匹配成功,则进行相应操作
      • 更最早回环检测帧
      • 得到 老关键帧 和 新当前关键帧的位姿
      • 获取当前帧与回环帧的相对位姿relative_q、relative_t
      • 基于老帧位姿+回环相对位姿,得到当前帧的位姿
      • 回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t
      • 将所有图像序列都合并到世界坐标系下 –这么草率么
        • 基于回环偏移计算 当前帧到世界帧的关系
        • 遍历所有帧,更新帧位姿
      • 将当前帧放入 优化队列中
  • 获取VIO当前帧的位姿P、R,根据偏移量得到实际位姿
  • 路径发布,若需保存闭环轨迹时保存其轨迹
  • 显示中添加 边等等

searchByBRIEFDes

  • brief:将关键帧与回环帧进行BRIEF描述子匹配

  • Param:

    • matched_2d_old 回环帧匹配后的二维坐标
    • matched_2d_old_norm 回环帧匹配后的二维归一化坐标
    • status 匹配状态,成功时为1
    • descriptors_old 回环帧的描述子
    • keypoints_old 回环帧的二维坐标
    • keypoints_old_norm 回环帧的二维归一化坐标
  • 步骤:

    • 遍历滑窗中的描述子,与回环帧的所有描述子匹配searchInAera
      • 遍历回环帧中的所有描述子,得到最佳匹配距离及下标
        • 计算 汉明距离 HammingDis
          • 异或描述子,然后统计其个数
        • 最佳汉明距离140,若小于该距离,则更新其距离和下标
      • 找到汉明距离小于80的最小值和索引即为该特征点的最佳匹配
        • 如果有,返回最佳匹配
        • 否则 返回false
    • 若匹配成功,则status 为1,否则为0

detectLoop

  • 查询字典数据库,得到与每一帧的相似度评分ret
  • 添加当前帧到字典的数据库中
  • 若 相似度评分大于 0.05
    • 若评分大于 0.015,则认为是回环候选帧

回调函数

image_callback

  • 图像数据回调函数,将image_msg放入image_buf,同时根据时间戳检测是否是新的图像序列

具体步骤:

  • 将数据放入image_buf ,数据互斥锁
  • 检测相机数据流
    • 若图片间隔大于1s 或 回调,则是新的图像序列new_sequence
      • 仅支持5个图像序列 ,大于5个时直接break
      • 重新初始化,重新构建地图
    • 更新上一次时间间隔

point_callback

  • 将点云point_msg放入 point_buf中,数据互斥锁

pose_callback

  • 把pose_msg放入pose_buf,数据互斥锁

others

imu_propagate

  • imu前向递推的回调函数,从IMU预积分的位姿得到IMU位姿和cam位姿,得到低延迟和高频率结果
  • imu递推的位姿 转换到世界坐标系后,经图优化参数后,发布

vio_callback

  • VIO回调函数,根据pose_msg中的位姿得到IMU位姿和cam位姿
  • 操作:
    • 转换到世界坐标系,并闭环处理的数据
    • imu坐标系转换到相机坐标系,保存到 odometry_buf

extrinsic_callback

  • 相机外参回调,实时更新 相机到imu的外参 tic+qic
  • 主线程互斥锁

PoseGraph optimize4DoF

  • while 循环,2s进行一次图优化
  • 取优化帧数据
    • 将优化buf的front帧作为当前帧cur_index,并pop_front
    • 最早闭环帧 作为 第一个闭环帧
  • 若当前帧有值,则 cur_index !=-1,否则continue
  • 定义一些数据:
    • 取当前关键帧 cur_kf
    • 因子图中的位姿
    • 求解器等
  • 遍历关键帧list中的 frame,对每个帧做如下操作:
    • 基于 第一个闭环帧 跳过无闭环的帧
    • 赋值帧数据
      • 位姿 t_array,q_array 欧拉角euler_array
      • 图序号 sequence_array
    • 优化器中添加 参数块:
      • 欧拉角和平移量
    • 优化器中固定第一闭环帧位姿
    • 添加当前帧的边
      • 当前帧与前5帧建立约束关系,约束为位姿帧间约束
    • 添加闭环边
      • 如果当前帧有闭环时,基于其闭环约束构建闭环边
  • ceres 求解
  • 更新 优化求解后的数据
    • 因子图中的位姿
    • 根据当前帧的位姿,计算漂移误差
    • 基于漂移误差更新关键帧list的位姿
  • 更新轨迹并发布

文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
上一篇 2022年6月15日 上午11:37
下一篇 2022年6月15日 上午11:39

相关推荐

本站注重文章个人版权,不会主动收集付费或者带有商业版权的文章,如果出现侵权情况只可能是作者后期更改了版权声明,如果出现这种情况请主动联系我们,我们看到会在第一时间删除!本站专注于人工智能高质量优质文章收集,方便各位学者快速找到学习资源,本站收集的文章都会附上文章出处,如果不愿意分享到本平台,我们会第一时间删除!