一、slam中的李群

1、群的定义

群是一种集合集合加上一种运算的代数结构。要满足：封闭性、结合律、幺元和逆。

2、旋转和群

三维空间中，描述一个物体的旋转（此处仅考虑刚体），旋转的表示有如下几种：旋转矩阵，四元数，欧拉角。其中旋转矩阵是一种特殊正交群，记为SO(3)，而用旋转矩阵表示旋转有一点小瑕疵，故我们引入了欧拉角和四元数，并学习使用她们计算旋转，理解她们和群之间的联系。

旋转矩阵：

首先，我们有一个世界坐标系，该坐标系是恒定不变的，然后我们选择一个刚体，比如，一个诺基亚，我们将其抛入空中，假设其由姿态A变为姿态B（此处不考虑平移，仅考虑旋转）。那么，我们可以通过这样一种方式来描述旋转，选择一个相对于世界坐标系固定的 point $a$ ，通过 point $a$ 相对于诺基亚视角中的位置变化来描述旋转，即我们以诺基亚机身的中点为原点构建一个三维坐标系，则 point $a$ 在诺基亚旋转前后的坐标可记为 $[x0,y0,z0]^T$ 和 $[x1,y1,z1]^T$ ，请记住，point $a$ 在世界坐标系中并未改变，坐标的改变是由于坐标系的变化，即 $[x0,y0,z0]^T$ 和 $[x1,y1,z1]^T$ 使用的并不是同一组正交基（相对于世界坐标），我们记为 $[\textbf{e1},\textbf{e2},\textbf{e3}]$ ，和 $[\textbf{e1'},\textbf{e2'},\textbf{e3'}]$ ，利用 point $a$ 在世界坐标系中的不变性，我们得到如下等式：

[e1, e2, e3] [x 0, y 0, z 0] T = [e1', e2', e3'] [x 1, y 1, z 1] T \dots \dots 式 1

$[\textbf{e1},\textbf{e2},\textbf{e3}][x0,y0,z0]^T = [\textbf{e1'},\textbf{e2'},\textbf{e3'}][x1,y1,z1]^T……式1$
注意，这里的

[e1,e2,e3] $[\textbf{e1},\textbf{e2},\textbf{e3}]$ 是一个

3X3 $3X3$ 的矩阵，且

e1 e2 e3 $\textbf{e1}~\textbf{e2}~\textbf{e3}$ 为正交基向量，由正交基性质易得：

[e1 T, e2 T, e3 T] T [e1, e2, e3] = I \dots \dots 式 2

$[\textbf{e1}^T,\textbf{e2}^T,\textbf{e3}^T]^T[\textbf{e1},\textbf{e2},\textbf{e3}] = I……式2$
在式1两边左乘

[e1T,e2T,e3T]T $[\textbf{e1}^T,\textbf{e2}^T,\textbf{e3}^T]^T$ 则得到下式：

[x 0, y 0, z 0] T = [e1 T, e2 T, e3 T] T [e1', e2', e3'] [x 1, y 1, z 1] T \dots \dots 式 3

$[x0,y0,z0]^T = [\textbf{e1}^T,\textbf{e2}^T,\textbf{e3}^T]^T[\textbf{e1'},\textbf{e2'},\textbf{e3'}][x1,y1,z1]^T……式3$
其中我们将矩阵

[e1T,e2T,e3T]T[e1',e2',e3'] $[\textbf{e1}^T,\textbf{e2}^T,\textbf{e3}^T]^T[\textbf{e1'},\textbf{e2'},\textbf{e3'}]$ 记为

R $R$ ，称为旋转矩阵。而这旋转矩阵正是一种群，我们称之为特殊正交群。

欧拉角：

在引入欧拉角之前，我们先来考虑一个最简单的旋转—绕某一轴的旋转，这种旋转我们可以简单的用外积来表示，即用一个向量来表示，该向量可表示为 $\textbf{a}θ$ 。（其实，对于任意一个旋转，一定可以看成是绕某个轴的旋转，这里便于叙述，不细讲）

我们不难想象，任意的一个旋转总能分解为绕一组正交基的三次旋转，特别的，当我们用三次连续的依次绕ZYX轴旋转时，得到的 $θ1,θ2,θ3$ 分为称为偏航角yaw，俯仰角pitch和滚转角roll，注意，此处强调连续，是指每次旋转后的正交基都发生了改变，故三个角度分别以最开始的坐标轴，绕Z轴旋转后的坐标轴，和再绕Y轴旋转后的坐标轴为坐标系。（ZYX这一顺序只是我们最常用的一种欧拉角，也可能采用其他顺序）

那么如何将欧拉角与旋转矩阵或者说群联系起来呢，一个绕X,Y,Z轴的旋转，可以分别用旋转矩阵 $R(θ_x)~R(θ_y)~R(θ_z)$ 来描述：

R x (θ) = ⎡ ⎣ ⎢ ⎢ 100 0 c o s θ s i n θ 0 - s i n θ c o s θ ⎤ ⎦ ⎥ ⎥

$R_x(θ)= \begin{bmatrix} 1 & 0 & 0 \\ 0 & cosθ & -sinθ \\ 0 & sinθ & cosθ \end{bmatrix}$

R y (θ) = ⎡ ⎣ ⎢ ⎢ c o s θ 0 - s i n θ 010 s i n θ 0 c o s θ ⎤ ⎦ ⎥ ⎥

$R_y(θ)= \begin{bmatrix} cosθ & 0 & sinθ \\ 0 & 1 & 0 \\ -sinθ & 0 & cosθ \end{bmatrix}$

R z (θ) = ⎡ ⎣ ⎢ ⎢ c o s θ s i n θ 0 - s i n θ c o s θ 0 001 ⎤ ⎦ ⎥ ⎥

$R_z(θ)= \begin{bmatrix} cosθ & -sinθ & 0\\ sinθ & cosθ & 0 \\ 0& 0 & 1 \end{bmatrix}$
根据我们使用的欧拉角的不同，如ZYX欧拉角，则旋转矩阵

R=Rx(θ)Ry(θ)Rz(θ) $R=R_x(θ)R_y(θ)R_z(θ)$ ，则我们通过这一转换式将欧拉角与李群联系在了一起。

欧拉角一个重大缺点是会面临万向锁难题，即在俯仰角pitch为 $±90^。$ 时，第一次旋转和第三次旋转将绕同一轴，这将会导致用欧拉角描述旋转时损失一个自由度。

四元数：

我们发现，使用旋转矩阵描述旋转时会产生冗余（用 $3X3$ 的矩阵即9个值去描述一个自由度为3的运动），而使用欧拉角（3个值描述自由度为3的运动）会面临万向锁难题，那么有没有折中的办法呢，四元数便是这样一个解决方案。

四元数是一种扩展的复数，用她来描述三维空间的旋转时既是紧凑的也是没有奇异性的，一个四元数 $q$ 看起来应该是这样的 $q=q_0+q_1i+q_2j+q_3k$ ，对于 $i~j~k$ ，我们有轮换式 $ab=-c$ 和 $i^2=j^2=k^2=-1$ 。

完整的四元数解释过于复杂，我们需要做的只有两件事：（1）找到四元数表示旋转的方法（2）知道表示旋转时四元数必要的运算方法

四元数的四则运算、共轭、模长计算与复数无异，不再赘述；四元数具有点乘运算，和matlab中矩阵点乘运算一样，对应位置数值相乘即可；一个四元数的逆 $q^{-1}=q^*/||q||^2$ 。

通常我们将旋转向量和四元数关联在一起。对于一个 $aθ$ 的旋转向量来说，当 $a=[n_x,n_y,n_z]$ 时，我们可以得到 $q=cos\frac{θ}{2}+n_xsin\frac{θ}{2}i+n_ysin\frac{θ}{2}j+n_zsin\frac{θ}{2}k$ （ $θ$ 加上 $2\pi$ 表示同一个旋转， $q$ 却变为 $-q$ ），反之我们可以得到

⎧ ⎩ ⎨ ⎪ ⎪ θ = 2 a r c o s q 0 [n x, n y, n z] T = [q 1, q 2, q 3] T / s i n θ 2

$\left\{ \begin{aligned} &θ=2arcosq_0\\ &[n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/sin\frac{θ}{2} \end{aligned} \right.$

那么问题来了，我们怎么去计旋转呢，与旋转矩阵类似，我们同样通过描述一个世界坐标系的不变点在旋转前后的刚体坐标系的坐标变化来描述旋转：
首先用一个虚四元数来描述坐标

p = [0, x, y, z] T

$p=[0,x,y,z]^T$ 然后用上述方法描述旋转

q = [c o s θ 2, n x s i n θ 2 i, n y s i n θ 2 j, n z s i n θ 2 k]

$q=[cos\frac{θ}{2},n_xsin\frac{θ}{2}i,n_ysin\frac{θ}{2}j,n_zsin\frac{θ}{2}k]$ 则有

p' = q p q - 1

$p'=qpq^{-1}$

知道了如何用四元数计算旋转，那么我们能否将四元数和群关联起来呢，有了四元数和旋转向量的转换式，似乎不难算出旋转矩阵，但计算过程过于麻烦，这里给出较为简洁的四元数和旋转矩阵的转换公式（公式不唯一）：

R = ⎡ ⎣ ⎢ ⎢ ⎢ 1 - 2 q 22 - 2 q 23 2 q 1 q 2 + 2 q 0 q 3 2 q 1 q 3 - 2 q 0 q 2 2 q 1 q 2 - 2 q 0 q 3 1 - 2 q 21 - 2 q 23 2 q 2 q 3 + 2 q 0 q 1 2 q 1 q 3 + 2 q 0 q 2 2 q 2 q 3 - 2 q 0 q 1 1 - 2 q 21 - 2 q 22 ⎤ ⎦ ⎥ ⎥ ⎥

$R= \begin{bmatrix} 1-2q_2^2-2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2-2q_2^2 \end{bmatrix}$

q 0 = t r ( R ) + 1 ‾ ‾ ‾ ‾ ‾ ‾ ‾ ‾ \sqrt 2, q 1 = m 23 - m 32 4 q 0, q 2 = m 31 - m 13 4 q 0, q 3 = m 12 - m 21 4 q 0

$q0=\frac{\sqrt{tr(R)+1}}{2},q1=\frac{m_{23}-m_{32}}{4q_0},q2=\frac{m_{31}-m_{13}}{4q_0},q3=\frac{m_{12}-m_{21}}{4q_0}$ 当

q0 $q_0$ 接近0时，分量会变得特别大，导致解不稳定，应换用其他方法。

旋转向量和旋转矩阵：
我们使用罗德里格斯公式将旋转向量转化为旋转矩阵：

R = c o s θ I + (1 - c o s θ) n n T + s i n θ n ̂

$R=cosθI+(1-cosθ)nn^T+sinθ\hat{n}$

紧接着给出从旋转矩阵到旋转向量的表达式：

⎧ ⎩ ⎨ ⎪ ⎪ θ = a r c o s (t r ( R ) - 1 2) R a = a

$\begin{equation} \left\{ \begin{aligned} &θ=arcos(\frac{tr(R)-1}{2}) \\ &R\textbf{a}=\textbf{a}\\ \end{aligned} \right. \end{equation}$
即转轴\textbf{a}是矩阵R特征值1对应的特征向量。

变换和群

我们用一个旋转矩阵来描述刚体在三维空间的旋转，而刚体在三维空间的运动可以看作是旋转+平移，为了使得这种变换是线性的，我们引入了齐次坐标，并且靠其得到了变换矩阵，变换矩阵便是一种特殊欧氏群，记为SE(3)。

旋转+平移看起来应该是这样的：

p' = R p + t

$p'=Rp+t$ 我们发现这种变化不是线性的。
为此我们引入了齐次坐标：

[p' 1] = [R 0 T t 1] [p 1] = T [p 1]

$\begin{bmatrix}p'\\1\end{bmatrix}=\begin{bmatrix}R&t\\0^T&1\end{bmatrix}\begin{bmatrix}p\\1\end{bmatrix}=T\begin{bmatrix}p\\1\end{bmatrix}$ 我们把矩阵T称为变换矩阵。我们给出变换矩阵的逆：

T - 1 = [R T 0 T - R T t 1]

$T^{-1}=\begin{bmatrix}R^T&-R^Tt\\0^T&1\end{bmatrix}$

二、slam中的李代数

1、李代数的定义

李代数由一个集合V，一个数域F和一个二元运算[,]（李括号）组成，需满足封闭性、双线性、自反性和雅可比等价。

（下面两个小结建议先看完三再看）

2、BCH公式的近似形式

对于旋转矩阵R和旋转向量Φ来说，我们难免产生一些想法，旋转矩阵连续左乘可以看作是多次连续的旋转，那么旋转向量合成是否也能看成是多次连续的旋转呢？也就是说两个旋转向量的和向量的指数映射是否等于两个旋转向量对应的旋转矩阵的乘积？

答案，是否定的。

首先给出三个结论：

l n (e x p (Φ 1^) e x p (Φ 2^)) \approx ⎧ ⎩ ⎨ ⎪ ⎪ (J l (Φ 2) - 1 Φ 1 + Φ 2^) 当 Φ 1 为 小 量 (J l (Φ 1) - 1 Φ 2 + Φ 1^) 当 Φ 2 为 小 量

$ln(exp(\hat{\textbf{Φ}_1})exp(\hat{\textbf{Φ}_2})) \approx \left\{ \begin{aligned} &\hat{(J_l(\textbf{Φ}_2)^{-1}\textbf{Φ}_1+\textbf{Φ}_2})~~~当\textbf{Φ}_1为小量\\ &\hat{(J_l(\textbf{Φ}_1)^{-1}\textbf{Φ}_2+\textbf{Φ}_1})~~~当\textbf{Φ}_2为小量 \end{aligned} \right.$

e x p (Δ Φ ̂) e x p (Φ ̂) = e x p ((Φ + J - 1 l (Φ) Δ Φ)^)

$exp(Δ\hat{\textbf{Φ}})exp(\hat{\textbf{Φ}})=exp(\hat{(\textbf{Φ}+J_l^{-1}(\textbf{Φ})Δ\textbf{Φ})})$

e x p ((Δ Φ + Φ)^) = e x p ((J l Δ Φ)^) e x p (Φ ̂) = e x p (Φ ̂) e x p ((J r Δ Φ)^)

$exp(\hat{(Δ\textbf{Φ}+\textbf{Φ})})=exp(\hat{(J_lΔ\textbf{Φ})})exp(\hat{\textbf{Φ}})=exp(\hat{\textbf{Φ}})exp(\hat{(J_rΔ\textbf{Φ})})$
其中：

⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ J = s i n θ θ I + (1 - s i n θ θ) a a T + 1 - c o s θ θ a ̂ J = θ 2 c o t θ 2 I + (1 - θ 2 c o t θ 2) a a T - θ 2 a ̂ J r (Φ) = J l (-Φ)

$\left\{ \begin{aligned} &J=\frac{sinθ}{θ}I+(1-\frac{sinθ}{θ})\textbf{a}\textbf{a}^T+\frac{1-cosθ}{θ}\hat{\textbf{a}}\\ &J=\frac{θ}{2}cot\frac{θ}{2}I+(1-\frac{θ}{2}cot\frac{θ}{2})\textbf{a}\textbf{a}^T-\frac{θ}{2}\hat{\textbf{a}}\\ &J_r(\textbf{Φ})=J_l(\textbf{-Φ}) \end{aligned} \right.$

3、李代数的求导

由于观测噪声的存在，观测值和实际值总会存在误差，为了使该误差最小，便要对李代数进行求导。求导的方法有二：1.根据李代数加法对李代数进行求导 2.对李代数左乘或右乘微小扰动，对该扰动进行求导。

SO(3)上李代数的求导：
这里让旋转后的坐标对旋转求导，直接给出结论：

d ( R p ) d Φ = (- R p)^J l

$\frac{d{(Rp)}}{dΦ}=\hat{(-Rp)}J_l$

扰动模型（左乘）：
设

ΔR $ΔR$ 对应的李代数为

φ $\textbf{φ}$ ，结论：

d ( R p ) d φ = (- R p)^

$\frac{d{(Rp)}}{dφ}=\hat{(-Rp)}$

SE(3)上李代数的求导：
这里让变换后的坐标对变换扰动求导，记

ΔT=exp(δξ̂ ) $ΔT=exp(δ\hat{\textbf{ξ}})$ ，结论：

d ( T p ) d δ ξ = [I 0 T - (R p + t)^0 T]

$\frac{d{(Tp)}}{dδ\textbf{ξ}}=\begin{bmatrix}I&-\hat{(Rp+t)}\\0^T&0^T\end{bmatrix}$

三、李群和李代数的联系

1、从李群到李代数

旋转矩阵R是正交矩阵，则 $RR^T=I$ ，当我们把 $R$ 看作关于时间的函数，则有 $R(t)R(t)^T = I$ ，同时对等式两边求导，则 $R'(t)R(t)^T=-(R'(t)R(t)^T)^T$ ，即 $R'(t)R(t)^T$ 为反对称矩阵，反对成矩阵都可以和向量相互转换，则 $R'(t)R(t)^T$ 可以表示为 $\hat{Φ}$ ，而 $Φ$ 就是一种李代数，在这里其就是 $R$ 对应的旋转向量，记作 $\mathfrak{SO}(3)$ ，位于 $\mathbb{R}^3$ 空间。

而变换矩阵对应的李代数，位于 $\mathbb{R}^6$ ，记作 $\mathfrak{SE}(3)$ ，对于其中每一个元素 $\textbf{ξ}$ 有 $\textbf{ξ}=\begin{bmatrix}ρ\\Φ\end{bmatrix}$ 前三维为平移（不同于t），后三维为旋转（就是旋转向量）。

2、指数映射和对数映射

SO(3)上的指数映射和 $\mathfrak{SO}(3)$ 上的对数映射：
首先给出SO(3)上的指数映射公式：

R = e x p (Φ ̂)

$R=exp(\hat{Φ})$ 接着给出指数映射的定义：

e x p (A) = \sum n = 0 \infty 1 n ! A n

$exp(A)=\sum_{n=0}^{\infty}\frac{1}{n!}A^n$ 即任意矩阵的指数映射可以写成一个泰勒展开，但是只有收敛时才有结果。

接着直接给出SO(3)上指数映射的推导结果（其实就是泰勒展开加上 $\textbfΦ=θ\textbf{a}$ $\hat{a}\hat{a}=aa^T-I$ 以及 $\hat{a}\hat{a}\hat{a}=-\hat{a}$ ）：

R = e x p (Φ ̂) = c o s θ I + (1 - c o s θ) a a T + s i n θ a ̂

$R=exp(\hat{\textbfΦ})=cosθI+(1-cosθ)\textbf{a}\textbf{a}^T+sinθ\hat{\textbf{a}}$ 很眼熟，对了，这就是罗德里格斯公式，即SO(3)上的指数映射就是罗德里格斯公式
。
既然指数映射是罗德里格斯公式，那么对数映射不难想象：

⎧ ⎩ ⎨ ⎪ ⎪ θ = a r c o s (t r ( R ) - 1 2) R a = a

$\begin{equation} \left\{ \begin{aligned} &θ=arcos(\frac{tr(R)-1}{2}) \\ &R\textbf{a}=\textbf{a}\\ \end{aligned} \right. \end{equation}$
SE(3)上的指数映射和 $\mathfrak{SE}(3)$ 上的对数映射：
同样，这里直接给出SE(3)上指数映射的公式：

T = e x p (ξ ̂) = [e x p (Φ ̂) 0 T J ρ 1]

$T=exp(\hat{\textbf{ξ}})=\begin{bmatrix}exp(\hat{\textbf{Φ}})&Jρ\\0^T&1\end{bmatrix}$ 其中

J=sinθθI+(1−sinθθ)aaT+1−cosθθâ $J=\frac{sinθ}{θ}I+(1-\frac{sinθ}{θ})\textbf{a}\textbf{a}^T+\frac{1-cosθ}{θ}\hat{\textbf{a}}$

即得到 $t=Jρ,R=exp(\hat{\textbf{Φ}})$

类似于上的对数映射：

⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ θ = a r c o s (t r ( R ) - 1 2) R a = a Φ = θ a t = J ρ

$\begin{equation} \left\{ \begin{aligned} &θ=arcos(\frac{tr(R)-1}{2}) \\ &R\textbf{a}=\textbf{a}\\ &\textbfΦ=θ\textbf{a}\\ &t=Jρ \end{aligned} \right. \end{equation}$ 即可得到

Φ $\textbfΦ$ 和

ρ $ρ$

slam中李群和李代数笔记