- 本节直接法与上节特征点法,为视觉里程计估计位姿的两大主流方法。而在引出直接法前,先介绍光流法(二者均对灰度值 I 做文章)。
- 至此,前端VO总算结束了。学下来一个感受就是前几章的数学基础很重要,尤其是构建最小二乘的非线性优化(BA),几乎每种方法都有其一席之地。
视觉SLAM学习打卡【8-1】-视觉里程计·直接法
一、光流法
- 特征点法需要同时提取两张图中的特征点,进而匹配描述子,最后进行位姿估计。而光流法只需要提取初始图像中的特征点,把匹配描述子换成光流跟踪,估计相机位姿仍需使用对极几何、PnP、ICP。
- 光流是一种描述像素随时间在图像之间运动的方法
- 代表有Lucas-Kanade光流(稀疏光流:只计算部分像素运动)& Horn-Schunck光流(稠密光流:计算全部像素运动)
(1)前提(实际中较难满足)
一个在 t 时刻,位于(x , y) 处的像素,它的灰度可以写成 I ( x , y , t ) I(x,y,t) I(x,y,t),设其在(t+dt)时刻,运动到(x+dx,y+dy)处,则其灰度变为 I ( x + d x , y + d y , t + d t ) \boldsymbol{I}(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t) I(x+dx,y+dy,t+dt)
- 灰度不变假设:同一个空间点的像素灰度值,在各个图像中是固定不变的. I ( x + d x , y + d y , t + d t ) = I ( x , y , t ) \boldsymbol{I}(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t)=\boldsymbol{I}(x,y,t) I(x+dx,y+dy,t+dt)=I(x,y,t)
- 一个w × w的窗口,一共有 w 2 {w^{2}} w2个像素,这个窗口内所有的像素都具有同样的运动,且每个像素都满足灰度不变假设.
(2)理论推导
把(t+dt)时刻的像素值进行泰勒展开
I
(
x
+
d
x
,
y
+
d
y
,
t
+
d
t
)
≈
I
(
x
,
y
,
t
)
+
∂
I
∂
x
d
x
+
∂
I
∂
y
d
y
+
∂
I
∂
t
d
t
I\left(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t\right)\approx I\left(x,y,t\right)+\frac{\partial I}{\partial x}\mathrm{d}x+\frac{\partial I}{\partial y}\mathrm{d}y+\frac{\partial I}{\partial t}\mathrm{d}t
I(x+dx,y+dy,t+dt)≈I(x,y,t)+∂x∂Idx+∂y∂Idy+∂t∂Idt由灰度不变假设可知,
∂
I
∂
x
d
x
+
∂
I
∂
y
d
y
+
∂
I
∂
t
d
t
=
0
\frac{\partial\boldsymbol{I}}{\partial x}\mathrm{d}x+\frac{\partial\boldsymbol{I}}{\partial y}\mathrm{d}y+\frac{\partial\boldsymbol{I}}{\partial t}\mathrm{d}t=0
∂x∂Idx+∂y∂Idy+∂t∂Idt=0移项、等式两边同除以dt得
∂
I
∂
x
d
x
d
t
+
∂
I
∂
y
d
y
d
t
=
−
∂
I
∂
t
\frac{\partial\boldsymbol{I}}{\partial x}\frac{\mathrm{d}x}{\mathrm{d}t}+\frac{\partial\boldsymbol{I}}{\partial y}\frac{\mathrm{d}y}{\mathrm{d}t}=-\frac{\partial\boldsymbol{I}}{\partial t}
∂x∂Idtdx+∂y∂Idtdy=−∂t∂I其中,
∂
I
∂
x
=
I
x
,
∂
I
∂
y
=
I
y
\frac{\partial I}{\partial x}=I_x,\frac{\partial I}{\partial y}=I_y
∂x∂I=Ix,∂y∂I=Iy,分别是图像灰度值在该点处x和y方向上的梯度;
d
x
d
t
=
u
\frac{\mathrm{d}x}{\mathrm{d}t}=u
dtdx=u,
d
y
d
t
=
v
\frac{\mathrm{d}y}{\mathrm{d}t}=v
dtdy=v,分别是像素在x轴和y轴上的运动速度;
∂
I
∂
t
=
I
t
\frac{\partial I}{\partial t}=I_t
∂t∂I=It为图像灰度值对时间变化量。
把上式写成矩阵形式
[
I
x
I
y
]
[
u
v
]
=
−
I
t
\begin{bmatrix}I_x&&I_y\end{bmatrix}\begin{bmatrix}u\\\\v\end{bmatrix}=-\boldsymbol I_t
[IxIy]
uv
=−It该矩阵方程是一个欠定方程组,存在自由度。根据前提2,有
w
2
{w^{2}}
w2个方程,即变为超定方程。
[
I
x
I
y
]
k
[
u
v
]
=
−
I
t
k
,
k
=
1
,
…
,
w
2
\begin{bmatrix}\boldsymbol{I}_x&\boldsymbol{I}_y\end{bmatrix}_k\begin{bmatrix}u\\\\v\end{bmatrix}=-\boldsymbol{I}_{tk},\quad k=1,\ldots,w^2
[IxIy]k
uv
=−Itk,k=1,…,w2解为
A
[
u
v
]
=
−
b
.
[
u
v
]
∗
=
−
(
A
T
A
)
−
1
A
T
b
.
\begin{aligned}&A\begin{bmatrix}u\\\\v\end{bmatrix}=-b.\\\\&\begin{bmatrix}u\\\\v\end{bmatrix}^*=-\left(A^\mathrm{T}A\right)^{-1}A^\mathrm{T}b.\end{aligned}
A
uv
=−b.
uv
∗=−(ATA)−1ATb.
(3)附:超定方程求解
- 超定方程组的数学推导主要涉及到最小二乘法的原理。
- 假设有超定方程组(方程组的个数多于未知数的个数): A x = b A\mathbf{x}=\mathbf{b} Ax=b,其中,A 是一个 m×n 的矩阵(m>n),x 是一个 n 维向量,b 是一个 m 维向量。
- 由于方程组的个数多于未知数的个数,该方程组通常没有精确解。因此,我们寻找一个近似解
x
a
p
p
r
o
x
x_{\mathrm{approx}}
xapprox,使得所有方程的残差平方和最小。残差定义为每个方程的观测值 b 与计算值
A
x
a
p
p
r
o
x
A\mathbf{x}_{\mathrm{approx}}
Axapprox之差,即
r
=
b
−
A
x
a
p
p
r
o
x
r=b-Ax_{approx}
r=b−Axapprox我们的目标是找到
x
a
p
p
r
o
x
x_{\mathrm{approx}}
xapprox,使得残差平方和 S 最小
S
=
r
T
r
=
(
b
−
A
x
approx
)
T
(
b
−
A
x
approx
)
S=\mathbf{r}^T\mathbf{r}=(\mathbf{b}-A\mathbf{x}_{\text{approx}})^T(\mathbf{b}-A\mathbf{x}_{\text{approx}})
S=rTr=(b−Axapprox)T(b−Axapprox)为了找到 S 的最小值,我们对 S 关于
x
a
p
p
r
o
x
x_{\mathrm{approx}}
xapprox求导,并令导数等于零,先展开 S
S
=
b
T
b
−
b
T
A
x
approx
−
x
approx
T
A
T
b
+
x
approx
T
A
T
A
x
approx
S=\mathbf{b}^T\mathbf{b}-\mathbf{b}^TA\mathbf{x}_\text{approx}-\mathbf{x}_\text{approx}^TA^T\mathbf{b}+\mathbf{x}_\text{approx}^TA^TA\mathbf{x}_\text{approx}
S=bTb−bTAxapprox−xapproxTATb+xapproxTATAxapprox求导
∂ S ∂ x approx = − 2 A T b + 2 A T A x approx \frac{\partial S}{\partial\mathbf{x}_{\text{approx}}}=-2A^T\mathbf{b}+2A^TA\mathbf{x}_{\text{approx}} ∂xapprox∂S=−2ATb+2ATAxapprox(可参考:矩阵求导公式大全)
令导数等于零,得到 − 2 A T b + 2 A T A x approx = 0 -2A^T\mathbf{b}+2A^TA\mathbf{x}_\text{ approx }=\mathbf{0} −2ATb+2ATAx approx =0整理后,得到正规方程(Normal Equation) A T A x a p p r o x = A T b A^TA\mathbf{x}_{\mathrm{approx}}=A^T\mathbf{b} ATAxapprox=ATb这个方程是一个 n×n 的线性方程组,其解 x a p p r o x x_{\mathrm{approx}} xapprox就是超定方程组的最小二乘解,即 x a p p r o x = ( A T A ) − 1 A T b x_{approx}=(A^{T}A)^{-1}A^{T}b xapprox=(ATA)−1ATb
二、直接法
不需要特征点(关键点),也不用匹配,只要求有像素梯度即可。特征点法和光流法分两步:先匹配后估计位姿;直接法两步一块走,以位姿为优化变量,求最优解(目标函数为运动前后像素光度误差,也算间接匹配)。
(1)理论推导
我们的目标是求第一个相机到第二个相机的相对位姿变换。我们以第一个相机为参照系,设第二个相机的旋转和平移为R , t(对应李群为T)。同时,两相机的内参相同,记为K。
p
1
=
[
u
v
1
]
1
=
1
Z
1
K
P
,
\boldsymbol{p}_1=\begin{bmatrix}u\\\\v\\\\1\end{bmatrix}_1=\frac1{Z_1}KP,
p1=
uv1
1=Z11KP,
p
2
=
[
u
v
1
]
2
=
1
Z
2
K
(
R
P
+
t
)
=
1
Z
2
K
(
T
P
)
1
:
3
\boldsymbol{p}_2=\begin{bmatrix}u\\\\v\\\\1\end{bmatrix}_2=\frac{1}{Z_2}\boldsymbol{K}\left(\boldsymbol{R}P+t\right)=\frac{1}{Z_2}\boldsymbol{K}\left(\boldsymbol{T}P\right)_{1:3}
p2=
uv1
2=Z21K(RP+t)=Z21K(TP)1:3构建优化函数,直接法中我们要求的是最小化光度误差,优化的参数是相机位姿T;特征点法中求的是最小化重投影误差,优化的参数也是相机位姿T.
e
=
I
1
(
p
1
)
−
I
2
(
p
2
)
.
e=\boldsymbol{I}_1\left(\boldsymbol{p}_1\right)-\boldsymbol{I}_2\left(\boldsymbol{p}_2\right).
e=I1(p1)−I2(p2).
min
T
J
(
T
)
=
∥
e
∥
2
.
\min_TJ\left(\boldsymbol{T}\right)=\|e\|^2.
TminJ(T)=∥e∥2.
min
T
J
(
T
)
=
∑
i
=
1
N
e
i
T
e
i
,
e
i
=
I
1
(
p
1
,
i
)
−
I
2
(
p
2
,
i
)
.
\min_{\boldsymbol{T}}J\left(\boldsymbol{T}\right)=\sum_{i=1}^{N}e_{i}^{\mathrm{T}}e_{i},\quad e_{i}=\boldsymbol{I}_{1}\left(\boldsymbol{p}_{1,i}\right)-\boldsymbol{I}_{2}\left(\boldsymbol{p}_{2,i}\right).
TminJ(T)=i=1∑NeiTei,ei=I1(p1,i)−I2(p2,i).(注:此处得J不代表雅可比矩阵,仅为误差2范数的一个表示符号)
定义两个中间变量,q是P在第二个相机坐标系下的坐标,而u是在第二个相机像素坐标系下的像素坐标。
q
=
T
P
,
u
=
1
Z
2
K
q
.
(此处
u
为上面的
p
2
即像素坐标,
q
为其相机坐标)
\begin{gathered}q=TP,\\u=\frac1{Z_2}Kq.\end{gathered}(此处u为上面的p2即像素坐标,q为其相机坐标)
q=TP,u=Z21Kq.(此处u为上面的p2即像素坐标,q为其相机坐标) 由于优化的参数是位姿,所以要对位姿求导。P1的像素点坐标是固定的常数(不随位姿变化而改变),所以上式对位姿求导后变为
∂
e
∂
T
=
∂
I
1
(
p
1
)
−
∂
I
2
(
p
2
)
∂
T
=
∂
I
1
(
p
1
)
−
∂
I
2
(
u
)
∂
T
=
−
∂
I
2
(
u
)
∂
T
\frac{\partial e}{\partial T}=\frac{\partial\boldsymbol{I}_1\left(\boldsymbol{p}_1\right)-\partial\boldsymbol{I}_2\left(\boldsymbol{p}_2\right)}{\partial T}=\frac{\partial\boldsymbol{I}_1\left(\boldsymbol{p}_1\right)-\partial\boldsymbol{I}_2\left(\boldsymbol{u}\right)}{\partial T}=-\frac{\partial\boldsymbol{I}_2\left(\boldsymbol{u}\right)}{\partial T}
∂T∂e=∂T∂I1(p1)−∂I2(p2)=∂T∂I1(p1)−∂I2(u)=−∂T∂I2(u)根据李代数的求导,我们选择左扰动模型(李群对加法不封闭,但李代数封闭),利用上面定义的中间变量,把u置换为位姿变化形式.(
∂
e
∂
T
=
∂
e
∂
δ
ξ
{{}}{\frac{\partial e}{\partial T}}={\frac{\partial e}{\partial\delta\xi}}
∂T∂e=∂δξ∂e)
∂
e
(
ξ
⊕
δ
ξ
)
∂
δ
ξ
=
−
∂
I
2
(
1
Z
2
K
exp
(
δ
ξ
∧
)
exp
(
ξ
∧
)
P
)
∂
δ
ξ
≈
−
∂
I
2
(
1
Z
2
K
(
1
+
δ
ξ
∧
)
exp
(
ξ
∧
)
P
)
∂
δ
ξ
=
−
∂
I
2
(
1
Z
2
K
exp
(
ξ
∧
)
P
+
1
Z
2
K
δ
ξ
∧
exp
(
ξ
∧
)
P
)
∂
δ
ξ
=
−
∂
I
2
(
u
+
1
Z
2
K
δ
ξ
∧
q
)
∂
δ
ξ
\begin{aligned} \frac{\partial e\left(\boldsymbol{\xi}\oplus\delta\boldsymbol{\xi}\right)}{\partial\delta\boldsymbol{\xi}}& =-\frac{\partial\boldsymbol{I}_2\left(\frac1{Z_2}\boldsymbol{K}\exp\left(\delta\boldsymbol{\xi}^\wedge\right)\exp\left(\boldsymbol{\xi}^\wedge\right)\boldsymbol{P}\right)}{\partial\boldsymbol{\delta}\boldsymbol{\xi}} \\ &\approx-\frac{\partial I_2\left(\frac1{Z_2} K\left(1+\delta\xi^\wedge\right)\exp\left(\xi^\wedge\right)P\right)}{\partial\boldsymbol{\delta}\boldsymbol{\xi}} \\ &=-\frac{\partial I_2\left(\frac1{Z_2} K\exp\left(\xi^{\wedge}\right)P+\frac1{Z_2} K\delta\xi^{\wedge}\exp\left(\xi^{\wedge}\right)P\right)}{\partial\delta\xi} \\ &=-\frac{\partial I_2\left(u+\frac1{Z_2}K\delta\xi^\wedge q\right)}{\partial\boldsymbol{\delta\xi}} \end{aligned}
∂δξ∂e(ξ⊕δξ)=−∂δξ∂I2(Z21Kexp(δξ∧)exp(ξ∧)P)≈−∂δξ∂I2(Z21K(1+δξ∧)exp(ξ∧)P)=−∂δξ∂I2(Z21Kexp(ξ∧)P+Z21Kδξ∧exp(ξ∧)P)=−∂δξ∂I2(u+Z21Kδξ∧q)其中,≈处运用了指数的泰勒一阶展示,
1
Z
2
K
δ
ξ
∧
q
\frac{1}{Z_{2}} K\delta\xi^{\wedge}q
Z21Kδξ∧q是像素点位置变化量,
δ
ξ
\delta\xi
δξ/
δ
ξ
∧
\delta\xi^{\wedge}
δξ∧均为扰动量的李代数形式(实际中不做详细区分)。
把
I
2
(
p
2
)
I_{2}\left(p_{2}\right)
I2(p2)在
p
2
=
u
p_{2}=u
p2=u处泰勒一阶展开
I
2
(
u
+
1
Z
2
K
δ
ξ
∧
q
)
≈
I
2
(
u
)
+
∂
I
2
∂
δ
ξ
δ
ξ
I_2\left(u+\frac{1}{Z_2} K\delta\xi^\wedge q\right)\approx I_2\left(u\right)+\frac{\partial I_2}{\partial\delta\xi} \delta\xi
I2(u+Z21Kδξ∧q)≈I2(u)+∂δξ∂I2δξ接着上面的推导,可得
∂
e
∂
δ
ξ
=
−
∂
I
2
(
u
+
1
Z
2
K
δ
ξ
∧
q
)
∂
δ
ξ
=
−
∂
(
I
2
(
u
)
+
∂
I
2
∂
δ
ξ
δ
ξ
)
∂
δ
ξ
=
0
−
∂
I
2
∂
δ
ξ
\begin{gathered} \frac{\partial e}{\partial\boldsymbol{\delta\xi}}=-\frac{\partial I_2\left(u+\frac1{Z_2} K\delta\xi^\wedge q\right)}{\partial\boldsymbol{\delta\xi}} \\ =-\frac{\partial(I_{2}\left(u\right)+\frac{\partial I_{2}}{\partial\delta\xi}\delta\xi)}{\partial\boldsymbol{\delta\xi}} \\ =0-\frac{\partial I_2}{\partial\delta\xi} \end{gathered}
∂δξ∂e=−∂δξ∂I2(u+Z21Kδξ∧q)=−∂δξ∂(I2(u)+∂δξ∂I2δξ)=0−∂δξ∂I2根据求导的链式法则
∂
e
∂
T
=
−
∂
I
2
∂
u
∂
u
∂
q
∂
q
∂
δ
ξ
\frac{\partial e}{\partial\boldsymbol{T}}=-\frac{\partial\boldsymbol{I}_2}{\partial\boldsymbol{u}}\frac{\partial\boldsymbol{u}}{\partial\boldsymbol{q}}\frac{\partial\boldsymbol{q}}{\partial\delta\boldsymbol{\xi}}
∂T∂e=−∂u∂I2∂q∂u∂δξ∂q关于
∂
e
∂
T
\frac{\partial e}{\partial T}
∂T∂e直接化为
I
2
I_{2}
I2的偏导,不用上述推导也可以想明白(因为以p1为参考,扰动在p2中)
- ∂ I 2 ∂ u \frac{\partial I_{2}}{\partial u} ∂u∂I2是图像像素的梯度,同光流法中计算一致(u是像素坐标2),即 ∂ I 2 ∂ u = [ I x I y ] T . \frac{\partial\boldsymbol{I}_{2}}{\partial\boldsymbol{u}}=[I_{x}~I_{y}]^{T}. ∂u∂I2=[Ix Iy]T.
- ∂ u ∂ q \frac{\partial u}{\partial q} ∂q∂u即像素坐标系下坐标2对相机坐标系下三维坐标2的导数. s u = k q su=kq su=kq [ s u s v s ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ] \begin{bmatrix}su\\sv\\s\end{bmatrix}=\begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}\begin{bmatrix}X\\Y\\Z\end{bmatrix} susvs = fx000fy0cxcy1 XYZ 利用第3行消去s,得 u = f x X Z + c x v = f y Y Z + c y u=f_x\frac{X}{Z}+c_x\quad v=f_y\frac{Y}{Z}+c_y u=fxZX+cxv=fyZY+cy所以 ∂ u ∂ q \frac{\partial u}{\partial q} ∂q∂u= ∂ u ∂ q = [ ∂ u ∂ X ∂ u ∂ Y ∂ u ∂ Z ∂ v ∂ X ∂ v ∂ Y ∂ v ∂ Z ] = [ f x Z 0 − f x X Z 2 0 f y Z − f y Y Z 2 ] 2 × 3 \frac{\partial\boldsymbol{u}}{\partial\boldsymbol{q}}=\begin{bmatrix}\dfrac{\partial u}{\partial X}&\dfrac{\partial u}{\partial Y}&\dfrac{\partial u}{\partial Z}\\\dfrac{\partial v}{\partial X}&\dfrac{\partial v}{\partial Y}&\dfrac{\partial v}{\partial Z}\end{bmatrix}=\begin{bmatrix}\dfrac{f_x}Z&0&-\dfrac{f_xX}{Z^2}\\\\0&\dfrac{f_y}Z&-\dfrac{f_yY}{Z^2}\end{bmatrix}_{2\times3} ∂q∂u= ∂X∂u∂X∂v∂Y∂u∂Y∂v∂Z∂u∂Z∂v = Zfx00Zfy−Z2fxX−Z2fyY 2×3
- ∂ q ∂ δ ξ \frac{\partial q}{\partial\delta\boldsymbol{\xi}} ∂δξ∂q即变换后的三维点q = Tp对位姿变换δξ的导数,这里具体过程同第四讲扰动模型求导。 ∂ T p ∂ δ ξ = ∂ q ∂ δ ξ = [ I , − q ∧ ] 3 × 6 \frac{\partial\boldsymbol{T}\boldsymbol{p}}{\partial\delta\boldsymbol{\xi}}=\frac{\partial\boldsymbol{q}}{\partial\delta\boldsymbol{\xi}}=[\boldsymbol{I},-\boldsymbol{q}^{\wedge}]_{3\times6} ∂δξ∂Tp=∂δξ∂q=[I,−q∧]3×6由于后两项只与三维点q 有关,而与图像无关,我们经常把它合并在一起 ∂ u ∂ δ ξ = [ f x Z 0 − f x X Z 2 − f x X Y Z 2 f x + f x X 2 Z 2 − f x Y Z 0 f y Z − f y Y Z 2 − f y − f y Y 2 Z 2 f y X Y Z 2 f y X Z ] 2 × 6 \frac{\partial u}{\partial\delta\boldsymbol{\xi}}=\begin{bmatrix}\frac{f_x}{Z}&0&-\frac{f_xX}{Z^2}&-\frac{f_xXY}{Z^2}&f_x+\frac{f_xX^2}{Z^2}&-\frac{f_xY}{Z}\\0&\frac{f_y}{Z}&-\frac{f_yY}{Z^2}&-f_y-\frac{f_yY^2}{Z^2}&\frac{f_yXY}{Z^2}&\frac{f_yX}{Z}\end{bmatrix}_{2\times6} ∂δξ∂u=[Zfx00Zfy−Z2fxX−Z2fyY−Z2fxXY−fy−Z2fyY2fx+Z2fxX2Z2fyXY−ZfxYZfyX]2×6终于,得到了残差函数e关于参数位姿T的导数,也就是我们需要的雅可比矩阵: J = − ∂ I 2 ∂ u ∂ u ∂ δ ξ . \boldsymbol{J}=-\frac{\partial\boldsymbol{I}_2}{\partial\boldsymbol{u}}\frac{\partial\boldsymbol{u}}{\partial\delta\boldsymbol{\xi}}. J=−∂u∂I2∂δξ∂u.
(2)直接法的优缺点
优点如下:
- 不需要特征点,只要有图像梯度即可.
- 省区计算特征点、描述子时间.
- 可以构建半稠密、稠密地图(特征点无法做到).
- 一般而言,直接法省去了光流计算时间(光流计算时间>计算并匹配描述子的时间).
缺点如下:
- 直接发完全依靠梯度搜索,若相机运动过快 / 初始值选取不当 / 图像具有非凸性,进入局部最小值(实际中可以引入金字塔).
- 单个像素误差太大,计算图像块,少数服从多数。因此,在选点较少时,结果变差.
- 比较前后光度值是建立在灰度不变假设的前提下,但现实中很明显做不到.