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的位姿
• 更新轨迹并发布