在SLAM中，通常用BA（Bundle Adjustment）来实现多个三维点和不同相机位姿的优化
本文描述如何建立基于线特征的图优化，并推导相应的雅克比矩阵，并用g2o实现相应的类
假设相机位姿为
T
c
w
T_{cw}
Tcw表示世界坐标系到相机坐标系的变换，或者表示成
R
c
w
,
t
c
w
R_{cw},t_{cw}
Rcw,tcw。三维空间直线表示成
世界坐标下
L
w
=
[
n
w
v
w
]
\mathcal{L}^{w}=\begin{bmatrix} n^{w} \\ v^{w}\end{bmatrix}
Lw=[nwvw]
相机坐标下
L
c
=
[
n
c
v
c
]
\mathcal{L}^{c}=\begin{bmatrix} n^{c} \\ v^{c}\end{bmatrix}
Lc=[ncvc]
相机坐标转换成直线方程系数矩阵为
K
=
[
f
y
0
0
0
f
x
0
−
f
y
c
x
−
f
x
c
y
f
x
f
y
]
\mathcal{K}=\begin{bmatrix} f_{y} & 0& 0 \\ 0 & f_{x} & 0 \\ -f_{y}c_{x} & -f_{x}c_{y} & f_{x}f_{y} \end{bmatrix}
K=⎣
⎡fy0−fycx0fx−fxcy00fxfy⎦
⎤
重投影误差为
e
l
=
[
p
s
T
l
′
l
1
2
+
l
2
2
p
e
T
l
′
l
1
2
+
l
2
2
]
e_{l} = \begin{bmatrix} \frac {p_{s}^{T}l^{'}} {\sqrt{l_{1}^{2} + l_{2}^{2}}} \\ \frac {p_{e}^{T}l^{'}} {\sqrt{l_{1}^{2} + l_{2}^{2}}} \end{bmatrix}
el=⎣
⎡l12+l22
psTl′l12+l22
peTl′⎦
⎤其中
l
′
=
[
l
1
l
2
l
3
]
T
l^{'}=\begin{bmatrix} l_{1} & l_{2} & l_{3} \end{bmatrix}^{T}
l′=[l1l2l3]T，表示空间直线投影到二维图像上的直线方程系数
p
s
=
[
u
s
v
s
1
]
T
，
p
e
=
[
u
e
v
e
1
]
T
p_{s}=\begin{bmatrix} u_{s} & v_{s} & 1\end{bmatrix}^{T}，p_{e}=\begin{bmatrix} u_{e} & v_{e} & 1\end{bmatrix}^{T}
ps=[usvs1]T，pe=[ueve1]T为与空间直线匹配的图像线特征起点和终点齐次坐标
坐标之间的变换为
[
n
c
v
c
]
=
[
R
c
w
[
t
c
w
]
×
R
c
w
0
R
c
w
]
[
n
w
v
w
]
=
H
c
w
[
n
w
v
w
]
l
′
=
[
f
y
0
0
0
f
x
0
−
f
y
c
x
−
f
x
c
y
f
x
f
y
]
n
c
=
K
n
c
\begin{aligned} \begin{bmatrix} n^{c} \\ v^{c} \end{bmatrix} &= \begin{bmatrix}R_{cw} & \left[t_{cw}\right]_\times R_{cw} \\ 0 & R_{cw}\end{bmatrix}\begin{bmatrix}n^{w} \\v^{w}\end{bmatrix} \\ &=\mathcal{H}_{cw}\begin{bmatrix}n^{w} \\v^{w}\end{bmatrix} \\l^{'} &=\begin{bmatrix} f_{y} & 0& 0 \\ 0 & f_{x} & 0 \\ -f_{y}c_{x} & -f_{x}c_{y} & f_{x}f_{y} \end{bmatrix}n^{c} \\ &= \mathcal{K}n^{c} \end{aligned}
[ncvc]l′=[Rcw0[tcw]×RcwRcw][nwvw]=Hcw[nwvw]=⎣
⎡fy0−fycx0fx−fxcy00fxfy⎦
⎤nc=Knc
利用链式法则，可以求得
∂
e
l
∂
δ
ξ
=
∂
e
l
∂
l
′
∂
l
′
∂
L
c
∂
L
c
∂
δ
ξ
\frac{\partial e_{l}}{\partial \delta\xi}=\frac{\partial e_{l}}{\partial l^{'}} \frac{\partial l^{'}}{\partial \mathcal{L}^{c}} \frac{\partial \mathcal{L}^{c}}{\partial \delta\xi}
∂δξ∂el=∂l′∂el∂Lc∂l′∂δξ∂Lc
这里注意，这里需要涉及到矩阵求导，矩阵求导通常分为分子布局和分母布局，这两种情况只是表达形式的不同，本身并没有两样，下面统一都用分母布局，具体求导结果可以看这篇文章
第一部分计算
∂
e
l
∂
l
′
=
[
∂
(
p
s
T
l
′
l
1
2
+
l
2
2
)
∂
l
1
∂
(
p
s
T
l
′
l
1
2
+
l
2
2
)
∂
l
2
∂
(
p
s
T
l
′
l
1
2
+
l
2
2
)
∂
l
3
∂
(
p
e
T
l
′
l
1
2
+
l
2
2
)
∂
l
1
∂
(
p
e
T
l
′
l
1
2
+
l
2
2
)
∂
l
2
∂
(
p
e
T
l
′
l
1
2
+
l
2
2
)
∂
l
3
]
=
[
u
s
l
2
2
−
v
s
l
1
l
2
−
l
1
l
3
(
l
1
2
+
l
2
2
)
3
2
v
s
l
2
2
−
u
s
l
1
l
2
−
l
1
l
3
(
l
1
2
+
l
2
2
)
3
2
1
l
1
2
+
l
2
2
u
e
l
2
2
−
v
e
l
1
l
2
−
l
1
l
3
(
l
1
2
+
l
2
2
)
3
2
v
e
l
2
2
−
u
e
l
1
l
2
−
l
1
l
3
(
l
1
2
+
l
2
2
)
3
2
1
l
1
2
+
l
2
2
]
\begin{aligned} \frac{\partial e_{l}}{\partial l^{'}} &= \begin{bmatrix} \frac{\partial(\frac{p_{s}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{1}} & \frac{\partial(\frac{p_{s}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{2}} & \frac{\partial(\frac{p_{s}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{3}} \\ \frac{\partial(\frac{p_{e}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{1}} & \frac{\partial(\frac{p_{e}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{2}} & \frac{\partial(\frac{p_{e}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{3}} \end{bmatrix} \\ &= \begin{bmatrix} \frac{u_{s}l_{2}^{2}-v_{s}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} & \frac{v_{s}l_{2}^{2}-u_{s}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} & \frac{1}{\sqrt{l_{1}^{2}+l_{2}^{2}}} \\ \frac{u_{e}l_{2}^{2}-v_{e}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} & \frac{v_{e}l_{2}^{2}-u_{e}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} & \frac{1}{\sqrt{l_{1}^{2}+l_{2}^{2}}} \end{bmatrix} \end{aligned}
∂l′∂el=⎣
⎡∂l1∂(l12+l22
psTl′)∂l1∂(l12+l22
peTl′)∂l2∂(l12+l22
psTl′)∂l2∂(l12+l22
peTl′)∂l3∂(l12+l22
psTl′)∂l3∂(l12+l22
peTl′)⎦
⎤=⎣
⎡(l12+l22)23usl22−vsl1l2−l1l3(l12+l22)23uel22−vel1l2−l1l3(l12+l22)23vsl22−usl1l2−l1l3(l12+l22)23vel22−uel1l2−l1l3l12+l22
1l12+l22
1⎦
⎤先化简一下，对于第一行第一列元素
u
s
l
2
2
−
v
s
l
1
l
2
−
l
1
l
3
(
l
1
2
+
l
2
2
)
3
2
=
u
s
l
2
2
+
(
u
s
l
1
2
−
u
s
l
1
2
)
−
v
s
l
1
l
2
−
l
1
l
3
(
l
1
2
+
l
2
2
)
3
2
=
u
s
(
l
1
2
+
l
2
2
)
−
l
1
(
u
s
l
1
+
v
s
l
2
+
l
3
)
(
l
1
2
+
l
2
2
)
3
2
=
u
1
l
1
2
+
l
2
2
−
l
1
p
s
T
l
′
(
l
1
2
+
l
2
2
)
3
2
=
1
l
1
2
+
l
2
2
(
u
1
−
l
1
p
s
T
l
′
l
1
2
+
l
2
2
)
\begin{aligned} \frac{u_{s}l_{2}^{2}-v_{s}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} &= \frac{u_{s}l_{2}^{2}+(u_{s}l_{1}^{2}-u_{s}l_{1}^{2})-v_{s}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} \\ &= \frac{u_{s}(l_{1}^{2}+l_{2}^{2})-l_{1}(u_{s}l_{1}+v_{s}l_{2}+l_{3})}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} \\ &= \frac{u_{1}}{\sqrt{l_{1}^{2}+l_{2}^{2}}}-\frac{l_{1}p_{s}^{T}l^{'}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} \\ &= \frac{1}{\sqrt{l_{1}^{2}+l_{2}^{2}}}(u_{1}-\frac{l_{1}p_{s}^{T}l^{'}}{l_{1}^{2}+l_{2}^{2}}) \end{aligned}
(l12+l22)23usl22−vsl1l2−l1l3=(l12+l22)23usl22+(usl12−usl12)−vsl1l2−l1l3=(l12+l22)23us(l12+l22)−l1(usl1+vsl2+l3)=l12+l22
u1−(l12+l22)23l1psTl′=l12+l22
1(u1−l12+l22l1psTl′)令
l
n
=
l
1
2
+
l
2
2
,
e
s
=
p
s
T
l
′
,
e
e
=
p
e
T
l
′
l_{n}=\sqrt{l_{1}^{2}+l_{2}^{2}},e_{s}=p_{s}^{T}l^{'},e_{e}=p_{e}^{T}l^{'}
ln=l12+l22
,es=psTl′,ee=peTl′则
u
s
l
2
2
−
v
s
l
1
l
2
−
l
1
l
3
(
l
1
2
+
l
2
2
)
3
2
=
1
l
n
(
u
s
−
l
1
e
s
l
n
2
)
\begin{aligned} \frac{u_{s}l_{2}^{2}-v_{s}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} &= \frac{1}{l_{n}}(u_{s}-\frac{l_{1}e_{s}}{l_{n}^{2}}) \end{aligned}
(l12+l22)23usl22−vsl1l2−l1l3=ln1(us−ln2l1es)对其他位置的元素也进行简化，最终得到
∂
e
l
∂
l
′
=
1
l
n
[
u
s
−
l
1
e
s
l
n
2
v
s
−
l
2
e
s
l
n
2
1
u
e
−
l
1
e
e
l
n
2
v
e
−
l
2
e
e
l
n
2
1
]
2
×
3
\begin{aligned} \frac{\partial e_{l}}{\partial l^{'}} &= \frac{1}{l_{n}} \begin{bmatrix} u_{s}-\frac{l_{1}e_{s}}{l_{n}^{2}}& v_{s}-\frac{l_{2}e_{s}}{l_{n}^{2}} & 1 \\ u_{e}-\frac{l_{1}e_{e}}{l_{n}^{2}} & v_{e}-\frac{l_{2}e_{e}}{l_{n}^{2}} & 1 \end{bmatrix}_{2 \times 3} \end{aligned}
∂l′∂el=ln1[us−ln2l1esue−ln2l1eevs−ln2l2esve−ln2l2ee11]2×3第二部分计算
∂
l
′
∂
L
c
=
∂
(
K
n
c
)
∂
[
n
c
v
c
]
=
[
K
0
]
3
×
6
\begin{aligned} \frac{\partial l^{'}}{\partial \mathcal{L}^{c}} &=\frac{\partial (\mathcal{K}n^{c})}{\partial \begin{bmatrix} n^{c} \\ v^{c} \end{bmatrix}} \\ &= \begin{bmatrix} \mathcal{K} & 0 \end{bmatrix}_{3 \times 6} \end{aligned}
∂Lc∂l′=∂[ncvc]∂(Knc)=[K0]3×6第三部分计算，
δ
ξ
=
[
δ
ϕ
δ
ρ
]
\delta\xi=\begin{bmatrix}\delta\phi \\ \delta\rho \end{bmatrix}
δξ=[δϕδρ]
∂
L
c
∂
δ
ξ
=
[
∂
L
c
∂
δ
ϕ
∂
L
c
∂
δ
ρ
]
6
×
6
\begin{aligned} \frac{\partial \mathcal{L}^{c}}{\partial \delta\xi} &= \begin{bmatrix} \frac{\partial \mathcal{L}^{c}}{\partial \delta\phi} & \frac{\partial \mathcal{L}^{c}}{\partial \delta\rho} \end{bmatrix}_{6 \times 6} \\ \end{aligned}
∂δξ∂Lc=[∂δϕ∂Lc∂δρ∂Lc]6×6这里的话分别计算这两部分，基本求解方式和李群的扰动模型类似，假设添加左扰动后变换矩阵为
H
c
w
∗
\mathcal{H}^{*}_{cw}
Hcw∗，则，
∂
L
c
∂
δ
ξ
=
lim
δ
ξ
→
0
H
c
w
∗
L
w
−
H
c
w
L
w
δ
ξ
\frac{\partial \mathcal{L}^{c}}{\partial \delta\xi} =\lim_{\delta\xi\rightarrow 0} \frac{\mathcal{H}^{*}_{cw}\mathcal{L}^{w}-\mathcal{H}_{cw}\mathcal{L}^{w}}{\delta\xi}
∂δξ∂Lc=δξ→0limδξHcw∗Lw−HcwLw先求
H
c
w
∗
\mathcal{H}^{*}_{cw}
Hcw∗
T
∗
=
e
x
p
(
δ
ξ
∧
)
T
c
w
≈
(
I
+
[
δ
ϕ
∧
δ
ρ
0
T
0
]
)
[
R
c
w
t
c
w
0
T
1
]
=
[
(
I
+
δ
ϕ
∧
)
R
c
w
(
I
+
δ
ϕ
∧
)
t
c
w
+
δ
ρ
0
T
1
]
H
c
w
∗
=
[
(
I
+
δ
ϕ
∧
)
R
c
w
[
(
I
+
δ
ϕ
∧
)
t
c
w
+
δ
ρ
]
×
(
I
+
δ
ϕ
∧
)
R
c
w
0
(
I
+
δ
ϕ
∧
)
R
c
w
]
\begin{aligned} T^{*} &= exp(\delta\xi^{\wedge})T_{cw} \\ &\approx (I+\begin{bmatrix} \delta\phi^{\wedge} & \delta\rho \\ 0^{T} & 0\end{bmatrix}) \begin{bmatrix} R_{cw} & t_{cw} \\ 0^{T} & 1 \end{bmatrix} \\ &= \begin{bmatrix} (I+\delta\phi^{\wedge})R_{cw} & (I+\delta\phi^{\wedge})t_{cw}+\delta\rho \\ 0^{T} & 1 \end{bmatrix} \\ \mathcal{H}^{*}_{cw} &= \begin{bmatrix} (I+\delta\phi^{\wedge})R_{cw} & \left[(I+\delta\phi^{\wedge})t_{cw}+\delta\rho\right]_\times (I+\delta\phi^{\wedge})R_{cw} \\ 0 & (I+\delta\phi^{\wedge})R_{cw} \end{bmatrix} \\ \end{aligned}
T∗Hcw∗=exp(δξ∧)Tcw≈(I+[δϕ∧0Tδρ0])[Rcw0Ttcw1]=[(I+δϕ∧)Rcw0T(I+δϕ∧)tcw+δρ1]=[(I+δϕ∧)Rcw0[(I+δϕ∧)tcw+δρ]×(I+δϕ∧)Rcw(I+δϕ∧)Rcw]计算
∂
L
c
∂
δ
ϕ
\frac{\partial \mathcal{L}^{c}}{\partial \delta\phi}
∂δϕ∂Lc的时候可以令
δ
ρ
=
0
\delta\rho=0
δρ=0，则
∂
L
c
∂
δ
ϕ
=
∂
[
(
I
+
δ
ϕ
∧
)
R
c
w
[
(
I
+
δ
ϕ
∧
)
t
c
w
]
×
(
I
+
δ
ϕ
∧
)
R
c
w
0
(
I
+
δ
ϕ
∧
)
R
c
w
]
[
n
w
v
w
]
∂
δ
ϕ
\begin{aligned} \frac{\partial \mathcal{L}^{c}}{\partial \delta\phi} &=\frac{\partial \begin{bmatrix} (I+\delta\phi^{\wedge})R_{cw} & \left[(I+\delta\phi^{\wedge})t_{cw}\right]_\times (I+\delta\phi^{\wedge})R_{cw} \\ 0 & (I+\delta\phi^{\wedge})R_{cw} \end{bmatrix}\begin{bmatrix}n^{w} \\v^{w}\end{bmatrix}}{\partial \delta\phi} \end{aligned}
∂δϕ∂Lc=∂δϕ∂[(I+δϕ∧)Rcw0[(I+δϕ∧)tcw]×(I+δϕ∧)Rcw(I+δϕ∧)Rcw][nwvw]这里使用一条性质
(
R
a
)
×
(
R
b
)
=
R
(
a
×
b
)
,
R
∈
S
O
(
3
)
(Ra) \times (Rb)=R(a \times b),R \in SO(3)
(Ra)×(Rb)=R(a×b),R∈SO(3)，则上式
∂
L
c
∂
δ
ϕ
=
∂
[
(
I
+
δ
ϕ
∧
)
R
c
w
(
I
+
δ
ϕ
∧
)
[
t
c
w
]
×
R
c
w
0
(
I
+
δ
ϕ
∧
)
R
c
w
]
[
n
w
v
w
]
∂
δ
ϕ
=
∂
[
(
I
+
δ
ϕ
∧
)
R
c
w
n
w
+
(
I
+
δ
ϕ
∧
)
[
t
c
w
]
×
R
c
w
v
w
(
I
+
δ
ϕ
∧
)
R
c
w
v
w
]
∂
δ
ϕ
=
[
δ
ϕ
∧
R
c
w
n
w
∂
δ
ϕ
+
δ
ϕ
∧
[
t
c
w
]
×
R
c
w
v
w
∂
δ
ϕ
δ
ϕ
∧
R
c
w
v
w
∂
δ
ϕ
]
=
[
−
[
R
c
w
n
w
]
×
−
[
[
t
c
w
]
×
R
c
w
v
w
]
×
−
[
R
c
w
v
w
]
×
]
6
×
3
\begin{aligned} \frac{\partial \mathcal{L}^{c}}{\partial \delta\phi} &=\frac{\partial \begin{bmatrix} (I+\delta\phi^{\wedge})R_{cw} & (I+\delta\phi^{\wedge})\left[t_{cw}\right]_\times R_{cw} \\ 0 & (I+\delta\phi^{\wedge})R_{cw} \end{bmatrix}\begin{bmatrix}n^{w} \\v^{w}\end{bmatrix}}{\partial \delta\phi} \\ &=\frac{\partial \begin{bmatrix} (I+\delta\phi^{\wedge})R_{cw}n^{w}+(I+\delta\phi^{\wedge})\left[t_{cw}\right]_\times R_{cw}v^{w} \\ (I+\delta\phi^{\wedge})R_{cw}v^{w} \end{bmatrix}}{\partial \delta\phi} \\ &= \begin{bmatrix} \frac{\delta\phi^{\wedge}R_{cw}n^{w}}{\partial \delta\phi} + \frac{\delta\phi^{\wedge}\left[t_{cw}\right]_{\times}R_{cw}v^{w}}{\partial \delta\phi} \\ \frac{\delta\phi^{\wedge}R_{cw}v^{w}}{\partial \delta\phi} \end{bmatrix} \\ &= \begin{bmatrix} -\left[R_{cw}n^{w}\right]_{\times}- \left[\left[t_{cw}\right]_{\times}R_{cw}v^{w}\right]_{\times}\\ -\left[R_{cw}v^{w}\right]_{\times} \end{bmatrix}_{6\times3} \end{aligned}
∂δϕ∂Lc=∂δϕ∂[(I+δϕ∧)Rcw0(I+δϕ∧)[tcw]×Rcw(I+δϕ∧)Rcw][nwvw]=∂δϕ∂[(I+δϕ∧)Rcwnw+(I+δϕ∧)[tcw]×Rcwvw(I+δϕ∧)Rcwvw]=[∂δϕδϕ∧Rcwnw+∂δϕδϕ∧[tcw]×Rcwvw∂δϕδϕ∧Rcwvw]=[−[Rcwnw]×−[[tcw]×Rcwvw]×−[Rcwvw]×]6×3同样，计算
∂
L
c
∂
δ
ρ
\frac{\partial \mathcal{L}^{c}}{\partial \delta\rho}
∂δρ∂Lc的时候可以令
δ
ϕ
=
0
\delta\phi=0
δϕ=0，则
∂
L
c
∂
δ
ρ
=
∂
[
R
c
w
[
t
c
w
+
δ
ρ
]
×
R
c
w
0
R
c
w
]
[
n
w
v
w
]
∂
δ
ϕ
=
∂
[
R
c
w
n
w
+
[
t
c
w
+
δ
ρ
]
×
R
c
w
v
w
R
c
w
v
w
]
∂
δ
ρ
=
[
−
[
R
c
w
v
w
]
×
(
t
c
w
+
δ
ρ
)
∂
δ
ρ
0
]
=
[
−
[
R
c
w
v
w
]
×
0
]
6
×
3
\begin{aligned} \frac{\partial \mathcal{L}^{c}}{\partial \delta\rho} &=\frac{\partial \begin{bmatrix} R_{cw} & \left[t_{cw}+\delta\rho\right]_\times R_{cw} \\ 0 & R_{cw} \end{bmatrix}\begin{bmatrix}n^{w} \\v^{w}\end{bmatrix}}{\partial \delta\phi} \\ &=\frac{\partial \begin{bmatrix} R_{cw}n^{w}+\left[t_{cw}+\delta\rho\right]_\times R_{cw}v^{w} \\ R_{cw}v^{w} \end{bmatrix}}{\partial \delta\rho} \\ &= \begin{bmatrix} \frac{-\left[R_{cw}v^{w}\right]_{\times}(t_{cw}+\delta\rho)}{\partial \delta\rho} \\ 0 \end{bmatrix} \\ &= \begin{bmatrix} -\left[R_{cw}v^{w}\right]_{\times} \\ 0 \end{bmatrix}_{6 \times 3} \end{aligned}
∂δρ∂Lc=∂δϕ∂[Rcw0[tcw+δρ]×RcwRcw][nwvw]=∂δρ∂[Rcwnw+[tcw+δρ]×RcwvwRcwvw]=[∂δρ−[Rcwvw]×(tcw+δρ)0]=[−[Rcwvw]×0]6×3综合以上，得到雅克比矩阵第三项为
∂
L
c
∂
δ
ξ
=
[
−
[
R
c
w
n
w
]
×
−
[
[
t
c
w
]
×
R
c
w
v
w
]
×
−
[
R
c
w
v
w
]
×
−
[
R
c
w
v
w
]
×
0
]
6
×
6
\begin{aligned} \frac{\partial \mathcal{L}^{c}}{\partial \delta\xi} &= \begin{bmatrix} -\left[R_{cw}n^{w}\right]_{\times}- \left[\left[t_{cw}\right]_{\times}R_{cw}v^{w}\right]_{\times} & -\left[R_{cw}v^{w}\right]_{\times} \\ -\left[R_{cw}v^{w}\right]_{\times} & 0 \end{bmatrix}_{6 \times 6} \\ \end{aligned}
∂δξ∂Lc=[−[Rcwnw]×−[[tcw]×Rcwvw]×−[Rcwvw]×−[Rcwvw]×0]6×6综上所述，线特征重投影误差关于位姿增量的雅克比矩阵为
∂
e
l
∂
δ
ξ
=
∂
e
l
∂
l
′
∂
l
′
∂
L
c
∂
L
c
∂
δ
ξ
∂
e
l
∂
l
′
=
1
l
n
[
u
s
−
l
1
e
s
l
n
2
v
s
−
l
2
e
s
l
n
2
1
u
e
−
l
1
e
e
l
n
2
v
e
−
l
2
e
e
l
n
2
1
]
2
×
3
∂
l
′
∂
L
c
=
[
K
0
]
3
×
6
∂
L
c
∂
δ
ξ
=
[
−
[
R
c
w
n
w
]
×
−
[
[
t
c
w
]
×
R
c
w
v
w
]
×
−
[
R
c
w
v
w
]
×
−
[
R
c
w
v
w
]
×
0
]
6
×
6
\begin{aligned} \frac{\partial e_{l}}{\partial \delta\xi} &=\frac{\partial e_{l}}{\partial l^{'}} \frac{\partial l^{'}}{\partial \mathcal{L}^{c}} \frac{\partial \mathcal{L}^{c}}{\partial \delta\xi}\\ \frac{\partial e_{l}}{\partial l^{'}} &= \frac{1}{l_{n}} \begin{bmatrix}u_{s}-\frac{l_{1}e_{s}}{l_{n}^{2}}& v_{s}-\frac{l_{2}e_{s}}{l_{n}^{2}} & 1 \\ u_{e}-\frac{l_{1}e_{e}}{l_{n}^{2}} & v_{e}-\frac{l_{2}e_{e}}{l_{n}^{2}} & 1\end{bmatrix}_{2 \times 3} \\ \frac{\partial l^{'}}{\partial \mathcal{L}^{c}} &= \begin{bmatrix} \mathcal{K} & 0 \end{bmatrix}_{3 \times 6} \\ \frac{\partial \mathcal{L}^{c}}{\partial \delta\xi} &= \begin{bmatrix} -\left[R_{cw}n^{w}\right]_{\times}-\left[\left[t_{cw}\right]_{\times}R_{cw}v^{w}\right]_{\times} &-\left[R_{cw}v^{w}\right]_{\times} \\ -\left[R_{cw}v^{w}\right]_{\times} &0\end{bmatrix}_{6 \times 6} \end{aligned}
∂δξ∂el∂l′∂el∂Lc∂l′∂δξ∂Lc=∂l′∂el∂Lc∂l′∂δξ∂Lc=ln1[us−ln2l1esue−ln2l1eevs−ln2l2esve−ln2l2ee11]2×3=[K0]3×6=[−[Rcwnw]×−[[tcw]×Rcwvw]×−[Rcwvw]×−[Rcwvw]×0]6×6其中
l
n
=
l
1
2
+
l
2
2
,
e
s
=
p
s
T
l
′
,
e
e
=
p
e
T
l
′
l_{n}=\sqrt{l_{1}^{2}+l_{2}^{2}},e_{s}=p_{s}^{T}l^{'},e_{e}=p_{e}^{T}l^{'}
ln=l12+l22
,es=psTl′,ee=peTl′
可以看出，该雅克比矩阵大小为2x6
普吕克坐标属于过参数表示，在后端优化中会增加优化的计算量，所以在后端优化时可以使用另一种四参数的正交表示。
设普吕克坐标为
L
=
[
n
T
,
v
T
]
T
\mathcal{L}=[n^{T},v^{T}]^{T}
L=[nT,vT]T，其正交表示为
(
U
,
W
)
∈
S
O
(
3
)
×
S
O
(
2
)
(U,W)\in SO(3) \times SO(2)
(U,W)∈SO(3)×SO(2)通过对矩阵
[
n
∣
v
]
[n|v]
[n∣v]进行QR分解即可获得，因为
n
T
v
=
0
n^{T}v=0
nTv=0，所以其分解为
Q
R
(
[
n
∣
v
]
)
=
U
[
w
1
0
0
w
2
0
0
]
W
=
[
c
o
s
(
θ
)
−
s
i
n
(
θ
)
s
i
n
(
θ
)
c
o
s
(
θ
)
]
=
[
w
1
−
w
2
w
2
w
1
]
QR([n|v])=U\begin{bmatrix} w_{1} & 0 \\ 0 & w_{2} \\ 0 & 0\end{bmatrix} \\ W=\begin{bmatrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \end{bmatrix}=\begin{bmatrix} w_{1} & -w_{2} \\ w_{2} & w_{1} \end{bmatrix}
QR([n∣v])=U⎣
⎡w1000w20⎦
⎤W=[cos(θ)sin(θ)−sin(θ)cos(θ)]=[w1w2−w2w1]其中
w
1
,
w
2
>
0
,
θ
∈
(
0
,
π
2
]
w_{1},w_{2}>0,\theta \in (0,\frac{\pi}{2}]
w1,w2>0,θ∈(0,2π]
事实上，从普吕克坐标到正交表示可以一步写出
[
n
∣
v
]
=
[
n
∥
n
∥
v
∥
v
∥
n
×
v
∥
n
×
v
∥
]
[
∥
n
∥
0
0
∥
v
∥
0
0
]
[n|v]=\begin{bmatrix} \frac{n}{\left \| n \right \|} & \frac{v}{\left \| v \right \|} & \frac{n \times v}{\left \| n \times v \right \|} \end{bmatrix} \begin{bmatrix} \left \| n \right \| & 0 \\ 0 & \left \| v \right \| \\ 0 & 0 \end{bmatrix}
[n∣v]=[∥n∥n∥v∥v∥n×v∥n×v]⎣
⎡∥n∥000∥v∥0⎦
⎤正交表示转化成普吕克坐标
L
=
[
w
1
u
1
w
2
u
2
]
\mathcal{L}=\begin{bmatrix} w_{1}u_{1} \\ w_{2}u_{2} \end{bmatrix}
L=[w1u1w2u2]其中
u
i
u_{i}
ui表示矩阵
U
U
U的第
i
i
i 列
正交表示在后端的BA中用于优化空间直线时来作为直线的表示形式，其增量可以定义为
δ
θ
=
[
δ
θ
U
T
,
δ
θ
W
]
\delta\theta=[\delta\theta_{U}^{T},\delta\theta_{W}]
δθ=[δθUT,δθW]，其中
δ
θ
U
\delta\theta_{U}
δθU用来更新
U
U
U，是一个三维向量，
δ
θ
W
\delta\theta_{W}
δθW用来更新
W
W
W，是一个单独的参数而非向量，所以直线正交表示可以定义
(
U
∗
,
W
∗
)
=
(
U
,
W
)
⊕
δ
θ
(U^{*},W^{*})=(U,W)\oplus \delta\theta
(U∗,W∗)=(U,W)⊕δθ其中
U
∗
=
e
x
p
(
[
δ
θ
U
]
×
)
U
W
∗
=
[
c
o
s
(
δ
θ
W
)
−
s
i
n
(
δ
θ
W
)
s
i
n
(
δ
θ
W
)
c
o
s
(
δ
θ
W
)
]
W
\begin{aligned} U^{*} &= exp([\delta\theta_{U}]_{\times})U\\ W^{*}&=\begin{bmatrix}cos(\delta\theta_{W}) & -sin(\delta\theta_{W}) \\ sin(\delta\theta_{W}) & \:\:\: cos(\delta\theta_{W}) \end{bmatrix}W \end{aligned}
U∗W∗=exp([δθU]×)U=[cos(δθW)sin(δθW)−sin(δθW)cos(δθW)]W利用链式法则，可以求得
∂
e
l
∂
δ
θ
=
∂
e
l
∂
l
′
∂
l
′
∂
L
c
∂
L
c
∂
L
w
∂
L
w
∂
δ
θ
\frac{\partial e_{l}}{\partial \delta\theta}=\frac{\partial e_{l}}{\partial l^{'}} \frac{\partial l^{'}}{\partial \mathcal{L}^{c}} \frac{\partial \mathcal{L}^{c}}{\partial \mathcal{L}^{w}}\frac{\partial \mathcal{L}^{w}}{\partial \delta\theta}
∂δθ∂el=∂l′∂el∂Lc∂l′∂Lw∂Lc∂δθ∂Lw其中第一项和第二项和前一节描述的一样，第三项为
∂
L
c
∂
L
w
=
∂
H
c
w
L
w
∂
L
w
=
H
c
w
\begin{aligned} \frac{\partial \mathcal{L}^{c}}{\partial \mathcal{L}^{w}} &=\frac{\partial \mathcal{H}_{cw}\mathcal{L}^{w}}{\partial \mathcal{L}^{w}} \\ &= \mathcal{H}_{cw} \end{aligned}
∂Lw∂Lc=∂Lw∂HcwLw=Hcw第四项为
∂
L
w
∂
δ
θ
=
∂
[
w
1
u
1
w
2
u
2
]
∂
δ
θ
=
[
∂
(
w
1
u
1
)
∂
δ
θ
U
∂
(
w
1
u
1
)
∂
δ
θ
W
∂
(
w
2
u
2
)
∂
δ
θ
U
∂
(
w
2
u
2
)
∂
δ
θ
W
]
\begin{aligned} \frac{\partial \mathcal{L}^{w}}{\partial \delta\theta} &=\frac{\partial \begin{bmatrix} w_{1}u_{1} \\ w_{2}u_{2} \end{bmatrix}}{\partial \delta\theta} \\ &=\begin{bmatrix} \frac{\partial(w_{1}u_{1})}{\partial \delta\theta_{U}} & \frac{\partial(w_{1}u_{1})}{\partial \delta\theta_{W}} \\ \frac{\partial(w_{2}u_{2})}{\partial \delta\theta_{U}} & \frac{\partial(w_{2}u_{2})}{\partial \delta\theta_{W}} \end{bmatrix} \\ \end{aligned}
∂δθ∂Lw=∂δθ∂[w1u1w2u2]=[∂δθU∂(w1u1)∂δθU∂(w2u2)∂δθW∂(w1u1)∂δθW∂(w2u2)]其中
∂
(
w
1
u
1
)
∂
δ
θ
U
=
w
1
lim
δ
θ
U
→
0
e
x
p
(
[
δ
θ
U
]
×
)
u
1
−
u
1
δ
θ
U
=
w
1
lim
δ
θ
U
→
0
(
I
+
[
δ
θ
U
]
×
)
u
1
−
u
1
δ
θ
U
=
w
1
lim
δ
θ
U
→
0
[
δ
θ
U
]
×
u
1
δ
θ
U
=
w
1
lim
δ
θ
U
→
0
−
[
u
1
]
×
δ
θ
U
δ
θ
U
=
−
[
w
1
u
1
]
×
\begin{aligned} \frac{\partial(w_{1}u_{1})}{\partial \delta\theta_{U}} &= w_{1}\lim_{\delta\theta_{U} \rightarrow0} \frac{exp([\delta\theta_{U}]_{\times})u_{1}-u_{1}}{\delta\theta_{U}} \\ &= w_{1}\lim_{\delta\theta_{U} \rightarrow0} \frac{(I+[\delta\theta_{U}]_{\times})u_{1}-u_{1}}{\delta\theta_{U}} \\ &= w_{1}\lim_{\delta\theta_{U} \rightarrow0} \frac{[\delta\theta_{U}]_{\times}u_{1}}{\delta\theta_{U}} \\ &= w_{1}\lim_{\delta\theta_{U} \rightarrow0} \frac{-[u_{1}]_{\times}\delta\theta_{U}}{\delta\theta_{U}} \\ &= -[w_{1}u_{1}]_{\times} \end{aligned}
∂δθU∂(w1u1)=w1δθU→0limδθUexp([δθU]×)u1−u1=w1δθU→0limδθU(I+[δθU]×)u1−u1=w1δθU→0limδθU[δθU]×u1=w1δθU→0limδθU−[u1]×δθU=−[w1u1]×同理
∂
(
w
2
u
2
)
∂
δ
θ
U
=
−
[
w
2
u
2
]
×
\frac{\partial(w_{2}u_{2})}{\partial \delta\theta_{U}}=-[w_{2}u_{2}]_{\times}
∂δθU∂(w2u2)=−[w2u2]×
∂
(
w
1
u
1
)
∂
δ
θ
W
=
∂
w
1
∂
δ
θ
W
u
1
=
lim
δ
θ
W
→
0
w
1
∗
−
w
1
δ
θ
W
u
1
\begin{aligned} \frac{\partial(w_{1}u_{1})}{\partial \delta\theta_{W}} &=\frac{\partial w_{1}}{\partial \delta\theta_{W}}u_{1} \\ &= \lim_{\delta\theta_{W} \rightarrow0} \frac{w_{1}^{*}-w_{1}}{\delta\theta_{W}}u_{1} \\ \end{aligned}
∂δθW∂(w1u1)=∂δθW∂w1u1=δθW→0limδθWw1∗−w1u1其中
W
=
[
c
o
s
(
θ
)
−
s
i
n
(
θ
)
s
i
n
(
θ
)
c
o
s
(
θ
)
]
=
[
w
1
−
w
2
w
2
w
1
]
W
∗
=
[
c
o
s
(
δ
θ
W
)
−
s
i
n
(
δ
θ
W
)
s
i
n
(
δ
θ
W
)
c
o
s
(
δ
θ
W
)
]
[
w
1
−
w
2
w
2
w
1
]
=
[
c
o
s
(
δ
θ
W
)
w
1
−
s
i
n
(
δ
θ
W
)
w
2
−
c
o
s
(
δ
θ
W
)
w
2
−
s
i
n
(
δ
θ
W
)
w
1
c
o
s
(
δ
θ
W
)
w
2
+
s
i
n
(
δ
θ
W
)
w
1
c
o
s
(
δ
θ
W
)
w
1
−
s
i
n
(
δ
θ
W
)
w
2
]
\begin{aligned} W&=\begin{bmatrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \end{bmatrix}=\begin{bmatrix} w_{1} & -w_{2} \\ w_{2} & w_{1} \end{bmatrix} \\ W^{*}&=\begin{bmatrix}cos(\delta\theta_{W}) & -sin(\delta\theta_{W}) \\ sin(\delta\theta_{W}) & \:\:\: cos(\delta\theta_{W}) \end{bmatrix} \begin{bmatrix} w_{1} & -w_{2} \\ w_{2} & w_{1} \end{bmatrix} \\ &= \begin{bmatrix} cos(\delta\theta_{W})w_{1}-sin(\delta\theta_{W})w_{2} & -cos(\delta\theta_{W})w_{2}-sin(\delta\theta_{W})w_{1} \\ cos(\delta\theta_{W})w_{2}+sin(\delta\theta_{W})w_{1} & \:\:\:cos(\delta\theta_{W})w_{1}-sin(\delta\theta_{W})w_{2} \end{bmatrix} \end{aligned}
WW∗=[cos(θ)sin(θ)−sin(θ)cos(θ)]=[w1w2−w2w1]=[cos(δθW)sin(δθW)−sin(δθW)cos(δθW)][w1w2−w2w1]=[cos(δθW)w1−sin(δθW)w2cos(δθW)w2+sin(δθW)w1−cos(δθW)w2−sin(δθW)w1cos(δθW)w1−sin(δθW)w2]则
∂
(
w
1
u
1
)
∂
δ
θ
W
=
lim
δ
θ
W
→
0
w
1
∗
−
w
1
δ
θ
W
u
1
=
lim
δ
θ
W
→
0
c
o
s
(
δ
θ
W
)
w
1
−
s
i
n
(
δ
θ
W
)
w
2
−
w
1
δ
θ
W
u
1
=
lim
δ
θ
W
→
0
−
(
1
−
c
o
s
(
δ
θ
W
)
)
w
1
−
s
i
n
(
δ
θ
W
)
w
2
δ
θ
W
u
1
=
lim
δ
θ
W
→
0
−
δ
θ
W
2
2
w
1
−
δ
θ
W
w
2
δ
θ
W
u
1
=
lim
δ
θ
W
→
0
(
−
δ
θ
W
2
w
1
−
w
2
)
u
1
=
−
w
2
u
1
\begin{aligned} \frac{\partial(w_{1}u_{1})}{\partial \delta\theta_{W}} &= \lim_{\delta\theta_{W} \rightarrow0} \frac{w_{1}^{*}-w_{1}}{\delta\theta_{W}}u_{1} \\ &= \lim_{\delta\theta_{W} \rightarrow0} \frac{cos(\delta\theta_{W})w_{1}-sin(\delta\theta_{W})w_{2}-w_{1}}{\delta\theta_{W}}u_{1} \\ &= \lim_{\delta\theta_{W} \rightarrow0} \frac{-(1-cos(\delta\theta_{W}))w_{1}-sin(\delta\theta_{W})w_{2}}{\delta\theta_{W}}u_{1} \\ &= \lim_{\delta\theta_{W} \rightarrow0} \frac{-\frac{\delta\theta_{W}^{2}}{2}w_{1}-\delta\theta_{W}w_{2}}{\delta\theta_{W}}u_{1} \\ &= \lim_{\delta\theta_{W} \rightarrow0}(-\frac{\delta\theta_{W}}{2}w_{1}-w_{2})u_{1} \\ &= -w_{2}u_{1} \end{aligned}
∂δθW∂(w1u1)=δθW→0limδθWw1∗−w1u1=δθW→0limδθWcos(δθW)w1−sin(δθW)w2−w1u1=δθW→0limδθW−(1−cos(δθW))w1−sin(δθW)w2u1=δθW→0limδθW−2δθW2w1−δθWw2u1=δθW→0lim(−2δθWw1−w2)u1=−w2u1同理
∂
(
w
2
u
2
)
∂
δ
θ
W
=
w
1
u
2
\frac{\partial(w_{2}u_{2})}{\partial \delta\theta_{W}}=w_{1}u_{2}
∂δθW∂(w2u2)=w1u2，所以
∂
L
w
∂
δ
θ
=
[
∂
(
w
1
u
1
)
∂
δ
θ
U
∂
(
w
1
u
1
)
∂
δ
θ
W
∂
(
w
2
u
2
)
∂
δ
θ
U
∂
(
w
2
u
2
)
∂
δ
θ
W
]
=
[
−
[
w
1
u
1
]
×
−
w
2
u
1
−
[
w
2
u
2
]
×
w
1
u
2
]
6
×
4
\begin{aligned} \frac{\partial \mathcal{L}^{w}}{\partial \delta\theta} &=\begin{bmatrix} \frac{\partial(w_{1}u_{1})}{\partial \delta\theta_{U}} & \frac{\partial(w_{1}u_{1})}{\partial \delta\theta_{W}} \\ \frac{\partial(w_{2}u_{2})}{\partial \delta\theta_{U}} & \frac{\partial(w_{2}u_{2})}{\partial \delta\theta_{W}} \\ \end{bmatrix} \\ &=\begin{bmatrix} -[w_{1}u_{1}]_{\times} & -w_{2}u_{1} \\ -[w_{2}u_{2}]_{\times} & \:\:\:w_{1}u_{2} \end{bmatrix}_{6 \times 4} \end{aligned}
∂δθ∂Lw=[∂δθU∂(w1u1)∂δθU∂(w2u2)∂δθW∂(w1u1)∂δθW∂(w2u2)]=[−[w1u1]×−[w2u2]×−w2u1w1u2]6×4综上所述，线特征重投影误差关于位姿增量的雅克比矩阵为
∂
e
l
∂
δ
θ
=
∂
e
l
∂
l
′
∂
l
′
∂
L
c
∂
L
c
∂
L
w
∂
L
w
∂
δ
θ
∂
e
l
∂
l
′
=
1
l
n
[
u
s
−
l
1
e
s
l
n
2
v
s
−
l
2
e
s
l
n
2
1
u
e
−
l
1
e
e
l
n
2
v
e
−
l
2
e
e
l
n
2
1
]
2
×
3
∂
l
′
∂
L
c
=
[
K
0
]
3
×
6
∂
L
c
∂
L
w
=
H
c
w
6
×
6
∂
L
w
∂
δ
θ
=
[
−
[
w
1
u
1
]
×
−
w
2
u
1
−
[
w
2
u
2
]
×
w
1
u
2
]
6
×
4
\begin{aligned} \frac{\partial e_{l}}{\partial \delta\theta}& =\frac{\partial e_{l}}{\partial l^{'}} \frac{\partial l^{'}}{\partial \mathcal{L}^{c}} \frac{\partial \mathcal{L}^{c}}{\partial \mathcal{L}^{w}}\frac{\partial \mathcal{L}^{w}}{\partial \delta\theta} \\ \frac{\partial e_{l}}{\partial l^{'}} &= \frac{1}{l_{n}} \begin{bmatrix}u_{s}-\frac{l_{1}e_{s}}{l_{n}^{2}}& v_{s}-\frac{l_{2}e_{s}}{l_{n}^{2}} & 1 \\ u_{e}-\frac{l_{1}e_{e}}{l_{n}^{2}} & v_{e}-\frac{l_{2}e_{e}}{l_{n}^{2}} & 1\end{bmatrix}_{2 \times 3} \\ \frac{\partial l^{'}}{\partial \mathcal{L}^{c}} &= \begin{bmatrix} \mathcal{K} & 0 \end{bmatrix}_{3 \times 6} \\ \frac{\partial \mathcal{L}^{c}}{\partial \mathcal{L}^{w}} &= \mathcal{H}_{cw \: 6 \times 6} \\ \frac{\partial \mathcal{L}^{w}}{\partial \delta\theta} &= \begin{bmatrix} -\left[w_{1}u_{1}\right]_{\times} & -w_{2}u_{1} \\ -\left[w_{2}u_{2}\right]_{\times} & \:\:\:\:w_{1}u_{2} \end{bmatrix}_{6 \times 4} \end{aligned}
∂δθ∂el∂l′∂el∂Lc∂l′∂Lw∂Lc∂δθ∂Lw=∂l′∂el∂Lc∂l′∂Lw∂Lc∂δθ∂Lw=ln1[us−ln2l1esue−ln2l1eevs−ln2l2esve−ln2l2ee11]2×3=[K0]3×6=Hcw6×6=[−[w1u1]×−[w2u2]×−w2u1w1u2]6×4可以看出，该雅克比矩阵大小为2x4。其中
l
n
=
l
1
2
+
l
2
2
,
e
s
=
p
s
T
l
′
,
e
e
=
p
e
T
l
′
l_{n}=\sqrt{l_{1}^{2}+l_{2}^{2}},e_{s}=p_{s}^{T}l^{'},e_{e}=p_{e}^{T}l^{'}
ln=l12+l22
,es=psTl′,ee=peTl′，
u
1
,
u
2
u_{1},u_{2}
u1,u2为正交表示
U
U
U的前两列,
w
1
,
w
2
w_{1},w_{2}
w1,w2为
W
W
W的元素，分别来自
[
n
∣
v
]
=
[
n
∥
n
∥
v
∥
v
∥
n
×
v
∥
n
×
v
∥
]
[
∥
n
∥
0
0
∥
v
∥
0
0
]
=
U
[
w
1
0
0
w
2
0
0
]
\begin{aligned} [n|v]&=\begin{bmatrix} \frac{n}{\left \| n \right \|} & \frac{v}{\left \| v \right \|} & \frac{n \times v}{\left \| n \times v \right \|} \end{bmatrix} \begin{bmatrix} \left \| n \right \| & 0 \\ 0 & \left \| v \right \| \\ 0 & 0 \end{bmatrix} \\ &= U\begin{bmatrix} w_{1} & 0 \\ 0 & w_{2} \\ 0 & 0\end{bmatrix} \end{aligned}
[n∣v]=[∥n∥n∥v∥v∥n×v∥n×v]⎣
⎡∥n∥000∥v∥0⎦
⎤=U⎣
⎡w1000w20⎦
⎤
更新中
[1] 谢晓佳. 基于点线综合特征的双目视觉SLAM方法[D]. 浙江:浙江大学,2017.
[2] BARTOLI A, STURM P. The 3D line motion matrix and alignment of line reconstructions[J]. International Journal of Computer Vision,2004,57(3):159-178.
[3] 高翔. 视觉SLAM十四讲[M].
