单层直接法
已知两帧图像,估计相机运动
设待估计的目标为
T
c
u
r
,
r
e
f
T_{cur, ref}
Tcur,ref,在参考帧(前一时刻)中取一组点
{
p
i
}
\{p_i\}
{pi}的位姿可以通过最小化目标函数求解:
T
c
u
r
,
r
e
f
=
1
N
∑
i
=
1
N
∑
W
i
∥
I
r
e
f
(
π
(
p
i
)
)
−
I
c
u
r
(
π
(
T
c
u
r
,
r
e
f
p
i
)
)
∥
2
2
.
\mathbf{T}_{\mathrm{cur}, \mathrm{ref}}=\frac{1}{N} \sum_{i=1}^{N} \sum_{W_{i}}\left\|I_{\mathrm{ref}}\left(\pi\left(\mathbf{p}_{i}\right)\right)-I_{\mathrm{cur}}\left(\pi\left(\mathbf{T}_{\mathrm{cur}, \mathrm{ref}} \mathbf{p}_{i}\right)\right)\right\|_{2}^{2}.
Tcur,ref=N1i=1∑NWi∑∥Iref(π(pi))−Icur(π(Tcur,refpi))∥22.
其中
N
N
N为点数,
π
\pi
π函数为针孔相机的投影函数
R
3
↦
R
2
\mathbb{R}^{3} \mapsto \mathbb{R}^{2}
R3↦R2,
W
i
W_i
Wi为第
i
i
i 个点周围的小窗口。同光流,可用Gauss-Newton函数求解。
-
该问题中的误差:
e ( T ) = I r e f ( π ( p i ) ) − I c u r ( π ( T c u r , r e f p i ) ) e(T) = I_{ref}(\pi(p_i)) - I_{cur}(\pi(T_{cur, ref}p_i)) e(T)=Iref(π(pi))−Icur(π(Tcur,refpi)) -
误差相对于自变量的梯度:
自变量:相机的位姿 ξ \xi ξ (李代数形式,6个自由度),故雅克比矩阵为 1 × 6 1\times6 1×6的矩阵。
给误差(其实是作用在T上)施加一个扰动:
e ( δ T ⊕ T ) = I r e f ( p 1 ) − I c u r ( 1 Z 2 K e x p ( δ ξ ∧ ) T P ) ≈ I r e f ( p 1 ) − I c u r ( 1 Z 2 K ( 1 + δ ξ ∧ ) T P ) = I r e f ( p 1 ) − I c u r ( u + 1 Z 2 K δ ξ ∧ T P ) ≈ I r e f ( P 1 ) − I c u r ( u ) − I c u r ′ ( u ) δ ξ = e ( T ) − ∂ I c u r ∂ u ⇀ u ⇀ ∂ q ∂ q ∂ δ ξ δ ξ e(\delta T \oplus T ) = I_{ref}(p_1) - I_{cur}(\frac{1}{Z_2}Kexp(\delta \xi^\wedge)TP) \\ \approx I_{ref}(p_1) - I_{cur}(\frac{1}{Z_2}K(1 + \delta \xi^{\wedge})TP) \\ = I_{ref}(p_1)- I_{cur}(u + \frac{1}{Z_2}K \delta \xi^{\wedge}TP) \\ \approx I_{ref}(P_1) - I_{cur}(u) -{ I_{cur}}'(u)\delta \xi \\ = e(T) - \frac{\partial \boldsymbol{I}_{cur}}{\partial \mathop{u}\limits^{\rightharpoonup}} \frac{\mathop{u}\limits^{\rightharpoonup}}{\partial \boldsymbol{q}} \frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}} \delta \boldsymbol{\xi} e(δT⊕T)=Iref(p1)−Icur(Z21Kexp(δξ∧)TP)≈Iref(p1)−Icur(Z21K(1+δξ∧)TP)=Iref(p1)−Icur(u+Z21Kδξ∧TP)≈Iref(P1)−Icur(u)−Icur′(u)δξ=e(T)−∂u⇀∂Icur∂qu⇀∂δξ∂qδξ完全按照一阶泰勒,上边近似后不应该是 δ ξ \delta\xi δξ,应该是上边的 u u u后边的那一坨。
难道是因为两个都是无穷小量,所以相等?q = T P u ⇀ = 1 Z 2 K q q = TP \\ \mathop{u}^{\rightharpoonup} = \frac{1}{Z_2}Kq q=TPu⇀=Z21Kq
-
注: q q q为 P P P在第二个相机坐标系下的坐标, u ⇀ \mathop{u}\limits^{\rightharpoonup} u⇀ 是它的像素坐标,由(4)可知, q q q是 T T T的函数, u ⇀ \mathop{u}\limits ^{\rightharpoonup} u⇀ 是 q q q的函数,这里的 P P P是已知位置的空间点。
-
lim δ ξ → 0 e ( δ ξ ⊕ ξ ) − e ( ξ ) δ ξ = − ∂ I c u r ∂ u ⇀ ∂ u ⇀ ∂ q ∂ q ∂ δ ξ = J \lim_{\delta \xi \to 0} \frac{e(\delta \xi \oplus \xi) - e(\xi)}{\delta \xi} = -\frac{\partial {I_{cur}}}{\partial \mathop{u}\limits^{\rightharpoonup}} \frac{\partial \mathop{u}\limits^{\rightharpoonup}}{\partial q}\frac{\partial q}{\partial \delta \xi} \\ = J δξ→0limδξe(δξ⊕ξ)−e(ξ)=−∂u⇀∂Icur∂q∂u⇀∂δξ∂q=J
-
梯度矩阵:
-
J = − ∂ I c u r ∂ u ⇀ ∂ u ⇀ ∂ q ∂ q ∂ δ ξ J = -\frac{\partial {I_{cur}}}{\partial \mathop{u}\limits^{\rightharpoonup}} \frac{\partial \mathop{u}\limits^{\rightharpoonup}}{\partial q}\frac{\partial q}{\partial \delta \xi} J=−∂u⇀∂Icur∂q∂u⇀∂δξ∂q
雅克比矩阵的维度为 1 × 6 1 \times 6 1×6 -
其中:
-
∂ 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 ] \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}}=\left[\begin{array}{ccc} \frac{\partial u}{\partial X} & \frac{\partial u}{\partial Y} & \frac{\partial u}{\partial Z} \\ \frac{\partial v}{\partial X} & \frac{\partial v}{\partial Y} & \frac{\partial v}{\partial Z} \end{array}\right]=\left[\begin{array}{ccc} \frac{f_{x}}{Z} & 0 & -\frac{f_{x} X}{Z^{2}} \\ 0 & \frac{f_{y}}{Z} & -\frac{f_{y} Y}{Z^{2}} \end{array}\right] ∂q∂u=[∂X∂u∂X∂v∂Y∂u∂Y∂v∂Z∂u∂Z∂v]=[Zfx00Zfy−Z2fxX−Z2fyY]
-
∂ q ∂ δ ξ = [ I , − q ∧ ] \frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}}=\left[\boldsymbol{I},-\boldsymbol{q}^{\wedge}\right] ∂δξ∂q=[I,−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 ] \frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}=\left[\begin{array}{cccccc} \frac{f_{x}}{Z} & 0 & -\frac{f_{x} X}{Z^{2}} & -\frac{f_{x} X Y}{Z^{2}} & f_{x}+\frac{f_{x} X^{2}}{Z^{2}} & -\frac{f_{x} Y}{Z} \\ 0 & \frac{f_{y}}{Z} & -\frac{f_{y} Y}{Z^{2}} & -f_{y}-\frac{f_{y} Y^{2}}{Z^{2}} & \frac{f_{y} X Y}{Z^{2}} & \frac{f_{y} X}{Z} \end{array}\right] ∂δξ∂u=[Zfx00Zfy−Z2fxX−Z2fyY−Z2fxXY−fy−Z2fyY2fx+Z2fxX2Z2fyXY−ZfxYZfyX]
-
-
若采用中值差分:
- ∂ I c u r ∂ u = 1 2 [ I c u r ( u + 1 , v ) − I c u r ( u − 1 , v ) , I c u r ( u , v + 1 − I c u r ( u , v − 1 ) ) ] \frac{\partial I_{cur}}{\partial u} = \frac 1 2 \begin{bmatrix} I_{cur}(u + 1, v) - I_{cur}(u - 1, v), I_{cur}(u, v +1 - I_{cur}(u, v - 1)) \end{bmatrix} ∂u∂Icur=21[Icur(u+1,v)−Icur(u−1,v),Icur(u,v+1−Icur(u,v−1))]
-
-
窗口尽量取大一点(上一帧图像中的点可能在投影后跑到下一帧图像对应窗口的外部),但也不能太大,否则计算时间太长。
窗口不要取单个点(投影之后点就没了,不在窗口范围内)
单层直接法:
- 可使用光流计算视差进而生成稀疏点云