二维空间和三维空间刚体变换中雅克比矩阵的推导

1 预备的知识

补充一些李群 $\mathrm{SE}(2)$ 和李代数 $\mathrm{se}(2)$ 的知识, 因为视觉 SLAM十四讲¹中只给出了 $\mathrm{SE}(3)$ 和 $\mathrm{se}(3)$ 的对应关系, 但是没有给出二维空间的对应关系。其中二维空间的李群李代数参考Lie Groups for 2D and 3D Transformations。²

1.1 李群$\mathrm{SE}(2)$

二维刚体变换公式为 $T,T\in \mathrm{SE}(2)$, 如下所示:
$$\begin{array}{rl}& \mathbf{R}=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\in \mathrm{SO}(2),\mathbf{t}=\left[\begin{array}{l}{t}_x\\ t_y\end{array}\right]\in {\mathbb{R}}^2\\ & T=\left(\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & t_x\\ \sin \theta & \cos \theta & t_y\\ 0& 0& 1\end{array}\right]\in \mathrm{SE}(2)\end{array}$$

1.2 李代数$se(2)$

与$\mathrm{SE}(2)$对应的李代数 ${\mathbf{\varphi }}^{\wedge }\in \mathrm{se}(2)$, 如下
$$\begin{array}{rlr}& G_1=\left(\begin{array}{lll}0& 0& 1\\ 0& 0& 0\\ 0& 0& 0\end{array}\right),G_2=\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right),G_3=\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right)& \\ & \mathbf{\varphi }={\left(\begin{array}{ll}{\mathrm{u}}_1\cdot {\mathrm{u}}_2& \theta \end{array}\right)}^T\in {\mathbb{R}}^3& \\ & {\mathbf{\varphi }}^{\wedge }={\mathbf{u}}_1{G}_1+{\mathbf{u}}_2{G}_2+\theta G_3=\left[\begin{array}{ccc}0& -\theta & {\mathrm{u}}_1\\ \theta & 0& {\mathrm{u}}_2\\ 0& 0& 0\end{array}\right]\in \text{ se (2) }& \end{array}$$ 上式中的 $\mathbf{\varphi }$ 即为我们理解的旋转向量, 或者称为角轴。注意 ${\mathbf{\varphi }}^{\wedge }$ 是反对称阵, 但是和 $\mathbf{so}(3)$ 中的反对称阵写法不同。在视觉 SLAM 十四讲中, 主要介绍的是三维空间, 李群 $\mathrm{SE}(3$ ) 和李代数 $\mathrm{se}(3)$ 是一一对应 (旋转角度在 $\pm \pi$ 之间, 李群和李代数元素是一一对应的) 的, $\mathrm{SE}(3)$ 为 $4\times$ 4 的旋转矩阵, $\mathrm{se}(3)$ 为 $6\times 1$ 的旋转向量。李群 $\mathrm{SO}(3)$ 和李代数 $\mathrm{so}(3)$ 是一一对应的, $\mathrm{SO}(3)$ 为 $3\times 3$ 的旋转矩阵, $\mathrm{so}(3)$ 为 $3\times 1$ 的旋转向量。

1.3 指数映射（将李代数 $\mathrm{se}(2)$ 转换为李群 $\mathrm{SE}(2)$ )

已知李代数 $\mathrm{se}(2)$ 如下:
$$\begin{array}{c}\mathbf{\varphi }=\left[\begin{array}{c}{\mathrm{u}}_1\\ {\mathrm{u}}_1\\ \theta \end{array}\right]\in {\mathbb{R}}^3\\ {\mathbf{\varphi }}^{\wedge }=\left[\begin{array}{ccc}0& -\theta & {\mathrm{u}}_1\\ \theta & 0& {\mathrm{u}}_2\\ 0& 0& 0\end{array}\right]\in \mathrm{se}(2)\end{array}$$那么对应的李群 $\mathrm{SE}(2)$ 为:
$$\mathrm{T}=\exp \left({\mathbf{\varphi }}^{\wedge }\right)=\left(\begin{array}{cc}\exp \left({\theta }^{\wedge }\right)& \mathbf{Vu}\\ 0& 0\end{array}\right)\dots \dots \dots \dots \text{ (1) }$$其中 $\exp \left({\theta }^{\wedge }\right)=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right],\mathbf{V}=\frac{1}{\theta }\cdot \left(\begin{array}{cc}\sin \theta & -(1-\cos \theta )\\ 1-\cos \theta & \sin \theta \end{array}\right)$, $\mathbf{u}=\left[\begin{array}{l}{\mathrm{u}}_1\\ {\mathrm{u}}_2\end{array}\right]$ 。那么
$$\mathrm{T}=\exp \left({\mathbf{\varphi }}^{\wedge }\right)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & \frac{\sin \theta }{\theta }{\mathrm{u}}_1-\frac{1-\cos \theta }{\theta }{\mathrm{u}}_2\\ \sin \theta & \cos \theta & \frac{1-\cos \theta }{\theta }{\mathrm{u}}_1+\frac{\sin {\delta }_{\theta }}{{\delta }_{\theta }}{\mathrm{u}}_2\\ 0& 0& 1\end{array}\right]\dots \dots \dots \dots \text{ (2) }$$从公式 (2) 可以知道, 李代数的 $\mathbf{\varphi }$ 这个向量中的三个量 ${\mathrm{u}}_1$ 、 ${\mathrm{u}}_2、\theta$, 刚好能对应到 $\mathbf{T}$ 中的 $\theta$, 因此李代数中的 $\theta$ 确实真实反映了旋转,可是 ${\mathrm{u}}_1、{\mathrm{u}}_2$ 只是部分反映了平移, 需要乘以 $\mathbf{V}$, 才是变换成 $\mathbf{T}$ 后真正的平移量。

