自动驾驶中的SLAM技术
第2章: 基础数学知识回顾
习题 1
分别使用左右扰动模型，计算 ∂ R − 1 p ∂ R \frac{\partial \mathbf{R}^{-1}\mathbf{p}}{\partial \mathbf{R}} ∂R∂R−1p。
左扰动模型
∂
R
−
1
p
∂
R
=
lim
δ
ϕ
→
0
(
E
x
p
(
δ
ϕ
)
R
)
−
1
p
−
R
−
1
p
δ
ϕ
=
lim
δ
ϕ
→
0
R
−
1
E
x
p
(
−
δ
ϕ
)
p
−
R
−
1
p
δ
ϕ
=
lim
δ
ϕ
→
0
R
−
1
(
I
−
δ
ϕ
∧
)
p
−
R
−
1
p
δ
ϕ
=
lim
δ
ϕ
→
0
−
R
−
1
δ
ϕ
∧
p
δ
ϕ
=
lim
δ
ϕ
→
0
R
−
1
p
∧
δ
ϕ
δ
ϕ
=
R
−
1
p
∧
\begin{align*} \frac{\partial \mathbf{R}^{-1}\mathbf{p}}{\partial \mathbf{R}} &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\left( \mathrm{Exp}\left( \delta \boldsymbol{\phi} \right) \mathbf{R} \right) ^{-1}\mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathbf{R}^{-1}\mathrm{Exp}\left( -\delta \boldsymbol{\phi} \right) \mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathbf{R}^{-1}\left( \mathbf{I}-\delta \boldsymbol{\phi} ^{\land} \right) \mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{-\mathbf{R}^{-1}\delta \boldsymbol{\phi} ^{\land}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathbf{R}^{-1}\mathbf{p}^{\land}\delta \boldsymbol{\phi}}{\delta \boldsymbol{\phi}} \\ &= \mathbf{R}^{-1}\mathbf{p}^{\land} \end{align*}
∂R∂R−1p=δϕ→0limδϕ(Exp(δϕ)R)−1p−R−1p=δϕ→0limδϕR−1Exp(−δϕ)p−R−1p=δϕ→0limδϕR−1(I−δϕ∧)p−R−1p=δϕ→0limδϕ−R−1δϕ∧p=δϕ→0limδϕR−1p∧δϕ=R−1p∧
右扰动模型
∂
R
−
1
p
∂
R
=
lim
δ
ϕ
→
0
(
R
E
x
p
(
δ
ϕ
)
)
−
1
p
−
R
−
1
p
δ
ϕ
=
lim
δ
ϕ
→
0
E
x
p
(
−
δ
ϕ
)
R
−
1
p
−
R
−
1
p
δ
ϕ
=
lim
δ
ϕ
→
0
(
I
−
δ
ϕ
∧
)
R
−
1
p
−
R
−
1
p
δ
ϕ
=
lim
δ
ϕ
→
0
−
δ
ϕ
∧
R
−
1
p
δ
ϕ
=
lim
δ
ϕ
→
0
(
R
−
1
p
)
∧
δ
ϕ
δ
ϕ
=
(
R
−
1
p
)
∧
\begin{align*} \frac{\partial \mathbf{R}^{-1}\mathbf{p}}{\partial \mathbf{R}} &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\left( \mathbf{R}\mathrm{Exp}\left( \delta \boldsymbol{\phi} \right) \right) ^{-1}\mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Exp}\left( -\delta \boldsymbol{\phi} \right) \mathbf{R}^{-1}\mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\left( \mathbf{I}-\delta \boldsymbol{\phi} ^{\land} \right) \mathbf{R}^{-1}\mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{-\delta \boldsymbol{\phi} ^{\land}\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\left( \mathbf{R}^{-1}\mathbf{p} \right) ^{\land}\delta \boldsymbol{\phi}}{\delta \boldsymbol{\phi}} \\ &= \left( \mathbf{R}^{-1}\mathbf{p} \right) ^{\land} \end{align*}
∂R∂R−1p=δϕ→0limδϕ(RExp(δϕ))−1p−R−1p=δϕ→0limδϕExp(−δϕ)R−1p−R−1p=δϕ→0limδϕ(I−δϕ∧)R−1p−R−1p=δϕ→0limδϕ−δϕ∧R−1p=δϕ→0limδϕ(R−1p)∧δϕ=(R−1p)∧
习题 2
分别使用左右扰动模型，计算 ∂ R 1 R 2 − 1 ∂ R 2 \frac{\partial \mathbf{R}1\mathbf{R}{2}^{-1}}{\partial \mathbf{R}_2} ∂R2∂R1R2−1。
注: 不能直接说 R 1 R 2 \mathbf{R}_1\mathbf{R}_2 R1R2 对 R 1 \mathbf{R}_1 R1 或 R 2 \mathbf{R}_2 R2 的导数，那样就变成矩阵对向量求导，无法使用矩阵来描述。在不引入张量的前提下，我们最多只能求向量到向量的导数。因此分子部分必须加上 Log 符号，取为矢量。
左扰动模型
∂
L
o
g
(
R
1
R
2
−
1
)
∂
R
2
=
lim
ϕ
→
0
L
o
g
(
R
1
(
E
x
p
(
ϕ
)
R
2
)
−
1
)
−
L
o
g
(
R
1
R
2
−
1
)
ϕ
=
lim
ϕ
→
0
L
o
g
(
R
1
R
2
−
1
E
x
p
(
−
ϕ
)
)
−
L
o
g
(
R
1
R
2
−
1
)
ϕ
=
lim
ϕ
→
0
L
o
g
(
R
1
R
2
−
1
)
+
J
r
−
1
(
L
o
g
(
R
1
R
2
−
1
)
)
(
−
ϕ
)
−
L
o
g
(
R
1
R
2
−
1
)
ϕ
=
lim
ϕ
→
0
−
J
r
−
1
(
L
o
g
(
R
1
R
2
−
1
)
)
ϕ
ϕ
=
−
J
r
−
1
L
o
g
(
R
1
R
2
−
1
)
\begin{align*} \frac{\partial \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\partial \mathbf{R}_2} &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\left( \mathrm{Exp}\left( \boldsymbol{\phi} \right) \mathbf{R}_2 \right) ^{-1} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1}\mathrm{Exp}\left( -\boldsymbol{\phi} \right) \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) +\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \left( -\boldsymbol{\phi} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{^{-1}} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{-\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \boldsymbol{\phi}}{\boldsymbol{\phi}} \\ &= -\mathbf{J}_{r}^{-1}\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \end{align*}
∂R2∂Log(R1R2−1)=ϕ→0limϕLog(R1(Exp(ϕ)R2)−1)−Log(R1R2−1)=ϕ→0limϕLog(R1R2−1Exp(−ϕ))−Log(R1R2−1)=ϕ→0limϕLog(R1R2−1)+Jr−1(Log(R1R2−1))(−ϕ)−Log(R1R2−1)=ϕ→0limϕ−Jr−1(Log(R1R2−1))ϕ=−Jr−1Log(R1R2−1)
右扰动模型
∂
L
o
g
(
R
1
R
2
−
1
)
∂
R
2
=
lim
ϕ
→
0
L
o
g
(
R
1
(
R
2
E
x
p
(
ϕ
)
)
−
1
)
−
L
o
g
(
R
1
R
2
−
1
)
ϕ
=
lim
ϕ
→
0
L
o
g
(
R
1
E
x
p
(
−
ϕ
)
R
2
−
1
)
−
L
o
g
(
R
1
R
2
−
1
)
ϕ
=
lim
ϕ
→
0
L
o
g
(
R
1
R
2
−
1
E
x
p
(
−
R
2
ϕ
)
)
−
L
o
g
(
R
1
R
2
−
1
)
ϕ
=
lim
ϕ
→
0
L
o
g
(
R
1
R
2
−
1
)
+
J
r
−
1
(
L
o
g
(
R
1
R
2
−
1
)
)
(
−
R
2
ϕ
)
−
L
o
g
(
R
1
R
2
−
1
)
ϕ
=
lim
ϕ
→
0
−
J
r
−
1
(
L
o
g
(
R
1
R
2
−
1
)
)
R
2
ϕ
ϕ
=
−
J
r
−
1
(
L
o
g
(
R
1
R
2
−
1
)
)
R
2
\begin{align*} \frac{\partial \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\partial \mathbf{R}_2} &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\left( \mathbf{R}_2\mathrm{Exp}\left( \boldsymbol{\phi} \right) \right) ^{-1} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathrm{Exp}\left( -\boldsymbol{\phi} \right) \mathbf{R}_{2}^{-1} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1}\mathrm{Exp}\left( -\mathbf{R}_2\boldsymbol{\phi} \right) \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) +\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \left( -\mathbf{R}_2\boldsymbol{\phi} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{-\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \mathbf{R}_2\boldsymbol{\phi}}{\boldsymbol{\phi}} \\ &= -\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \mathbf{R}_2 \end{align*}
∂R2∂Log(R1R2−1)=ϕ→0limϕLog(R1(R2Exp(ϕ))−1)−Log(R1R2−1)=ϕ→0limϕLog(R1Exp(−ϕ)R2−1)−Log(R1R2−1)=ϕ→0limϕLog(R1R2−1Exp(−R2ϕ))−Log(R1R2−1)=ϕ→0limϕLog(R1R2−1)+Jr−1(Log(R1R2−1))(−R2ϕ)−Log(R1R2−1)=ϕ→0limϕ−Jr−1(Log(R1R2−1))R2ϕ=−Jr−1(Log(R1R2−1))R2
注:
L o g ( A B ) ≈ { L o g ( B ) + J l − 1 ( L o g ( B ) ) L o g ( A ) , 当 A ≈ I L o g ( A ) + J r − 1 ( L o g ( A ) ) L o g ( B ) , 当 B ≈ I \mathrm{Log}\left( \mathbf{A}\mathbf{B} \right) \approx \begin{cases} \mathrm{Log}\left( \mathbf{B} \right) +\mathbf{J}_{l}^{-1}\left( \mathrm{Log}\left( \mathbf{B} \right) \right) \mathrm{Log}\left( \mathbf{A} \right) ,\text{当}\mathbf{A}\approx \mathbf{I} \\ \mathrm{Log}\left( \mathbf{A} \right) +\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{A} \right) \right) \mathrm{Log}\left( \mathbf{B} \right) ,\text{当}\mathbf{B}\approx \mathbf{I} \end{cases} Log(AB)≈{Log(B)+Jl−1(Log(B))Log(A),当A≈ILog(A)+Jr−1(Log(A))Log(B),当B≈I
E x p ( ϕ ) R = R E x p ( R ⊤ ϕ ) \mathrm{Exp}\left( \boldsymbol{\phi } \right) \boldsymbol{R}=\boldsymbol{R}\mathrm{Exp}\left( \boldsymbol{R}^{\top}\boldsymbol{\phi } \right) Exp(ϕ)R=RExp(R⊤ϕ)
