《视觉SLAM十四讲》学习笔记-李代数求导与扰动模型的一些重要公式

最新推荐文章于 2024-03-15 15:10:19 发布

teddyluo

最新推荐文章于 2024-03-15 15:10:19 发布

阅读量4.8k

点赞数 1

分类专栏： slam

slam 专栏收录该内容

20 篇文章 17 订阅

订阅专栏

BCH(Baker-Campbell-Hausdorff)公式：
$ln (exp (A) exp (B)) = A + B + 1 2 [A, B] + 1 12 [A, [A, B]] - 1 12 [B, [A, B]] + \dots$ $\ln (\exp (\mathbf{A}) \exp (\mathbf{B}) ) = \mathbf{A} + \mathbf{B} + \frac{1}{2}[\mathbf{A}, \mathbf{B}] + \frac{1}{12}[\mathbf{A}, [\mathbf{A}, \mathbf{B}] ] - \frac{1}{12}[\mathbf{B}, [\mathbf{A}, \mathbf{B}] ] + \cdots$

当为SO(3)上的李代数 $\ln (\exp (\Phi_1^\wedge) \exp (\Phi_2^\wedge) )^\vee$ 时，BCH变为：

ln (exp (Φ \land 1) exp (Φ \land 2)) \lor \approx {J l (Φ 2) - 1 Φ 1 + Φ 2 J r (Φ 1) - 1 Φ 2 + Φ 1 i f Φ 1 i s s m a l l, i f Φ 2 i s s m a l l .

$\ln (\exp (\Phi_1^\wedge) \exp (\Phi_2^\wedge) )^\vee \approx \begin{cases} \mathbf{J}_l(\Phi_2)^{-1}\Phi_1 + \Phi_2 & if~\Phi_1 ~is ~small,\\ \mathbf{J}_r(\Phi_1)^{-1}\Phi_2 + \Phi_1 & if~\Phi_2 ~is ~small. \end{cases}$
这其中，左乘BCH近似的Jacobi

Jl J l $\mathbf{J}_l$ 为：

J l = J = sin θ θ I + (1 - sin θ θ) a ⃗ a ⃗ ⊤ + 1 - cos θ θ a ⃗ \land

$\mathbf{J}_l = \mathbf{J} = \frac{\sin \theta}{\theta}\mathbf{I} + \left( 1-\frac{\sin\theta}{\theta} \right)\vec{a}\vec{a}^\top + \frac{1-\cos\theta}{\theta}\vec{a}^\wedge$

Jl J l $\mathbf{J}_l$ 的逆为

J - 1 l = θ 2 cot θ 2 I + (1 - θ 2 cot θ 2) a ⃗ a ⃗ ⊤ - θ 2 a ⃗ \land

$\mathbf{J}_l^{-1} = \frac{\theta}{2}\cot\frac{\theta}{2}\mathbf{I} + \left( 1-\frac{\theta}{2}\cot \frac{\theta}{2} \right)\vec{a}\vec{a}^\top -\frac{\theta}{2}\vec{a}^\wedge$
另外，右乘Jacobi

Jr J r $\mathbf{J}_r$ 可对自变量取负号即可得到：

J r (Φ) = J l (- Φ)

$\mathbf{J}_r(\Phi) = \mathbf{J}_l(-\Phi)$

BCH近似的意义. 旋转 $\mathbf{R}$ 对应的李代数为 $\Phi$ , 左乘微小旋转 $\triangle\mathbf{R}$ 所对应的李代数为 $\triangle\Phi$ .
则李群上的结果为 $\triangle\mathbf{R}\cdot\mathbf{R}$ ,李代数的结果根据BCH变为 $\mathbf{J}_l^{-1}(\Phi)\triangle\Phi + \Phi$ :

$exp (△ Φ \land) exp (Φ \land) = exp ((Φ + J - 1 l (Φ) △ Φ) \land)$ $\exp(\triangle\Phi^\wedge)\exp(\Phi^\wedge) = \exp\left( (\Phi+ \mathbf{J}_l^{-1}(\Phi)\triangle\Phi)^\wedge \right)$
若在李代数上做加法， $\Phi\rightarrow \Phi + \triangle \Phi$ :
$exp ((Φ + △ Φ) \land) = exp ((J l △ Φ) \land) exp (Φ \land) = exp (Φ \land) exp ((J r △ Φ) \land)$ $\exp \left( (\Phi + \triangle\Phi)^\wedge \right) = \exp\left( (\mathbf{J}_l\triangle\Phi)^\wedge\right)\exp(\Phi^\wedge) = \exp(\Phi^\wedge)\exp\left( (\mathbf{J}_r\triangle\Phi)^\wedge \right)$
SO(3)李代数的求导。两种思路：
- [x] 用李代数表示姿态，根据李代数加法对李代数求导；
- [x] 对李群左乘或右乘微小的扰动，对该扰动求导.

3.1 问题：对一个空间点 $\vec{p}$ 进行旋转，得到 $\mathbf{R}\vec{p}$ ,求旋转后的点相对于旋转的导数，即 $\frac{\partial\mathbf{R}\vec{p}}{\partial\mathbf{R}}$ .
设 $\mathbf{R}$ 对应的李代数为 $\Phi$ , 则问题表述为:

\partial ( exp ( Φ \land ) p ⃗ ) \partial Φ

$\frac{\partial(\exp(\Phi^\wedge)\vec{p})}{\partial \Phi}$

\partial ( exp ( Φ \land ) p ⃗ ) \partial Φ = lim δ Φ \to 0 exp ( ( Φ + δ Φ ) \land ) p ⃗ - exp ( Φ \land ) p ⃗ δ Φ = lim δ Φ \to 0 exp ( ( J l δ Φ ) \land ) exp ( Φ \land ) p ⃗ - exp ( Φ \land ) p ⃗ δ Φ (BCH linear approximation) \approx lim δ Φ \to 0 I + ( ( J l δ Φ ) \land ) exp ( Φ \land ) p ⃗ - exp ( Φ \land ) p ⃗ δ Φ (Taylor's formula) = lim δ Φ \to 0 ( J l δ Φ ) \land exp ( Φ \land ) p ⃗ δ Φ = lim δ Φ \to 0 - ( exp ( Φ \land ) p ⃗ ) \land J l δ Φ δ Φ (cross product) = - (R p ⃗) \land J l .

$\begin{aligned} \frac{\partial(\exp(\Phi^\wedge)\vec{p})}{\partial \Phi} &= \lim\limits_{\delta\Phi\rightarrow 0} \frac{\exp \left( (\Phi+\delta\Phi)^\wedge\right)\vec{p} -\exp(\Phi^\wedge)\vec{p}}{\delta\Phi}\\ &= \lim\limits_{\delta\Phi\rightarrow 0} \frac{ \exp \left( (\mathbf{J}_l\delta\Phi)^\wedge\right)\exp(\Phi^\wedge)\vec{p} -\exp(\Phi^\wedge)\vec{p}}{\delta\Phi} ~~(\text{BCH linear approximation})\\ &\approx \lim\limits_{\delta\Phi\rightarrow 0} \frac{ \mathbf{I} + \left( (\mathbf{J}_l\delta\Phi)^\wedge\right)\exp(\Phi^\wedge)\vec{p} -\exp(\Phi^\wedge)\vec{p}}{\delta\Phi}~~(\text{Taylor's formula}) \\ &= \lim\limits_{\delta\Phi\rightarrow 0} \frac{ (\mathbf{J}_l\delta\Phi)^\wedge \exp(\Phi^\wedge)\vec{p} }{\delta\Phi} \\ &= \lim\limits_{\delta\Phi\rightarrow 0} \frac{ - (\exp(\Phi^\wedge)\vec{p})^\wedge \mathbf{J}_l\delta\Phi }{\delta\Phi}~~(\text{cross product}) \\ &= -(\mathbf{R}\vec{p})^\wedge\mathbf{J}_l. \end{aligned}$

所以旋转后的点相对于李代数的导数为：

\partial ( R p ⃗ ) \partial Φ = (- R p ⃗) \land J l

$\frac{\partial(\mathbf{R}\vec{p})}{\partial\Phi} = (-\mathbf{R}\vec{p})^\wedge\mathbf{J}_l$

3.2 扰动模型的求导公式推导

问题：对 $\mathbf{R}$ 的一次扰动量为 $\triangle\mathbf{R}$ 。设左扰动 $\triangle\mathbf{R}$ 对应的李代数为 $\varphi$ ，对 $\varphi$ 求导即为问题解：

\partial R p ⃗ \partial φ = lim φ \to 0 exp ( φ \land ) exp ( Φ \land ) p ⃗ - exp ( Φ \land ) p ⃗ φ .

$\frac{\partial\mathbf{R\vec{p}}}{\partial\varphi}=\lim\limits_{\varphi\rightarrow 0}\frac{\exp(\varphi^\wedge) \exp(\Phi^\wedge)\vec{p} - \exp(\Phi^\wedge)\vec{p} }{\varphi}.$

\partial R p ⃗ \partial φ = lim φ \to 0 exp ( φ \land ) exp ( Φ \land ) p ⃗ - exp ( Φ \land ) p ⃗ φ \approx lim φ \to 0 ( 1 + φ \land ) exp ( Φ \land ) p ⃗ - exp ( Φ \land ) p ⃗ φ = lim φ \to 0 φ \land exp ( Φ \land ) p ⃗ φ = lim φ \to 0 φ \land R p ⃗ φ = lim φ \to 0 - ( R p ⃗ ) \land φ φ = - (R p ⃗) \land .

$\begin{aligned} \frac{\partial\mathbf{R\vec{p}}}{\partial\varphi} &=\lim\limits_{\varphi\rightarrow 0}\frac{\exp(\varphi^\wedge) \exp(\Phi^\wedge)\vec{p} - \exp(\Phi^\wedge)\vec{p} }{\varphi}\\ &\approx \lim\limits_{\varphi\rightarrow 0} \frac{ (1 + \varphi^\wedge) \exp(\Phi^\wedge)\vec{p} - \exp(\Phi^\wedge)\vec{p} }{\varphi}\\ &= \lim\limits_{\varphi\rightarrow 0} \frac{\varphi^\wedge \exp(\Phi^\wedge)\vec{p} }{\varphi} = \lim\limits_{\varphi\rightarrow 0} \frac{\varphi^\wedge \mathbf{R}\vec{p} }{\varphi}\\ &= \lim\limits_{\varphi\rightarrow 0} \frac{ - (\mathbf{R}\vec{p})^\wedge \varphi }{\varphi}=-(\mathbf{R}\vec{p})^\wedge. \end{aligned}$

teddyluo

关注

1
点赞
踩
2

收藏

觉得还不错? 一键收藏
10
评论
《视觉SLAM十四讲》学习笔记-李代数求导与扰动模型的一些重要公式

BCH(Baker-Campbell-Hausdorff)公式： ln(exp(A)exp(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+⋯ln⁡(exp⁡(A)exp⁡(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+⋯\ln (\exp (\mathbf{A}) \exp (\mathbf{B}) ) = \mat...
复制链接

扫一扫