1.4 求极限

当 $x\to 0$ 时, $\sin x\sim x,1-\cos x\sim \frac{1}{2}{x}^2,\cos x\sim 1,x^2\sim 0$
当 $x,y\to 0$ 时, $x\cdot y\sim 0$

2 二维和三维刚体变换求雅可比矩阵

2.1 问题描述

在 gtsam 的官方文档gtsam.pdf 中的 3.2 节 Defining Custom Factors,构造一个类似于 GPS 的因子, 它是二维的坐标。在自定义因子时，需要提供雅可比矩阵。
因此如何求导雅可比矩阵是关键问题，而雅可比矩阵是误差方程对状态量或者状态增量的求导。问题也就转换为在旋转矩阵 $\mathrm{SE}(2)$ 上做扰动,然后对扰动量（状态增量）求导，即为我们需要的雅可比矩阵。但是由于李群只对乘法封闭, 不对加分封闭, 因此我们没法对这个扰动量求导，不过李群和李代数是一一对应的, 而李代数是对加法封闭的。我们在求导时, 只能对线性空间求导, 李群不是线性空间, 李代数是线性空间, 因此我们将问题转换为对扰动后的李代数求导。误差方程为:
$$E(q)\triangleq h(q)-m$$其中, $E(q)$ 为误差函数, $m$ 为二维的观测量, 观测量是不准确的, $h(q)$ 认为是一个广义的测量函数, 它能求出入参送进去之后的二维坐标, 我们希望优化的就是 $h(q)$ 函数, 最终得到 $E(q)$ 最小的目的, 也就是最小二乘, 如下所示:
$$h(q)=\left[\begin{array}{l}{q}_x\\ q_y\end{array}\right]$$

2.2 方法 1: 对扰动的量 $\Delta T$ 对应的李代数 $\xi$ 进行求导

在 gtsam 中, 定义雅可比矩阵如下所示, 注意我们最终要的是 $H$ :
$$\mathbf{h}\left(\mathbf{T}\cdot \exp {\xi }^{\wedge }\right)\approx \mathbf{h}(\mathbf{T})+\mathbf{H}\xi \dots \dots \dots \dots \text{ (3) }$$ 换言之，
$$\mathbf{E}(\mathbf{q})=\mathbf{h}\left(\mathbf{T}\cdot \exp {\xi }^{\wedge }\right)-\mathbf{h}(\mathbf{T})=\mathbf{H}\xi \dots \dots \dots \dots \text{ (4) }$$其中 $T=\left[\begin{array}{ccc}\cos q_{\theta }& -\sin q_{\theta }& q_x\\ \sin q_{\theta }& \cos q_{\theta }& q_x\\ 0& 0& 1\end{array}\right]$, 对 $T$ 做扰动, 扰动的量为 $\Delta T,\xi$ 为 $\Delta T$对应的李代数。

