SE(3)和se(3)，左扰动模型

1. SE(3)和se(3)

李群 S E ( 3 ) = { T = Δ [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ R 3 × 3 , R R ⊤ = I , det ⁡ ( R ) = 1 , t ∈ R 3 } SE(3)=\{\mathbf{T} \stackrel{\Delta}{=}\begin{bmatrix} \mathbf{R} &\mathbf{t} \\ \mathbf{0} &1 \\ \end{bmatrix} \in R^{4 \times 4}| \mathbf{R} \in R^{3 \times 3}, \mathbf{R}\mathbf{R}^\top = \mathbf{I}, \det(\mathbf{R}) = 1, \mathbf{t} \in R^{3}\} SE(3)={T=Δ[R0​t1​]∈R4×4∣R∈R3×3,RR⊤=I,det(R)=1,t∈R3}

李代数 s e ( 3 ) = { ξ = Δ [ r τ ] ∈ R 6 ∣ r ∈ R 3 , τ ∈ R 3 } se(3) = \{ \boldsymbol{\xi} \stackrel{\Delta}{=}\begin{bmatrix} \mathbf{r} \\ \boldsymbol{\tau} \\ \end{bmatrix} \in R^{6}| \mathbf{r} \in R^{3}, \boldsymbol{\tau} \in R^3 \} se(3)={ξ=Δ[rτ​]∈R6∣r∈R3,τ∈R3}

$${\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{r}}^{\wedge }& \mathbf{\tau }\\ 0& 1\end{array}\right]$$

从 $\mathbf{\xi }$ 到 $\mathbf{T}$:
$$\begin{array}{rl}\mathbf{T}& =\exp ({\mathbf{\xi }}^{\wedge })\\ & =\left[\begin{array}{cc}\sum _{n=0}^{\infty }\frac{1}{n!}({\mathbf{r}}^{\wedge }{)}^n& \sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\mathbf{r}}^{\wedge }{)}^n\mathbf{\tau }\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{R}& \mathbf{J}\mathbf{\tau }\\ 0& 1\end{array}\right]\end{array}$$

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

$$\theta =||\mathbf{r}||,\mathbf{a}=\frac{\mathbf{r}}{\theta }$$

于是
$$\mathbf{t}=\mathbf{J}\mathbf{\tau }$$

2. 左扰动模型

$$\mathbf{y}=\mathbf{T}\left[\begin{array}{c}\mathbf{x}\\ 1\end{array}\right]=\exp ({\mathbf{\xi }}^{\wedge })\cdot \left[\begin{array}{c}\mathbf{x}\\ 1\end{array}\right]=\exp ({\mathbf{\xi }}^{\wedge })\cdot \tilde{\mathbf{x}}$$

$$\begin{array}{rl}\frac{\partial \mathbf{y}}{\partial \mathbf{\xi }}& =\underset{\Delta \mathbf{\xi }\to \infty }{\lim }\frac{\Delta \mathbf{y}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to \infty }{\lim }\frac{\exp ((\mathbf{\xi }+\Delta \mathbf{\xi }{)}^{\wedge })\cdot \tilde{\mathbf{x}}-\exp ({\mathbf{\xi }}^{\wedge })\cdot \tilde{\mathbf{x}}}{\Delta \mathbf{\xi }}\\ & \stackrel{\Delta }{=}\underset{\Delta \mathbf{\xi }\to \infty }{\lim }\frac{\exp (\Delta {\mathbf{\xi }}^{\wedge })\cdot \exp ({\mathbf{\xi }}^{\wedge })\cdot \tilde{\mathbf{x}}-\exp ({\mathbf{\xi }}^{\wedge })\cdot \tilde{\mathbf{x}}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to \infty }{\lim }\frac{(\mathbf{I}+\Delta {\mathbf{\xi }}^{\wedge })\cdot \exp ({\mathbf{\xi }}^{\wedge })\cdot \tilde{\mathbf{x}}-\exp ({\mathbf{\xi }}^{\wedge })\cdot \tilde{\mathbf{x}}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to \infty }{\lim }\frac{\Delta {\mathbf{\xi }}^{\wedge }\exp ({\mathbf{\xi }}^{\wedge })\cdot \tilde{\mathbf{x}}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to \infty }{\lim }\frac{\left[\begin{array}{cc}\Delta {\mathbf{r}}^{\wedge }& \Delta \mathbf{\tau }\\ 0& 1\end{array}\right]\cdot \left[\begin{array}{c}\mathbf{Rx}+\mathbf{t}\\ 1\end{array}\right]}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to \infty }{\lim }\frac{\left[\begin{array}{c}\Delta {\mathbf{r}}^{\wedge }\left(\mathbf{Rx}+\mathbf{t}\right)+\Delta \mathbf{\tau }\\ 1\end{array}\right]}{\Delta \mathbf{\xi }}\\ & =\left[\begin{array}{cc}-{\left(\mathbf{Rx}+\mathbf{t}\right)}^{\wedge }& \mathbf{I}\\ 0_{1\times 3}& 0_{1\times 3}\end{array}\right]\end{array}$$