从世界坐标系看到的点的坐标(X,Y,Z)和相机看到的点(u,v)之间的关系
[
u
v
1
]
=
1
Z
[
f
x
,
0
,
c
x
0
,
f
y
,
c
y
0
,
0
,
1
]
[
X
Y
Z
]
=
1
Z
K
P
\begin{bmatrix}u\\v\\1\end{bmatrix} =\frac{1}{Z}\begin{bmatrix}f_x,0,c_x\\0,f_y,c_y\\0,0,1\end{bmatrix}\begin{bmatrix}X \\Y\\Z\end{bmatrix}=\frac{1}{Z}KP
⎣⎡uv1⎦⎤=Z1⎣⎡fx,0,cx0,fy,cy0,0,1⎦⎤⎣⎡XYZ⎦⎤=Z1KP
其中K是内参矩阵。也就是如下关系:
Z
p
=
K
P
Zp=KP
Zp=KP
一般的,在相机外参R,t的情况下,就变成了
Z
p
=
K
(
R
P
+
t
)
Zp=K(RP+t)
Zp=K(RP+t)
其中相机看到的点
P
j
c
a
m
P^{cam}_j
Pjcam和世界坐标系中的点
P
j
w
P^{w}_j
Pjw的关系如下:
P
j
c
a
m
=
w
c
R
P
j
w
+
w
c
P
P^{cam}_j = _w^{c}RP^w_j+_w^{c}P
Pjcam=wcRPjw+wcP
∂ u j ∂ X w = ∂ ( r 11 X w + r 12 Y w + r 13 Z w + w c X r 31 X w + r 32 Y w + r 33 Z w + w c Z ) ∂ X w = − r 11 ( r 31 X w + r 32 Y w + r 33 Z w + w c Z ) 2 = − r 11 ( Z j c a m ) 2 \frac{\partial u_j}{\partial X^w}=\frac{\partial(\frac{r_{11}X^w+r_{12}Y^w+r_{13}Z^w+_w^{c}X}{r_{31}X^w+r_{32}Y^w+r_{33}Z^w+_w^{c}Z})}{\partial X^w}=-\frac{r_{11}}{(r_{31}X^w+r_{32}Y^w+r_{33}Z^w+_w^{c}Z)^2}=-\frac{r_{11}}{{(Z_j^{cam}})^2} ∂Xw∂uj=∂Xw∂(r31Xw+r32Yw+r33Zw+wcZr11Xw+r12Yw+r13Zw+wcX)=−(r31Xw+r32Yw+r33Zw+wcZ)2r11=−(Zjcam)2r11
∂
u
j
∂
Y
w
=
−
r
12
(
r
31
X
w
+
r
32
Y
w
+
r
33
Z
w
+
w
c
Z
)
2
=
−
r
12
(
Z
j
c
a
m
)
2
\frac{\partial u_j}{\partial Y^w}=-\frac{r_{12}}{(r_{31}X^w+r_{32}Y^w+r_{33}Z^w+_w^{c}Z)^2}=-\frac{r_{12}}{{(Z_j^{cam}})^2}
∂Yw∂uj=−(r31Xw+r32Yw+r33Zw+wcZ)2r12=−(Zjcam)2r12
…
所以,图像看到的点相对于其世界坐标中的点的导数是:
J
23
=
p
j
c
∂
X
w
Y
w
Z
w
=
−
1
(
Z
j
c
a
m
)
2
[
r
11
,
r
12
,
r
13
r
21
,
r
22
,
r
23
]
J_{23}=\frac{p_j^c}{\partial X^wY^wZ^w}=-\frac{1}{(Z_j^{cam})^2}\begin{bmatrix}r_{11},r_{12},r_{13}\\r_{21},r_{22},r_{23} \end{bmatrix}
J23=∂XwYwZwpjc=−(Zjcam)21[r11,r12,r13r21,r22,r23]
从而有:
Δ
p
j
c
=
J
23
Δ
X
Y
Z
\Delta p_j^c=J_{23}\Delta XYZ
Δpjc=J23ΔXYZ
但是这个似乎和文章或者网上的推导不一样。
2D特征点的"微分"
按照文章公式(23)的理解,应该是:
J
23
=
∂
p
∂
X
Y
Z
=
[
∂
u
∂
X
Y
Z
∂
v
∂
X
Y
Z
]
=
[
∂
(
X
Z
)
∂
X
Y
Z
∂
(
Y
Z
)
∂
X
Y
Z
]
=
[
1
Z
,
0
,
−
X
Z
2
0
,
1
Z
,
−
Y
Z
2
]
=
1
Z
[
1
,
0
,
−
X
Z
0
,
1
,
−
Y
Z
]
J_{23}=\frac{\partial p}{\partial XYZ}=\begin{bmatrix}\frac{\partial u}{\partial XYZ}\\\frac{\partial v}{\partial XYZ}\end{bmatrix}=\begin{bmatrix}\frac{\partial (\frac{X}{Z})}{\partial XYZ}\\\frac{\partial (\frac{Y}{Z})}{\partial XYZ}\end{bmatrix}=\begin{bmatrix}\frac{1}{Z},0,-\frac{X}{Z^2}\\0,\frac{1}{Z},-\frac{Y}{Z^2}\end{bmatrix}=\frac{1}{Z}\begin{bmatrix}1,0,-\frac{X}{Z}\\0,1,-\frac{Y}{Z}\end{bmatrix}
J23=∂XYZ∂p=[∂XYZ∂u∂XYZ∂v]=[∂XYZ∂(ZX)∂XYZ∂(ZY)]=[Z1,0,−Z2X0,Z1,−Z2Y]=Z1[1,0,−ZX0,1,−ZY]
从而:
p
−
p
^
=
Δ
p
c
=
J
23
Δ
X
Y
Z
=
J
23
Δ
P
c
p-\hat{p}=\Delta p^c=J_{23}\Delta XYZ=J_{23}\Delta P^c
p−p^=Δpc=J23ΔXYZ=J23ΔPc
等方程求解的时候,
Δ
p
\Delta p
Δp和
J
23
J_{23}
J23都是已知量(其中
p
p
p是图像获取,
p
^
\hat{p}
p^是估计量,是根据
P
P
P的估计
P
^
\hat{P}
P^来的),求得
Δ
P
\Delta P
ΔP这个未知量。这个未知量的新鲜血液来自于图像的2D特征点坐标
p
p
p。其实这个小p和大P通过雅可比的转换是相机坐标系下的自说自话。
根据3.3的说法,也即是公式(14),相机的pose是IMU的pose而来。相机和IMU之间的外参是固定的,所以这两者知道一个就知道另一个了。
根据3.4的说法,要先搞定相机的pose,然后再用观测的点的2D坐标来做最小二乘法来确定2D坐标的估计值。所以附录解释了如何确定相机的pose。
公式(19),
P
c
=
w
c
R
(
P
w
−
P
c
a
m
w
)
P^c = _w^cR(P^w-P^w_{cam})
Pc=wcR(Pw−Pcamw), 其中
P
c
a
m
w
P_{cam}^w
Pcamw代表相机的原点在世界坐标系中的位置。换种说法,
P
w
=
c
w
R
P
c
+
P
c
a
m
w
P^w = _c^wRP^c+P^w_{cam}
Pw=cwRPc+Pcamw,这个公式和本文开头的公式
P
j
c
a
m
=
w
c
R
P
j
w
+
w
c
P
P^{cam}_j = _w^{c}RP^w_j+_w^{c}P
Pjcam=wcRPjw+wcP的意义是一样的。 但是对于公式(19)两边进行旋转和平移的求导是可以的。
说说公式(21)是怎么来的,首先在相机坐标系的框架内有:
Δ
p
=
J
X
Δ
X
+
J
23
Δ
P
c
\Delta p^=J_{X}\Delta X_{}+J_{23}\Delta P^c
Δp=JXΔX+J23ΔPc
这个公式的来源有两部分,分别是
Δ
X
\Delta X
ΔX和
Δ
P
c
\Delta P^c
ΔPc, 这两部分都和
Δ
p
\Delta p
Δp有着直接的关系:
和最后一个相机的状态有关系吗?
Δ
p
=
p
−
p
^
\Delta p=p-\hat{p}
Δp=p−p^
p
^
=
p
N
+
R
P
\hat{p}=p_N+RP
p^=pN+RP
其中,
Δ
X
=
[
Δ
X
I
M
U
,
Δ
θ
1
c
,
Δ
P
1
w
,
Δ
θ
2
c
,
Δ
P
2
w
,
.
.
.
,
Δ
θ
N
c
,
Δ
P
N
w
]
\Delta{X}=[\Delta X_{IMU}, \Delta \theta^c_1,\Delta P^w_1, \Delta \theta^c_2,\Delta P^w_2,..., \Delta \theta^c_N,\Delta P^w_N]
ΔX=[ΔXIMU,Δθ1c,ΔP1w,Δθ2c,ΔP2w,...,ΔθNc,ΔPNw]
Δ X I M U = [ Δ θ I M U , Δ P I M U , Δ v I M U , Δ b a , Δ b g ] \Delta X_{IMU}=[\Delta\theta_{IMU},\Delta P_{IMU},\Delta v_{IMU},\Delta b_a,\Delta b_g] ΔXIMU=[ΔθIMU,ΔPIMU,ΔvIMU,Δba,Δbg]
∂ Δ p ∂ Δ X I M U = [ ∂ Δ p ∂ Δ θ I M U , ∂ Δ p ∂ Δ P I M U , ∂ Δ p ∂ Δ v I M U , 0 , 0 ] = = [ 0 , 0 , 0 , 0 , 0 ] \frac{\partial \Delta p}{\partial \Delta X_{IMU}}=[\frac{\partial\Delta{p}}{\partial \Delta\theta_{IMU}},\frac{\partial\Delta{p}}{\partial \Delta P_{IMU}},\frac{\partial\Delta{p}}{\partial \Delta v_{IMU}},\textbf{0},\textbf{0} ]==[\textbf{0},\textbf{0},\textbf{0},\textbf{0},\textbf{0}] ∂ΔXIMU∂Δp=[∂ΔθIMU∂Δp,∂ΔPIMU∂Δp,∂ΔvIMU∂Δp,0,0]==[0,0,0,0,0]
著名的李代数求导:
∂ Δ p j ∂ Δ θ j = ∂ Δ p j ∂ Δ P j ∂ Δ P j ∂ Δ θ j = J 23 ∂ Δ R ∂ Δ θ = J 23 R p ^ \frac{\partial \Delta p_j}{\partial \Delta \theta_j}=\frac{\partial \Delta p_j}{\partial \Delta P_j}\frac{\partial \Delta P_j}{\partial \Delta \theta_j}=J_{23}\frac{\partial \Delta R}{\partial \Delta\theta}=J_{23}Rp_{} \hat{} ∂Δθj∂Δpj=∂ΔPj∂Δpj∂Δθj∂ΔPj=J23∂Δθ∂ΔR=J23Rp^
∂ Δ p j ∂ Δ P j = J 23 \frac{\partial \Delta p_j}{\partial \Delta P_j}=J_{23} ∂ΔPj∂Δpj=J23
其中,
J
23
Δ
P
c
=
J
23
w
c
R
Δ
P
w
=
J
f
Δ
P
w
J_{23}\Delta P^c=J_{23}{_w^c}R\Delta P^w=J_f\Delta P^w
J23ΔPc=J23wcRΔPw=JfΔPw
所以:
Δ
p
=
J
X
Δ
X
i
m
u
+
J
f
Δ
P
w
\Delta p^=J_{X}\Delta X_{imu}+J_f\Delta P^w
Δp=JXΔXimu+JfΔPw
其中
J
f
=
J
23
w
c
R
J_f=J_{23}{_w^c}R
Jf=J23wcR
特征点三角化
从最小二乘法的原理开始理解,就是有一堆已知坐标的点,{
x
i
,
y
i
x_i,y_i
xi,yi},现在需要求
y
=
k
x
+
b
y=kx+b
y=kx+b的k,b,使得
e
=
Σ
∣
y
i
−
(
k
x
i
+
b
)
∣
2
e=\Sigma|y_i-(kx_i+b)|^2
e=Σ∣yi−(kxi+b)∣2最小。
∂
e
∂
k
=
−
Σ
(
y
i
−
(
k
x
i
+
b
)
)
x
i
=
0
\frac{\partial e}{\partial k}=-\Sigma (y_i-(kx_i+b))x_i=0
∂k∂e=−Σ(yi−(kxi+b))xi=0
∂
e
∂
b
=
−
Σ
(
y
i
−
(
k
x
i
+
b
)
)
=
0
\frac{\partial e}{\partial b}=-\Sigma (y_i-(kx_i+b))=0
∂b∂e=−Σ(yi−(kxi+b))=0
轻松得出:
k
^
=
Σ
x
i
Σ
y
i
−
n
Σ
x
i
y
i
n
Σ
x
i
2
−
(
Σ
x
i
)
2
\hat k=\frac{\Sigma x_i\Sigma y_i-n\Sigma x_iy_i}{n\Sigma x_i^2-(\Sigma x_i)^2}
k^=nΣxi2−(Σxi)2ΣxiΣyi−nΣxiyi
b
^
=
Σ
x
i
Σ
x
i
y
i
−
n
Σ
x
i
2
Σ
y
i
n
Σ
x
i
2
−
(
Σ
x
i
)
2
\hat b=\frac{\Sigma x_i\Sigma x_iy_i-n\Sigma x_i^2\Sigma y_i}{n\Sigma x_i^2-(\Sigma x_i)^2}
b^=nΣxi2−(Σxi)2ΣxiΣxiyi−nΣxi2Σyi
现在有一堆已知pose的相机
R
C
i
,
t
C
i
R^{C_i},t^{C_i}
RCi,tCi,和一个未知的世界点P(X,Y,Z)在每个相机当中的2D观测量
(
u
i
,
v
i
)
(u_i,v_i)
(ui,vi),求世界点的坐标估计值.
世界点在相机系的2D坐标是(忽略相机内参):
(
u
i
,
v
i
,
1
)
′
=
1
Z
C
i
(
R
C
i
P
+
t
C
i
)
(u_i,v_i,1)'=\frac{1}{Z^{C_i}}(R^{C_i}P+t^{C_i})
(ui,vi,1)′=ZCi1(RCiP+tCi)
其中,相机坐标系下的
P
c
P^c
Pc都是世界坐标系的P的元素的线性组合:
X
c
=
r
11
X
w
+
r
12
Y
w
+
r
13
Z
w
+
w
c
X
X^c=r_{11}X^w+r_{12}Y^w+r_{13}Z^w+_w^{c}X
Xc=r11Xw+r12Yw+r13Zw+wcX
…
误差函数:
e = Σ [ ( u i − X C i Z C i ) 2 + ( v i − Y C i Z C i ) 2 ] e=\Sigma[(u_i-\frac{X^{C_i}}{Z^{C_i}})^2+(v_i-\frac{Y^{C_i}}{Z^{C_i}})^2] e=Σ[(ui−ZCiXCi)2+(vi−ZCiYCi)2]
∂ e ∂ X = Σ 2 [ ( u i − X C i Z C i ) ( − 1 Z C i ∂ X C i ∂ X + X C i ( Z C i ) 2 ∂ Z C i ∂ X ) + ( v i − Y C i Z C i ) ( − 1 Z C i ∂ Y C i ∂ X + Y C i ( Z C i ) 2 ∂ Z C i ∂ X ) ] = Σ 2 [ ( u i − X C i Z C i ) ( − 1 Z C i r 11 + X C i ( Z C i ) 2 r 13 ) + ( v i − Y C i Z C i ) ( − 1 Z C i r 12 + Y C i ( Z C i ) 2 r 13 ) ] = 0 \frac{\partial e}{\partial X}=\Sigma 2[(u_i-\frac{X^{C_i}}{Z^{C_i}})(-\frac{1}{Z^{C_i}}\frac{\partial X^{C_i}}{\partial X}+\frac{X^{C_i}}{(Z^{C_i})^2}\frac{\partial Z^{C_i}}{\partial X})+(v_i-\frac{Y^{C_i}}{Z^{C_i}})(-\frac{1}{Z^{C_i}}\frac{\partial Y^{C_i}}{\partial X}+\frac{Y^{C_i}}{(Z^{C_i})^2}\frac{\partial Z^{C_i}}{\partial X})]\\=\Sigma 2[(u_i-\frac{X^{C_i}}{Z^{C_i}})(-\frac{1}{Z^{C_i}}r_{11}+\frac{X^{C_i}}{(Z^{C_i})^2}r_{13})+(v_i-\frac{Y^{C_i}}{Z^{C_i}})(-\frac{1}{Z^{C_i}}r_{12}+\frac{Y^{C_i}}{(Z^{C_i})^2}r_{13})]=0 ∂X∂e=Σ2[(ui−ZCiXCi)(−ZCi1∂X∂XCi+(ZCi)2XCi∂X∂ZCi)+(vi−ZCiYCi)(−ZCi1∂X∂YCi+(ZCi)2YCi∂X∂ZCi)]=Σ2[(ui−ZCiXCi)(−ZCi1r11+(ZCi)2XCir13)+(vi−ZCiYCi)(−ZCi1r12+(ZCi)2YCir13)]=0
但是 [ ∂ e ∂ X ∂ e ∂ Y ∂ e ∂ Z ] = 0 \begin{bmatrix}\frac{\partial e}{\partial X}\\\frac{\partial e}{\partial Y}\\\frac{\partial e}{\partial Z}\end{bmatrix}=\textbf{0} ⎣⎡∂X∂e∂Y∂e∂Z∂e⎦⎤=0不构成一个线性方程组,如何求得P?
原文中的替代方法什么 α = X Z \alpha=\frac{X}{Z} α=ZX之类的也是没有什么用的,因为改变不了原文中z是一个分数的结构。最终没有办法得出一个线性方程组。我也比较疑惑这么有名的文章为啥会浆糊。
重投影误差的思考
其实我觉得这里的目标函数
e
=
Σ
[
(
u
i
−
X
C
i
Z
C
i
)
2
+
(
v
i
−
Y
C
i
Z
C
i
)
2
]
e=\Sigma[(u_i-\frac{X^{C_i}}{Z^{C_i}})^2+(v_i-\frac{Y^{C_i}}{Z^{C_i}})^2]
e=Σ[(ui−ZCiXCi)2+(vi−ZCiYCi)2]是重投影误差。只是这里需要借助一些非线性最小二乘的方法了。