公式 (4) 中的 $\mathrm{H}$ 即为我们要的雅可比矩阵, 直观上看 $\mathbf{H}=\mathbf{E}(\mathbf{q}){\xi }^{-1}$，可惜的是 $\xi$ 向量是没有逆的, 因此只能求导，则
$$\mathbf{H}=\frac{\partial \mathbf{E}(\mathbf{q})}{\partial \mathbf{\xi }}\dots \dots \dots \dots \dots \dots \text{ (5) }$$为了更好的理解公式 (5), 我们做一个类比, 比如线性空间 $y=$ $kx$, 如果已知一组 $y_i$ 和对应的 $x_i$, 那么导数 $y^{\prime }=k=\frac{y_i}{x_i}$, 另一种方式是已知了表达式 $y=kx$, 那么 $y^{\prime }=\frac{\partial y}{\partial x}$, 两种方式都可以求出来 $k$ 。可是对于公式 (4), 我们是没法直接对 $\xi$ 这个向量求逆, 因此只能通过求导的方式来求出 $H$ 。
由于我们无法对非线性空间求导, 只能对线性空间求导, 定义 $\xi$ 李代数如下所示。注意这里面的三个量需要转换到李群 $\Delta T$, 才能找到对 $q_x$ 和$q_y$ 的扰动, 也就是说我们后面在做优化的过程中, 求的是 $\xi$ 变化量, 然后根据指数映射求出 $\Delta T$, 最终 $T_{\text{result }}=T\Delta T$, 最终的位置和姿态从 $T_{\text{result }}$ 中取出来即可。
$$\xi =\left[\begin{array}{l}\delta x\\ \delta y\\ \delta \theta \end{array}\right]$$利用李代数 $\xi$ 求出李群 $\Delta T$, 即指数映射公式 (2)。注意，我也参考学习了GTSAM曲线拟合与自定义因子的部分内容，指数映射公式写的是错误的，其他地方都写的很好。可得:
$$\Delta T=\left[\begin{array}{ccc}\cos \delta \theta & -\sin \delta \theta & \frac{\sin \delta \theta }{\delta \theta }\delta x-\frac{1-\cos \delta \theta }{\delta \theta }\delta y\\ \sin \delta \theta & \cos \delta \theta & \frac{1-\cos \delta \theta }{\delta \theta }\delta x+\frac{\sin {\delta }_{\theta }}{{\delta }_{\theta }}\delta y\\ 0& 0& 1\end{array}\right]$$利用目录 1.4 中的极限法则, $\xi$ 的三个量为小量, 也就是趋近于0，那么 $\cos \delta \theta \sim 1,\sin \delta \theta \sim$ $\delta \theta$ 。对 $\Delta T$ 右上角两个式子展开求, 那么:
$$\begin{array}{rl}& \underset{\xi \to 0}{\lim }\frac{\sin {\delta }_{\theta }}{{\delta }_{\theta }}{\delta }_x-\frac{1-\cos {\delta }_{\theta }}{{\delta }_{\theta }}{\delta }_y\\ =& \underset{\xi \to 0}{\lim }1\cdot {\delta }_x-\frac{\frac{1}{2}{\delta }_{\theta }^2}{{\delta }_{\theta }}\cdot {\delta }_y\\ =& \underset{\xi \to 0}{\lim }{\delta }_x\\ & \underset{\xi \to 0}{\lim }\frac{1-\cos {\delta }_{\theta }}{{\delta }_{\theta }}{\delta }_x+\frac{\sin {\delta }_{\theta }}{{\delta }_{\theta }}{\delta }_y\\ =& \underset{\xi \to 0}{\lim }\frac{\frac{1}{2}{\delta }_{\theta }^2}{{\delta }_{\theta }}{\delta }_x+{\delta }_y.\\ =& \underset{\xi \to 0}{\lim }{\delta }_y.\end{array}$$因此, $\Delta T\approx \left[\begin{array}{ccc}1& -\delta \theta & \delta x\\ \delta \theta & 1& \delta y\\ 0& 0& 1\end{array}\right]$。
那么 $\mathbf{h}\left(\mathbf{T}\cdot \exp {\xi }^{\wedge }\right)\approx h\left(\left[\begin{array}{ccc}\cos \left(q_{\theta }\right)& -\sin \left(q_{\theta }\right)& q_x\\ \sin \left(q_{\theta }\right)& \cos \left(q_{\theta }\right)& q_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& -\delta \theta & \delta x\\ \delta \theta & 1& \delta y\\ 0& 0& 1\end{array}\right]\right)$
$$=h\left(\left[\begin{array}{ccc}\cos q_{\theta }-\sin \left(q_{\theta }\right)\cdot {\delta }_{\theta }& -\cos \left(q_{\theta }\right){\delta }_{\theta }-\sin \left(q_{\theta }\right)& \cos \left(q_{\theta }\right){\delta }_x-\sin \left(q_{\theta }\right){\delta }_y+q_x\\ \sin q_{\theta }+\cos \left(q_{\theta }\right)\cdot {\delta }_{\theta }& -\sin \left(q_{\theta }\right){\delta }_{\theta }+\cos \left(q_{\theta }\right)& \sin \left(q_{\theta }\right){\delta }_x+\cos \left(q_{\theta }\right){\delta }_y+q_y\\ 0& 0& 1\end{array}\right]\right)$$$$=\left[\begin{array}{l}{q}_x+\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ q_y+\sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]$$那么, 由 $\mathbf{E}(\mathbf{q})=\mathbf{h}\left(\mathbf{T}\cdot \exp {\xi }^{\wedge }\right)-\mathbf{h}(\mathbf{T})$, 可得

