BA问题:
已知n个相机位姿(估计值),m个特征点世界坐标系坐标(估计值)和第j个特征点在第i个相机中像的像素坐标(观测值);
目标是求解使重投影误差最小化的相机位姿和特征点坐标。
【符号约定】
待估计状态:
X
=
[
ξ
1
,
ξ
2
,
⋯
 
,
ξ
n
,
P
1
w
,
P
2
w
,
⋯
 
,
P
m
w
]
T
\Chi=[\xi_1,\xi_2,\cdots,\xi_n,P^w_1,P^w_2,\cdots,P^w_m]^T
X=[ξ1,ξ2,⋯,ξn,P1w,P2w,⋯,Pmw]T;
ξ
i
\xi_i
ξi:第i个相机的位姿,
6
×
1
6\times 1
6×1的李代数;
P
j
w
,
P
j
c
i
P^w_j,P^{c_i}_j
Pjw,Pjci:分别表示世界坐标系、第i个相机坐标系下,第j个特征点的坐标,
3
×
1
3\times 1
3×1;
u
^
i
j
,
u
i
j
\hat u_{ij},u_{ij}
u^ij,uij:分别表示第j个特征点在第i个相机中像的像素坐标(的估计值、观测值),
2
×
1
2\times 1
2×1;
e
i
j
=
u
^
i
j
−
u
i
j
e_{ij}=\hat u_{ij}- u_{ij}
eij=u^ij−uij:表示像素坐标重投影误差,
2
×
1
2\times 1
2×1。
符号化BA问题:
(1)
(
ξ
∗
,
P
j
w
∗
)
=
arg
min
(
ξ
,
P
j
w
)
1
2
∑
(
i
,
j
)
=
(
1
,
1
)
(
n
,
m
)
∥
u
^
i
j
−
u
i
j
∥
Σ
2
(\xi^*,P^{w*}_j)=\arg \min_{(\xi,P^w_j)}\frac{1}{2}\sum_{(i,j)=(1,1)}^{(n,m)}\| \hat u_{ij}- u_{ij}\|^2_{\Sigma} \tag{1}
(ξ∗,Pjw∗)=arg(ξ,Pjw)min21(i,j)=(1,1)∑(n,m)∥u^ij−uij∥Σ2(1)
式中,
u
^
i
j
\hat u_{ij}
u^ij为依据估计的特征点坐标和估计的相机位姿计算得到的估计的像素坐标,详见公式(2);
e
i
j
=
u
^
i
j
−
u
i
j
e_{ij}=\hat u_{ij}- u_{ij}
eij=u^ij−uij为像素坐标重投影误差【表示,估计的的像素坐标与观测到的像素坐标(特征点在相机上的投影)相比较得到的误差】;
Σ
\Sigma
Σ为观测噪声;
(2)
s
i
j
u
^
i
j
=
K
[
e
x
p
(
ξ
i
∧
)
P
j
w
]
1
:
3
s
i
j
u
^
i
j
=
K
P
j
c
i
[
s
i
j
μ
^
i
j
s
i
j
ν
^
i
j
s
i
j
]
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
x
j
c
i
y
j
c
i
z
j
c
i
]
\begin{aligned} s_{ij}\hat u_{ij} &= K[exp(\xi_i^\wedge)P^w_j]_{1:3} \\ s_{ij}\hat u_{ij} &= KP^{c_i}_j \\ \begin{bmatrix} s_{ij}\hat\mu_{ij} \\ s_{ij}\hat\nu_{ij}\\ s_{ij} \end{bmatrix} &= \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x^{c_i}_j \\ y^{c_i}_j \\ z^{c_i}_j \end{bmatrix} \tag{2} \end{aligned}
siju^ijsiju^ij⎣⎡sijμ^ijsijν^ijsij⎦⎤=K[exp(ξi∧)Pjw]1:3=KPjci=⎣⎡fx000fy0cxcy1⎦⎤⎣⎡xjciyjcizjci⎦⎤(2)
式中,定义
P
j
c
i
≜
[
e
x
p
(
ξ
i
∧
)
P
j
w
]
1
:
3
P^{c_i}_j\triangleq [exp(\xi_i^\wedge)P^w_j]_{1:3}
Pjci≜[exp(ξi∧)Pjw]1:3。
【注】公式中有一些非齐次、齐次坐标之间的转换。
欲根据
ξ
i
\xi_i
ξi,
P
j
w
P^w_j
Pjw求
u
^
i
j
\hat u_{ij}
u^ij,而公式(2)中包含额外变量
s
i
j
s_{ij}
sij,故消去如下:
{
μ
^
i
j
=
f
x
x
j
c
i
z
j
c
i
+
c
x
ν
^
i
j
=
f
y
y
j
c
i
z
j
c
i
+
c
y
\begin{cases} \hat \mu_{ij} =f_x\frac{ x^{c_i}_j}{z^{c_i}_j}+c_x \\ \hat \nu_{ij} =f_y\frac{ y^{c_i}_j}{z^{c_i}_j}+c_y \\ \end{cases}
⎩⎨⎧μ^ij=fxzjcixjci+cxν^ij=fyzjciyjci+cy
则残差函数:
{
e
μ
i
j
=
μ
^
i
j
−
μ
i
j
=
f
x
x
j
c
i
z
j
c
i
+
c
x
−
μ
i
j
e
ν
i
j
=
ν
^
i
j
−
ν
i
j
=
f
y
y
j
c
i
z
j
c
i
+
c
y
−
ν
i
j
\begin{cases} e_{\mu _{ij}} = \hat \mu_{ij} - \mu_{ij} =f_x\frac{ x^{c_i}_j}{z^{c_i}_j}+c_x - \mu_{ij}\\ e_{\nu _{ij}} =\hat \nu_{ij} -\nu_{ij} =f_y\frac{ y^{c_i}_j}{z^{c_i}_j}+c_y -\nu_{ij} \\ \end{cases}
⎩⎨⎧eμij=μ^ij−μij=fxzjcixjci+cx−μijeνij=ν^ij−νij=fyzjciyjci+cy−νij
使用LM算法求解该问题,需要计算 e i j = [ e μ i j , e ν i j ] T e_{ij}=[e_{\mu _{ij}} ,e_{\nu _{ij}} ]^T eij=[eμij,eνij]T关于状态 X \Chi X的雅可比矩阵。
在雅可比中,仅状态
ξ
i
\xi_i
ξi和
P
j
w
P^w_j
Pjw对应的值非零,其余状态与
e
i
j
e_{ij}
eij的无关故为0,即
∂
e
i
j
∂
X
=
[
∂
e
i
j
∂
δ
ξ
1
⋯
∂
e
i
j
∂
δ
ξ
i
⋯
∂
e
i
j
∂
δ
ξ
n
∂
e
i
j
∂
P
1
w
⋯
∂
e
i
j
∂
P
j
w
⋯
∂
e
i
j
∂
P
m
w
]
2
×
(
6
n
+
3
m
)
=
[
0
⋯
∂
e
i
j
∂
δ
ξ
i
⋯
0
0
⋯
∂
e
i
j
∂
P
j
w
⋯
0
]
\begin{aligned} \frac{\partial e_{ij}}{\partial\Chi} &= \begin{bmatrix} \frac{\partial e_{ij}}{\partial\delta\xi_1}& \cdots& \frac{\partial e_{ij}}{\partial\delta\xi_i}& \cdots& \frac{\partial e_{ij}}{\partial\delta\xi_n}& \frac{\partial e_{ij}}{\partial P^w_1}& \cdots& \frac{\partial e_{ij}}{\partial P^w_j}& \cdots & \frac{\partial e_{ij}}{\partial P^w_m} \end{bmatrix}_{2\times (6n+3m)} \\ &=\begin{bmatrix} 0& \cdots& \frac{\partial e_{ij}}{\partial\delta\xi_i}& \cdots& 0& 0& \cdots& \frac{\partial e_{ij}}{\partial P^w_j}& \cdots& 0 \end{bmatrix} \end{aligned}
∂X∂eij=[∂δξ1∂eij⋯∂δξi∂eij⋯∂δξn∂eij∂P1w∂eij⋯∂Pjw∂eij⋯∂Pmw∂eij]2×(6n+3m)=[0⋯∂δξi∂eij⋯00⋯∂Pjw∂eij⋯0]
【注】对李代数的求导采用扰动形式。
根据信息矩阵的定义:
Λ
i
j
=
(
∂
e
i
j
∂
X
)
T
Σ
i
j
−
1
∂
e
i
j
∂
X
=
[
0
⋮
∂
e
i
j
∂
δ
ξ
i
⋮
0
0
⋮
∂
e
i
j
∂
P
j
w
⋮
0
]
(
6
n
+
3
m
)
×
2
[
Σ
i
j
−
1
]
2
×
2
[
0
⋯
∂
e
i
j
∂
δ
ξ
i
⋯
0
0
⋯
∂
e
i
j
∂
P
j
w
⋯
0
]
2
×
(
6
n
+
3
m
)
=
[
0
⋯
0
⋯
0
0
⋯
0
⋯
0
⋮
⋮
⋮
⋮
⋮
⋮
0
⋯
(
∂
e
i
j
∂
δ
ξ
i
)
T
Σ
i
j
−
1
∂
e
i
j
∂
δ
ξ
i
⋯
0
0
⋯
(
∂
e
i
j
∂
δ
ξ
i
)
T
Σ
i
j
−
1
∂
e
i
j
∂
P
j
w
⋯
0
⋮
⋮
⋮
⋮
⋮
⋮
0
⋯
0
⋯
0
0
⋯
0
⋯
0
0
⋯
0
⋯
0
0
⋯
0
⋯
0
⋮
⋮
⋮
⋮
⋮
⋮
0
⋯
(
∂
e
i
j
∂
P
j
w
)
T
Σ
i
j
−
1
∂
e
i
j
∂
δ
ξ
i
⋯
0
0
⋯
(
∂
e
i
j
∂
P
j
w
)
T
Σ
i
j
−
1
∂
e
i
j
∂
P
j
w
⋯
0
⋮
⋮
⋮
⋮
⋮
⋮
0
⋯
0
⋯
0
0
⋯
0
⋯
0
]
(
6
n
+
3
m
)
×
(
6
n
+
3
m
)
\begin{aligned} \Lambda_{ij} &=(\frac{\partial e_{ij}}{\partial\Chi})^T\Sigma^{-1}_{ij}\frac{\partial e_{ij}}{\partial\Chi} \\ &=\begin{bmatrix} 0 \\ \vdots \\ \frac{\partial e_{ij}}{\partial\delta\xi_i} \\ \vdots \\0 \\0 \\ \vdots \\ \frac{\partial e_{ij}}{\partial P^w_j} \\ \vdots \\ 0 \end{bmatrix}_{ (6n+3m)\times 2} [\Sigma^{-1}_{ij}]_{2\times 2} \begin{bmatrix} 0& \cdots& \frac{\partial e_{ij}}{\partial\delta\xi_i}& \cdots& 0& 0& \cdots& \frac{\partial e_{ij}}{\partial P^w_j} & \cdots& 0 \end{bmatrix}_{2\times (6n+3m)} \\ &=\begin{bmatrix} 0 & \cdots & 0 & \cdots & 0 & 0 & \cdots &0 & \cdots & 0\\ \vdots & & \vdots & & \vdots & \vdots & & \vdots & &\vdots \\ 0 & \cdots & (\frac{\partial e_{ij}}{\partial\delta\xi_i})^T \Sigma^{-1}_{ij} \frac{\partial e_{ij}}{\partial\delta\xi_i} & \cdots & 0 & 0 & \cdots & (\frac{\partial e_{ij}}{\partial\delta\xi_i})^T \Sigma^{-1}_{ij} \frac{\partial e_{ij}}{\partial P^w_j} & \cdots & 0 \\ \vdots & & \vdots & & \vdots & \vdots & & \vdots & &\vdots \\ 0 & \cdots & 0 & \cdots & 0 & 0 & \cdots &0 & \cdots & 0 \\ 0 & \cdots & 0 & \cdots & 0 & 0 & \cdots &0 & \cdots & 0 \\ \vdots & & \vdots & & \vdots & \vdots & & \vdots & &\vdots \\ 0& \cdots & (\frac{\partial e_{ij}}{\partial P^w_j})^T \Sigma^{-1}_{ij} \frac{\partial e_{ij}}{\partial\delta\xi_i} &\cdots & 0 & 0 & \cdots & (\frac{\partial e_{ij}}{\partial P^w_j})^T \Sigma^{-1}_{ij} \frac{\partial e_{ij}}{\partial P^w_j} & \cdots & 0\\ \vdots & & \vdots & & \vdots & \vdots & & \vdots & &\vdots \\ 0 & \cdots & 0 & \cdots & 0 & 0 & \cdots &0 & \cdots & 0 \\ \end{bmatrix}_{ (6n+3m)\times (6n+3m)} \end{aligned}
Λij=(∂X∂eij)TΣij−1∂X∂eij=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡0⋮∂δξi∂eij⋮00⋮∂Pjw∂eij⋮0⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(6n+3m)×2[Σij−1]2×2[0⋯∂δξi∂eij⋯00⋯∂Pjw∂eij⋯0]2×(6n+3m)=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡0⋮0⋮00⋮0⋮0⋯⋯⋯⋯⋯⋯0⋮(∂δξi∂eij)TΣij−1∂δξi∂eij⋮00⋮(∂Pjw∂eij)TΣij−1∂δξi∂eij⋮0⋯⋯⋯⋯⋯⋯0⋮0⋮00⋮0⋮00⋮0⋮00⋮0⋮0⋯⋯⋯⋯⋯⋯0⋮(∂δξi∂eij)TΣij−1∂Pjw∂eij⋮00⋮(∂Pjw∂eij)TΣij−1∂Pjw∂eij⋮0⋯⋯⋯⋯⋯⋯0⋮0⋮00⋮0⋮0⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(6n+3m)×(6n+3m)
故仅需要计算 e i j = [ e μ i j , e ν i j ] T e_{ij}=[e_{\mu _{ij}} ,e_{\nu _{ij}} ]^T eij=[eμij,eνij]T关于状态 ξ i \xi_i ξi和 P j w P^w_j Pjw的雅可比矩阵。
已定义
P
j
c
i
≜
[
e
x
p
(
ξ
i
∧
)
P
j
w
]
1
:
3
P^{c_i}_j\triangleq [exp(\xi_i^\wedge)P^w_j]_{1:3}
Pjci≜[exp(ξi∧)Pjw]1:3,故利用链式法则可先对
e
i
j
=
[
e
μ
i
j
,
e
ν
i
j
]
T
e_{ij}=[e_{\mu _{ij}} ,e_{\nu _{ij}} ]^T
eij=[eμij,eνij]T关于
P
j
c
i
P^{c_i}_j
Pjci求导,再计算
P
j
c
i
P^{c_i}_j
Pjci关于状态
ξ
i
\xi_i
ξi和
P
j
w
P^w_j
Pjw的雅可比矩阵,即:
∂
e
i
j
∂
δ
ξ
i
=
∂
e
i
j
∂
P
j
c
i
∂
P
j
c
i
∂
δ
ξ
i
∂
e
i
j
∂
P
j
w
=
∂
e
i
j
∂
P
j
c
i
∂
P
j
c
i
∂
P
j
w
\begin{aligned} \frac{\partial e_{ij}}{\partial\delta\xi_i} &= \frac{\partial e_{ij}}{\partial P^{c_i}_j} \frac{\partial P^{c_i}_j}{\partial\delta\xi_i} \\ \frac{\partial e_{ij}}{\partial P^w_j}&= \frac{\partial e_{ij}}{\partial P^{c_i}_j} \frac{\partial P^{c_i}_j}{\partial P^w_j} \end{aligned}
∂δξi∂eij∂Pjw∂eij=∂Pjci∂eij∂δξi∂Pjci=∂Pjci∂eij∂Pjw∂Pjci
式中,三个偏导分别为:
(1)
∂
e
i
j
∂
P
j
c
i
=
[
∂
e
μ
i
j
∂
x
j
c
i
∂
e
μ
i
j
∂
y
j
c
i
∂
e
μ
i
j
∂
z
j
c
i
∂
e
ν
i
j
∂
x
j
c
i
∂
e
ν
i
j
∂
y
j
c
i
∂
e
ν
i
j
∂
z
j
c
i
]
=
[
f
x
z
j
c
i
0
−
f
x
x
j
c
i
(
z
j
c
i
)
2
0
f
y
z
j
c
i
−
f
y
y
j
c
i
(
z
j
c
i
)
2
]
\begin{aligned} \frac{\partial e_{ij}}{\partial P^{c_i}_j} &= \begin{bmatrix} \frac{ \partial e_{\mu_{ij}} }{\partial x^{c_i}_j} & \frac{ \partial e_{\mu_{ij}} }{\partial y^{c_i}_j} & \frac{ \partial e_{\mu_{ij}} }{\partial z^{c_i}_j} \\ \frac{ \partial e_{\nu_{ij}} }{\partial x^{c_i}_j} & \frac{ \partial e_{\nu_{ij}} }{\partial y^{c_i}_j} & \frac{ \partial e_{\nu_{ij}} }{\partial z^{c_i}_j} \\ \end{bmatrix} \\ &= \begin{bmatrix} \frac{ f_x }{z^{c_i}_j} &0 & -\frac{ f_x x^{c_i}_j }{(z^{c_i}_j)^2} \\ 0 & \frac{ f_y }{z^{c_i}_j} & -\frac{ f_y y^{c_i}_j }{(z^{c_i}_j)^2} \\ \end{bmatrix} \\ \end{aligned}
∂Pjci∂eij=⎣⎡∂xjci∂eμij∂xjci∂eνij∂yjci∂eμij∂yjci∂eνij∂zjci∂eμij∂zjci∂eνij⎦⎤=⎣⎡zjcifx00zjcify−(zjci)2fxxjci−(zjci)2fyyjci⎦⎤
(2)
∂
P
j
c
i
∂
P
j
w
=
R
c
i
w
P
j
w
+
t
∂
P
j
w
=
R
c
i
w
\begin{aligned} \frac{\partial P^{c_i}_j}{\partial P^w_j} &= \frac{R_{c_iw}P^w_j +t}{\partial P^w_j} =R_{c_iw} \end{aligned}
∂Pjw∂Pjci=∂PjwRciwPjw+t=Rciw
(3)
∂
P
j
c
i
∂
δ
ξ
i
=
∂
e
x
p
(
ξ
i
∧
)
P
j
w
∂
δ
ξ
i
=
lim
δ
ξ
i
→
0
e
x
p
(
δ
ξ
i
∧
)
e
x
p
(
ξ
i
∧
)
P
j
w
−
e
x
p
(
ξ
i
∧
)
P
j
w
∂
δ
ξ
i
=
lim
δ
ξ
i
→
0
(
I
+
δ
ξ
i
∧
)
e
x
p
(
ξ
i
∧
)
P
j
w
−
e
x
p
(
ξ
i
∧
)
P
j
w
∂
δ
ξ
i
=
lim
δ
ξ
i
→
0
δ
ξ
i
∧
e
x
p
(
ξ
i
∧
)
P
j
w
∂
δ
ξ
i
=
lim
δ
ξ
i
→
0
[
δ
ϕ
∧
δ
ρ
0
T
0
]
[
P
j
c
i
1
]
∂
[
δ
ρ
δ
ϕ
]
=
lim
δ
ξ
i
→
0
[
δ
ϕ
∧
P
j
c
i
+
δ
ρ
0
]
∂
[
δ
ρ
δ
ϕ
]
=
[
I
−
(
P
j
c
i
)
∧
0
T
0
T
]
\begin{aligned} \frac{\partial P^{c_i}_j}{\partial\delta\xi_i} &= \frac{\partial exp(\xi^\wedge_i)P^{w}_j} {\partial\delta\xi_i} \\ &=\lim_{\delta \xi_i \to 0} \frac{exp(\delta\xi^\wedge_i)exp(\xi^\wedge_i)P^{w}_j -exp(\xi^\wedge_i)P^{w}_j } {\partial\delta\xi_i} \\ &=\lim_{\delta \xi_i \to 0} \frac{(I+\delta\xi^\wedge_i)exp(\xi^\wedge_i)P^{w}_j -exp(\xi^\wedge_i)P^{w}_j } {\partial\delta\xi_i} \\ &=\lim_{\delta \xi_i \to 0} \frac{\delta\xi^\wedge_i exp(\xi^\wedge_i)P^{w}_j } {\partial\delta\xi_i} \\ &=\lim_{\delta \xi_i \to 0} \frac{ \begin{bmatrix} \delta\phi^\wedge & \delta \rho \\ 0^T & 0 \end{bmatrix} \begin{bmatrix} P^{c_i}_j \\ 1\\ \end{bmatrix} } {\partial \begin{bmatrix} \delta\rho\\ \delta \phi\\ \end{bmatrix} } \\ &=\lim_{\delta \xi_i \to 0} \frac{ \begin{bmatrix} \delta\phi^\wedge P^{c_i}_j +\delta \rho \\ 0 \end{bmatrix} } {\partial \begin{bmatrix} \delta\rho\\ \delta \phi\\ \end{bmatrix} } \\ &= \begin{bmatrix} I & -(P^{c_i}_j)^\wedge \\ 0^T & 0^T \end{bmatrix} \\ \end{aligned}
∂δξi∂Pjci=∂δξi∂exp(ξi∧)Pjw=δξi→0lim∂δξiexp(δξi∧)exp(ξi∧)Pjw−exp(ξi∧)Pjw=δξi→0lim∂δξi(I+δξi∧)exp(ξi∧)Pjw−exp(ξi∧)Pjw=δξi→0lim∂δξiδξi∧exp(ξi∧)Pjw=δξi→0lim∂[δρδϕ][δϕ∧0Tδρ0][Pjci1]=δξi→0lim∂[δρδϕ][δϕ∧Pjci+δρ0]=[I0T−(Pjci)∧0T]
取前3行。
【注】如果
s
e
(
3
)
\mathfrak{se}(3)
se(3)的定义方式是旋转在前、平移在后,只需将这个矩阵的前3列与后3列对调即可。
结论:
∂
e
i
j
∂
δ
ξ
i
=
∂
e
i
j
∂
P
j
c
i
∂
P
j
c
i
∂
δ
ξ
i
=
[
f
x
z
j
c
i
0
−
f
x
x
j
c
i
(
z
j
c
i
)
2
0
f
y
z
j
c
i
−
f
y
y
j
c
i
(
z
j
c
i
)
2
]
R
c
i
w
∂
e
i
j
∂
P
j
w
=
∂
e
i
j
∂
P
j
c
i
∂
P
j
c
i
∂
P
j
w
=
[
f
x
z
j
c
i
0
−
f
x
x
j
c
i
(
z
j
c
i
)
2
0
f
y
z
j
c
i
−
f
y
y
j
c
i
(
z
j
c
i
)
2
]
[
I
−
(
P
j
c
i
)
∧
]
=
[
f
x
z
j
c
i
0
−
f
x
x
j
c
i
(
z
j
c
i
)
2
0
f
y
z
j
c
i
−
f
y
y
j
c
i
(
z
j
c
i
)
2
]
[
1
0
0
0
z
j
c
i
−
y
j
c
i
0
1
0
−
z
j
c
i
0
x
j
c
i
0
0
1
y
j
c
i
−
x
j
c
i
0
]
=
[
f
x
z
j
c
i
0
−
f
x
x
j
c
i
(
z
j
c
i
)
2
−
f
x
x
j
c
i
y
j
c
i
(
z
j
c
i
)
2
f
x
+
f
x
(
x
j
c
i
)
2
(
z
j
c
i
)
2
−
f
x
y
j
c
j
z
j
c
i
0
f
y
z
j
c
i
−
f
y
y
j
c
i
(
z
j
c
i
)
2
−
f
y
−
f
y
(
y
j
c
i
)
2
(
z
j
c
i
)
2
f
y
x
j
c
i
y
j
c
i
(
z
j
c
i
)
2
f
y
x
j
c
j
z
j
c
i
]
\begin{aligned} \frac{\partial e_{ij}}{\partial\delta\xi_i} &= \frac{\partial e_{ij}}{\partial P^{c_i}_j} \frac{\partial P^{c_i}_j}{\partial\delta\xi_i} \\ &=\begin{bmatrix} \frac{ f_x }{z^{c_i}_j} &0 & -\frac{ f_x x^{c_i}_j }{(z^{c_i}_j)^2} \\ 0 & \frac{ f_y }{z^{c_i}_j} & -\frac{ f_y y^{c_i}_j }{(z^{c_i}_j)^2} \\ \end{bmatrix} R_{c_iw} \\ \frac{\partial e_{ij}}{\partial P^w_j} &= \frac{\partial e_{ij}}{\partial P^{c_i}_j} \frac{\partial P^{c_i}_j}{\partial P^w_j} \\ &= \begin{bmatrix} \frac{ f_x }{z^{c_i}_j} &0 & -\frac{ f_x x^{c_i}_j }{(z^{c_i}_j)^2} \\ 0 & \frac{ f_y }{z^{c_i}_j} & -\frac{ f_y y^{c_i}_j }{(z^{c_i}_j)^2} \\ \end{bmatrix} \begin{bmatrix} I & -(P^{c_i}_j)^\wedge \end{bmatrix} \\ &= \begin{bmatrix} \frac{ f_x }{z^{c_i}_j} &0 & -\frac{ f_x x^{c_i}_j }{(z^{c_i}_j)^2} \\ 0 & \frac{ f_y }{z^{c_i}_j} & -\frac{ f_y y^{c_i}_j }{(z^{c_i}_j)^2} \\ \end{bmatrix} \begin{bmatrix} 1 &0 & 0 & 0 & z^{c_i}_j & -y^{c_i}_j \\ 0& 1& 0 & -z^{c_i}_j & 0 & x^{c_i}_j \\ 0& 0& 1 & y^{c_i}_j & -x^{c_i}_j & 0 \end{bmatrix} \\ &= \begin{bmatrix} \frac{ f_x }{z^{c_i}_j} &0 & -\frac{ f_x x^{c_i}_j }{(z^{c_i}_j)^2} & -\frac{ f_x x^{c_i}_j y^{c_i}_j }{(z^{c_i}_j)^2} & f_x+\frac{ f_x (x^{c_i}_j)^2 }{(z^{c_i}_j)^2} & -\frac{ f_x y^{c_j}_{j} }{z^{c_i}_j} \\ 0 & \frac{ f_y }{z^{c_i}_j} & -\frac{ f_y y^{c_i}_j }{(z^{c_i}_j)^2} & -f_y -\frac{ f_y (y^{c_i}_j)^2 }{(z^{c_i}_j)^2} & \frac{ f_y x^{c_i}_j y^{c_i}_j }{(z^{c_i}_j)^2} & \frac{ f_y x^{c_j}_{j} }{z^{c_i}_j} \\ \end{bmatrix} \end{aligned}
∂δξi∂eij∂Pjw∂eij=∂Pjci∂eij∂δξi∂Pjci=⎣⎡zjcifx00zjcify−(zjci)2fxxjci−(zjci)2fyyjci⎦⎤Rciw=∂Pjci∂eij∂Pjw∂Pjci=⎣⎡zjcifx00zjcify−(zjci)2fxxjci−(zjci)2fyyjci⎦⎤[I−(Pjci)∧]=⎣⎡zjcifx00zjcify−(zjci)2fxxjci−(zjci)2fyyjci⎦⎤⎣⎡1000100010−zjciyjcizjci0−xjci−yjcixjci0⎦⎤=⎣⎢⎡zjcifx00zjcify−(zjci)2fxxjci−(zjci)2fyyjci−(zjci)2fxxjciyjci−fy−(zjci)2fy(yjci)2fx+(zjci)2fx(xjci)2(zjci)2fyxjciyjci−zjcifxyjcjzjcifyxjcj⎦⎥⎤
对应的代码:
Eigen::Matrix<double,2,3> jacobian_uv_Pc;
jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
0, fy/z, -y * fy/z_2;
Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw; // 2*3
Eigen::Matrix<double,2,6> jacobian_Ti;
jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
-(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;