SLAM中求导相关的公式总结

求导相关的公式总结"/>

SLAM中求导相关的公式总结

李代数与李群的关系
R ˙ R T \dot{R}R^{T} R˙RT 是一个反对称矩阵，所以这个矩阵可以用一个1×3向量进行反对称来表示
R ˙ R T = Φ ^ \dot{R}R^{T}=Φ^{\hat{}} R˙RT=Φ^ ，
根据十四讲 4.8 的推导，最后则有 R ( t ) ˙ = Φ ^ ⋅ R ( t ) \dot{R(t)}=Φ^{\hat{}}·R(t) R(t)˙​=Φ^⋅R(t)
这个李代数 Φ Φ Φ 反映了 R R R 的导数性质，所以李代数是李群 S O ( 3 ) SO(3) SO(3) 的正切空间，因为这里李群是9维的，所以切向量也是一个空间

李代数由一个方向和夹角构成， Φ = θ a ， ∣ ∣ a ⃗ ∣ ∣ = 1 Φ=θa，||\vec{a}||=1 Φ=θa，∣∣a ∣∣=1
指数映射，也是罗德里格斯公式
e x p ( Φ ∧ ) = e x p ( θ a ∧ ) = c o s θ I + ( 1 − c o s θ ) a a T + s i n θ a ∧ = R exp(Φ^{\wedge})=exp(θa^{\wedge})=cosθI+(1-cosθ)aa^{T}+sinθa^{\wedge}=R exp(Φ∧)=exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧=R
e x p ( Φ ∧ ) = R exp(Φ^{\wedge})=R exp(Φ∧)=R
意思就是李代数 Φ Φ Φ 可以由角度 θ θ θ 和方向向量 a ⃗ \vec{a} a 表示，通过罗德里格斯公式可以变换成对应的旋转矩阵 S O ( 3 ) SO(3) SO(3)

用李代数表示旋转会有个问题，就是周期性，就是多个李代数可以对应一个旋转矩阵，如果固定旋转角度在±π时就是唯一对应的

由旋转矩阵 R R R 求李代数 Φ Φ Φ ， θ = a r c c o s t r ( R ) − 1 2 , R a = a θ=arccos\frac{tr(R)-1}{2},Ra=a θ=arccos2tr(R)−1​,Ra=a，由 l n ( R ) ∨ = Φ ln(R)^{\vee}=Φ ln(R)∨=Φ 表示

BCH一阶线性近似表达
l n ( e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) ) ∨ ln(exp(\phi^{\wedge}_{1})exp(\phi^{\wedge}_{2}))^{\vee} ln(exp(ϕ1∧​)exp(ϕ2∧​))∨

当左边的 ϕ 1 \phi_{1} ϕ1​ 为小量时，此时相当于左乘，则
= > J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 =>J_{l}(\phi_{2})^{-1}\phi_{1}+\phi_{2} =>Jl​(ϕ2​)−1ϕ1​+ϕ2​
其实就是相当于在 ϕ 2 \phi_{2} ϕ2​ 的基础上加上微量 J l ( ϕ 2 ) − 1 ϕ 1 J_{l}(\phi_{2})^{-1}\phi_{1} Jl​(ϕ2​)−1ϕ1​

当右边的 ϕ 2 \phi_{2} ϕ2​ 为小量时，此时相当于右乘，则
= > J l ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 =>J_{l}(\phi_{1})^{-1}\phi_{2}+\phi_{1} =>Jl​(ϕ1​)−1ϕ2​+ϕ1​

SLAM中我们构建了与位姿相关的函数后，需要讨论该函数对位姿的求导，以估计当前值
有两种方法求导
1、用李代数表示位姿，根据李代数加法对李代数求导
2、对李群进行左乘或右乘进行扰动，对扰动求导

由于使用李代数求导要计算 雅可比 J J J ，这个形式比较复杂，工程中不用这个方法，都是用扰动模型，所以只看扰动模型
∂ ( R p ) ∂ φ = l i m φ → 0 e x p ( φ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p φ \frac{\partial(Rp)}{\partial\varphi}=lim_{\varphi\rightarrow0}\frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi} ∂φ∂(Rp)​=limφ→0​φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p​
e x p ( φ ∧ ) exp(\varphi^{\wedge}) exp(φ∧) 是微量，相当于是对旋转的导数，则等于 ( I + φ ∧ ) (I+\varphi^{\wedge}) (I+φ∧)
具体推导如下
左乘扰动求导
这里是旋转 R R R 对向量 p p p 进行旋转，不停地左乘扰动来改变向量 p p p 的方向，左扰动 Δ R \Delta{R} ΔR 对应的李代数 φ \varphi φ ， R p Rp Rp 的结果也是向量