$\mathbf{E}(\mathbf{q})=\left[\begin{array}{l}{q}_x+\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ q_y+\sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]-\left[\begin{array}{l}{q}_x\\ q_y\end{array}\right]={\left[\begin{array}{c}\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ \sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]}_{2\times 1}$, 则 $H$ 是函数 $E(q)$ 对 $\xi$ 求导数，那么
$$\begin{array}{rl}& H=\frac{\partial E(q)}{\partial \xi }\\ & =\frac{\partial {\left[\begin{array}{l}\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ \sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]}_{2\times 1}}{\partial {\left[\begin{array}{l}\delta x\\ \delta y\\ \delta \theta \end{array}\right]}_{3\times 1}}\\ & ={\left[\begin{array}{ccc}\cos q_{\theta }& -\sin q_{\theta }& 0\\ \sin q_{\theta }& \cos q_{\theta }& 0\end{array}\right]}_{2\times 3}\\ & =\left[\begin{array}{cc}{R}_{2\times 2}& 0_{2\times 1}\end{array}\right]\end{array}$$ $H$即为我们要的三维空间中误差方程的雅可比矩阵, 其中$R_{2\times 2}\in \mathrm{SO}(2)$。至此, 我们求出来了雅可比矩阵，这个矩阵和gtsam.pdf 中的 3.2 节 Defining Custom Factors中的雅可比矩阵一样，我们推导成功了。

2.2 方法 2: 直接用公式推导 ³

用误差扰动公式求导，不过这个公式是怎么来的，我还是很纳闷:
$$\begin{array}{rl}& {\hat{q}}_A=q_A+\delta q\\ & \cdots \cdots =q_A+\left[\begin{array}{cc}\cos \left(q_{\theta }\right)& -\sin \left(q_{\theta }\right)\\ \sin \left(q_{\theta }\right)& \cos \left(q_{\theta }\right)\end{array}\right]\left[\begin{array}{l}\delta x\\ \delta y\end{array}\right]\end{array}$$ ${\hat{q}}_A$ 为A点的坐标估计量, $q_A$ 为A点的坐标真值, $\delta q=E(q)=\left[\begin{array}{cc}\cos \left(q_{\theta }\right)& -\sin \left(q_{\theta }\right)\\ \sin \left(q_{\theta }\right)& \cos \left(q_{\theta }\right)\end{array}\right]\left[\begin{array}{l}\delta x\\ \delta y\end{array}\right]=$ ${\left[\begin{array}{l}\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ \sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]}_{2\times 1}$ 为误差项, 而 $H$ 是函数 $\mathbf{E}(\mathbf{q})$ 对 $\xi$ 求导数,那么
$$\begin{array}{rl}& H=\frac{\partial E(q)}{\partial \xi }\\ & =\frac{\partial {\left[\begin{array}{l}\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ \sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]}_{2\times 1}}{\partial {\left[\begin{array}{l}\delta x\\ \delta y\\ \delta \theta \end{array}\right]}_{3\times 1}}\\ & ={\left[\begin{array}{ccc}\cos q_{\theta }& -\sin q_{\theta }& 0\\ \sin q_{\theta }& \cos q_{\theta }& 0\end{array}\right]}_{2\times 3}\\ & =\left[\begin{array}{cc}{R}_{2\times 2}& 0_{2\times 1}\end{array}\right]\end{array}$$ $H$即为我们要的三维空间中误差方程的雅可比矩阵, 其中$R_{2\times 2}\in \mathrm{SO}(2)$。

2.3 将方法 2 类比推导到三维空间

