OKVIS 笔记

版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/fuxingyin/article/details/53368649

OKVIS 属于 VIO(Visual Inertial Odometry),视觉融合 IMU 做 odometry。

系统配置

视觉和 IMU 关系图如下:

待估计的状态量包括机器人在图像 k 获取时刻的 state xkR 和特征点的坐标 xL .

其中 xR 包括:
机器人在世界系下的位置坐标 WrS
旋转矩阵(用四元数表示) qWS
sensor 系(IMU 系)下机器人的速度 SvWS(简写为 Sv
陀螺仪的偏置 bg,加速度的偏置 ba

特征用齐次坐标表示:xLj:=Wlj

相机相对于 IMU 的外参:xCi:=[SrTCi qTSCi]T

相机相对于 IMU 的外参可以在线调整(系统受温度影响机械结构可能产生形变),如果标定的足够准,并且系统运行时参数保持不变,也可以将参数在优化时,设置成常量。

图像 reprojection error 和 IMU error 误差一起优化

OKVIS 是紧耦合的,视觉和 IMU 的误差项和状态量在一起优化。

下图左边表示纯视觉 SLAM 示意图,右边表示加上 IMU 后 SLAM 的示意图。IMU 对相邻两帧的 pose 之间添加约束,而且对每一帧添加了状态量:陀螺仪和加速度计的 bias 和 speed(速度)。

这里写图片描述

系统优化的能量函数如下,能量函数包括视觉 reprojection error 和 IMU error。

上式中, k 表示 frame 索引,i 表示 camera 索引,j 表示 feature 的索引,Wi,j,kr 表示特征的 information matrix,Wks 表示 IMU error 的 information matrix。

视觉 reprojection error 形式为:

ei,j,kr=zi,j,khi(TkCiSTkSWWlj)
其中 hi() 表示相机的投影模型,zi,j,k 表示特征的图像坐标。

reprojection error 从公式中可以看出可以优化系统的 pose TkSW,IMU 和相机的外参 TCiS,和特征坐标 kWlj

IMU 和相机外参的优化项还包括 pose error 和 relative pose error,pose error 是系统初始化时读入的手工标定的相机和 IMU 间的 pose,relative pose 是前后帧相机和 IMU 间的 pose 不能变化太大。

error 还有几项,从代码中也可以看出来,还包括: IMU error,speed and bias error,marginalization error。

**IMU error:**IMU error 代码实现和论文 IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation 一致,也就是说 OKVIS 优化也是预积分的思路。

OKVIS 代码也是按照 preintegration 的思路将前后帧 IMU 测量值做积分,因为积分会用到 IMU 的 bias,而 bias 是状态量,每次迭代时是变化的。所以,每次迭代时,会根据状态量相对于 bias 的雅可比重新计算预积分值,当 bias 变化太大时,不能再用雅可比近似计算预积分值,这时会根据 IMU 测量值重新积分。沈老师的 VINS 系统也是这个思路。

speed and bias error:系统初始化时,会读入手工设定的 IMU 的 bias 参数(包含陀螺仪和加速度计),读入的 bias 作为先验,读入的 bias 和计算的 bias 相减作为 error。speed 没有手工设定,赋值为 0。

marginalization error 和 marginalization 的过程未完,待续。

系统前端过程

Keypoint detection

算法用 SSE-optimized Harris corner detector 做角点检测,用 BRISK descriptor 作为特征描述子。在提取特征的时候尽量使得特征均摊在图像平面上。

在图像特征提取前,先根据系统状态量和 IMU 测量值对当前系统状态做 propagation,根据预测的系统状态(TWSTSCi)提取特征的描述子。

Keypoint matching

特征匹配有:current frame 和 last keyframes 匹配,current 和 last frames 匹配,和当前帧左右图像间匹配。
通过 3D-2D 匹配,建立 3D 特征和 2D 特征 reprojection error;通过 2D-2D 匹配初始化新的特征点。

3D-2D 的匹配:通过 IMU 测量值和上一帧系统的状态,预测当前帧系统的状态,首先根据预测的当前帧位姿筛选当前帧可见的特征点(特征点之前初始化获得),然后当前帧的每个特征点和这些可见的特征点做暴力匹配,暴力匹配的方式对于当前帧的每个特征点,计算和当前帧下所有可见的特征点之间特征描述子之间的距离(这里不用类似于 orb-slam 用的 guided matching 策略,作者说因为使用的特征描述子匹配较快,用这种暴力匹配的方式还要快),通过描述子之间的距离确定匹配关系。特征匹配后还进行误匹配的移除,移除分为两步:第一步根据位姿预测的不确定计算 Mahalanobis 距离,判断 3D 到 2D 的匹配是否有效;第二部通过 RANSAC 算法和 OpenGV 库来移除误匹配。

2D-2D 的匹配:匹配也是先通过暴力的方式得到初始匹配,然后通过三角化计算特征的位置,并且移除外点。如果说系统还没初始化,2D-2D 匹配还用于系统的初始化。

Keyframe selection

对于关键帧的选取采取以下策略:
如果当前帧匹配的特征点在图像中所占的面积不足一定的阈值,或者匹配的特征数目和检测到的特征点数目不足一定阈值,则认为是关键帧。

Keyframes and marginalization

初始阶段:边缘化过程如上图所示,开始的 M 帧都会被当做关键帧,边缘化的时候会消掉开始几帧 speed/bias 项,如下图所示:

添加的新的一帧时,当 temporal window 最老的一帧不是 keyframe 时,如上图所示,temporal window 中最老的一帧是 c3 帧,而 c3 帧不是关键帧,这时边缘化掉 c3 帧,边缘化的时候先把 c3 帧所有特征点的观测在优化中全部删除掉(每个观测构成一项重投影误差),然后把 c3 帧的 pose 和 speed/bias 边缘化掉,如下图所示:

temporal window 最老的一帧是 keyframe 时,如上图所示,temporal window 中最老的一帧是 c3 帧,而 c3 帧是关键帧,这时要边缘化掉关键帧 KF1 帧,在边缘化的时候会边缘化掉在 KF1 中可见而在最新关键帧和当前帧不可见的特征。

下面这幅图也很好的说明了这个问题:

上图中,当 temporal window 中最老的一帧 c3 不是关键帧时,边缘化掉 c3 帧,先把 c3 帧对特征 2,3,4 的观测移除(优化的目标函数中移除),然后边缘化掉 c3 帧的 pose 和 speed/bias。

当 temporal window 中最老的一帧 c3 是关键帧时,KF1 帧能观测到的特征有 1 2 3,当前帧能够观测到的特征有 4,在边缘化 KF1 帧时,由于特征 1 在最新关键帧 c3 和当前帧都不可见,所以要被边缘化掉,同时会移除特征 2,3 在 KF1 中的观测。

版权声明:本文为博主原创文章,未经博主允许不得转载。

没有更多推荐了,返回首页

私密
私密原因:
请选择设置私密原因
  • 广告
  • 抄袭
  • 版权
  • 政治
  • 色情
  • 无意义
  • 其他
其他原因:
120
出错啦
系统繁忙,请稍后再试

关闭