g2o之SE3-SE3约束
1. g2o之SE3-SE3约束
假设位姿图中某两帧的位姿为
T
i
T_i
Ti和
T
j
T_j
Tj，且由两帧之间的测量约束为
T
i
j
T_{ij}
Tij，则有下式：
Δ
T
=
T
i
j
−
1
T
i
−
1
T
j
(1.1)
\Delta T=T_{ij}^{-1}T_i^{-1}T_j \tag{1.1}
ΔT=Tij−1Ti−1Tj(1.1)
利用对数映射，可得到该约束对应的残差为：
e
i
j
=
ln
(
Δ
T
)
∨
=
ln
(
T
i
j
−
1
T
i
−
1
T
j
)
∨
∈
R
6
(1.2)
e_{ij}=\ln(\Delta T)^{\vee}=\ln(T_{ij}^{-1}T_i^{-1}T_j)^{\vee}\in R^6 \tag{1.2}
eij=ln(ΔT)∨=ln(Tij−1Ti−1Tj)∨∈R6(1.2)
在利用Gauss-Newton或Levenberg-Marquat算法求解非线性最小二乘问题时，需要求解残差向量
e
i
j
e_{ij}
eij相对于SE节点
T
i
T_i
Ti和
T
j
T_j
Tj的Jacobian。为求解Jacobian，给
T
i
T_i
Ti添加左扰动
exp
(
ξ
i
∧
)
\exp(\xi_i^{\wedge})
exp(ξi∧)，带入公式1.2则有：
Δ
T
(
ξ
i
)
=
T
i
j
−
1
T
i
−
1
exp
(
−
ξ
i
∧
)
T
j
(1.3)
\Delta T(\xi_i)=T_{ij}^{-1}T_i^{-1}\exp(-\xi_i^{\wedge})T_j \tag{1.3}
ΔT(ξi)=Tij−1Ti−1exp(−ξi∧)Tj(1.3)
e
i
j
(
ξ
i
)
=
ln
(
T
i
j
−
1
T
i
−
1
exp
(
−
ξ
i
∧
)
T
j
)
∨
(1.4)
e_{ij}(\xi_i)=\ln(T_{ij}^{-1}T_i^{-1}\exp(-\xi_i^{\wedge})T_j)^{\vee} \tag{1.4}
eij(ξi)=ln(Tij−1Ti−1exp(−ξi∧)Tj)∨(1.4)
则残差
e
i
j
e_{ij}
eij相对于
T
i
T_i
Ti的Jacobian可表示为：
∂
e
i
j
∂
T
i
=
∂
e
i
j
(
ξ
i
)
∂
ξ
i
∣
ξ
i
=
0
(1.5)
\frac{\partial e_{ij}}{\partial T_i}=\frac{\partial e_{ij}(\xi_i)}{\partial \xi_i}_{\mid \xi_i=0} \tag{1.5}
∂Ti∂eij=∂ξi∂eij(ξi)∣ξi=0(1.5)
利用如公式1.6所示的伴随矩阵的性质，可以将公式1.5化简为公式1.7:
A
exp
(
b
∧
)
=
exp
(
A
a
d
j
b
∧
)
A
(1.6)
A\exp(b^\wedge)=\exp(A_{adj}b^\wedge)A \tag{1.6}
Aexp(b∧)=exp(Aadjb∧)A(1.6)
e
i
j
(
ξ
i
)
=
ln
(
exp
(
−
(
(
T
i
j
−
1
T
i
−
1
)
a
d
j
ξ
i
∧
)
)
T
i
j
−
1
T
i
−
1
T
j
)
∨
(1.7)
e_{ij}(\xi_i)=\ln(\exp(-((T_{ij}^{-1}T_i^{-1})_{adj}\xi_i^\wedge))T_{ij}^{-1}T_i^{-1}T_j)^{\vee} \tag{1.7}
eij(ξi)=ln(exp(−((Tij−1Ti−1)adjξi∧))Tij−1Ti−1Tj)∨(1.7)
将公式1.2带入公式1.7，可进一步化简为：
e
i
j
(
ξ
i
)
=
ln
(
exp
(
−
(
(
T
i
j
−
1
T
i
−
1
)
a
d
j
ξ
i
∧
)
)
exp
(
e
i
j
∧
)
)
∨
(1.8)
e_{ij}(\xi_i)=\ln(\exp(-((T_{ij}^{-1}T_i^{-1})_{adj}\xi_i^\wedge))\exp(e_{ij}^{\wedge}))^\vee \tag{1.8}
eij(ξi)=ln(exp(−((Tij−1Ti−1)adjξi∧))exp(eij∧))∨(1.8)
为进一步化简上式，回顾一下BCH公式：
exp
(
a
∧
+
b
∧
)
≈
exp
(
J
l
(
b
)
a
∧
)
e
x
p
(
b
∧
)
(1.9)
\exp(a^\wedge+b^\wedge)\approx\exp(J_l(b)a^\wedge)exp(b^\wedge) \tag{1.9}
exp(a∧+b∧)≈exp(Jl(b)a∧)exp(b∧)(1.9)
式中，a为无穷小量，
J
l
J_l
Jl为SE(3)的左Jacobian。则也有：
exp
(
a
∧
)
exp
(
b
∧
)
≈
exp
(
J
l
−
1
(
b
)
a
∧
+
b
∧
)
(1.10)
\exp(a^\wedge)\exp(b^\wedge)\approx\exp(J_l^{-1}(b)a^\wedge+b^\wedge) \tag{1.10}
exp(a∧)exp(b∧)≈exp(Jl−1(b)a∧+b∧)(1.10)
利用公式1.10，可以将公式1.8化简为：
e
i
j
(
ξ
i
)
≈
ln
(
exp
(
(
e
i
j
−
J
l
−
1
(
e
i
j
)
(
T
i
j
−
1
T
i
−
1
)
a
d
j
ξ
i
)
∧
)
)
∨
(1.11)
e_{ij}(\xi_i)\approx\ln(\exp((e_{ij}-J_l^{-1}(e_{ij})(T_{ij}^{-1}T_i^{-1})_{adj}\xi_i)^\wedge))^\vee \tag{1.11}
eij(ξi)≈ln(exp((eij−Jl−1(eij)(Tij−1Ti−1)adjξi)∧))∨(1.11)
进一步化简为：
e
i
j
(
ξ
i
)
=
e
i
j
−
J
l
(
e
i
j
)
−
1
(
T
i
j
−
1
T
i
−
1
)
a
d
j
ξ
i
(1.12)
e_{ij}(\xi_i)=e_{ij}-J_{l(e_{ij})}^{-1}(T_{ij}^{-1}T_i^{-1})_{adj}\xi_i \tag{1.12}
eij(ξi)=eij−Jl(eij)−1(Tij−1Ti−1)adjξi(1.12)
则
e
i
j
e_{ij}
eij相对于
T
i
T_i
Ti的Jacobian等于：
∂
e
i
j
∂
T
i
=
∂
e
i
j
(
ξ
i
)
∂
ξ
i
∣
ξ
i
=
0
=
−
J
l
(
e
i
j
)
−
1
(
T
i
j
−
1
T
i
−
1
)
a
d
j
(1.13)
\frac{\partial e_{ij}}{\partial T_i}=\frac{\partial e_{ij}(\xi_i)}{\partial \xi_i}_{\mid \xi_i=0}=-J_{l(e_{ij})}^{-1}(T_{ij}^{-1}T_i^{-1})_{adj} \tag{1.13}
∂Ti∂eij=∂ξi∂eij(ξi)∣ξi=0=−Jl(eij)−1(Tij−1Ti−1)adj(1.13)