用方法 2 进行方法类比, 扩展到三维空间, 利用方法 $2,q=\left[\begin{array}{l}{q}_x\\ q_y\\ q_z\\ q_{\alpha }\\ q_{\beta }\\ q_{\gamma }\end{array}\right]$， 此时${\xi }_{6\times 1}=\left[\begin{array}{l}\delta x\\ \delta y\\ \delta z\\ \delta \alpha \\ \delta \beta \\ \delta \gamma \end{array}\right]$ 为六维向量。那么 $\delta q$ 如下:
$$\begin{array}{c}E=\delta q={\left[\begin{array}{ccc}\cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha -\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha +\sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha +\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha -\cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha \end{array}\right]}_{3\times 3}{\left[\begin{array}{l}\delta x\\ \delta y\\ \delta z\end{array}\right]}_{3\times 1}\\ ={\left[\begin{array}{r}\cos \gamma \cos \beta \cdot {\delta }_x+(\cos \gamma \sin \beta \sin \alpha -\sin \gamma \cos \alpha )\cdot \delta y+(\cos \gamma \sin \beta \cos \alpha +\sin \gamma \sin \alpha )\cdot \delta z\\ \sin \gamma \cos \beta \cdot {\delta }_x+(\sin \gamma \sin \beta \sin \alpha +\cos \gamma \cos \alpha )\cdot \delta y+(\sin \gamma \sin \beta \cos \alpha -\cos \gamma \sin \alpha )\cdot \delta z\\ -\sin \beta \cdot {\delta }_x+\cos \beta \sin \alpha \cdot {\delta }_y+\cos \beta \cos \alpha \cdot {\delta }_z\end{array}\right]}_{3\times 1}\end{array}$$那么雅可比矩阵 $H$ 如下：
$$\begin{array}{rlrl}& H=\frac{\partial E(q)}{\partial \xi }={\left[\begin{array}{llllll}\frac{\partial E_1}{\partial {\delta }_x}& \frac{\partial E_1}{\partial {\delta }_y}& \frac{\partial E_1}{\partial {\delta }_z}& \frac{\partial E_1}{\partial {\delta }_{\alpha }}& \frac{\partial E_1}{\partial {\delta }_{\beta }}& \frac{\partial E_1}{\partial {\delta }_{\gamma }}\\ \frac{\partial E_2}{\partial {\delta }_x}& \frac{\partial E_2}{\partial {\delta }_y}& \frac{\partial E_2}{\partial {\delta }_z}& \frac{\partial E_2}{\partial {\delta }_{\alpha }}& \frac{\partial E_2}{\partial {\delta }_{\beta }}& \frac{\partial E_2}{\partial {\delta }_{\gamma }}\\ \frac{\partial E_3}{\partial {\delta }_x}& \frac{\partial E_3}{\partial {\delta }_y}& \frac{\partial E_3}{\partial {\delta }_z}& \frac{\partial E_3}{\partial {\delta }_{\alpha }}& \frac{\partial E_3}{\partial {\delta }_{\beta }}& \frac{\partial E_3}{\partial {\delta }_{\gamma }}\end{array}\right]}_{3\times 6}& & \\ & ={\left[\begin{array}{llllll}\cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha -\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha +\sin \gamma \sin \alpha & 0& 0& 0\\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha +\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha -\cos \gamma \sin \alpha & 0& 0& 0\\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha & 0& 0& 0\end{array}\right]}_{3\times 6}& & \\ & =\left[R_{3\times 3}{0}_{3\times 3}\right]& & \end{array}$$ $H$即为我们要的三维空间中误差方程的雅可比矩阵, 其中$R_{3\times 3}\in \mathrm{SO}(3)$。这个矩阵对应于gtsam中的gtsam::GPSFactor中的雅可比矩阵，因为gtsam::GPSFactor的因子是在三维空间中的优化，上面这个雅可比矩阵和gtsam代码中gtsam::GPSFactor的雅可比矩阵是对应的。
接下来为了更好的说明，我们来展示下gtsam中的两个函数，gtsam::GPSFactor的雅可比矩阵是从Pose3::translation函数中来的，是*Hself << Z_3x3, rotation().matrix()，也就是由一个3x3的零矩阵和R阵组成，和我们上面推导的 $H$ 阵对应，只不过它把3x3的零矩阵写在前面了，这是因为代码中${\xi }_{6\times 1}$先写角度的扰动，后写位置的扰动，参考a question about GPSFactor。

Vector GPSFactor::evaluateError(const Pose3& p,
    OptionalMatrixType H) const {
  return p.translation(H) -nT_;
}
const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
  if (Hself) *Hself << Z_3x3, rotation().matrix();
  return t_;
}

---

1. 高翔, 张涛, 刘毅, 等. 视觉SLAM十四讲——从理论到实践[M]. 北京: 电子工业出版社, 2017: 17-22. ↩︎

2. Eade E. Lie groups for 2d and 3d transformations[J]. URL http://ethaneade. com/lie. pdf, revised Dec, 2013, 117: 118. ↩︎

3. 武汉大学卫星导航中心张全老师的指导 ↩︎