背景
李群/李代数，因为其特殊的性质，在无人驾驶领域，应用得比较广泛。当在构建数学模型中应用到对应的李群/李代数时，这时当知道一些李群/李代数的性质，以及对应的映射关系时，对一些数学推导可以起到事半功倍的效果。这篇文章主要是对文献[1][2]中用提到的一些公式，进行记录。
定义
1 SO(3)
一个李群SO(3)中的元素\(R\)，总能在李代数中找到包含3个元素的向量\(\phi\)与其对应，这里\(R\)是一个\(3 \times 3\)的旋转矩阵.
对于本文的旋转矩阵\(R\)，有如下性质
(1.1)$$
|R|=1\\
R^T = R^{-1}
$$
约定如下的反对称符号
(1.2)$$
\phi ^\wedge =\begin{bmatrix}
0 & -\phi_2 & \phi_1\\
\phi_2 & 0 & -\phi_0\\
-\phi_1 & \phi_0 & 0
\end{bmatrix}_{3 \times 3}
\\
\phi =\begin{bmatrix}
0 & -\phi_2 & \phi_1\\
\phi_2 & 0 & -\phi_0\\
-\phi_1 & \phi_0 & 0
\end{bmatrix}^\vee = \begin{bmatrix}
\phi_0 \\
\phi _1\\
\phi _2
\end{bmatrix}
$$
对于\(R\)与\(\phi\)通过指数映射有如下关系
(1.3)$$\begin{aligned}
R &= \exp(\phi^\wedge)
\\
\phi^\wedge &= \log(R)
\\
\phi &= \log(R)^\vee\\
(\phi^\wedge)^T &=-\phi^\wedge=(-\phi)^\wedge
\end{aligned}$$
因为一个旋转向量\(\mathbf{a}\)与旋转角\(\theta\)可以表示一个旋转，因此对于如下表达式
(1.4)$$
\phi = \theta \mathbf{a}
\\
||\mathbf{a}||_2 = 1 \\
\theta = |\phi|
$$
则可以建立联系
(1.5)$$
R = \cos \theta I + (1-\cos\theta)\mathbf{a}\mathbf{a}^T+ \sin\theta \mathbf{a}^\wedge
$$
其中\(R\)的左右雅克比矩阵\(J_l,J_r\)可表示为
(1.6)$$\begin{cases}
J_l &=I+\dfrac{1-\cos\theta}{\theta^2}\phi^\wedge+\dfrac{\theta-\sin\theta}{\theta^3}(\phi^\wedge)^2
\\
\\
J_r &=I-\dfrac{1-\cos\theta}{\theta^2}\phi^\wedge+\dfrac{\theta-\sin\theta}{\theta^3}(\phi^\wedge)^2
\\
\\
J_l^{-1}&=I-\dfrac{1}{2}\phi^\wedge+(\dfrac{1}{\theta^2}-\dfrac{1+\cos\theta}{2\theta\sin\theta})(\phi^\wedge)^2
\\
\\
J_r^{-1}&=I+\dfrac{1}{2}\phi^\wedge+(\dfrac{1}{\theta^2}-\dfrac{1+\cos\theta}{2\theta\sin\theta})(\phi^\wedge)^2
\end{cases}
$$
当轴角趋近0时，可简化为如下公式
(1.7)$$\begin{cases}
J_l|_{\theta \rightarrow 0} &\approx I + \dfrac{1}{2}\phi^\wedge+\dfrac{1}{6}(\phi^\wedge)^2
\\
\\
J_r|_{\theta \rightarrow 0} &\approx I – \dfrac{1}{2}\phi^\wedge+\dfrac{1}{6}(\phi^\wedge)^2
\\
\\
J_l^{-1}|_{\theta \rightarrow 0} &\approx I-\dfrac{1}{2}\phi^\wedge+\dfrac{1}{12}(\phi^\wedge)^2
\\
\\
J_r^{-1}|_{\theta \rightarrow 0} &\approx I+\dfrac{1}{2}\phi^\wedge+\dfrac{1}{12}(\phi^\wedge)^2
\end{cases}$$
对于(1.7)忽略二阶小量，可得到如下结果
(1.8)$$\begin{cases}
J_l|_{\theta \rightarrow 0} &\approx I + \dfrac{1}{2}\phi^\wedge
\\
\\
J_r|_{\theta \rightarrow 0} &\approx I – \dfrac{1}{2}\phi^\wedge
\\
\\
J_l^{-1}|_{\theta \rightarrow 0} &\approx I-\dfrac{1}{2}\phi^\wedge
\\
\\
J_r^{-1}|_{\theta \rightarrow 0} &\approx I+\dfrac{1}{2}\phi^\wedge
\end{cases}$$
根据（1.6）定义，\(R\)的左雅克比矩阵与\(R^T\)的右雅克比矩阵相同，\(R\)的右雅克比矩阵与\(R^T\)的左雅克比矩阵相同。
对于一个比较小的量\(\delta \phi\)，对旋转矩阵进行左右扰动，有如下结论
(1.9)$$
\log(\exp(\phi^\wedge)\exp(\delta \phi^\wedge))^\vee \approx J_r^{-1} \delta \phi +\phi
\\
\log(\exp(\delta \phi^\wedge)\exp(\phi^\wedge))^\vee \approx J_l^{-1} \delta \phi +\phi
$$
注意，仔细观察(1.6)与(1.9)式，可以发现\(\theta\)加减\(2n\) \(\pi\)，旋转矩阵\(R\)的值不变。
同样，有如下近似
(1.10)$$
\exp(\delta \phi^\wedge) \exp(\phi^\wedge) \approx \exp((\phi + J_l^{-1}\delta \phi)^\wedge)
\\
\exp(\phi^\wedge)\exp(\delta \phi^\wedge) \approx \exp((\phi + J_r^{-1}\delta \phi)^\wedge)
\\
\exp((\phi +\delta \phi)^\wedge) \approx \exp((J_l\delta \phi)^\wedge)\exp(\phi^\wedge) \approx \exp(\phi^\wedge)\exp((J_r\delta \phi)^\wedge)$$
下面列举一些常见的性质
(1.11)$$
J_l(\phi) = J_r(-\phi)
\\
J_l = RJ_r
$$
(1.12)$$
R J_r = J_rR
\\
RJ_l=J_lR
$$
(1.13)$$\begin{aligned}
J_l&=RJ_r=J_rR=J_r^T
\\
J_r&=R^TJ_l=J_lR^T=J_l^T\end{aligned}
$$
(1.14)$$
R(J_r\mathbf{x})^\wedge=(J_l\mathbf{x})^\wedge R
$$
(1.15)$$R = I + \phi^\wedge J_l$$
(1.16)$$I = R^T + \phi^\wedge J_r$$
(1.17)$$\theta = \arccos(\frac{\mathbf{Tr}(R)-1}{2})$$
(1.18)$$\mathbf{a}^\wedge = \frac{\theta}{\sin\theta}\frac{R-R^T}{2}$$
(1.19)$$R\phi = \phi$$
(1.20)$$R\phi^\wedge = \phi^\wedge R$$
对于任意3个元素的向量\(\mathbf{x}, \mathbf{y}\)，以及任意\(3 \times 3\)矩阵\(W\)有
(1.21)$$
\mathbf{x}^\wedge \mathbf{x} = 0
$$
(1.22)$$
\mathbf{x}^T\mathbf{x}^\wedge = \mathbf{0}^T
$$
(1.23)$$
\mathbf{x}^T\mathbf{y}^\wedge \mathbf{x} = 0
$$
(1.24)$$
\mathbf{x}^\wedge \mathbf{y}^\wedge =\mathbf{y}\mathbf{x}^T- (\mathbf{x}^T\mathbf{y})I
$$
(1.25)$$
\mathbf{x}^\wedge \mathbf{y} = – \mathbf{y}^\wedge \mathbf{x}
$$
(1.26)$$
(\mathbf{x}^\wedge)^T = -\mathbf{x}^\wedge
$$
(1.27)$$
(\mathbf{x} + \mathbf{y})^\wedge = \mathbf{x}^\wedge + \mathbf{y}^\wedge
$$
(1.28)$$(\mathbf{x}^{\wedge}\mathbf{y})^\wedge= \mathbf{x}^{\wedge} \mathbf{y}^\wedge – \mathbf{y}^{\wedge} \mathbf{x}^\wedge = \mathbf{y}\mathbf{x}^T – \mathbf{x}\mathbf{y}^T$$
(1.29)$$
\mathbf{x} \times \mathbf{y} = \mathbf{x}^\wedge \mathbf{y}
$$
(1.30)$$
(\mathbf{x}^\wedge \mathbf{y}) \cdot \mathbf{z} =(\mathbf{y}^\wedge \mathbf{z}) \cdot \mathbf{x}=(\mathbf{z}^\wedge \mathbf{x}) \cdot \mathbf{y}
$$
上面公式中的\(\times\)表示叉乘.
(1.31)$$
(W\mathbf{x})^\wedge = \mathbf{x}^\wedge(\mathbf{Tr}(W)I – W) – W^T\mathbf{x}^\wedge
$$
(1.32)$$
\mathbf{x}^\wedge W \mathbf{y}^\wedge =\mathbf{Tr}(W^T\mathbf{yx}^T)I-W^T\mathbf{yx}^T +(\mathbf{Tr}(\mathbf{yx}^T) – \mathbf{yx}^T)(W^T-\mathbf{Tr}(W)I)$$
(1.33)$$
(R\mathbf{x})^\wedge = R\mathbf{x}^\wedge R^T =(2\cos \theta + 1)\mathbf{x}^\wedge -\mathbf{x}^\wedge R – R^T\mathbf{x}^\wedge
$$
(1.34)$$
\exp((R\mathbf{x})^\wedge) = R \exp(\mathbf{x}^\wedge)R^T
$$
(1.35)$$
R\exp((R^T\mathbf{x})^\wedge) = \exp(\mathbf{x}^\wedge)R
$$
(1.36)$$
\frac{\partial \mathbf{a}^\wedge \mathbf{b}}{\partial \mathbf{a}}= -\mathbf{b}^\wedge
$$
(1.37)$$
\log(\exp(\phi_1^\wedge)\exp(\phi_2^\wedge))^\vee=\phi_1+\phi_2+\dfrac{1}{2}\phi_1^\wedge\phi_2+\dfrac{1}{12}(\phi_1^\wedge\phi_1^\wedge\phi_2+\phi_2^\wedge\phi_2^\wedge\phi_1)+……
$$
(1.38)$$R(\mathbf{x}\times\mathbf{y})=(R\mathbf{x})\times(R\mathbf{y})$$
(1.39)$$\mathbf{x}\cdot\mathbf{y}=(R\mathbf{x})\cdot(R\mathbf{y})$$
(1.40)$$(R\mathbf{x})\cdot\mathbf{y}=\mathbf{x}\cdot(R^T\mathbf{y})$$
对于车辆局部坐标系下的角速度\(\mathbf{w}_l\)，以及全局坐标系下的角速度\(\mathbf{w}_g\)，与车辆位姿本身的旋转矩阵\(R\)(局部到全局坐标系表示)有如下关系
(1.41)$$
\begin{cases} R &=\exp(\phi^\wedge)
\\
\dot{R}&=R\mathbf{w}_l^\wedge=\mathbf{w}_g^\wedge R
\\
\mathbf{w}_l&=J_r\dot{\phi}
\\
\mathbf{w}_g&=J_l\dot{\phi}=R\mathbf{w}_l
\\
\dot{\phi} &=J_l^{-1}\mathbf{w}_g=J_r^{-1}\mathbf{w}_l
\\
\\
\dot{J}_l&=\mathbf{w}_g^\wedge J_l + \dfrac{\partial \mathbf{w}_g}{\partial \phi}=\mathbf{w}_g^\wedge J_l-\mathbf{w}_g^\wedge J_l+R\dfrac{\partial \mathbf{w}_l}{\partial \phi}=R\dfrac{\partial \mathbf{w}_l}{\partial \phi}
\\
&=\dot{R}J_r+R\dot{J}_r
\\
&=\mathbf{w}_g^\wedge R J_r+R\dot{J}_r
\\
&=\mathbf{w}_g^\wedge J_l+R\dot{J}_r
\\
\\
\dot{J}_r &=R^T\dfrac{\partial \mathbf{w}_g}{\partial \phi} \end{cases}
$$
2 SE(3)
一个李群SE(3)中的元素\(T\)，总能在李代数中找到包含6个元素的向量\(\xi\)与其对应， 其中\(\xi\)表示如下
(2.1)$$
\xi =
\begin{bmatrix}
\rho\\
\phi
\end{bmatrix}
$$
其中\(\rho\)与\(\phi\)均为大小为3的向量，这里约定如下的符号
(2.2)$$
\xi^\wedge =
\begin{bmatrix}
\phi^\wedge & \rho \\
\mathbf{0}^T & 0
\end{bmatrix}_{4 \times 4}
\\
\xi =
\begin{bmatrix}
\phi^\wedge & \rho \\
\mathbf{0}^T & 0
\end{bmatrix}^\vee
\\
\xi ^ \curlywedge=\begin{bmatrix}
\phi ^\wedge & \rho^\wedge\\
0 & \phi^\wedge
\end{bmatrix}_{6 \times 6}
\\
\xi=\begin{bmatrix}
\phi ^\wedge & \rho^\wedge\\
0 & \phi^\wedge
\end{bmatrix}^ \curlyvee
\\
\mathbf{p}^{\odot}= \begin{bmatrix}
\mathbf{e}\\
\eta
\end{bmatrix}^{\odot}_{4 \times 1}
= \begin{bmatrix}
\eta I & -\mathbf{e}^\wedge\\
\mathbf{0}^T & \mathbf{0}^T
\end{bmatrix}_{4 \times 6}
\\
\mathbf{p}^{\circledcirc}= \begin{bmatrix}
\mathbf{e}\\
\eta
\end{bmatrix}^{\circledcirc}_{4 \times 1}
= \begin{bmatrix}
\mathbf{0} & \mathbf{e}^\wedge\\
-\mathbf{e}^\wedge & \mathbf{0}
\end{bmatrix}_{6 \times 4}
$$
上面公式中的\(\mathbf{e}\)为大小为3的向量，\(\eta\)为一个实数。
下面建立向量与矩阵的转换关系
(2.3)$$
\begin{aligned}
T &= \exp(\xi^\wedge)\\
&=
\begin{bmatrix}
\exp(\phi^\wedge) & J_l\rho\\
\mathbf{0}^T & 1
\end{bmatrix}\\
&=
\begin{bmatrix}
R & \mathbf{t}\\
\mathbf{0}^T & 1
\end{bmatrix}_{4 \times 4}\\
&=I +\xi^\wedge + \frac{1-\cos \theta}{\theta^2}(\xi^\wedge)^2+\frac{\theta- \sin\theta}{\theta^3}(\xi^\wedge)^3
\end{aligned}
$$
(2.4)$$
\xi^\wedge = \log(T)$$
(2.5)$$
T^{-1}=\begin{bmatrix}
R^T & -R^T\mathbf{t}\\
\mathbf{0}^T & 1
\end{bmatrix}
$$
同样的\(T\)也有对应的左右雅克比矩阵\(\mathcal{J}_l,\mathcal{J}_r\)
(2.6)$$
\mathcal{J}_l = \begin{bmatrix}
J_l & Q_l\\
0 & J_l
\end{bmatrix}
\\
\mathcal{J}_r = \begin{bmatrix}
J_r & Q_r\\
0 & J_r
\end{bmatrix}
\\
\mathcal{J}_l^{-1} = \begin{bmatrix}
J_l^{-1} & -J_l^{-1}Q_lJ_l^{-1}\\
0 & J_l^{-1}
\end{bmatrix}
\\
\mathcal{J}_r^{-1}= \begin{bmatrix}
J_r^{-1} & -J_r^{-1}Q_rJ_r^{-1}\\
0 & J_r^{-1}
\end{bmatrix}
$$
其中\(Q_l,Q_r\)表达式如下
(2.7)$$\begin{cases}
Q_l&=\dfrac{1}{2}\rho^\wedge
\\
\\
&+\dfrac{\theta-\sin\theta}{\theta^3}(\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge+\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
&+\dfrac{\theta^2+2\cos\theta-2}{2\theta^4}(\phi^\wedge\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge\phi^\wedge-3\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
&+\dfrac{2\theta-3\sin\theta+\theta\cos\theta}{2\theta^5}(\phi^\wedge\rho^\wedge\phi^\wedge\phi^\wedge+\phi^\wedge\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
Q_r&=-\dfrac{1}{2}\rho^\wedge
\\
\\
&+\dfrac{\theta-\sin\theta}{\theta^3}(\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge-\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
&-\dfrac{\theta^2+2\cos\theta-2}{2\theta^4}(\phi^\wedge\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge\phi^\wedge-3\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
&+\dfrac{2\theta-3\sin\theta+\theta\cos\theta}{2\theta^5}(\phi^\wedge\rho^\wedge\phi^\wedge\phi^\wedge+\phi^\wedge\phi^\wedge\rho^\wedge\phi^\wedge)
\end{cases}$$
对于(2.7)式，当轴角趋近0时，有
(2.8)$$\begin{cases}Q_l|_{\theta \rightarrow 0}&\approx \dfrac{1}{2}\rho^\wedge
\\
\\
&+\dfrac{1}{6}(\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge+\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
&+\dfrac{1}{24}(\phi^\wedge\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge\phi^\wedge-3\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
&+\dfrac{1}{120}(\phi^\wedge\rho^\wedge\phi^\wedge\phi^\wedge+\phi^\wedge\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
Q_r|_{\theta \rightarrow 0}&\approx -\dfrac{1}{2}\rho^\wedge
\\
\\
&+\dfrac{1}{6}(\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge-\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
&-\dfrac{1}{24}(\phi^\wedge\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge\phi^\wedge-3\phi^\wedge\rho^\wedge\phi^\wedge)
\\
\\
&+\dfrac{1}{120}(\phi^\wedge\rho^\wedge\phi^\wedge\phi^\wedge+\phi^\wedge\phi^\wedge\rho^\wedge\phi^\wedge)
\end{cases}$$
对于(2.8)忽略二阶小量有
(2.9)$$\begin{cases}
Q_l|_{\theta \rightarrow 0}&\approx
\dfrac{1}{6}(\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge)+\dfrac{1}{2}\rho^\wedge
\\
\\
Q_r|_{\theta \rightarrow 0}&\approx \dfrac{1}{6}(\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge)-\dfrac{1}{2}\rho^\wedge
\end{cases}$$
(2.10)$$
Q_r(\xi) = Q_l(-\xi) = RQ_l(\xi) + (J_l\rho)^\wedge RJ_l
$$
对于\(T\)，其伴随矩阵定义为
(2.11)$$\begin{aligned}
\mathcal{T}(T) &=\exp(\xi^ \curlywedge)\\
&=I +\xi^\curlywedge \mathcal{J}_l\\
&=\begin{bmatrix}
R & \mathbf{t}^\wedge R\\
0 & R
\end{bmatrix}_{6 \times 6}\\
&=I
\\&+\frac{3\sin\theta – \theta\cos \theta}{2\theta}\xi^\curlywedge
\\&+
\frac{4-\theta\sin\theta- 4\cos \theta}{2\theta^2}(\xi^\curlywedge)^2
\\&+\frac{\sin\theta – \theta\cos\theta}{2\theta^3}(\xi^\curlywedge)^3
\\&+\frac{2-\theta\sin\theta -2\cos\theta}{2\theta^4}(\xi^\curlywedge)^4
\end{aligned}$$
对于一个比较小的量\(\delta \xi\)，对其进行左右扰动有如下结论
(2.12)$$
\log(\exp(\xi^\wedge)\exp(\delta \xi^\wedge))^\vee \approx \mathcal{J}_r^{-1} \delta \xi +\xi
\\
\log(\exp(\delta \xi^\wedge)\exp(\xi^\wedge))^\vee \approx \mathcal{J}_l^{-1} \delta \xi +\xi
$$
同样，有如下近似
(2.13)$$
\exp(\delta \xi^\wedge) \exp(\xi^\wedge) \approx \exp((\xi + \mathcal{J}_l^{-1}\delta \xi)^\wedge)
\\
\exp(\xi^\wedge)\exp(\delta \xi^\wedge) \approx \exp((\xi + \mathcal{J}_r^{-1}\delta \xi)^\wedge)
\\
\exp((\xi +\delta \xi)^\wedge) \approx \exp((\mathcal{J}_l\delta \xi)^\wedge)\exp(\xi^\wedge) \approx \exp(\xi^\wedge)\exp((\mathcal{J}_r\delta \xi)^\wedge)$$
下面列举一些常见性质
(2.14)$$
(-\xi)^\wedge = -\xi^\wedge
$$
(2.15)$$
(-\xi)^ \curlywedge = -\xi^ \curlywedge
$$
(2.16)$$
T\xi^\wedge = \xi^\wedge T
$$
(2.17)$$
\mathcal{T}\xi^\curlywedge= \xi^\curlywedge \mathcal{T}
$$
(2.18)$$
\mathcal{T}\xi = \xi
$$
(2.19)$$
(\xi^\wedge)^4 + \theta^2(\xi^\wedge)^2 = \mathbf{0}
$$
(2.20)$$
(\xi^{ \curlywedge})^5+ 2\theta^2(\xi^{ \curlywedge})^3+\theta^4\xi^{ \curlywedge}=\mathbf{0}
$$
(2.21)$$
T^{-1}= \exp((-\xi)^\wedge) = \exp(-\xi^\wedge) =\begin{bmatrix}
R^T & -R^T\mathbf{t}\\
\mathbf{0}^T &1
\end{bmatrix}
$$
(2.22)$$
\mathcal{T}^{-1}(T) =\mathcal{T}(T^{-1}) =\begin{bmatrix}
R^T & -R^T\mathbf{t}^\wedge\\
0 & R^T
\end{bmatrix}
$$
(2.23)$$
\mathcal{T}(T_1) \mathcal{T}(T_2) = \mathcal{T}(T_1T_2)$$
(2.24)$$
\mathcal{J}_l(\xi) =\mathcal{T}(T) \mathcal{J}_r(\xi) =\mathcal{J}_r(-\xi)$$
(2.25)$$\begin{aligned}
\mathcal{J}_l&=I\\
&+\frac{4-\theta \sin \theta – 4 \cos \theta}{2\theta^2}\xi^ \curlywedge \\
&+ \frac{4\theta- 5\sin\theta + \theta\cos \theta}{2\theta^3}(\xi^ \curlywedge)^2\\
&+ \frac{2-\theta \sin\theta- 2 \cos\theta}{2\theta^4}(\xi^ \curlywedge)^3\\
&+ \frac{2\theta -3\sin\theta + \theta \cos \theta}{2\theta^5}(\xi ^ \curlywedge)^4
\end{aligned}$$
对于SE(3)中对应的李代数向量\(\mathbf{h},\mathbf{g}\),以及任意大小为4的向量\(\mathbf{p}\)，存在如下性质
(2.26)$$(\mathbf{h}+\mathbf{g})^\wedge = \mathbf{h}^\wedge + \mathbf{g}^\wedge$$
(2.27)$$
(\mathbf{h}+\mathbf{g})^\curlywedge = \mathbf{h}^\curlywedge + \mathbf{g}^\curlywedge
$$
(2.28)$$
\mathbf{h}^\curlywedge \mathbf{g}= – \mathbf{g}^\curlywedge \mathbf{h}
$$
(2.29)$$(\mathbf{h}^\curlywedge \mathbf{g})^\wedge= \mathbf{h}^\wedge \mathbf{g}^\wedge – \mathbf{g}^\wedge \mathbf{h}^\wedge $$
(2.30)$$
\mathbf{h}^\curlywedge \mathbf{h}= 0
$$
(2.31)$$
\mathbf{g}^\wedge \mathbf{p}=\mathbf{p}^{\odot}\mathbf{g}
$$
(2.32)$$
\mathbf{p}^T \mathbf{g}^\wedge = \mathbf{g}^T\mathbf{p}^{\circledcirc}
$$
(2.33)$$(\mathcal{T}(T)\mathbf{g})^\wedge = T\mathbf{g}^\wedge T^{-1}$$
(2.34)$$
(\mathcal{T}(T)\mathbf{g})^\curlywedge=\mathcal{T}(T)\mathbf{g}^\curlywedge \mathcal{T}(T)^{-1}
$$
(2.35)$$
\exp((\mathcal{T}(T)\mathbf{g})^\wedge ) = T \exp(\mathbf{g}^\wedge) T^{-1}
$$
(2.36)$$
\exp(\mathbf{g}^\wedge ) T= T \exp((\mathcal{T}(T)^{-1}\mathbf{g})^\wedge)
$$
(2.37)$$
\exp((\mathcal{T}(T)\mathbf{g})^\curlywedge) = \mathcal{T}(T) \exp(\mathbf{g}^\curlywedge) \mathcal{T}(T)^{-1}
$$
由于左右雅克比逆在一些算法中, 可能会被使用到且其计算复杂, 当\(\phi\)均接近0时, 这时左右雅克比逆的表达式, 根据上面的公式，直接将二阶小量忽略掉可得到如下公式
(2.38)$$\begin{cases}
\mathcal{J}_l^{-1} &\approx I- \dfrac{1}{2}\begin{bmatrix}
\phi^\wedge & \rho^\wedge-\dfrac{1}{6}(\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge)
\\
\mathbf{0} & \phi^\wedge
\end{bmatrix}
\\
\\
\mathcal{J}_r^{-1} &\approx I+ \dfrac{1}{2}\begin{bmatrix}
\phi^\wedge & \rho^\wedge+\dfrac{1}{6}(\phi^\wedge\rho^\wedge+\rho^\wedge\phi^\wedge)
\\
\mathbf{0} & \phi^\wedge
\end{bmatrix}
\end{cases}$$
特别地，当\(\xi\)接近0时，同样忽略二阶小量，则上面公式可变为如下形式
(2.39)$$\begin{cases}
\mathcal{J}_l^{-1} &\approx I- \dfrac{1}{2}\begin{bmatrix}
\phi^\wedge & \rho^\wedge
\\
\mathbf{0} & \phi^\wedge
\end{bmatrix}
\\
\\
\mathcal{J}_r^{-1} &\approx I+ \dfrac{1}{2}\begin{bmatrix}
\phi^\wedge & \rho^\wedge
\\
\mathbf{0} & \phi^\wedge
\end{bmatrix}
\end{cases}$$
由于\(\xi\)由\(\phi\)与\(\rho\)组成，而在实际微扰中，\(\phi\)的值更加容易接近0，而\(\rho\)的值通常与空间位置值量级一致(一个远离0的值)，因此(2.39)形式的二阶小量忽略往往不成立，这里更建议使用(2.38)公式来进行近似(即只考虑旋转部分的微扰来获取二阶近似公式)。
根据上面的公式，当\(\phi\)的值接近0时，忽略一阶以上小量，可以大致得到如下结果
(2.40)$$
2\mathcal{J}_r^{-1} -\mathcal{T}(T)\approx I
\\
2\mathcal{J}_r+\mathcal{T}(T)\approx 3I
$$
3 Sim(3)
一个李群Sim(3)中的元素\(S\)，总能在李代数中找到包含7个元素的向量\(\zeta\)与其对应， 其中\(\zeta\)表示如下
(3.1)$$
\zeta =\begin{bmatrix}
\rho\\
\phi \\
\sigma
\end{bmatrix}
$$
约定如下符号
(3.2)$$
\zeta ^\wedge =\begin{bmatrix}
\sigma I + \phi ^\wedge & \rho\\
\mathbf{0}^T & 0
\end{bmatrix}
$$
对应的指数映射如下
(3.3)$$
\begin{aligned}
S&=\exp(\zeta ^\wedge)\\
&=\begin{bmatrix}
e^\sigma \exp(\phi^\wedge) & J_s\rho\\
\mathbf{0}^T & 1
\end{bmatrix}\\
&=
\begin{bmatrix}
sR & J_s\rho\\
\mathbf{0}^T & 1
\end{bmatrix}\\
&=
\begin{bmatrix}
sR & \mathbf{t}\\
\mathbf{0}^T & 1
\end{bmatrix}
\end{aligned}
$$
上面公式中\(J_s\)为
(3.4)$$
J_s=\frac{e^\sigma – 1}{\sigma}I+
\frac{\sigma e^\sigma \sin\theta + (1-e^\sigma \cos\theta)\theta}{\sigma ^2 + \theta^2}a^\wedge +
(\frac{e^\sigma – 1}{\sigma} – \frac{(e^\sigma \cos\theta – 1)\sigma + \theta e^\sigma \sin\theta}{\sigma ^2 + \theta^2})a^\wedge a^\wedge
$$
(3.5)$$
S^{-1}=\begin{bmatrix}
\frac{R^T}{s} & – \frac{R^T \mathbf{t}}{s}\\
\mathbf{0}^T & 1
\end{bmatrix}
$$
4 矩阵指数
对于一个方阵\(A\)，其矩阵指数为
(4.1)$$
\exp(A) = I + A + \frac{1}{2!}A^2 + \frac{1}{3!}A^3+… = \sum_{n=0}^{\infty}\frac{1}{n!}A^n
$$
当\(A\)是一个趋近0矩阵的小量时，根据公式(4.1)，忽略高阶小量有
(4.2)$$
\exp(A) \approx I + A
$$
对于方阵\(A\)，还有如下性质
(4.3)$$
\exp(A)^{-1}=\exp(-A)$$
(4.4)$$
\exp(A^T)=\exp(A)^T
$$
(4.5)$$
\det(\exp(A))=\exp(\mathbf{Tr}(A))$$
(4.6)$$
\exp(BAB^{-1})=B\exp(A)B^{-1}
$$
如果方阵\(A\)与\(B\)可交换，即\(AB=BA\)，那么它们指数函数满足
(4.7)$$
\exp(A+B)=\exp(A)\exp(B)$$
5 应用
5.1 相机位姿
可以想象，一个相机，当相对原点先旋转\(R\)，然后再进行\(\mathbf{t}\)的偏移。因为原点已知，因此如果知道\(R\)和\(\mathbf{t}\)，那么我们就可以确定出相机的位置与朝向，也因此，可以使用如下的\(T\)来表示相机的位姿：
(5.1)$$
T=\begin{bmatrix}
R & \mathbf{t}\\
\mathbf{0}^T & 1
\end{bmatrix}
$$
仔细观察(5.1)的形式，可以发现和上面SE(3)的形式类似，因此一般会使用\(\xi\)来描述相机的位姿。
注意：
(1)当使用\(\xi\)来描述相机位姿时 ， \(\xi\)当中的\(\phi\)可以用来描述位姿的旋转，但是 \(\rho\)却不是很好理解。毕竟按这种表示方式，偏移还和旋转相关。
(2)其实，我们也可以直接使用\(\phi\)与\(\mathbf{t}\)来描述相机位姿，
同样的，如果相对原点先旋转\(R\)，然后再进行\(s\)的缩放，最后再进行\(t\)的偏移，则根据这几个参数，同样可以确定出相机的位置与朝向，因此也可以使用如下的\(T\)来表示相机的位姿：
(5.2)$$
T=\begin{bmatrix}
sR & \mathbf{t}\\
\mathbf{0}^T & 1
\end{bmatrix}
$$
仔细观察(5.2)的形式，可以发现和上面Sim(3)的形式类似，因此一般会使用\(\zeta\)来描述相机的位姿。
对比(5.1)与(5.2)可以发现，(5.1)是(5.2)的特殊形式，对于没有特殊说明，当使用到(5.1)的表示形式时，这里都默认缩放系数\(s=1\)。
5.2 相机光心
空间中一个3D点\(\mathbf{p}\)变换到相机\(T\)坐标系下到位姿为\(\mathbf{p}'\)，这里我们约定：变换的方式为先旋转，再缩放，最后再偏移。则3D点的变换可表示为
(5.3)$$
\mathbf{p}'=sR\mathbf{p}+\mathbf{t}
$$
一般地，我们约定相机在自己相机坐标系下的坐标为原点，如果相机在空间中地坐标为\(\mathbf{c}\)（光心），那么根据公式(5.3)它们有如下关系
(5.4)$$
\mathbf{0}=sR\mathbf{c}+\mathbf{t}
$$
通过公式(5.4)可以得到光心的表达式
(5.5)$$
\mathbf{c}=-\frac{1}{s}R^T\mathbf{t}
$$
特别地，当时缩放系数为1时，则光心可表示为
(5.6)$$
\mathbf{c}=-R^T\mathbf{t}
$$
5.3 点的变换
公式(5.3)可以写成如下形式
(5.7)$$
\begin{bmatrix}
\mathbf{p}'\\
1
\end{bmatrix}
=
\begin{bmatrix}
sR & \mathbf{t}\\
\mathbf{0}^T & 1
\end{bmatrix}
\begin{bmatrix}
\mathbf{p}\\
1
\end{bmatrix}
= T
\begin{bmatrix}
\mathbf{p}\\
1
\end{bmatrix}
$$
对于一个点\(\mathbf{p}\)，先经过\(T_0\)的变换，再经过\(T_1\)的变换，最终得到\(\mathbf{p}'\)，则可表达为
(5.8)$$
\begin{bmatrix}
\mathbf{p}'\\
1
\end{bmatrix}
= T_1T_0
\begin{bmatrix}
\mathbf{p}\\
1
\end{bmatrix}
= T'
\begin{bmatrix}
\mathbf{p}\\
1
\end{bmatrix}
$$
因此中间的变换可表示为
(5.9)$$
T' = T_1T_0
$$
5.4 位姿变换
我们知道，对于空间中一堆3D点云，我们按照(5.7)让点云在刚体变换\(T\)下进行坐标的变换，可以发现，点云中各个点的相对位置不会改变。
同样地，对于空间中一些相机和一些点云组成的整体，现在让它们进行一个刚体变换\(T\)，我们希望变换之后，相机之间，以及相机与点云之间，它们的相对位置信息不会改变。
首先，相机与点的相对位置关系不变可以体现在点在相机坐标系下的坐标不变，也因此，如果相机原始位姿为\(T_0\)，经过\(T\)变换后的位姿\(T_1\)，对于一个空间原始的空间点\(\mathbf{p}\)有
(5.10)$$
T_0
\begin{bmatrix}
\mathbf{p}\\
1
\end{bmatrix}
=
\begin{bmatrix}
\mathbf{p}'\\
1
\end{bmatrix}
=T_1(T\begin{bmatrix}
\mathbf{p}\\
1
\end{bmatrix})
=T_1T\begin{bmatrix}
\mathbf{p}\\
1
\end{bmatrix}
$$
从上面的公式可以得到
(5.11)$$
T_1 = T_0T^{-1}
$$
按照(5.11)式定义的相机变换，可以满足相机与空间3D点之间的相对位置关系，而对于相机间的相对位置关系，要使其既满足(5.11)的变换关系，又能使得相对位置关系不因(5.11)的变换而改变，则对于位姿为\(T_i\)与\(T_j\)的相机，它们的相对位置关系\(T_{ij}\)，可以定义为如下形式
(5.12)$$
T_{ij}=T_iT_j^{-1}
$$
公式(5.12)就是一些PGO优化中边的测量形式。
需要注意的是，文献[1]中提到的另一种测量形式如下
(5.13)$$
T_{ij}=T_i^{-1}T_j
$$
对于(5.13)这种测量形式不建议使用，因为这种测量在优化中，如果有一个顶点不变的情况下，理论上没有问题，但是，往往我们更加注重的是满足位姿与点可共同变换的测量，因此建议尽量使用(5.12)的形式，因为这个公式具有更强的物理意义！
6 矩阵向量运算
下面记录一些矩阵向量的公式[3]，所写公式中点矩阵变量/向量均满足维度要求下成立
(6.1)$$
ABC=A(BC)$$
(6.2)$$
(AB)^T=B^TA^T
$$
(6.3)$$
(A^T)^{-1}=(A^{-1})^T
$$
(6.4)$$
(AB)^{-1}=B^{-1}A^{-1}
$$
(6.5)$$
\mathbf{Tr}(ABC) = \mathbf{Tr}(BCA) = \mathbf{Tr}(CAB)$$
(6.6)$$
\frac{\partial A^{-1}}{\partial \mathbf{x}}=A^{-1}\frac{\partial A }{\partial \mathbf{x}}A^{-1}
$$
(6.7)$$
\frac{\partial AB}{\partial \mathbf{x}}=\frac{\partial A}{\partial \mathbf{x}}B+A\frac{\partial B}{\partial \mathbf{x}}
$$
(6.8)$$\frac{\partial (aB)}{\partial \mathbf{x}}=B \otimes \frac{\partial a}{\partial \mathbf{x}} + a\frac{\partial B}{\partial \mathbf{x}}$$
上面公式中的\(a\)为一个数，\(\otimes\)为 Kronecker product运算符。
(6.9)$$\frac{\partial \mathbf{Tr}(A)}{\partial \mathbf{x}}=\mathbf{Tr}(\frac{\partial A}{\partial \mathbf{x}})$$
(6.10)$$\frac{\partial \mathbf{x}^TA \mathbf{x}}{\partial \mathbf{x}}=(A+A^T)\mathbf{x}$$
(6.11)$$\det(I+\mathbf{a}\mathbf{b}^T)=1+\mathbf{a}^T\mathbf{b}$$
(6.12)$$
\det(A^{-1})=\frac{1}{\det(A)}
$$
(6.13)$$
||A||_F=\sqrt{\mathbf{Tr}(AA^T)}
$$
(6.14)$$
||\mathbf{x}+\mathbf{y}||_2^2 = ||\mathbf{x}||_2^2 + ||\mathbf{y}||_2^2 + 2\mathbf{x}^T\mathbf{y}
$$
(6.15)$$
(I+\mathbf{xy}^T)^{-1}=I-\frac{\mathbf{xy}^T}{1+\mathbf{y}^T\mathbf{x}}
$$
Sherman–Morrison 公式
(6.16)$$
(A+\mathbf{xy}^T)^{-1}=A^{-1}-\frac{A^{-1}\mathbf{xy}^TA^{-1}}{1+\mathbf{y}^TA^{-1}\mathbf{x}}
$$
Sherman–Morrison–Woodbury公式
(6.17)$$
\begin{aligned}
(A+UCV)^{-1}&=A^{-1}-A^{-1}U(C^{-1}+VA^{-1}U)^{-1}VA^{-1}
\\
AB(D+CAB)^{-1}&=(A^{-1}+BD^{-1}C)^{-1}BD^{-1}
\\
(D+CAB)^{-1}CA&=D^{-1}C(A^{-1}+BD^{-1}C)^{-1}
\\
(A+BC)^{-1}&=A^{-1}-A^{-1}B(I+CA^{-1}B)^{-1}CA^{-1}
\end{aligned}
$$
(6.18)$$
\begin{aligned}
(A+B)^{-1} &= A^{-1} – A^{-1}B(B+BA^{-1}B)^{-1}BA^{-1}
\\
&=A^{-1}-A^{-1}(I+BA^{-1})^{-1}BA^{-1}
\\
&=A^{-1}-(A+AB^{-1}A)^{-1}
\end{aligned}
$$
(6.19)$$
\begin{bmatrix}
A & 0\\
C & D
\end{bmatrix}
^{-1}
=
\begin{bmatrix}
A^{-1} & 0\\
-D^{-1}CA^{-1} & D^{-1}
\end{bmatrix}
$$
(6.20)$$
\begin{bmatrix}
A & B\\
C & D
\end{bmatrix}^{-1}
=
\begin{bmatrix}
-C^{-1}D(B-AC^{-1}D)^{-1} & C^{-1}+C^{-1}D(B-AC^{-1}D)^{-1}AC^{-1}\\
(B-AC^{-1}D)^{-1} & -(B-AC^{-1}D)^{-1}AC^{-1}
\end{bmatrix}
$$
(6.21)$$ \begin{bmatrix}
A & B\\
C & D
\end{bmatrix}^{-1}
=
\begin{bmatrix}
-(C-DB^{-1}A)^{-1}DB^{-1} & (C-DB^{-1}A)^{-1}
\\
B^{-1}+B^{-1}A(C-DB^{-1}A)^{-1}DB^{-1} & -B^{-1}A(C-DB^{-1}A)^{-1}
\end{bmatrix}
$$
(6.22)$$
\begin{bmatrix}
A & B\\
C & D
\end{bmatrix}^{-1}
=
\begin{bmatrix}
A^{-1}+A^{-1}B(D-CA^{-1}B)^{-1}CA^{-1} & -A^{-1}B(D-CA^{-1}B)^{-1}
\\
-(D-CA^{-1}B)^{-1}CA^{-1} & (D-CA^{-1}B)^{-1}
\end{bmatrix}
$$
(6.23)$$
\begin{bmatrix}
A & B\\
C & D
\end{bmatrix}^{-1}
=
\begin{bmatrix}
(A-BD^{-1}C)^{-1} &-(A-BD^{-1}C)^{-1}BD^{-1}
\\
-D^{-1}C(A-BD^{-1}C)^{-1} & D^{-1}+D^{-1}C(A-BD^{-1}C)^{-1}BD^{-1}
\end{bmatrix}
$$
如果\(A\)为对称矩阵，则必然存在一个旋转矩阵\(R\)使得
(6.24)$$
R^TAR=\mathbf{diag}(d_0,d_1,d_2)$$
对于一个复数方阵\(A\)，有如下性质
(6.25)$$
\det(\exp(A))=\exp(\mathbf{Tr}(A))
\\
\exp(A)^{-1}=\exp(-A)
\\
\exp(A^T)=\exp(A)^T
\\
\exp(BAB^{-1})=B\exp(A)B^{-1}
$$
如果方阵\(A\)与\(B\)可交换，即\(AB=BA\)，那么它们指数函数满足
(6.26)$$
\exp(A+B)=\exp(A)\exp(B)$$
对于同维度的向量\(\mathbf{x}\),\(\mathbf{y}\)，以及一个向量\(\mathbf{z}\)，它们之间满足如下关系
(6.27)$$\begin{aligned}
\mathbf{x}^T \mathbf{yz}
&= \mathbf{y}^T \mathbf{xz}\\
&= \mathbf{z} \otimes \mathbf{x}^T \mathbf{y}\\
&= \mathbf{z} \otimes \mathbf{y}^T \mathbf{x}
\end{aligned}$$
(6.28)$$\begin{bmatrix}
R_2 & t_2
\\
0 & 1
\end{bmatrix}^{-1} \begin{bmatrix}
R_1 & t_1
\\
0 & 1
\end{bmatrix} = \begin{bmatrix}
R_2^TR_1 & R_2^T(t_1-t_2)
\\
0 & 1
\end{bmatrix}
$$
(6.29) 在右手坐标系中，z轴逆时针旋转\(\theta\)角度得到的旋转矩阵为如下的\(R_z\)(旋转前的状态，通过\(R_z\)变换为旋转后的状态）
$$R_z = \begin{bmatrix}
\cos \theta & -\sin \theta & 0
\\
\sin \theta & \cos\theta & 0
\\
0 & 0& 1
\end{bmatrix}
$$
(6.30) 在右手坐标系中，z轴顺时针旋转\(\theta\)角度得到的旋转矩阵为如下 \(R_z\)(旋转前的状态，通过\(R_z\)变换为旋转后的状态）
$$R_z = \begin{bmatrix}
\cos \theta & \sin \theta & 0
\\
-\sin \theta & \cos\theta & 0
\\
0 & 0& 1
\end{bmatrix}
$$
(6.31) 在右手坐标系中，x轴逆时针旋转\(\theta\)角度得到的旋转矩阵为如下\(R_x\)(旋转前的状态，通过\(R_x\)变换为旋转后的状态）
$$R_x = \begin{bmatrix}
1 & 0 & 0
\\
0 & \cos\theta & -\sin\theta
\\
0 & \sin\theta& \cos\theta
\end{bmatrix}
$$
(6.32) 在右手坐标系中，y轴逆时针旋转\(\theta\)角度得到的旋转矩阵为如下\(R_y\)(旋转前的状态，通过\(R_y\)变换为旋转后的状态）
$$R_y = \begin{bmatrix}
\cos \theta & 0 & -\sin\theta
\\
0 & 1 & 0
\\
\sin\theta & 0& \cos\theta
\end{bmatrix}
$$
7 叉乘运算
设向量\(\mathbf{a}=[a_0,a_1,a_2]^T、\mathbf{b}=[b_0,b_1,b_2]^T、\mathbf{c}、\mathbf{d}\)为大小为3的向量。这里定义叉乘
(7.1)
$$\mathbf{a} \times \mathbf{b} =\begin{bmatrix}
a_1b_2-a_2b_1
\\
a_2b_0-a_0b_2
\\
a_0b_1-a_1b_0
\end{bmatrix}$$
上面公式里以右手规则来确定叉乘后的向量方向：右手食指指向\(\mathbf{a}\)的方向，右手中指指向\(\mathbf{b}\)的方向，右手大拇指指向\(\mathbf{a}\times\mathbf{b}\)的方向。
对于叉乘有如下性质
(7.2)$$\mathbf{a} \times \mathbf{0} = \mathbf{0}$$
(7.3)$$\mathbf{a} \times \mathbf{a} = \mathbf{0}$$
(7.4)$$\mathbf{a} \times \mathbf{b} = -\mathbf{b} \times \mathbf{a}$$
(7.5)$$(k\mathbf{a}) \times \mathbf{b} = \mathbf{a} \times (k\mathbf{b})=k(\mathbf{a}\times\mathbf{b})$$
(7.6)$$\mathbf{a} \times (\mathbf{b} + \mathbf{c})= \mathbf{a} \times \mathbf{b}+\mathbf{a} \times \mathbf{c}$$
(7.7)$$(\mathbf{b} + \mathbf{c})\times \mathbf{a}=\mathbf{b} \times \mathbf{a} + \mathbf{c} \times \mathbf{a}$$
(7.8)$$\mathbf{a} \cdot (\mathbf{b} \times \mathbf{c}) = (\mathbf{a} \times \mathbf{b})\cdot \mathbf{c}=(\mathbf{c}\times\mathbf{a})\cdot \mathbf{b}$$
(7.9)$$\mathbf{a} \times (\mathbf{b} \times \mathbf{c}) = (\mathbf{c}\times \mathbf{a})\times\mathbf{b}+(\mathbf{a}\times \mathbf{b})\times \mathbf{c}=(\mathbf{a} \cdot\mathbf{c})\mathbf{b} – (\mathbf{a} \cdot\mathbf{b})\mathbf{c}$$
(7.10)$$(\mathbf{a} \times \mathbf{b})\times \mathbf{c} = \mathbf{a}\times (\mathbf{b}\times \mathbf{c}) – \mathbf{b}\times(\mathbf{a}\times\mathbf{c})=(\mathbf{a}\cdot\mathbf{c})\mathbf{b}-(\mathbf{b}\cdot\mathbf{c})\mathbf{a}$$
(7.11)$$||\mathbf{a}\times\mathbf{b}||^2+(\mathbf{a}\cdot \mathbf{b})^2=||\mathbf{a}||^2||\mathbf{b}||^2$$
(7.12)$$\dfrac{\partial (\mathbf{a} \times \mathbf{b})}{\partial t}=\dfrac{\partial \mathbf{a}}{\partial t}\times \mathbf{b}+\mathbf{a} \times \dfrac{\partial \mathbf{b}}{\partial t}$$
(7.13)$$|\mathbf{a} \times \mathbf{b}|=|\mathbf{a}||\mathbf{b}|\sin \theta_{a,b}$$
公式里\(\theta_{a,b}\)为向量\(\mathbf{a}\)和\(\mathbf{b}\)的夹角。
8 G2O库
这里是直接下载下来的最新g2o库。
参考
[1] 高翔,张涛. 视觉SLAM十四讲:从理论到实践[M]. 电子工业出版社,2017.
[2] 高翔,谢晓佳. 机器人学中的状态估计[M]. 西安交通大学出版社,2018
[3] Petersen K B, Pedersen M S. The matrix cookbook[M]. Technical University of Denmark, 2012