本文主要补充slam十四讲中关于ba推导有疑问的地方。
设输入位姿为世界坐标系在当前cam坐标系下的位姿，表示为
T
w
c
T^c_w
Twc。
输入的点为当前点在世界坐标系下的位置，表示为
P
w
P^w
Pw,点在cam坐标系下表示为
P
c
P^c
Pc
得到点在cam坐标系下的位姿
P
c
＝
R
w
c
∗
P
w
+
t
w
c
(
１
)
P^c＝R^c_w*P^w+t^c_w \quad (１)
Pc＝Rwc∗Pw+twc (１)
ba的残差雅克比求导需要对于位置求导，链式法则中这一项求导是关键
设ba的残差为
e
=
u
−
1
s
K
e
x
p
(
ς
)
P
w
e = u-\frac{1}{s} Kexp( \varsigma )P^w
e=u−s1Kexp(ς)Pw
由链式法则可得
∂
e
∂
δ
ς
=
∂
e
∂
P
c
∂
P
c
∂
δ
ς
\frac{\partial e}{\partial \delta \varsigma }=\frac{\partial e}{\partial P^c} \frac{\partial P^c}{\partial \delta \varsigma }
∂δς∂e=∂Pc∂e∂δς∂Pc
前面一项分别求导就行了，不展开推导
这里写成了李代数扰动的形式，后面一项就是公式（１）的求导，可以对旋转和平移分开来求导，旋转和平移用李代数表示为 [ ϕ , ρ ] [\phi,\rho] [ϕ,ρ],扰动表示为 [ δ ϕ , δ ρ ] [\delta\phi,\delta\rho] [δϕ,δρ]，也可以写成直接对变量求导。注意，下面公式中的反对称符号。因为旋转矩阵和另外一个反对称矩阵通过指数关系发生了联系。所以旋转写成指数形式后要加这个反对称符号。
中间用到了泰勒展开
e
x
p
(
ϕ
Λ
)
＝
１
＋
δ
ϕ
Λ
exp({ \phi}^{\Lambda})＝１＋{ \delta \phi}^{\Lambda}
exp(ϕΛ)＝１＋δϕΛ
还用到了叉乘的性质负交换律
a
×
b
=
a
Λ
b
=
−
b
Λ
a
a\times b=a^{\Lambda}b=-b^{\Lambda}a
a×b=aΛb=−bΛa
证明方法为利用叉乘表示平行四边形的有向面积参考
https://jingyan.baidu.com/article/414eccf62f611e2b431f0ad7.html
∂
P
c
∂
R
w
c
=
∂
(
R
w
c
∗
P
w
+
t
w
c
)
∂
R
w
c
＝
lim
δ
ϕ
→
0
(
e
x
p
(
δ
ϕ
Λ
)
e
x
p
(
ϕ
Λ
)
P
w
+
t
w
c
)
−
e
x
p
(
ϕ
Λ
)
P
w
+
t
w
c
)
δ
ϕ
=
lim
δ
ϕ
→
0
e
x
p
(
δ
ϕ
Λ
)
e
x
p
(
ϕ
Λ
)
P
w
−
e
x
p
(
ϕ
Λ
P
w
)
δ
ϕ
=
lim
δ
ϕ
→
0
(
1
+
δ
ϕ
Λ
)
e
x
p
(
ϕ
Λ
)
P
w
−
e
x
p
(
ϕ
Λ
P
w
)
δ
ϕ
=
lim
δ
ϕ
→
0
δ
ϕ
Λ
e
x
p
(
ϕ
Λ
)
P
w
δ
ϕ
=
lim
δ
ϕ
→
0
−
(
R
w
c
P
w
)
Λ
δ
ϕ
δ
ϕ
=
−
(
R
w
c
P
w
)
Λ
\frac{\partial{ P^c}}{\partial{R^c_w}}=\frac{\partial{ (R^c_w*P^w+t^c_w)}}{\partial{R^c_w}}＝ \lim_{\delta \phi \to 0}\frac{(exp({\delta \phi}^{\Lambda})exp({ \phi}^{\Lambda})P^w+t^c_w)-exp({ \phi}^{\Lambda})P^w+t^c_w)}{{\delta \phi}}\\ =\lim_{\delta \phi \to 0}\frac{exp({\delta \phi}^{\Lambda})exp({ \phi}^{\Lambda})P^w-exp({ \phi}^{\Lambda}P^w)}{{\delta \phi}}\\ =\lim_{\delta \phi \to 0}\frac{(1+{\delta \phi}^{\Lambda})exp({ \phi}^{\Lambda})P^w-exp({ \phi}^{\Lambda}P^w)}{{\delta \phi}}\\ =\lim_{\delta \phi \to 0}\frac{{\delta \phi}^{\Lambda}exp({ \phi}^{\Lambda})P^w}{{\delta \phi}}\\ =\lim_{\delta \phi \to 0}-\frac{(R^c_wP^w)^{\Lambda}{\delta \phi}}{{\delta \phi}}\\ =-(R^c_wP^w)^{\Lambda}
∂Rwc∂Pc=∂Rwc∂(Rwc∗Pw+twc)＝δϕ→0limδϕ(exp(δϕΛ)exp(ϕΛ)Pw+twc)−exp(ϕΛ)Pw+twc)=δϕ→0limδϕexp(δϕΛ)exp(ϕΛ)Pw−exp(ϕΛPw)=δϕ→0limδϕ(1+δϕΛ)exp(ϕΛ)Pw−exp(ϕΛPw)=δϕ→0limδϕδϕΛexp(ϕΛ)Pw=δϕ→0lim−δϕ(RwcPw)Λδϕ=−(RwcPw)Λ
平移求导
∂
P
c
∂
t
w
c
=
∂
(
R
w
c
∗
P
w
+
t
w
c
)
∂
t
w
c
＝
I
\frac{\partial{ P^c}}{\partial{t^c_w}}=\frac{\partial{ (R^c_w*P^w+t^c_w)}}{\partial{t^c_w}}＝ I
∂twc∂Pc=∂twc∂(Rwc∗Pw+twc)＝I