概述
文章目录
- 一、视觉SLAM中的Bundle Adjustment问题
- 二、最小二乘问题的求解
- 1、迭代下降法
- 1.1、最速下降法
- 1.2、牛顿法
- 1.3、阻尼法
- 1.4、高斯牛顿
- 1.5、LM
- 2、鲁棒核函数
- 三、VIO残差函数的构建
- 1、系统需要优化的状态量
- 2、VIO中基于逆深度的重投影误差
- 3、IMU测量值积分
- 4、IMU预积分
- 四、残差Jacobian的推导
- 五、结论
一、视觉SLAM中的Bundle Adjustment问题
已知:
- 状态初始值,根据里程计大致位置,可以计算出机器人的位置与特征的位置。如: q = [0.96, 0, 0, 0.25], p = [1, 2, 0], f = [2, 4, 0].
- 观测量:图像观测到的特征的像素坐标 z = [100, 0];
目标函数:
最小化所有观测像素的残差:
a
r
g
m
i
n
Σ
∣
∣
z
i
−
π
(
q
,
p
,
f
)
∣
∣
arg min Sigma ||z_i - pi (q, p, f)||
argminΣ∣∣zi−π(q,p,f)∣∣
这里的
π
(
q
,
p
,
f
)
pi(q, p, f)
π(q,p,f)是两个步骤,首先将特征点转换到局部坐标,然后把局部坐标转换到像素坐标,第二步的过程为:
x
p
i
x
=
f
x
x
z
x_{pix} = f_x frac{x}{z}
xpix=fxzx
相机局部坐标系为:前z,右x,下y,在二维情况中,y一直为0.
二、最小二乘问题的求解
关于泰勒展开式的理解: 泰勒展开通过一定阶数的表达式来代表(近似)一个复杂的函数。一般来说,求取泰勒展开式有两种:
- 已知复杂表达式,且复杂表达式可导,那直接通过一阶导数和二阶导数来求取。
- 当不知道复杂表达式,或者不可导时,可以通过在其上采样,通过采样点来求取泰勒展开。
在自动驾驶中,一般采用三次曲线来描述车道线,它其实就是现实生活中,复杂车道线的一个三次泰勒展开式。车身坐标系其实就是展开点。
一种比较简单的记忆泰勒展开式的方法:一次项使用直线,二次项使用抛物线,分别在0点处的情况来说明。
关于线性问题,如果是线性问题,其泰勒展开式,就可以描述其在整个可行解区间的近似,直接使用最小二乘解一次即可。
Hessian H的性质:当一阶导数为0时:
- 如果H正定,特征值都大于0,则一阶导数单增,此处为最小值。
- 如果H负定,特征值都小于0,则一阶导数单减,此处为最大值。
- 如果H不定,特征值有正有负,此处不确定,为鞍点。
1、迭代下降法
下降法的初衷,就是找到一个方向,然后确定步长。正如下楼梯,如果简化为一个1维的问题,那就是超楼下指的方向就下降方向(-x), 然后确定使用线搜索确定步长。
1.1、最速下降法
梯度的负方向下降速度最快。也就是沿着切线方向。在最后容易震荡。适合一开始
用
y
=
sin
(
x
)
y = sin(x)
y=sin(x)来理解。
1.2、牛顿法
求二阶最优,最稳定,但是速度慢,适合最后几步。
牛顿法,本质就是初中时学的,二次方程的根:对于方程
a
x
2
+
b
x
+
c
=
0
ax^2 + bx +c = 0
ax2+bx+c=0, 其最值在
x
=
−
b
2
a
x = -frac{b}{2a}
x=−2ab, 其中
a
=
1
/
2
∗
H
,
b
=
J
a = 1/2*H, b = J
a=1/2∗H,b=J, 显然,这种解法不一定能找到得最小值,只适用于快要接近于最优值了。
1.3、阻尼法
增加一个阻尼因子,在二次项上增加,调节a的大小。
1.4、高斯牛顿
高斯牛顿法,本质就是F函数被表示为了一个二次型,即 F = f ∗ f F = f*f F=f∗f. 然后把牛顿法用上,就成了高斯牛顿,真简单。
1.5、LM
将阻尼因子动态调节,就成了列文伯格马夸尔特算法。
2、鲁棒核函数
三、VIO残差函数的构建
1、系统需要优化的状态量
滑动窗口中,待优化的变量包含:body的PVQ(位置速度姿态),特征点的位置,角速度和角速度的偏置量。
2、VIO中基于逆深度的重投影误差
因为前面已经假设了二维的场景,相机坐标系下的feature与图像坐标系下的观测之间的关系为:
u
=
f
∗
x
z
u = f*frac{x}{z}
u=f∗zx, 因为是二维场景,所以相机的观测都在一条水平线上
v
=
f
∗
y
z
=
0
v = f*frac{y}{z} = 0
v=f∗zy=0. 实例检验:当x = 1m, z = 1m, y = 0m, f = 500, u = 500, v = 0;
残差的定义则为
r
c
=
[
u
−
f
∗
x
z
v
−
f
∗
y
z
]
=
[
u
−
f
∗
x
z
0
]
r_c = [begin{matrix}u - f*frac{x}{z} \ v - f*frac{y}{z}end{matrix}] =[begin{matrix}u - f*frac{x}{z} \ 0end{matrix}]
rc=[u−f∗zxv−f∗zy]=[u−f∗zx0]
3、IMU测量值积分
我们的状态量,是怎么与IMU测量值发生关系的?前一时刻的PVQ,通过对IMU测量值积分,得到下一个时刻的PVQ。具体来说:P-通过假设匀加速直线运动模型;V-通过匀加速直线运动模型,Q-通过匀角速度运动模型。实例检验:角速度1°/s, 加速度1m/s^2, 速度1m/s, 时间t=0-1s; 初始状态:
P
0
=
[
0
,
0
]
,
V
0
=
[
0
,
0
]
,
Q
0
=
0
°
P_0 = [0, 0], V_0 = [0, 0], Q_0 = 0°
P0=[0,0],V0=[0,0],Q0=0°。 经过一秒后,
P
1
=
[
0.5
,
0.5
]
,
V
1
=
[
1
,
1
]
,
Q
1
=
1
°
P_1 = [0.5, 0.5], V_1 = [1, 1], Q_1 = 1°
P1=[0.5,0.5],V1=[1,1],Q1=1°$
写成通用形式:
q
j
=
∫
q
t
⊗
[
0
,
1
/
2
w
]
δ
t
q_j = int q_t otimes [0, 1/2w] delta t
qj=∫qt⊗[0,1/2w]δt
v
j
=
v
i
+
∫
q
t
a
δ
t
v_j = v_i + int q_t a delta t
vj=vi+∫qtaδt
p
j
=
p
i
+
∫
v
t
δ
t
+
1
/
2
∫
∫
q
t
a
δ
t
p_j = p_i + int v_t delta t + 1/2intint q_ta delta t
pj=pi+∫vtδt+1/2∫∫qtaδt
这里的观测值a与w都是局部坐标系下的观测值。
缺点: 我们可以看到,积分里面的量包含了观测量,也包含了待优化的姿态,那么,我们每一步迭代都需要重新计算积分里面的量。而积分是一个计算量比较大的操作。比如两个相邻的姿态之间,有是个加速度值,那么就是计算10*3次,如果迭代10次,那么就需要对积分量进行300次计算,如果有100个姿态,则计算量高达3万次。
4、IMU预积分
将上面的三个式子变成:
q
j
=
q
i
∫
q
i
t
⊗
[
0
,
1
/
2
w
]
δ
t
q_j = q_i int q_{it} otimes [0, 1/2w] delta t
qj=qi∫qit⊗[0,1/2w]δt
v
j
=
v
i
+
q
i
∫
q
i
t
a
δ
t
v_j = v_i + q_iint q_{it} a delta t
vj=vi+qi∫qitaδt
p
j
=
p
i
+
∫
v
t
δ
t
+
1
/
2
q
i
∫
∫
q
i
t
a
δ
t
p_j = p_i + int v_t delta t + 1/2q_iintint q_{it}a delta t
pj=pi+∫vtδt+1/2qi∫∫qitaδt
这样变换之后,与积分相关的内容,其实是一个常数了,只需要计算一次,也就是说,上面的三万次计算量,变成了30*100 = 3千次,计算量缩小了十倍。
预计分的本质: 将状态量变成了观测量。
四、残差Jacobian的推导
(@todo)
五、结论
- 通过预计分,计算量优化了n倍,n是迭代的次数。这个倍数很重要,比如以前的1hz, 可以提高到10hz, 就能衍生出更多的应用场景了。
最后
以上就是虚心香菇为你收集整理的简单理解VIO(三)--基于优化的IMU与视觉信息融合的全部内容,希望文章能够帮你解决简单理解VIO(三)--基于优化的IMU与视觉信息融合所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错,欢迎将靠谱客网站推荐给程序员好友。
发表评论 取消回复