接着上篇:
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)
再次说明一下,本博客目前只是我在看深蓝时记录的,当然会有很多 PPT 上的内容,加上一点自己学的时候的理解,或者补全没有写全的推导,目的是能够留下一个电子形式的资料,直接目的是留给自己之后反复查看。每次视频我都是一点点啃下去的,属实对我这种本科生并不友好(本科大牛请绕路),所有内容都是一点点用 LaTeX 打上去的,不是截图搬运。也感谢贺博、高博及深蓝的精美课程!
VIO 残差函数的构建
带权重的残差计算
(18)
其中,
![[公式]](https://www.zhihu.com/equation?tex=f%28x%29)
服从高斯分布,协方差为
![[公式]](https://www.zhihu.com/equation?tex=%5CSigma)
。
- 协方差
用来 归一化,多传感器融合
单位不统一,如:PnP的误差方程的单位是像素,即实际像素点和理论像素点之间的误差;IMU 的误差单位为米(
)和 度(
)。
- 权重。协方差越小代表数据越集中,
就越大,代表权重越高。如说残差方程相差大,协方差大,说明可能存在误匹配 - 坏值,那么
较小权重低。
基于滑动窗口的 VIO Bundle Adjustment
(19)
高博的视觉SLAM十四讲里,对滑动窗口理论没有做过多的讲述,从上式(19)
![[公式]](https://www.zhihu.com/equation?tex=prior)
是先验,是后入数据时扔掉的前数据转化成的part,
![[公式]](https://www.zhihu.com/equation?tex=IMU%5Cquad+error)
和
![[公式]](https://www.zhihu.com/equation?tex=image+%5Cquad+error)
是新窗口内数据的残差方程,
![[公式]](https://www.zhihu.com/equation?tex=%5Crho)
是上篇提到的鲁棒核函数。
定义优化的系统状态量:
(20)
是
时刻 IMU 在惯性坐标系中的位置,姿态,速度,IMU 机体坐标系下加速度和角速度的偏置量估计。
是
时刻的观测点。
分别是滑动窗口里机体状态量和观测路标的起始时刻。N 代表状态量在窗口内的关键帧数,M 是窗口内所有关键帧观测到的路标点数。
视觉重投影误差
一个特征点在
归一化相机坐标系 下的估计值和观测值的差。
(21)
其中,待估计的状态量为特征点的三维空间坐标
![[公式]](https://www.zhihu.com/equation?tex=%28x%2Cy%2Cz%29%5ET)
,观测值
![[公式]](https://www.zhihu.com/equation?tex=%28u%2Cv%29%5ET)
为特征在相机归一化平面的坐标。
逆深度参数化 Inverse Depth Parametrization
特征点在
归一化相机坐标系 与
相机坐标系 下的坐标关系为:
(22)
其中
![[公式]](https://www.zhihu.com/equation?tex=%5Clambda+%3D+%5Cfrac%7B1%7D%7Bz%7D)
称为
逆深度[1]。逆深度更容易表示无穷远处的点,当
![[公式]](https://www.zhihu.com/equation?tex=%5Clim_%7B%5Clambda%5Cto+0%7Dz%3D%5Cinfty)
,可以用来表示无穷远处;且
![[公式]](https://www.zhihu.com/equation?tex=%5Clambda)
更接近正态分布。在实际应用中,逆深度也具有更好的数值稳定性。
特征点逆深度在第
![[公式]](https://www.zhihu.com/equation?tex=i)
帧中初始化得到,在第
![[公式]](https://www.zhihu.com/equation?tex=j)
帧又被观测到,预测其在第
![[公式]](https://www.zhihu.com/equation?tex=j)
中的坐标为:
(23)
这里的变换矩阵其实就是先从一帧的相机坐标系
![[公式]](https://www.zhihu.com/equation?tex=%5Cto)
这帧的 IMU body 坐标系
![[公式]](https://www.zhihu.com/equation?tex=%5Cto)
World 坐标系
![[公式]](https://www.zhihu.com/equation?tex=%5Cto)
另一帧的 body 坐标系
![[公式]](https://www.zhihu.com/equation?tex=%5Cto)
另一帧的相机坐标系。第一帧的 u-v 是可以通过亚像素的方法配准的,故用
![[公式]](https://www.zhihu.com/equation?tex=%5Clambda)
这一个参数表示第一帧的观测。
视觉重投影误差为:
(24)
IMU 测量值积分
IMU 的真实值
![[公式]](https://www.zhihu.com/equation?tex=%5Comega%2Ca)
,测量值为
![[公式]](https://www.zhihu.com/equation?tex=%5Cwidetilde%7B%5Comega%7D%2C%5Cwidetilde%7Ba%7D)
,则有:
(25)
(26)
上标
![[公式]](https://www.zhihu.com/equation?tex=g)
表示 gyroscope,
![[公式]](https://www.zhihu.com/equation?tex=a)
表示 Accelerometer,
![[公式]](https://www.zhihu.com/equation?tex=w)
表示世界坐标系 world,
![[公式]](https://www.zhihu.com/equation?tex=b)
表示imu 机体坐标系 body。
PVQ 对时间的导数可写成:
(27)

从第
![[公式]](https://www.zhihu.com/equation?tex=i)
时刻的 PVQ 对 IMU 的测量值进行积分得到第 j 时刻的 PVQ:
(28)
目标→解决:每次
![[公式]](https://www.zhihu.com/equation?tex=q_%7Bwb_t%7D)
优化更新,都需要对之后时间线上的重新积分。
IMU 预积分[2]
积分模型到预积分模型:
(29)
那么,PVQ 积分公式中的积分项则变成相对于第
![[公式]](https://www.zhihu.com/equation?tex=i)
时刻的姿态,而不是相对于世界坐标系的姿态:
(30)
预积分量
预积分量仅仅跟 IMU 测量值有关,它将一段时间内的 IMU 数据直接积分起来就得到了
预积分量:
(31)
PVQ 的积分公式:
(32)
认为短时间内的 bias 是不变的。
预积分误差:一段时间内 IMU 构建的预计分量做为测量值,对两时刻之间的状态量进行约束。
(33)
![[公式]](https://www.zhihu.com/equation?tex=%5B%C2%B7%5D_%7Bxyz%7D)
表示只取四元数的虚部
![[公式]](https://www.zhihu.com/equation?tex=%28x%2Cy%2Cz%29)
组成的三维向量,
![[公式]](https://www.zhihu.com/equation?tex=q_%7Bb_jb_i%7D)
是预积分估计值,四元数
![[公式]](https://www.zhihu.com/equation?tex=q%5Cotimes+q%5E%7B-1%7D)
的结果是
![[公式]](https://www.zhihu.com/equation?tex=%5Cbegin%7Bbmatrix%7D1%5C%5C0%5Cend%7Bbmatrix%7D)
即

代表转角
![[公式]](https://www.zhihu.com/equation?tex=%5Cfrac%7B%5Ctheta%7D%7B2%7D)
接近0,这里只取出 xyz 就是取出旋转角的残差乘以2得到
![[公式]](https://www.zhihu.com/equation?tex=%5Ctheta)
。等号左边每项都是一个三维度的共十五维,除四元数外,其他项均为 j 处世界坐标系下的真值减去 预积分以外的估计值(通过
![[公式]](https://www.zhihu.com/equation?tex=q_%7Bb_iw%7D)
从世界坐标系转换到 i 坐标系),再在 i 坐标系下与预积分值相减得到预积分误差。
预积分的离散形式
这里使用
中值法,即两个相邻时刻 k 到 k+1 的位姿用两个时刻的测量值
![[公式]](https://www.zhihu.com/equation?tex=a%2C%5Comega)
的平均值计算:
(34)
测量数据
![[公式]](https://www.zhihu.com/equation?tex=a%2C%5Comega)
都减去了 bias 可以用过标定获得,bias 服从随机游走,故有了后两个式子。随后的目的是 如何求算离散形式的多个 IMU 数据积分形成的
累计预积分量的
协方差。
协方差的传递 Covariance Propagation
已知
![[公式]](https://www.zhihu.com/equation?tex=y%3DAx%2Cx%5Cin%5Ccal+N%280%2C%5CSigma_x%29)
,则有
![[公式]](https://www.zhihu.com/equation?tex=%5CSigma_y%3DA%5CSigma_xA%5ET)
,证明其实在之前提过:

假设已知相邻时刻误差的线性传递方程:
(35)
比如:状态量误差为
![[公式]](https://www.zhihu.com/equation?tex=%5Ceta_%7Bik%7D%3D%5Cbegin%7Bbmatrix%7D%5Cdelta+%5Ctheta_%7Bik%7D%2C%5Cdelta+v_%7Bik%7D%2C%5Cdelta+p_%7Bik%7D%5Cend%7Bbmatrix%7D)
,测量噪声为
![[公式]](https://www.zhihu.com/equation?tex=n_k%3D%5Cbegin%7Bbmatrix%7Dn_k%5Eg%2Cn_k%5Ea%5Cend%7Bbmatrix%7D)
。
误差的传递由两部分组成:
当前时刻的误差传递给下一时刻,
当前时刻测量噪声传递给下一刻。
协方差矩阵可以通过递推得到:
(36)
其中,
![[公式]](https://www.zhihu.com/equation?tex=%5CSigma_n)
是测量噪声的协方差矩阵,方差从 i 时刻开始递推,
![[公式]](https://www.zhihu.com/equation?tex=%5CSigma_%7Bii%7D%3D0)
;Allan 方差计算出了测量噪声的方差量级。
状态误差线性递推公式的推导
- 基于一阶泰勒展开的误差递推方程。
- 基于误差随时间变化的递推方程。
基于一阶泰勒展开的误差递推方程
令状态量为
![[公式]](https://www.zhihu.com/equation?tex=x%3D%5Chat%7Bx%7D%2B%5Cdelta+x)
,其中,真值为
![[公式]](https://www.zhihu.com/equation?tex=%5Chat%7Bx%7D)
,误差为
![[公式]](https://www.zhihu.com/equation?tex=%5Cdelta+x)
。另外,输入量
![[公式]](https://www.zhihu.com/equation?tex=u)
的噪声为
![[公式]](https://www.zhihu.com/equation?tex=n)
。
非线性系统
![[公式]](https://www.zhihu.com/equation?tex=x_k%3Df%28x_%7Bk-1%7D%2Cu_%7Bk-1%7D%29)
的状态误差的线性递推关系如下:
(37)
其中,
![[公式]](https://www.zhihu.com/equation?tex=F)
是状态量
![[公式]](https://www.zhihu.com/equation?tex=x_k)
对状态量
![[公式]](https://www.zhihu.com/equation?tex=x_%7Bk-1%7D)
的雅可比矩阵,
![[公式]](https://www.zhihu.com/equation?tex=G)
是状态量
![[公式]](https://www.zhihu.com/equation?tex=x_k)
对输入量
![[公式]](https://www.zhihu.com/equation?tex=u_%7Bk-1%7D)
的雅可比矩阵。
对非线性状态方程进行一阶泰勒展开有:
(38)
基于误差随时间变化的递推方程
如果能够推导状态误差随时间变化的导数关系,如:
(39)
则误差状态的传递方程为:
(40)
对比两个方法有:
(41)
为什么会有误差随时间变化这种方法?
因为 VIO 系统中已知状态的导数和状态之间的转移矩阵。比如说速度的导数与状态量之间的关系:
(42)
那么就可以推导速度的误差和状态误差之间的关系,再每一项上都加上各自的误差就有:
(43)
预积分的误差递推公式推导
白噪声的值不能像 bias 一样估计,故白噪声不能像 bias 一样减去。
下面误差的 Jacobian 是,在递推公式(
![[公式]](https://www.zhihu.com/equation?tex=%5Comega%2Ca)
用了中值法)等式左右分别对偏导分子分母加上一个误差
![[公式]](https://www.zhihu.com/equation?tex=%5Cdelta+X)
后求出该 Jacobian。对于
![[公式]](https://www.zhihu.com/equation?tex=%5Ctheta)
是用
![[公式]](https://www.zhihu.com/equation?tex=%5Cotimes)
。
这个递推公式,可以理解为 k 时刻的该项误差,是由 k-1 时刻的误差及高斯白噪声(不像 bias 可以估计出来减去)递推得出的,但是为了简化,使用了对各项的一阶泰勒展开近似,下面↓来求这里的 F 和 G。

用前面一阶泰勒展开的推导方式,我们希望推导出误差的递推公式:
(44)
![[公式]](https://www.zhihu.com/equation?tex=F%2CG)
为两个时刻间的协方差传递矩阵,
![[公式]](https://www.zhihu.com/equation?tex=%5Cdelta%28%C2%B7%29)
表示各时刻的误差。
![[公式]](https://www.zhihu.com/equation?tex=F%5Cin+%5CBbb+R%5E%7B5%C3%975%7D%2CG%5Cin%5CBbb+R%5E%7B5%C3%976%7D)
。
(45)
(46)
其中系数为:
(47)
对部分项做详细推导↓对于导数中与变量无关的项省去:
对各状态量的雅可比推导,
第三行
速度预计分量
![[公式]](https://www.zhihu.com/equation?tex=%5Cbeta)
的递推计算形式:

(48)
:对上一时刻速度预积分量的 Jacobian
(49)
:对角度预积分量的 Jacobian
(50)
(51)
Part 1 对应的雅可比为:
(52)
Part 2 对应的雅可比为:
(53)
合起来就是(47)中的
![[公式]](https://www.zhihu.com/equation?tex=f_%7B32%7D)
。
:速度预积分量对 k 时刻角速度 bias 的 Jacobian

就有了这样的雅可比推导过程:
(54)
旋转预计分量的 Jacobian,
第二行
旋转预积分递推公式:
:前一时刻的旋转误差
如何影响当前旋转误差 ![[公式]](https://www.zhihu.com/equation?tex=%5Cdelta%5Ctheta_%7Bb_%7Bk%2B1%7D%7D)
假设两个时刻的真值是
![[公式]](https://www.zhihu.com/equation?tex=q_%7Bb_ib_%7Bk%2B1%7D%7D%2Cq_%7Bb_ib_k%7D)
,不考虑受到角速度 bias 的影响,两时刻的增量

三元组四元数相乘有如下性质(其实就是伴随性质,在李群李代数证明过):
(56)
其中
![[公式]](https://www.zhihu.com/equation?tex=R)
是和
![[公式]](https://www.zhihu.com/equation?tex=q)
对应的旋转矩阵,
![[公式]](https://www.zhihu.com/equation?tex=p_w)
为
![[公式]](https://www.zhihu.com/equation?tex=p)
的实部,
![[公式]](https://www.zhihu.com/equation?tex=p_v)
为
![[公式]](https://www.zhihu.com/equation?tex=p)
的虚部。
(57)
(58)
故第 k+1 时刻的旋转预积分的误差相对于第 k 时刻的 Jacobian 为:
(59)
残差 Jacobian 的推导
归一化平面视觉残差为:
对于第 i 帧中的特征点,它投影到第 j 帧相机坐标系下的值为:
写成三维坐标形式为:
(60)
这里的
![[公式]](https://www.zhihu.com/equation?tex=-p_%7Bwb_j%7D%2C-p_%7Bbc%7D)
是因为是从
![[公式]](https://www.zhihu.com/equation?tex=w%5Cto+j%2Cbody%5Cto+camera)
的变化,是相反的。此处把旋转和平移拆解开,平移部分为,
![[公式]](https://www.zhihu.com/equation?tex=T_%7Bbc%7D)
的平移部分
![[公式]](https://www.zhihu.com/equation?tex=p_%7Bbc%7D)
会经过三次旋转加平移的变换直到 j 的 camera 坐标系下。
变量定义:
(61)
Jacobian 对状态量,外参和逆深度求导:
(62)
step 1:先求error对
的导数,与 《视觉SLAM十四讲》不同之处是这里在归一化平面。
(63)
step 2:
对 i 时刻的状态量求导。
对位移求导↓
(64)
对角度求导↓
(65)
只保留与
![[公式]](https://www.zhihu.com/equation?tex=R_%7Bwb_i%7D)
有关的项:
(66)
这里的
![[公式]](https://www.zhihu.com/equation?tex=f_%7Bb_i%7D%3DR_%7Bbc%7Df_%7B%7Bc_i%7D%7D%2Bp_%7Bbc%7D)
即 i 坐标系下的坐标转换到 world 坐标系下。
Jacobian 为:
(67)
step 3:
对 j 时刻的状态量求导。
对位移求导↓
(68)
负号的原因可以看式(65),
![[公式]](https://www.zhihu.com/equation?tex=-p_%7Bwb_j%7D)
在式中与
![[公式]](https://www.zhihu.com/equation?tex=R%5ET_%7Bbc%7D%2CR%5ET_%7Bwb_j%7D)
相乘,求导固然结果如此。
对角度求导↓
(69)
Jacobian 为:
(70)
step 4:对 imu 与相机之间的外参求导。
就是对两传感器之间的旋转和平移求导。
对位移求导↓
(71)
对角度求导↓Jacobian 为:
(72)
step 5:视觉误差对特征逆深度的求导
(73)
IMU 误差相对于优化变量的 Jacobian
预积分误差如下图↓
这里的误差和上部分提到的误差是两码事,我是这么理解的,上部分提到的误差是通过递推 k 到 k+1 时刻间的误差传递;这里的误差是预积分误差,是求出的两时刻的位置、速度与真实值之间的误差。

上式中,
![[公式]](https://www.zhihu.com/equation?tex=%5Calpha_%7Bb_ib_j%7D%2C%5Cbeta_%7Bb_ib_j%7D%2Cq_%7Bb_ib_j%7D)
是估计值。
由于 i 时刻的 bias 相关的预积分计算是通过迭代一步一步累计递推的,很复杂。故预积分量直接在 i 时刻的 bias 附近用一阶泰勒展开近似。
(74)
对
进行推导:
- 对 i 时刻位移:由于与位移无关,故 Jacobian 为 0;
- 对 i 时刻旋转:
(75)
- 对 i 时刻速度:
;
- 对 i 时刻的加速度 bias 的 Jacobian,由于加速度只影响预积分,故
。
对
进行推导:
(76)
这里的
![[公式]](https://www.zhihu.com/equation?tex=%5Cbegin%7Bbmatrix%7D0%26I%5Cend%7Bbmatrix%7D)
就是提取 xyz 虚部,乘以系数 2,就是微小转动的
![[公式]](https://www.zhihu.com/equation?tex=%5Cdelta%5Ctheta)
,前面有解释。然而(76)式中对分子几个四元数
![[公式]](https://www.zhihu.com/equation?tex=%5Cotimes)
,取共轭后的
![[公式]](https://www.zhihu.com/equation?tex=%5B%C2%B7%5D_%7Bxyz%7D)
等于在整个式子前加负号。
- 对 i 时刻陀螺仪偏置
:
(77)
式中对角速度 bias 求 Jacobian,用一阶泰勒展开的形式,最后矩阵内 1 对
![[公式]](https://www.zhihu.com/equation?tex=%5Cdelta+b%5Eg_i)
求导为 0。
参考
- ^Civera J , Davison A J , J. M Martínez Montiel. Inverse depth parametrization for monocular SLAM[J]. IEEE Transactions on Robotics, 2008, 24(5):932-945. http://web.mit.edu/2.166/www/handouts/davison_tro2008.pdf
- ^Forster C , Carlone L , Dellaert F , et al. On-Manifold Preintegration for Real-Time Visual--Inertial Odometry[J]. ieee transactions on robotics, 2017, 33(1):1-21. https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=7557075