ORB-SLAM3学习
第零章–研究综述
第一章–非线性最小二乘法
文章目录
前言
视觉SLAM中后端优化使用了诸多算法,非线性最小二乘法作为滤波算法的继承者,同时也是优化诸多算法中较为基础和简单的算法,对它的深入学习无疑是极为重要的。
参考视频资料:
1.最小二乘法简述 【「珂学原理」No.94「最小二乘估计了什么」】
https://www.bilibili.com/video/BV1sb411e79E/?share_source=copy_web&vd_source=9cb1868d1f25bd19d49d293bfbe568ac
2.高斯-牛顿公式推导【最优化算法之高斯牛顿法】
https://www.bilibili.com/video/BV1zE41177WB/?share_source=copy_web&vd_source=9cb1868d1f25bd19d49d293bfbe568ac
一、非线性最小二乘法在V-SLAM算法中的应用和简单理解
1.1 在V-SLAM中的简单说明
~~~~ 简单来说,属于一种优化算法,用于后端优化,优化过程中只有来自前端处理好的数据(orb特征处理的结果),不关心前端数据来自哪些传感器。具体来说,在slam十四讲中,最小二乘法用于状态估计,看书的时候就很在意这个状态是什么。举例来说,机器人在运动时,我们可以用运动方程去描述机器人的状态,对于观测点(观察到的图像点)我们可以用观测方程去描述它,那么问题来了,机器人和观测点的关系怎么去描述,这也就是状态问题(ps:并不是最小二乘法要解决的问题,有公式可以解决,最小二乘法是优化这个函数)。
1.2 最小二乘法的简单引入
~~~~
接下来,让我们来达成一个共识:任何系统都会产生误差。所谓优化就是尽可能的接近真实值,将误差考虑进问题中,做状态估计的时候直接加入一个误差项,通过最小二乘法是误差项达到最小。
~~~~
我们从最经典的“尺子测量”问题来体会一下什么叫“优化”,人永远无法用尺子测得一个物体的长度,首先,测量时会出现各种各样的误差(看错了、没对好),其次,小数点后精确多少位也是无法测出的。我们测量了四次,次次不一样,完了,这个砖头没长度了。
~~~~
接下来我们引入最小二乘法,首先将问题抽象化。砖头测量抽象起来就是:
A
x
=
b
Ax=b
Ax=b
~~~~
A是测量矩阵,x是砖块的长度,b是测量的结果,我们找不到一个完美的x来满足所有的等式,退而求其次吧,找到一个尽可能解释所有方程的x。把这个等式展开
a
1
1
x
1
+
a
1
2
x
2
+
…
+
a
1
n
x
n
=
b
1
a
2
1
x
1
+
a
2
2
x
2
+
…
+
a
2
n
x
n
=
b
2
⋮
a
m
1
x
1
+
a
m
2
x
2
+
…
+
a
m
n
x
n
=
b
m
a_11x_1+a_12x_2+\ldots+a_1nx_n=b_1 \\ a_21x_1+a_22x_2+\ldots+a_2nx_n=b_2 \\ \vdots\\ a_m1x_1+a_m2x_2+\ldots+a_mnx_n=b_m
a11x1+a12x2+…+a1nxn=b1a21x1+a22x2+…+a2nxn=b2⋮am1x1+am2x2+…+amnxn=bm
~~~~
那么我们如何找到那个“万金油”x呢,让所有的等式都基本满足呗,引入代价函数:
J
(
x
)
=
(
b
1
−
(
a
1
1
x
1
+
…
+
a
1
n
x
n
)
)
2
+
(
b
2
−
(
a
2
1
x
1
+
…
+
a
2
n
x
n
)
)
2
+
⋮
(
b
m
−
(
a
m
1
x
1
+
…
+
a
m
n
x
n
)
)
2
\begin{aligned} J(\mathbf{x}) = & ( b_1-(a_11x_1+\ldots+a_1nx_n))^2+ \\ & (b_2-(a_21x_1+\ldots +a_2nx_n))^2+\\ &\vdots \\ & (b_m-(a_m1x_1+\ldots+a_mnx_n))^2 \end{aligned}
J(x)=(b1−(a11x1+…+a1nxn))2+(b2−(a21x1+…+a2nxn))2+⋮(bm−(am1x1+…+amnxn))2
~~~~
什么叫代价函数呢,就是偏差的具象表现,优化就是要把这部分给尽量减小,找到那个x让J(x)最小。让一个函数值最小,求导呗,令其导数为零,x就出来了。
[
1
1
1
1
]
x
=
[
24.11
24.05
24.13
24.12
]
\left[ \begin{matrix} 1 \\ 1\\ 1\\ 1 \end{matrix} \right]x= \left[ \begin{matrix} 24.11 \\ 24.05\\ 24.13\\ 24.12 \end{matrix} \right]
⎣⎢⎢⎡1111⎦⎥⎥⎤x=⎣⎢⎢⎡24.1124.0524.1324.12⎦⎥⎥⎤
~~~~
代入公式:
J
(
x
)
=
(
24.11
−
x
)
2
+
(
24.05
−
x
)
2
+
(
24.13
−
x
)
2
+
(
24.12
−
x
)
2
J(\mathbf{x})=(24.11-x)^2+(24.05-x)^2+(24.13-x)^2+(24.12-x)^2
J(x)=(24.11−x)2+(24.05−x)2+(24.13−x)2+(24.12−x)2
~~~~
求导:
d
J
x
=
−
2
(
24.11
−
x
)
−
2
(
24.05
−
x
)
−
2
(
24.13
−
x
)
−
2
(
24.12
−
x
)
=
0
\frac{d J}{x}=-2(24.11-x)-2(24.05-x)-2(24.13-x)-2(24.12-x)=0
xdJ=−2(24.11−x)−2(24.05−x)−2(24.13−x)−2(24.12−x)=0
~~~~
解得:
x
=
24.11
+
24.05
+
24.13
+
24.12
4
x=\frac{24.11+24.05+24.13+24.12}{4}
x=424.11+24.05+24.13+24.12
~~~~
这不就是我们常说的“测不准多测几次求平均值嘛”,当然这是最最最简单的一个例子。总结下来两点:1.求出代价函数。2.让代价函数最小化。至此,我们很浅显的了解了最小二乘法的基本思想,当然,这是线性的例子,很好理解,我们接下来要解决的问题是非线性的,主题思想还是那么两点。
二、牛顿公式推导
2.1 一维函数求最小值
设函数
f
(
x
)
f(x)
f(x),
x
∈
R
x\in R
x∈R,将函数进行二阶泰勒展开:
f
(
x
k
)
=
f
(
x
k
−
1
)
+
f
′
(
x
k
−
1
)
(
x
k
−
x
k
−
1
)
+
1
2
f
′
′
(
x
k
−
1
)
(
x
k
−
x
k
−
1
)
2
+
o
f(x_k)=f(x_{k-1})+f'(x_{k-1})(x_k-x_{k-1})+\frac{1}{2}f''(x_{k-1})(x_k-x_{k-1})^2+o
f(xk)=f(xk−1)+f′(xk−1)(xk−xk−1)+21f′′(xk−1)(xk−xk−1)2+o
对其求导,令导数为0:
x
k
=
x
k
−
1
−
f
′
(
x
k
−
1
)
f
′
′
(
x
k
−
1
)
x_{k}=x_{k-1}-\frac{f'(x_{k-1})}{f''(x_{k-1})}
xk=xk−1−f′′(xk−1)f′(xk−1)
2.2 二(多)维函数求最小值
直接上多维确实是很抽象,我们先从二维出发,看一下具体是怎么来的。设二元函数
f
(
x
,
y
)
f(x,y)
f(x,y)在点
(
a
,
b
)
(a,b)
(a,b)进行二阶泰勒展开:
f
(
x
,
y
)
≈
f
(
a
,
b
)
+
[
∂
f
x
(
a
,
b
)
,
∂
f
y
(
a
,
b
)
]
[
x
−
a
y
−
b
]
+
1
2
[
x
−
a
y
−
b
]
[
∂
2
f
∂
x
2
(
a
,
b
)
∂
2
f
∂
x
∂
y
(
a
,
b
)
∂
2
f
∂
y
∂
x
(
a
,
b
)
∂
2
f
∂
y
2
(
a
,
b
)
]
[
x
−
a
y
−
b
]
=
f
(
a
,
b
)
+
(
x
−
a
)
f
x
′
(
a
,
b
)
+
(
y
−
b
)
f
y
′
(
a
,
b
)
+
1
2
!
(
x
−
a
)
2
f
x
x
′
′
(
a
,
b
)
+
1
2
!
(
x
−
a
)
(
y
−
b
)
f
x
y
′
′
(
a
,
b
)
+
1
2
!
(
x
−
a
)
(
y
−
b
)
f
y
x
′
′
(
a
,
b
)
+
1
2
!
(
y
−
b
)
2
f
y
y
′
′
(
a
,
b
)
\begin{aligned} f(x,y)\approx & f(a,b)+\left[\frac{\partial f}{x} (a,b),\frac{\partial f}{y} (a,b)\right]\begin{bmatrix}x-a\\y-b\end{bmatrix}\\+ &\frac{1}{2} \begin{bmatrix}x-a & y-b\end{bmatrix}\begin{bmatrix}\dfrac{\partial ^{2} f}{\partial x^{2}}( a,b) & \dfrac{\partial ^{2} f}{\partial x \ \partial y}( a,b)\\ \dfrac{\partial ^{2} f}{\partial y \ \partial x}( a,b) & \dfrac{\partial ^{2} f}{\partial y^{2}}( a,b)\end{bmatrix}\begin{bmatrix}x-a\\y-b\end{bmatrix}\\= & f(a,b)+(x-a)f'_{x} (a,b)+(y-b)f'_{y} (a,b)\\+ & \frac{1}{2!} (x-a)^{2} f''_{xx} (a,b)+\frac{1}{2!} (x-a)(y-b)f''_{xy} (a,b)\\+ & \frac{1}{2!} (x-a)(y-b)f''_{yx} (a,b)+\frac{1}{2!} (y-b)^{2} f''_{yy} (a,b) \end{aligned}
f(x,y)≈+=++f(a,b)+[x∂f(a,b),y∂f(a,b)][x−ay−b]21[x−ay−b]⎣⎢⎢⎡∂x2∂2f(a,b)∂y ∂x∂2f(a,b)∂x ∂y∂2f(a,b)∂y2∂2f(a,b)⎦⎥⎥⎤[x−ay−b]f(a,b)+(x−a)fx′(a,b)+(y−b)fy′(a,b)2!1(x−a)2fxx′′(a,b)+2!1(x−a)(y−b)fxy′′(a,b)2!1(x−a)(y−b)fyx′′(a,b)+2!1(y−b)2fyy′′(a,b)
从结果可以发现一阶导数是两个向量的偏导矩阵,二阶导数是两个向量的二阶偏导矩阵。
二、高斯–牛顿公式推导
2.1 基础知识
设有向量函数f(x)及向量x:
f
:
[
f
1
(
x
)
,
f
2
(
x
)
,
…
,
f
m
(
x
)
]
f:[f_1(\mathbf{x}),f_2(\mathbf{x}),\ldots,f_m(\mathbf{x})]
f:[f1(x),f2(x),…,fm(x)]
x
:
[
x
1
,
x
2
,
…
,
x
n
]
x:[x_1,x_2,\ldots,x_n]
x:[x1,x2,…,xn]
接下来回顾雅可比矩阵,它就是f对x的一阶导数,只不过上升到矩阵层面了:
J
f
=
[
∂
f
∂
x
1
,
∂
f
∂
x
2
,
…
,
∂
f
∂
x
n
]
=
[
∂
f
1
∂
x
1
⋯
∂
f
1
∂
x
n
⋮
⋱
⋮
∂
f
m
∂
x
1
⋯
∂
f
m
∂
x
n
]
J_f=\left[ \begin{matrix} \frac{\partial f}{\partial x_1} , \frac{\partial f}{\partial x_2},\ldots , \frac{\partial f}{\partial x_n}\\ \end{matrix} \right]=\begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \cdots & \frac{\partial f_m}{\partial x_n} \end{bmatrix}
Jf=[∂x1∂f,∂x2∂f,…,∂xn∂f]=⎣⎢⎡∂x1∂f1⋮∂x1∂fm⋯⋱⋯∂xn∂f1⋮∂xn∂fm⎦⎥⎤
对向量f进行泰勒展开(按照泰勒公式),进行一阶近似:
f
(
x
+
Δ
x
)
=
f
(
x
)
+
∇
f
(
x
)
Δ
x
+
1
2
Δ
x
T
H
(
f
(
x
)
)
Δ
x
f(\mathbf x+\mathbf \Delta x)=f(\mathbf x)+\nabla f(\mathbf x)\Delta x+\frac{1}{2}\Delta x^TH(f(\mathbf x))\Delta x
f(x+Δx)=f(x)+∇f(x)Δx+21ΔxTH(f(x))Δx
对
f
(
x
0
)
f(x_0)
f(x0)求导并令其导数为0:
x
n
+
1
=
x
n
−
[
H
(
f
(
x
n
)
)
]
−
1
∇
f
(
x
n
)
x_{n+1}=x_n-[H(f(x_n))]^{-1}\nabla f(x_n)
xn+1=xn−[H(f(xn))]−1∇f(xn)
何为非线性,最直观的理解就是拟合的图像不是一条直线,可能是各种各样的曲线(各种函数的组合),所以在此处不能像上面那个例子一样求个导就能求出x的值,就像
e
x
e^x
ex去求导,永远不可能把x消掉,求出x的具体值,我们只能去一点一点逼近x,直到偏差在可接受的范围内。
2.2 抛出问题
现有一个向量函数
f
f
f,有m个观测点(上图中的散点),每个点取决于p个变量(
x
x
x矩阵有p个分量)。模型函数如下,
β
\beta
β参数来反映函数:
f
(
x
1
,
x
2
,
…
,
x
p
;
β
1
,
β
2
,
…
,
β
n
)
f(x_1,x_2,\dots,x_p;\beta_1,\beta_2,\dots,\beta_n)
f(x1,x2,…,xp;β1,β2,…,βn)
2.3 优化目标函数
根据上面的例子,我们知道优化函数是向量函数预测值和真实值只差的平方和,表示如下:
m
i
n
S
=
∑
i
=
1
m
(
f
(
x
i
;
β
)
−
y
i
)
2
min S=\sum_{i = 1} ^m(f(x_i;\beta)-y_i)^2
minS=i=1∑m(f(xi;β)−yi)2
第i次观测点的预测偏差:
r
i
=
f
(
x
i
;
β
)
−
y
i
r_i=f(x_i;\beta)-y_i
ri=f(xi;β)−yi
将ri表示为矩阵:
r
=
[
r
1
,
…
,
r
m
]
T
r=[r_1,\ldots,r_m]^T
r=[r1,…,rm]T
于是目标函数
S
S
S可写为:
S
=
∑
i
=
1
m
r
i
2
=
r
T
r
(1x1)
S=\sum_{i = 1} ^mr_i^2=r^Tr\tag{1x1}
S=i=1∑mri2=rTr(1x1)
接下来要做的就是让目标函数最小化,换而言之准备泰勒展开求导吧,
S
S
S对
β
\beta
β求导,先对
r
r
r求导再对
β
\beta
β求导。
∂
S
∂
β
j
=
2
∑
i
=
1
m
r
i
∂
r
i
∂
β
j
\frac{\partial S}{\partial \beta_j}=2\sum_{i = 1} ^mr_i\frac{\partial r_i}{\partial \beta_j}
∂βj∂S=2i=1∑mri∂βj∂ri
梯度为:
∇
S
(
β
)
=
[
∂
S
∂
β
1
,
…
,
∂
S
∂
β
n
]
\nabla S(\beta)=\left[ \begin{matrix} \frac{\partial S}{\partial \beta_1},\ldots,\frac{\partial S}{\partial \beta_n}\end{matrix}\right]
∇S(β)=[∂β1∂S,…,∂βn∂S]
用
J
(
r
(
β
)
)
J(r(\beta))
J(r(β))代换,梯度为:
∇
S
=
2
J
T
r
\nabla S=2J^Tr
∇S=2JTr
接下来是高斯–牛顿法的重点,用
J
J
J去近似
H
H
H,实际上就是去求二阶导,再去近似:
H
=
∂
2
S
∂
β
k
β
j
=
2
∑
i
=
1
m
(
∂
r
i
∂
β
k
∂
r
i
∂
β
j
+
r
i
∂
2
r
i
∂
β
k
∂
β
j
)
H=\frac{\partial^2 S}{\partial \beta_k \beta_j}=2\sum_{i=1}^m(\frac{\partial r_i}{\partial \beta_k}\frac{\partial r_i}{\partial \beta_j}+r_i\frac{\partial ^2r_i}{\partial \beta_k\partial \beta_j})
H=∂βkβj∂2S=2i=1∑m(∂βk∂ri∂βj∂ri+ri∂βk∂βj∂2ri)
舍弃二阶导数项:
H
≈
2
∑
i
=
1
m
∂
r
i
∂
β
k
∂
r
i
∂
β
j
=
2
∑
i
=
1
m
J
i
k
J
i
j
H\approx2\sum_{i=1}^m\frac{\partial r_i}{\partial \beta_k}\frac{\partial r_i}{\partial \beta_j}= 2\sum_{i=1}^mJ_{ik}J_{ij}
H≈2i=1∑m∂βk∂ri∂βj∂ri=2i=1∑mJikJij
即:
H
=
2
J
T
J
H=2J^TJ
H=2JTJ
代入牛顿法结论:
x
n
+
1
=
x
n
−
[
H
(
f
(
x
n
)
)
]
−
1
∇
f
(
x
n
)
x_{n+1}=x_n-[H(f(x_n))]^{-1}\nabla f(x_n)
xn+1=xn−[H(f(xn))]−1∇f(xn)
β
(
k
+
1
)
=
β
(
k
)
−
H
−
1
g
=
β
(
k
)
−
(
2
J
T
J
)
−
1
2
J
T
r
\beta_{(k+1)}=\beta_{(k)}-H^{-1}g=\beta_{(k)}-(2J^TJ)^{-1}2J^Tr
β(k+1)=β(k)−H−1g=β(k)−(2JTJ)−12JTr
整理完成:
β
(
k
+
1
)
=
β
(
k
)
−
(
J
T
J
)
−
1
J
T
r
β
\beta_{(k+1)}=\beta_{(k)}-(J^TJ)^{-1}J^Tr_\beta
β(k+1)=β(k)−(JTJ)−1JTrβ