∂ ( R p ) ∂ φ = l i m φ → 0 e x p ( φ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p φ \frac{\partial(Rp)}{\partial\varphi}=lim_{\varphi\rightarrow0}\frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi} ∂φ∂(Rp)​=limφ→0​φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p​
= l i m φ → 0 ( I + φ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p φ =lim_{\varphi\rightarrow0}\frac{(I+\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi} =limφ→0​φ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p​
乘进去相减，很明显
= l i m φ → 0 φ ∧ R p φ =lim_{\varphi\rightarrow0}\frac{\varphi^{\wedge}Rp}{\varphi} =limφ→0​φφ∧Rp​

叉乘有一个性质， a ⃗ × b ⃗ = − b ⃗ × a ⃗ \vec{a}×\vec{b}=-\vec{b}×\vec{a} a ×b =−b ×a
a ⃗ × b ⃗ = a ∧ ⋅ b \vec{a}×\vec{b}=a^{\wedge}·b a ×b =a∧⋅b
− b ⃗ × a ⃗ = − b ∧ ⋅ a -\vec{b}×\vec{a}=-b^{\wedge}·a −b ×a =−b∧⋅a
a ∧ ⋅ b = − b ∧ ⋅ a a^{\wedge}·b=-b^{\wedge}·a a∧⋅b=−b∧⋅a

则上式等于
= l i m φ → 0 − ( R p ) ∧ φ φ =lim_{\varphi\rightarrow0}\frac{-(Rp)^{\wedge}\varphi}{\varphi} =limφ→0​φ−(Rp)∧φ​
约掉 φ \varphi φ 则 l i m φ → 0 = − ( R p ) ∧ lim_{\varphi\rightarrow0}=-(Rp)^{\wedge} limφ→0​=−(Rp)∧
这里省去了雅可比 J J J 的计算
对右乘也是一样的方法

矩阵转置的性质
( A + B ) T = A T + B T (A+B)^{T}=A^{T}+B^{T} (A+B)T=AT+BT
( λ A ) T = λ A T (\lambda{A})^{T}=\lambda{A^{T}} (λA)T=λAT
( A B ) T = B T A T (AB)^{T}=B^{T}A^{T} (AB)T=BTAT

SO(3)的伴随性质
R T E x p ( ϕ ) R = E x p ( R T ϕ ) R^{T}Exp(\phi)R=Exp(R^{T}\phi) RTExp(ϕ)R=Exp(RTϕ)
ϕ \phi ϕ 为扰动量 Δ R \Delta{R} ΔR 对应的李代数

对复合旋转进行求导

∂ L o g ( R 1 R 2 ) ∂ R 1 \frac{\partial Log(R_{1}R_{2})}{\partial R_{1}} ∂R1​∂Log(R1​R2​)​

这里是对两个相乘的旋转矩阵中的其中一个旋转进行求导，上面的左乘扰动例子是对矩阵相乘向量进行求导的，所以可以直接对矩阵进行扰动

但是这里不能直接对矩阵 S O ( 3 ) SO(3) SO(3) 进行扰动，不能直接说 R 1 R 2 R_{1}R_{2} R1​R2​ 对 R 1 R_{1} R1​ 或 R 2 R_{2} R2​ 的导数，这样就变成矩阵对向量的求导，因为扰动量是可以用向量表示的，前面的例子 R p Rp Rp 相乘后也是个向量，所以可以直接对扰动量进行求导，但是这里两个矩阵相乘还是矩阵，所以得用 L o g Log Log 将矩阵相乘结果变为李代数，这样才符合求导的定义
L o g ( R ) = l o g ( R ) ∨ Log(R)=log(R)^{\vee} Log(R)=log(R)∨，用 L o g Log Log 就是为了懒得写 ∨ \vee ∨

对 R 1 R_{1} R1​ 进行右扰动
∂ L o g ( R 1 R 2 ) ∂ R 1 = l i m φ → 0 L o g ( R 1 E x p ( ϕ ) R 2 ) − L o g ( R 1 R 2 ) ϕ \frac{\partial Log(R_{1}R_{2})}{\partial R_{1}}=lim_{\varphi\rightarrow0}\frac{Log(R_{1}Exp(\phi)R_{2})-Log(R_{1}R_{2})}{\phi} ∂R1​∂Log(R1​R2​)​=limφ→0​ϕLog(R1​Exp(ϕ)R2​)−Log(R1​R2​)​
推导明天再写了