李群与李代数大全（1） 在计算机视觉应用详细描述

  本次博文要讲述的是在上一次博文中没有完全展开描述的李群和李代数，可以参考博主这篇博文，大致了解一下李群和李代数以及刚体运动，算算本博文的简洁精炼版本，本博文的系列对其进行详细补充和描述，并且使得可以对李代数类似于字典一般便于查找。

  博主只是进行了大致浏览和概括，但是完全没有达到博主的期望，现在博主大刀阔斧的再次操刀，要进行一次最为齐全的李群李代数的讲解。

  本次的博文会十分的长，会包括部分公式的推导，用于处理表示2D和3D空间变换的李群。

  本博文主要是介绍关于李群和李代数在计算机视觉中的应用，原先博文的李群李代数为此博文的简化版本。后面系列的博文就讲纯粹的李群李代数理论及其推算以及相应的公式。

  首先我们要知道，李群是一个拓扑群，也是一个光滑的流形，具有一些其他不错的属性。 与每个李群相关联的是李代数，它是下面讨论的向量空间。 重要的是，李群及其李代数密切相关，允许将一个中的计算有效地映射到另一个中【或者相互转换，这点在我写过的博文指出过，指数映射和对数，这是一对反函数】。

  我们给出一些略微的李群总结

群	描述	维度	矩阵表示
$SO(3)$	3D旋转	3	3D旋转矩阵
$SE(3)$	3D刚性变换	6	在齐次4维矢量上线性变换
$SO(2)$	2D旋转	1	2D旋转矩阵
$SE(2)$	2D刚性变换	3	在齐次3维矢量上线性变换
$Sim(3)$	3D相似变换 刚性变换+尺度	7	在齐次4维矢量上线性变换

为什么说李群可以使用在机器人学和计算机视觉中

  机器人学和计算机视觉中的许多问题涉及3D几何中的操纵和估计。 如果没有一个连贯而强大的框架来表示和处理3D转换，这些任务繁重而且非常危险。 必须对变换进行组合，反转，分解和插值。 李群及其相关机制解决了所有这些操作，并以条理化方式进行，因此这样的事实一旦形成，就可以令人信服。

三维欧氏空间

  我们将使用 ${\mathbb{E}}^3$ 来表示熟悉的三维欧氏空间。 一般来说，欧氏空间是一个元素，其元素满足欧氏的五个公理。 在分析上，三维欧氏空间完全可以用笛卡尔坐标系表示：每个点$\rho \in {\mathbb{E}}^3$可以用${\mathbb{R}}^3$中的一个点用三个坐标来识别

$$X\dot{=}[x_1,x_2,x_3{]}^T=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\in {\mathbb{R}}^3$$

  通过这种形式使得我们可以知道，在笛卡尔坐标下，${\mathbb{E}}^3$和${\mathbb{R}}^3$是一一对应关系，这样可以假设在谈论点和其坐标，二者是等价的。

  笛卡尔坐标是使测量距离和角度成为可能。 为此，必须为${\mathbb{E}}^3$ 赋予度量标准。 度量的精确定义依赖于向量的概念。

向量

  在欧氏空间中，向量$v$由一对点$p,q\in {\mathbb{E}}^3$确定，并且被定义为连接p到q的有向箭头，表示为$v=\overrightarrow{pq}$。

  点$p$通常被称为向量$v$的基点(base point)。在坐标中，向量$v$由三元组$[v_1,v_2,v_3]\in {\mathbb{R}}^3$表示，其中每个坐标是两点的对应坐标之间的差：如果$p$具有坐标$X$并且$q$具有坐标$Y$，则$v$具有坐标

$$v\dot{=}Y-X\in {\mathbb{R}}^3$$

  前面的矢量定义称为绑定矢量(bound vector)。我们还可以引入自由向量的概念，向量的定义不依赖于其基点。

  如果我们有两对坐标满足$Y-X=Y^{\prime }-X^{\prime }$的点$(p,q)$和$(p^{\prime },q^{\prime })$，我们说它们定义了相同的自由向量。

  直观地，这允许向量$v$在${\mathbb{E}}^3$中的任何地方移动。 特别是，在不失一般性的情况下，可以假设基点是笛卡尔坐标系的原点，因此$X=0$且$Y=v$。

  但是，请注意，这种符号令人困惑：这里的$Y$表示恰好与点$q$的坐标相同的矢量坐标，因为我们选择了点$p$作为原点。点和矢量是不同的几何对象。 这将很重要，我们将很快看到，因为刚体运动对点和向量的作用不同。

  所有自由向量的集合形成线性向量空间，其中两个向量$v，u\in {\mathbb{R}}^3$的线性组合由

$$\alpha v+\beta u=[\alpha v_1+\beta u_1,\alpha v_2+\beta u_2,\alpha v_3+\beta u_3{]}^T\in {\mathbb{R}}^3,\forall \alpha ,\beta \in \mathbb{R}$$

  然后，${\mathbb{E}}^3$的欧氏度量仅由向量空间${\mathbb{R}}^3$上的内积定义。 可以证明，通过适当选择笛卡尔坐标系，${\mathbb{E}}^3$中的任何内积可以转换为以下标准形

$$<u,v>\dot{=}{u}^{T}v=u_1{v}_1+u_2{v}_2+u_3{v}_3,\forall u,v\in {\mathbb{R}}^3$$

  该内积也称为标准欧氏度量。 后面，我们将使用内积标准形式$<u,v>=u^{T}v$。因此，向量$v$的范数（或长度）是$||v||=\sqrt{<v,v>}=\sqrt{v_1^2+v_2^2+v_3^3}$。

  当两个矢量之间的内积为零时，即$(u,v)=0$时，它们被认为是正交的。

  最后，欧氏空间${\mathbb{E}}^3$可以形式地描述为相对于笛卡尔坐标系可以用${\mathbb{R}}^3$识别并且具有由上述内积给出的度量（在其向量空间上）的空间。 利用这样的度量，不仅可以测量矢量之间的点或角度之间的距离，还可以计算曲线（如果${\mathbb{E}}^3$中的运动粒子$\rho$的轨迹由曲线$\gamma (\cdot )：t\mapsto X(t)\in {\mathbb{R}}^3,t\in [0,1]$描述，则曲线的总长度为，由下式给出）

$$l(\gamma (\cdot ))\equiv {\int }_0^1||\dot{X}(t)||dt$$

  的长度或区域的体积,其中$\dot{X}(t)=\frac{d}{dt}(X(t))\in {\mathbb{R}}^3$是曲线的所谓切向量。

叉积（Cross product）

  给定两个向量$u，v\in {\mathbb{R}}^3$，它们的叉积是第三个向量，坐标由下式给出

$$u\times v\dot{=}\left(\begin{array}{c}{u}_2{v}_3-u_3{v}_2\\ u_3{v}_1-u_1{v}_3\\ u_1{v}_2-u_2{v}_1\end{array}\right)\in {\mathbb{R}}^3$$

  从这个定义可以看出，两个向量的叉积在每个参数中是线性的，如下式所示。

$$u\times (\alpha v+\beta w)\equiv \alpha u\times v+\beta u\times w,\forall \alpha ,\beta \in \mathbb{R}$$

  此外，还可以验证如下的式子：

$$<u\times v,u>=<u\times v,v>=0，u\times v=-v\times u$$

利用上面定义的坐标换算可以求证，在此不进行证明

  因此，两个向量的叉积与其每个因子正交，并且因子的顺序定义了方向（如果我们改变因子的顺序，则叉积改变符号）。

  如果我们固定$u$，则叉积可以用从${\mathbb{R}}^3$到${\mathbb{R}}^3$的映射表示:$v\mapsto u\times v$。这个映射在$v$中是线性的，因此可以用矩阵表示。 我们用$\hat{u}\in {\mathbb{R}}^3$表示这个矩阵，发音为“u hat”。 通过代入求解可得出矩阵如下的形式

$$\hat{u}=\left(\begin{array}{ccc}0& -u_3& u_2\\ u_3& 0& -u_1\\ -u_2& u_1& 0\end{array}\right)\in {\mathbb{R}}^{3\times 3}\text{\hspace{1em}(1)}$$

值得一提,$\hat{u}$和$u_{\times }$以及$[u{]}_{\times }$的表示意思等价

  因此叉积$u\times v=\hat{u}v$。现在你应该要注意到$\hat{u}$是$3\times 3$的斜对称矩阵，其中${\hat{u}}^T=-\hat{u}$。

叉积例子(右手定则，Right-hand rule)

  对于$e_1\dot{=}[1,0,0{]}^T,e_2\dot{=}[0,1,0{]}^T\in {\mathbb{R}}^3$，我们有$e_1\times e_2=[0,0,1{]}^T\dot{=}{e}_3$。 也就是说，对于标准笛卡尔坐标系，轴$X$和$Y$的叉积给出轴$Z$。因此，叉积符合右手定则，见图1。

图1：右手定则坐标系

  因此，叉积自然地定义了矢量$u$和$3\times 3$斜对称矩阵$\hat{u}$之间的映射。通过检查，该陈述的反过来显然是正确的，因为我们可以容易地识别与每个相关联的三维矢量。 $3\times 3$斜对称矩阵（仅从式（1）中提出$u_1,u_2,u_3$）。

反对称矩阵

  矩阵$M\in {\mathbb{R}}^{3\times 3}$是斜对称的，当且仅当对某个$u\in {\mathbb{R}}^3$使得满足$M=\hat{u}$时。

  因此，向量空间${\mathbb{R}}^3$和所有斜对称3×3矩阵的空间(被称为$\mathfrak{s}\mathfrak{o}(3)$是同构的（即，存在保持向量空间结构的一对一映射）。 同构是所谓的hat算子(hat operator):

$$\wedge :\mathbb{R}\to \mathfrak{s}\mathfrak{o}(3):u\mapsto \hat{v}$$

  它的逆映射，称为vee算子，从斜对称矩阵$\hat{u}$中提取向量$u$的分量，由下式给出：

$$\vee :\mathfrak{s}\mathfrak{o}(3)\to {\mathbb{R}}^3;\hat{u}\mapsto {\hat{u}}^{\vee }=u$$

刚体运动

  考虑在相机前移动的物体。 为了描述其运动，原则上应该指定物体上每个单点的轨迹，例如，通过将点的坐标指定为时间$X(t)$的函数。 幸运的是，对于刚性物体，我们不需要指定每个点的运动。 正如我们稍后将看到的，仅指定一个（而不是每个）点的运动以及连接到该点的三个坐标轴的运动就足够了，如图2所示。 原因在于，对于每个刚性物体，物体上任何两点之间的距离不随物体移动而随时间变化。

图2。 刚体的运动保持其上任何一对点$（p，q）$之间的距离$d$。

  因此，如果$X(t)$和$Y(t)$分别是对象上任意两个点$p$和$q$的坐标，则它们之间的距离是恒定的：

$$||X(t)-Y(t)||\equiv \mathrm{c}\mathrm{o}\mathrm{n}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{t},\forall t\in \mathbb{R}\text{\hspace{1em}(2)}$$

  刚体运动（或刚体变换）是一系映射，描述刚性物体上每个点的坐标在满足式（2）时如何随时间变化。 我们用这个映射来表示

$$g(t):{\mathbb{R}}^3\to {\mathbb{R}}^3;X\mapsto g(t)(X),t\in [0,T]$$

  如果不是看着移动物体的整个连续路径，而是集中在它的初始和最终位姿之间的映射上，我们有一个刚体位移，用

$$g:{\mathbb{R}}^3\to {\mathbb{R}}^3;X\mapsto g(X)$$

  除了变换点的坐标之外，$g$还引起向量的变换。 假设$v$是由坐标$v=Y-X$的两个点$p$和$q$定义的向量; 然后，在变换$g$的作用下之后，我们获得一个新的向量

$$u=g_{\ast }(v)\dot{=}g(Y)-g(X)$$

使用$g_{\ast }$ 这与微分几何中所谓的前推(推进，push-forward)映射或$g$的微分算子一致，它表示可微分映射在其域的切空间上的作用。

  由于$g$保留了点之间的距离，我们得到对于所有自由向量$v\in {\mathbb{R}}^3$,都有$g_{\ast }(v)=||v||$。

  保留距离的映射称为欧氏变换。 在3-D空间中，所有欧氏变换的集合由$E(3)$表示。 请注意，保持点之间的距离不足以表征在空间中移动的刚性物体。 实际上，存在保留距离的变换，但它们不是物理上可实现的。 例如，映射

$$f:[X_1,X_2,X_3{]}^T\mapsto [X_1,X_2,-X_3{]}^T$$

  保留距离但不保留方向。 它对应于XY平面中的点作为双面镜的反射。 为了排除这种映射，我们要求任何刚体运动除了保持距离外，还要保留方向。 也就是说，除了保留向量的范数之外，它还必须保留它们的叉积。 由刚体运动引起的映射或变换称为特殊欧氏变换。 “特殊”一词表示变换是保向的。

刚体运动或特殊欧氏变换

  映射$g:{\mathbb{R}}^3\to {\mathbb{R}}^3$是一个刚体运动或特殊欧氏变换，仅当它保留任何两个向量的范数和叉积，

1. 范数：$||g_{\ast }(v)||=||v||,\forall v\in {\mathbb{R}}^3$
2. 叉积：$g_{\ast }(u)\times g_{\ast }(v)=g_{\ast }(u\times v),\forall u,v\in {\mathbb{R}}^3$

所有这些运动或变换的集合由$SE(3)$表示。

  在刚体运动的上述定义中，保持矢量之间的角度并不是显而易见的。 但是，内积$<\cdot ,\cdot >$可以通过极化恒等式（Polarization Identity）用范数$||\cdot ||$表示。

$$<u,v>=\frac{1}{4}(||u+v|{|}^2-||u-v|{|}^2)$$

  由于$||u+v||=||g_{\ast }(u)||+||g_{\ast }(v)||$，可以得出结论，对于任何刚体运动$g$，

$$<u,v>=<g_{\ast }(u),g_{\ast }(v)>,\forall u,v\in {\mathbb{R}}^3$$

  换句话说，刚体运动也可以定义为保留内积和叉积的运动。

例子：三重积和体积

  从刚体运动的定义可以看出，它还保留了三个向量中所谓的三重积（Triple Product）：

$$<g_{\ast }(u),g_{\ast }(v)\times g_{\ast }(w)>=<u,v\times w>,\forall u,v,w\in {\mathbb{R}}^3$$

  由于三重积对应于三个矢量所构成的平行六面体的体积，因此刚体运动也保持了体积。

  这些属性如何帮助我们简洁地描述刚体运动？ 通过刚体运动保持距离和方向的事实意味着各个点不能相对于彼此移动。 因此，刚体运动可以通过身体上所选点的运动和附着于该点的坐标系的旋转来描述。 为了看到这一点，我们通过将笛卡尔坐标系附加到刚体上的某个点来表示刚体的配置，并且我们将跟踪该坐标系相对于固定世界（参考）系的运动。

  为此，考虑一个坐标系，其轴由三个正交向量$e_1,e_2,e_3\in {\mathbb{R}}^3$给出; 也就是说，它们满足

$$e_i^T{e}_j={\delta }_{ij}\dot{=}\{\begin{array}{ll}{\delta }_{ij}=1& fori=j\\ {\delta }_{ij}=0& fori̸=j\end{array}$$

  通常，对向量是排序的以至于形成右手坐标系：$e_1\times e_2=e_3$。 然后，在刚体运动$g$后，我们有：

$$g_{\ast }(e_i{)}^T{g}_{\ast }(e_j)={\delta }_{ij},g_{\ast }(e_1)\times g_{\ast }(e_2)=g_{\ast }(e_3)$$

  也就是说，得到的三个矢量仍然形成右手正交坐标系。因此，刚性物体总是可以与右手正交坐标系相关联，并且其刚体运动可以完全由这种坐标系的运动来指定，我们称之为物体坐标系。

  也就是说，得到三个向量$g_{\ast }(e_1),g_{\ast }(e_2),g_{\ast }(e_3)$仍然形成右手正交坐标系。因此，刚性对象总是可以与右手正交坐标系相关联，我们将其称为对象坐标系或体坐标系，其刚体运动可以完全由这种坐标系的运动来指定。

  在图3中，我们示出了一个物体，在这种情况下是一个相机，相对于预先选择的世界参考系$W:(X,Y,Z)$移动。

图3。相机坐标系，$C=(x,y,z)$与世界坐标坐标系$W:(X,Y,Z)$之间的刚体运动。

  为了指定相机相对于世界坐标系$W$的配置，可以在相机上选择固定点$o$并且在其上附接体坐标系，在这种情况下称为相机坐标系，$C=(x,y,z)$。 相机移动时，相机坐标系也会随相机移动。 然后，摄像机的配置由两个部分决定：

1. 世界坐标系的原点$o$与相机坐标系的原点$o$之间的向量$g(o)$，称为“平移”部分，用$T$表示;
2. 相对于具有坐标轴$（X，Y，Z）$的固定世界坐标系$W$，具有坐标轴$（x，y，z）$的相机坐标系$C$的相对方向，称为“旋转”部分，由$R$表示。

  在我们考虑的问题中，世界参考系及其原点$o$没有明确的选择。 因此，我们可以选择要附加到摄像机的世界坐标系并指定场景相对于该坐标系的平移和旋转（只要它是刚性的），或者我们可以将世界坐标系附加到场景并指定相对于该坐标系的相机运动。 重要的是场景和相机之间的相对运动; 从几何学的角度来看，世界参考坐标系系的选择是任意的。

  如果我们可以将刚性物体（例如，相机）从一个地方移动到另一个地方，我们当然可以反转动作并将其恢复到原始位置。 同样，我们可以组合几个动作来生成一个新的动作。 粗略地说，这种可逆性和组成的性质可以用“群”的概念在数学上进行表征。

  我们很快就会看到，刚体运动集确实是一个群，即所谓的特殊欧氏群。 然而，在我们给出一个显式表示并将其用于计算之前，群的抽象概念是没有用的。 在接下来将重点研究如何用矩阵表示刚体运动。 更具体地说，我们将证明任何刚体运动都可以表示为$4\times 4$矩阵。 为简单起见，我们从刚体运动的旋转分量开始。

旋转运动及其表示

旋转的正交矩阵表示

  假设我们有一个刚性物体绕固定点$o\in \mathbb{E}$旋转。 我们如何描述相对于选定坐标系的方向，比如$W？$。 不失一般性，我们可能总是假设世界坐标系的起源是旋转中心$o$。 如果不是这种情况，只需将原点转换为点$o$即可。 我们现在将另一个坐标系（例如$C$）附加到旋转对象，比如相机，其原点也在$o$处。 这两个坐标系之间的关系如图4所示。

图4。 刚体绕固定点$o$和沿轴线$w$旋转。 坐标系$W$（实线）固定不动，坐标框$C$（虚线）安装在旋转刚体上。

  坐标系$C$相对于坐标系$W$的配置（或“方向”）由相对于世界坐标系$W$的三个正交矢量$r_1=g_{\ast }(e_1),r_2=g_{\ast }(e_2),r_3=g_{\ast }(e_3)\in {\mathbb{R}}^3$的坐标确定,如图4所示。 三个矢量$r_1,r_2,r_3$分别简单是沿着相机坐标系$C$上的轴$x,y,z$的单位矢量。然后，旋转物体的配置完全由$3\times 3$矩阵确定

$$R_{wc}\dot{=}[r_1,r_2,r_3]\in {\mathbb{R}}^{3\times 3}$$

  其中$r_1,r_2,r_3$按顺序堆叠为三列。 由于$r_1,r_2,r_3$形成正交坐标系，因此遵循这一点

r i T r j = δ i j = ˙ { 1 f o r i = j , 0 f o r i ≠ j , ∀ i , j ∈ { 1 , 2 , 3 } r_i^Tr_j=\delta_{ij}\dot=\begin{cases}1 &amp;for \quad i= j ,\\0 &amp;for \quad i \neq j ,\end{cases}\forall i,j\in \{1,2,3\} riT​rj​=δij​=˙{10​fori=j,fori̸​=j,​∀i,j∈{1,2,3}

  这可以用矩阵形式写成

$$R_{wc}^T{R}_{wc}=R_{wc}{R}_{wc}^T=I$$

在线性代数上可以这么证明，一个向量在某基$e$在的坐标为$a$，经过旋转后在新基$e^{\prime }$下坐标为$a^{\prime }$，这里我们假设向量并没有随着坐标系旋转而运动，则满足$ea=e^{\prime }{a}^{\prime }$。然后两边左乘乘以$e^T$,可以得到$a=e^T{e}^{\prime }{a}^{\prime }$，也就是能得到两者之间的线性变换矩阵$e^T{e}^{\prime }$，也是旋转矩阵。

  满足上述恒等式的任何矩阵称为正交矩阵。 从上面的定义得出，正交矩阵的逆是简单的转置：$R_{wc}^{-1}=R_{wc}^T$。由于$r_1,r_2,r_3$形成右手坐标系，我们还有进一步的条件，即$R_{wc}$的行列式必须是$\pm 1$。 因此，$R_{wc}$是一个特殊正交矩阵，与之前一样，“特殊”一词表示它是保向的。${\mathbb{R}}^{3\times 3}$中所有这些特殊正交矩阵的空间通常表示为

S O ( 3 ) = ˙ { R ∈ R 3 × 3 ∣ R R T = I , det ⁡ ( R ) = ± 1 } SO(3)\dot=\{R\in \mathbb{R}^{3\times 3}|RR^T=I,\det(R)=\pm 1\} SO(3)=˙{R∈R3×3∣RRT=I,det(R)=±1}

  一般的，$3\times 3$特殊正交矩阵被称为旋转矩阵。 可以证实$SO(3)$在矩阵乘法下满足群的所有四个公理。简单说旋转矩阵乘法符合分配律，结合律。其逆是其转置也是旋转矩阵，单位元素是$I$即不旋转。

  因此，空间$SO(3)$也被称为${\mathbb{R}}^{3\times 3}$的特殊正交群，或简称为旋转群。 直接从定义中可以看出，旋转确实保留了向量的内积和叉积。

旋转矩阵

  表示围绕$Z$轴旋转角度$\theta$的矩阵是

$$R_Z(\theta )=\left[\begin{array}{ccc}cos(\theta )& -sin(\theta )& 0\\ sin(\theta )& cos(\theta )& 0\\ 0& 0& 1\end{array}\right]$$

  回到图4，每个旋转矩阵$R_{wc}\in SO(3)$表示物体围绕点$o$旋转。 除此之外，$R_{wc}$还表示从坐标系$C$到坐标系$W$的实际坐标变换的矩阵。

  为了看到这一点，假设对于给定的点$p\in {\mathbb{E}}^3$，其相对于坐标系$W$的坐标是 $X_w=[X_{1w},X_{2w},X_{3w}{]}^T\in {\mathbb{R}}^3$。 自$r_1,r_2,r_3$形成${\mathbb{R}}^3$的基（是坐标系$C$的轴），$X_w$也可以表示为这三个矢量的线性组合，例如$X_w=X_{1c}{r}_1+X_{2c}{r}_2+X_{3c}{r}_3$,其中 $X_c=[X_{1c},X_{2c},X_{3c}{]}^T\in {\mathbb{R}}^3$。 显然， $X_c=[X_{1c},X_{2c},X_{3c}{]}^T$是相对于坐标系$C$的相同点$p$的坐标。因此，我们有：

$$X_w=X_{1c}{r}_1+X_{2c}{r}_2+X_{3c}{r}_3=R_{wc}{X}_c$$

  在该等式中，矩阵$R_{wc}$将点$p$相对于坐标系$C$的坐标$X_c$变换为相对于坐标系$W$的坐标$X_w$。由于$R_{wc}$是旋转矩阵，因此其逆是简单的转置：

$$X_c=R_{wc}^{-1}{X}_w=R_{wc}^T{X}_w$$

请各位注意下标关系，莫要混淆

  也就是说，旋转的逆变换也是旋转; 我们称之为$R_{cw}$:

$$R_{cw}=R_{wc}^{-1}=R_{wc}^T$$

  然后可以将物体的连续旋转描述为空间$SO(3)$中的轨迹$R(t):t\mapsto SO(3)$。 当起始时间不是$t=0$时，时间$t_2$和时间$t_1$之间的相对运动将表示为$R(t_2,t_1)$。由旋转群的结合律可以得到

$$R(t_2,t_0)=R(t_2,t_1)R(t_1,t_0),\forall t_0<t_1<t_2\in \mathbb{R}$$

  然后，对于旋转摄像机，固定的3D点$p$的世界坐标$X_w$通过以下方式变换为相对于摄像机坐标系C的坐标：

$$X_c(t)=R_{cw}{X}_w$$

  或者，如果相对于具有坐标$X_c$的相机坐标系的点$p$，则其作为$t$的函数在其世界坐标$X_w(t)$由下式给出：

$$X_w(t)=R_{wc}(t)X_c$$

回忆上面所讲的刚体运动，认为点轨迹变化是随着时间变动的函数

$SO(3)$上的指数坐标（映射）

  到目前为止，我们已经证明${\mathbb{E}}^3$中的旋转刚体运动可以用$3\times 3$旋转矩阵$R\in SO(3)$表示。 在我们到目前为止的矩阵表示中，每个旋转矩阵R由其$3\times 3=9$个元素描述和确定。 但是，这9个元素不是自由参数 - 它们必须满足约束$R^{T}R=I$。这实际上对9个元素施加了6个独立约束(两两正交约束)。 因此，旋转矩阵空间$SO(3)$的维度仅为$3$，并且9中的6个参数实际上是多余的。 稍后我们将为旋转矩阵引入一些表示（或参数化）。

  给定曲线$R(t):\mathbb{R}\to SO(3)$ 描述连续旋转运动，旋转必须满足以下约束：

$$R(t)R^T(t)=I$$

想想点坐标是随着时间而变化的函数，自然每次都会变化$R$，所以旋转矩阵在刚体运动过程中也是个时间函数

  计算上述方程相对于时间$t$的导数，注意到右侧是一个常数矩阵，我们得到：

$$\frac{d}{dt}(R(t)R^T(t))=\dot{R}(t)R^T(t)+R(t){\dot{R}}^T(t)=0\Rightarrow \dot{R}(t)R^T(t)=-(\dot{R}(t)R^T(t){)}^T$$

  得到的等式反映了矩阵$\dot{R}(t)R^T(t)\in {\mathbb{R}}^{3\times 3}$是斜对称矩阵的事实。 然后，正如我们在前面叉积时所说的，必存在一个向量，比如$\omega (t)\in {\mathbb{R}}^3$，使得：

$$\dot{R}(t)R^T(t)=\hat{\omega }(t)$$

  将$R(t)$乘以等式两端右侧得到：

$$\dot{R}(t)=\hat{\omega }(t)R(t)$$

  根据上面的等式，如果对于$t=t_0$，如果$R(t_0)=I$，则我们有$\dot{R}(t_0)=\hat{\omega }(t_0)$。 因此，在单位矩阵$I$周围，斜对称矩阵给出了旋转矩阵的一阶近似（一阶泰勒展开）：

$$R(t_0+dt)\approx I+\hat{\omega }(t_0)dt$$

  所有斜对称矩阵的空间都表示为

s o ( 3 ) = ˙ { ω ^ ∈ R 3 × 3 ∣ ω ∈ R 3 } \mathfrak{so}(3)\dot=\{\hat{\omega}\in \mathbb{R}^{3\times 3}|\omega\in \mathbb{R}^3\} so(3)=˙{ω^∈R3×3∣ω∈R3}

  它也被称为旋转群$SO(3)$在原点（单位矩阵，identity）处的切空间。

$SO(3)$被称为李群，$\mathfrak{s}\mathfrak{o}(3)$被称为李代数

切空间，在$x$处的切空间$T_{x}M$。来源于维基百科。

  如果$R(t)$不是原点，则$R(t)$处的切空间简单地通过$R(t)$右乘$\mathfrak{s}\mathfrak{o}(3)$变到$R(t)$：$\dot{R}(t)=\hat{\omega }(t)R(t)$。 这也表明$SO(3)$的元素仅取决于三个参数$({\omega }_1,{\omega }_2,{\omega }_3)$。

  在理解了它的局部近似之后，我们现在将使用这些知识来获得旋转矩阵的表示。 我们假设矩阵$\hat{\omega }$在$\dot{R}(t)=\hat{\omega }(t)R(t)$是常数：

$$\dot{R}(t)=\hat{\omega }R(t)$$

  在上面的等式中，$\hat{\omega }$可以解释为以下线性常微分方程（ODE）的状态转移矩阵（state transition matrix）：

$$\dot{x}(t)=\hat{\omega }x(t),x(t)\in {\mathbb{R}}^3$$

  然后立即验证上述ODE的解是由下式给出的：

$$x(t)=e^{\hat{\omega }t}x(0)\text{\hspace{1em}(3)}$$

  其中$e^{\hat{\omega }t}$是矩阵指数函数：

$$e^{\hat{\omega }t}=\sum _{n=0}^{\infty }\frac{(\hat{\omega }t{)}^n}{n!}=I+\hat{\omega }t+\frac{(\hat{\omega }t{)}^2}{2!}+\cdots +\frac{(\hat{\omega }t{)}^n}{n!}+\cdots \text{\hspace{1em}(4)}$$

  $e^{\hat{\omega }t}$也被记为$\exp (\hat{\omega }t)$。由于式子（3）的ODE解唯一性，并假设$R(0)=I$作为初始条件，我们有：

$$R(t)=e^{\hat{\omega }t}\text{\hspace{1em}(5)}$$

  为了证明矩阵$e^{\hat{\omega }t}$确实是一个旋转矩阵，可以直接从矩阵指数的定义中看出：

$$(e^{\hat{\omega }t}{)}^{-1}=e^{-\hat{\omega }t}=e^{{\hat{\omega }}^{T}t}=(e^{\hat{\omega }t}{)}^T$$

斜对称矩阵$-\hat{w}={\hat{w}}^T$

  因此$(e^{\hat{\omega }t}{)}^T{e}^{\hat{\omega }t}=I$。仍然表明$\det (e^{\dot{\omega }t})=\pm 1$。

   等式（5）的物理解释是：如果$||\omega ||=1$，则$R(t)=e^{\hat{\omega }t}$只是围绕旋转轴$\omega \in {\mathbb{R}}^3$旋转$t$弧度。

我们也可以使用$e^{\hat{\omega }\theta }$，其中$\theta$明确地表示旋转角度和$||\omega ||=1$，或者更简单地写成$e^{\hat{\omega }}$，其中$||\omega ||$表示旋转角度。

   因此，矩阵指数（等式4）确实定义了从空间$\mathfrak{s}\mathfrak{o}(3)$到$SO(3)$的映射，即所谓的指数映射：

$$\exp :\mathfrak{s}\mathfrak{o}(3)\to SO(3)$$

$$\hat{\omega }\in \mathfrak{s}\mathfrak{o}(3)\mapsto e^{\hat{\omega }}\in SO(3)$$

备注：$e^{\hat{\omega }}$是旋转矩阵$R$

   注意，我们通过假设等式（5）中的$\omega (t)$是常数来获得等式（4）的。 但情况并非总是如此。 因此，这里自然会出现一个问题：每个旋转矩阵$R\in SO(3)$能否以等式（5）中的指数形式表示？ 答案是肯定的。

$SO(3)$中的李对数

   对于任何旋转矩阵$R\in SO(3)$，存在$\omega \in {\mathbb{R}}^3$，使得$R=\exp (\hat{\omega })$。 这样的元素用$\hat{\omega }=\log (R)$表示(指数映射的逆)。

指数映射存在满射性。

   $j$即对于任何$R\in SO(3)$，存在（不一定是唯一的）$\omega \in {\mathbb{R}}^3,||\omega ||=1$和$t\in \mathbb{R}$使得$R(t)=e^{\hat{\omega }t}$。

证明

   该定理的证明是通过构造来证明。

   如果旋转矩阵$R$给出为：

$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

   相应的$t$和$\omega$ 由下给出：

$$t={\cos }^{-1}\left(\frac{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(R)-1}{2}\right),\omega =\frac{1}{2\sin (t)}\left(\begin{array}{c}{r}_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{array}\right)$$

   该定理的重要性在于，可以通过围绕某个轴旋转一定角度来实现任何旋转矩阵。

   然而，该定理仅保证了从$\mathfrak{s}\mathfrak{o}(3)$到$SO(3)$的指数映射的可靠性。 不幸的是，这个映射不是单射的，因此不是一对一的。 在我们引入所谓的罗德里格斯公式计算$R(t)=e^{\hat{\omega }t}$的公式后，这将变得清晰。

   现在我们现在知道如何计算给定旋转矩阵$R\in SO(3)$的指数坐标$\omega$。 另一方面，给定$\omega$，我们如何有效地计算相应的旋转矩阵$R=e^{\hat{\omega }}$？ 人们当然可以使用等式5来求解。 然而，以下定理大大简化了计算：

罗德里格斯公式（Rodrigues’ formula）及证明

   给定$\omega \in {\mathbb{R}}^3$，其中$||w||=1$而且$t\in \mathbb{R}$,则矩阵指数$R(t)=e^{\hat{\omega }t}$可以通过下面的公式给出：

$$e^{\hat{\omega }t}=I+\hat{\omega }\sin (t)+{\hat{\omega }}^2(1-\cos (t))$$

证明1

   这是直接验证$\hat{\omega }$的幂！ 可以通过以下两个公式来减少幂的次方：

$${\hat{\omega }}^2=\hat{\omega }{\hat{\omega }}^T-I$$

$${\hat{\omega }}^3=-\hat{\omega }$$

   因此，等式4的指数级数可以简化为：

$$e^{\hat{\omega }t}=I+\underset{\text{sin(t)}}{\underbrace{(t-\frac{t^3}{3!}+\frac{t^5}{5!}-\cdots )}}\hat{\omega }+\underset{\text{1-cos(t)}}{\underbrace{(\frac{t^2}{2!}-\frac{t^4}{4!}+\frac{t^6}{6!}-\cdots )}}{\hat{\omega }}^2$$

   所以得到

$$e^{\hat{\omega }t}=I+\hat{\omega }\sin (t)+{\hat{\omega }}^2(1-\cos (t))$$

证明2

   该证明摘自维基百科。

   如上图所示，$k$是单位向量，它定义了旋转轴，而$v$是任意的一个向量，但是围绕$k$轴旋转$\theta$角得到$v_{rot}$.

   向量$v$被分解为与$k$轴平行$v_{\Vert }$和与$k$垂直的$v_{\perp }$分量，由内积可以得到这两个分量与$v$的关系为$v=v_{\Vert }+v_{\perp }$。

   与$k$轴平行的分量$v_{\Vert }$表示为：

$$v_{\Vert }=(v\cdot k)k$$

$v\cdot k$是$v$在向量$k$上的投影长度，然后乘以单位向量$k$就得到$v$以$k$方向为分量的$v_{\Vert }$

   上式被称为$v$对$k$的向量投影。

   而垂直于$k$的分量为$v_{\perp }$，通过前两个式子可以得到：

$$v_{\perp }=v-v_{\Vert }=v-(k\cdot v)k=-k\times (k\times v)$$

   称为来自$k$的$v$的向量拒绝（ vector rejection ）。

   从上图来说明，$k\times v$可以看作是$v_{\perp }$关于$k$逆时针旋转$90^{\circ }$的副本，所以它们大小相等，但是方向垂直【你得注意到$k$是单位向量的这个假设】。同样，向量$k\times (k\times v)$是$v_{\perp }$关于$k$逆时针旋转$180^{\circ }$的一个副本，所以$k\times (k\times v)$与$v_{\perp }$在大小上相等，但方向相反（因为他们互为相反数，所以是减号）。

   三重积展开式建立了平行分量和垂直分量间的联系：

$$a\times (b\times c)=(a\cdot c)b-(a\cdot b)c,\forall a,b,c$$

   平行于轴的分量在旋转下不会改变幅度和方向，所以$v_{\Vert rot}=v_{\Vert }$。

   只有垂直分量会改变方向，但会保持其大小（仅旋转）。

$$|v_{\perp rot}=|v_{\perp }|$$

$$v_{\perp rot}=\cos \theta v_{\perp }+\sin \theta k\times v_{\perp }$$

   由于$k$和$v_{\Vert }$是平行的，所以叉积为零，即

$$k\times v_{\Vert }$$

   所以可以得到

$$k\times v_{\perp }=k\times v-k\times v_{\Vert }=k\times v$$

   因此

$$v_{\perp rot}=\cos \theta v_{\perp }+\sin \theta k\times v$$

   这种选择是正确的，这是因为向量$v_{\perp }$和$k\times v$具有相同的长度，而且$k\times v$是$v_{\perp }$绕着$k$轴逆时针旋转$90^{\circ }$得到的。

   使用三角函数正弦和余弦对$v_{\perp }$和$k\times v$进行适当缩放给出旋转的垂直分量。

   旋转分量形式类似于笛卡尔基的2D平面极坐标$(r,\theta )$中给出的径向向量

$$r=r\cos \theta e_x+r\sin \theta e_y$$

   其中$e_x,e_y$则是坐标轴方向指向的单位向量。

   那么完整的选择矢量就有了表达：

$$\begin{array}{rl}{v}_{rot}& =v_{\Vert }+\cos \theta v_{\perp }+\sin \theta k\times v\\ & =v_{\Vert }+\cos \theta (v-v_{\Vert })+\sin \theta k\times v\\ & =\cos \theta v+(1-\cos \theta )v_{\Vert }+\sin \theta k\times v\\ & =\cos \theta v+(1-\cos \theta )(k\cdot v)k+\sin \theta k\times v\end{array}$$

  以上是旋转向量的推导，然后将$v$和$k\times v$表示为列矩阵，叉积可以表示为矩阵乘积(之前的博文和本博文之前有描述叉积，帽运算)：

$$k\times v=\left[\begin{array}{c}(k\times v{)}_x\\ (k\times v{)}_y\\ (k\times v{)}_z\end{array}\right]=\left[\begin{array}{c}{k}_y{v}_z-k_z{v}_y\\ k_z{v}_x-k_x{v}_z\\ k_x{v}_y-k_y{v}_x\end{array}\right]=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& -0\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$$

  令$K$表示单位向量$k$的叉积矩阵

$$K=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& -0\end{array}\right]$$

  所以可得到下面等式

$$k\times v=Kv$$

  对于任何向量$v$（实际上，$K$是这个性质唯一的矩阵，它具有特征值$0$和$\pm 1$）。

  迭代右边叉积相当于左乘叉积矩阵：

$$K(Kv)=K^{2}v=k\times (k\times v)$$

  由于$k$是单位向量，所以具有单位2-范数。

  所以在矩阵语言上，我们有矩阵公式形式的旋转公式：

$$v_{rot}=v+(\sin \theta )Kv+(1-\cos \theta )K^{2}v,||K|{|}_2=1$$

  上述形式的由来是这样的。

  我们知道$v_{rot}=\cos \theta v+(1-\cos \theta )(k\cdot v)k+\sin \theta k\times v$,而且$v_{\perp }=v-v_{\Vert }=v-(k\cdot v)k=-k\times (k\times v)$。

  所以我们可以得到

$$(k\cdot v)k=v+k\times (k\times v)$$

  所以

$$\begin{array}{rl}{v}_{rot}& =\cos \theta v+(1-\cos \theta )(k\cdot v)k+\sin \theta k\times v\\ & =\cos \theta v+(1-\cos \theta )（v+k\times (k\times v)）+\sin \theta k\times v\\ & =\cos \theta v+(1-\cos \theta )v+(1-\cos \theta )k\times (k\times v)+\sin \theta k\times v\\ & =v+(1-\cos \theta )\underset{K^{2}v}{\underbrace{k\times (k\times v)}}+\sin \theta \underset{Kv}{\underbrace{k\times v}}\\ & =v+(\sin \theta )Kv+(1-\cos \theta )K^{2}v\end{array}$$

  值得注意的是，这个公式的主要项现在是1。

  将$v$因式分解可以得到紧凑的表达式：

$$v_{rot}=Rv$$

  其中

$$R=I+(\sin \theta )K+(1-cos(\theta ))K^2$$

  是围绕$k$轴旋转为$\theta$的旋转矩阵，$I$是$3\times 3$的单位矩阵。其中

$$K=\hat{k}$$

$$K^2=\hat{k}\hat{k}=k{k}^T-I$$

  所以

$$\begin{array}{rl}R& =I+\sin \theta \hat{k}+(1-\cos \theta )(k{k}^T-I)\\ & =\cos \theta I+\sin \theta \hat{k}+(1-\cos \theta )k{k}^T\\ & =e^{\hat{k}\theta }\end{array}$$

  证明完毕。

---

罗德里格斯公式中元素与旋转矩阵的关系及证明

  下面我们证明，由罗德里格斯的公式：

$$R=e^{\hat{\omega }t}=I+\hat{\omega }\sin (t)+{\hat{\omega }}^2(1-\cos (t))$$

  如何得到下面的等式？

$$t={\cos }^{-1}\left(\frac{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(R)-1}{2}\right),\omega =\frac{1}{2\sin (t)}\left(\begin{array}{c}{r}_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{array}\right)$$

  首先，证明：

$$t={\cos }^{-1}\left(\frac{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(R)-1}{2}\right)$$

  我们知道

$$R=e^{\hat{\omega }t}=I+\hat{\omega }\sin (t)+{\hat{\omega }}^2(1-\cos (t))$$

$$\iff R=\cos \theta I+\hat{\omega }\sin (t)+(1-\cos (t))\omega {\omega }^T$$

  两边对其求迹【常数的迹还是常数】，可以得到

$$\begin{array}{rl}\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(R)& =\cos (t)\underset{=3}{\underbrace{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(I)}}+\sin (t)\underset{=0}{\underbrace{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(\hat{w})}}+(1-\cos (t))\underset{=1}{\underbrace{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(\omega {\omega }^T)}}\\ & =3\cos (t)+(1-\cos (t))\\ & =1+2\cos (t)\end{array}$$

单位矩阵的迹为3，反对称矩阵的迹为0，我们已经指出$||\omega ||=1$，而$||\omega ||=\sqrt{{\omega }_1^2+{\omega }_2^2+{\omega }_3^3}=\sqrt{\omega {\omega }^T}=1$,所以很容易推断出$\omega {\omega }^T$的迹为1。

  这就能得到:

$$t={\cos }^{-1}\left(\frac{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(R)-1}{2}\right)=\arccos \left(\frac{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(R)-1}{2}\right)$$

  对于$\omega$,求解可能较复杂,我们用最愚蠢的方式证明，假设

$$\omega =\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right]$$

  则：

$$\hat{\omega }=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& -0\end{array}\right]$$

$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

  由于

$${\hat{\omega }}^T=-\hat{\omega }$$

  所以

$$({\hat{w}}^2{)}^T=({\hat{\omega }}^T{)}^2={\hat{\omega }}^2$$

  所以我们让$R=I+\hat{\omega }\sin (t)+{\hat{\omega }}^2(1-\cos (t))$两边都取转置得到：

$$R^T=I-\hat{\omega }\sin (t)+{\hat{\omega }}^2(1-\cos (t))$$

  所以很容易得到

$$\hat{\omega }\sin (t)=\frac{R-R^T}{2}$$

  由于

$$\hat{\omega }\sin (t)=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& -0\end{array}\right]\sin (t)$$

  而且

$$R-R^T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]-\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$$

$$\iff R-R^T=\left[\begin{array}{ccc}0& r_{12}-r_{21}& r_{13}-r_{31}\\ r_{21}-r_{12}& 0& r_{23}-r_{32}\\ r_{31}-r_{13}& r_{32}-r_{23}& 0\end{array}\right]$$

  然后两者联立得到

$$\hat{\omega }=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& -0\end{array}\right]=\frac{1}{2\sin (t)}\left[\begin{array}{ccc}0& r_{12}-r_{21}& r_{13}-r_{31}\\ r_{21}-r_{12}& 0& r_{23}-r_{32}\\ r_{31}-r_{13}& r_{32}-r_{23}& 0\end{array}\right]$$

  故而得到

$${\omega }_1=r_{32}-r_{23},{\omega }_2=r_{13}-r_{31},{\omega }_3=r_{21}-r_{12}$$

  所以

$$\omega =\frac{1}{2\sin (t)}\left(\begin{array}{c}{r}_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{array}\right)$$

---

  使用罗德里格斯的公式，可以看出，如果$t=2k\pi ,k\in \mathbb{Z}$，我们有

$$e^{\hat{\omega }2k\pi }=I,\forall k$$

  因此，对于给定的旋转矩阵$R\in SO(3)$，通常存在非常多的指数坐标$(\omega ,t)$，使得$e^{\hat{\omega }t}=R$。指数映射$\exp :\mathfrak{s}\mathfrak{o}(3)\to SO(3)$因此不是一对一的。

  知道指数映射也不是可交换的也是有用的，即对于两个${\hat{\omega }}_1,{\hat{\omega }}_2\in \mathfrak{s}\mathfrak{o}(3)$

$$e^{{\hat{\omega }}_1}{e}^{{\hat{\omega }}_2}̸=e^{{\hat{\omega }}_2}{e}^{{\hat{\omega }}_1}̸=e^{{\hat{\omega }}_1+{\hat{\omega }}_2}$$

  除非${\hat{\omega }}_1{\hat{\omega }}_2={\hat{\omega }}_2{\hat{\omega }}_1$。

李括号

  一般来说，${\hat{\omega }}_1{\hat{\omega }}_2$和${\hat{\omega }}_2{\hat{\omega }}_1$之间的差异在$\mathfrak{s}\mathfrak{o}(3)$上称为李括号，表示为：

$$[{\hat{\omega }}_1,{\hat{\omega }}_2]={\hat{\omega }}_1{\hat{\omega }}_2-{\hat{\omega }}_2{\hat{\omega }}_1,\forall {\hat{\omega }}_1,{\hat{\omega }}_2\in \mathfrak{s}\mathfrak{o}(3)$$

  显然，$[{\hat{\omega }}_1,{\hat{\omega }}_2]$也是$\mathfrak{s}\mathfrak{o}(3)$中的斜对称矩阵。 $\mathfrak{s}\mathfrak{o}(3)$的线性结构与李括号一起形成（Lie）群$SO(3)$的李代数。 有关$SO(3)$的李群结构的更多细节，会在此博文系列的下一博文描述。

  所有旋转矩阵的集合$e^{\hat{\omega }t},t\in \mathbb{R}$,然后被称为$SO(3)$的一个参数子群，并且这种子群中的乘法是可交换的，即相同的$\omega \in {\mathbb{R}}^3$，我们有：

$$e^{\hat{\omega }{t}_1}{e}^{\hat{\omega }{t}_2}=e^{\hat{\omega }{t}_2}{e}^{\hat{\omega }{t}_1}=e^{\hat{\omega }(t_1+t_2)},\forall t_1,t_2\in \mathbb{R}$$

  上面介绍的指数坐标为旋转矩阵提供局部参数化。 还有其他方法可以参数化旋转矩阵，无论是全局还是局部，其中四元数和欧拉角（或者更正式地说，Lie-Cartan坐标）是两种流行的选择。 我们使用指数坐标，因为它们更简单，更直观。

四元数（Quaternions）和Lie-Cartan坐标

四元数

  有关四元数的知识，请阅读我的这篇博文。不再赘述，避免冗余。

Lie-Cartan坐标

  有关四元数的知识，请阅读我的这篇博文。不再赘述，避免冗余。

刚体运动及其表征

  前面，我们研究了纯粹的旋转刚体运动以及如何表示和计算旋转矩阵。 在本节中，我们将研究如何表示一般的刚体运动，即具有旋转和平移的运动。

  图5显示了一个移动的刚性物体，其上附有坐标系$\mathrm{C}$。

图5。 移动坐标系$C$和世界坐标系$W$之间的刚体运动。

  为了描述相对于坐标系$\mathrm{W}$下对象上的点 $p$ 的坐标，从图中很容易看出，向量${\mathrm{X}}_w$简单的是坐标系$\mathrm{C}$原点相对世界坐标系$\mathrm{W}$的原点平移$T_{wc}\in {\mathbb{R}}^3$和相对坐标系$\mathrm{W}$表达的向量${\mathrm{X}}_c$的和。由于${\mathrm{X}}_c$是点$p$ 相对于坐标系$\mathrm{C}$的坐标，但相对于坐标系$\mathrm{W}$，它变成了$R_{wc}{\mathrm{X}}_c$，其中$R_{wc}\in SO(3)$是两坐标系之间的相对旋转。 因此，坐标$X_w$由下式给出:

$${\mathrm{X}}_w=R_{wc}{\mathrm{X}}_c+T_{wc}$$

  通常，如果所涉及的坐标系从上下文中能够很明确清楚，我们将完全刚性运动表示为$g_{wc}=(R_{wc},T_{wc})$或简单地表示$g=(R,T)$。 然后$g$不仅表示刚体对象的配置的描述，而且表示坐标系之间的坐标的变换。 在紧凑的形式中，我们可以简单地写：

$${\mathrm{X}}_w=g_{wc}({\mathrm{W}}_c)$$

  然后可以将刚体的所有可能配置的集合描述为：

$$SE(3)=g=(R,T)|R\in SO(3),T\in {\mathbb{R}}^3=SO\times {\mathbb{R}}^3$$

  这就是所谓的特殊欧式群$SE(3)$。 为了简化，我们引入所谓的齐次坐标。

齐次表示

  从式${\mathrm{X}}_w=R_{wc}{\mathrm{X}}_c+T_{wc}$中注意到，与纯旋转情况不同，完全刚体运动的坐标变换不是线性的，而是仿射的。

  我们说对于某个矩阵$A$,若两个向量$u,v$ 是线性变换而相关仅当$u=Av$。对于某个矩阵$A$和向量$b$，如果说两个向量$u,v$是仿射变换相关仅当$u=Av+b$。

  但是，我们可以将仿射变换转换为线性变换。通过使用所谓的齐次坐标：将1加到一个点$p\in {\mathbb{E}}^3$的坐标$\mathrm{X}=[X_1,X_2,X_3{]}^T\in {\mathbb{R}}^3$，产生一个${\mathbb{R}}^4$中的向量，表示为$\bar{\mathrm{X}}$：

$$\bar{\mathrm{X}}=\left[\begin{array}{c}\mathrm{X}\\ 1\end{array}\right]=\left[\begin{array}{c}{\mathrm{X}}_1\\ X_2\\ X_3\\ 1\end{array}\right]\in {\mathbb{R}}^4$$

  在这种情况下，坐标的这种扩展已经将欧氏空间${\mathbb{E}}^3$嵌入到${\mathbb{R}}^4$中的超平面而不是${\mathbb{R}}^3$中。 矢量$v=\mathrm{X}(q)-\mathrm{X}(p)$的齐次坐标定义为两点的齐次坐标之间的差异，因此形式为：

$$\bar{v}=\left[\begin{array}{c}v\\ 0\end{array}\right]=\left[\begin{array}{c}\mathrm{X}(q)\\ 1\end{array}\right]-\left[\begin{array}{c}\mathrm{X}(p)\\ 1\end{array}\right]=\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\\ 0\end{array}\right]\in {\mathbb{R}}^4$$

  注意，在${\mathbb{R}}^4$中，上述形式的矢量产生子空间，因此原始矢量$v\in {\mathbb{R}}^3$的所有线性结构都被新的表示完美地保留。 使用新表示法，转换${\mathrm{X}}_w=R_{wc}{\mathrm{X}}_c+T_{wc}$可以重写为：

$${\mathrm{X}}_w=\left[\begin{array}{c}{\mathrm{X}}_w\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{wc}& T_{wc}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{X}}_c\\ 1\end{array}\right]\dot{=}{\hat{g}}_{wc}{\bar{\mathrm{X}}}_c$$

  其中$4\times 4$矩阵${\hat{g}}_{wc}\in {\mathbb{R}}^4$被称为刚性运动$g_{wc}=(R_{wc},T_{wc})$的齐次表示。 一般来说，如果$g=(R,T)$，则齐次表示为：

$$\bar{g}=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

  请注意，通过在符号中引入一点冗余，我们通过线性矩阵乘法表示坐标的刚体变换。 上式中$g$的齐次表示产生了特殊欧式群$SE(3)$的自然矩阵表示：

$$SE(3)\equiv \left\{\bar{g}=\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)|R\in SO(3),T\in {\mathbb{R}}^3\right\}\subset {\mathbb{R}}^{4\times 4}$$

  然后，直接验证确实满足了群的所有要求（群的四个性质，在博文中有描述。）。 特别是$\forall {\bar{g}}_1,{\bar{g}}_2$和$\bar{g}\in SE(3)$，我们有：

$${\bar{g}}_1{\bar{g}}_2=\left[\begin{array}{cc}{R}_1& T_1\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_2& T_2\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_1{R}_2& R_1{T}_2+T_1\\ 0& 1\end{array}\right]\in SE(3)$$

  而且

$${\bar{g}}^{-1}={\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]}^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}T\\ 0& 1\end{array}\right]\in SE(3)$$

  在齐次表示中，刚体变换$g\in SE(3)$对向量$v=\mathrm{X}(q)-\mathrm{X}(p)$的作用变为：

$${\bar{g}}_{\ast }(\bar{v})=\bar{g}\bar{\mathrm{X}}(q)-\bar{g}\bar{\mathrm{X}}(p)=\bar{g}\bar{v}$$

  也就是说，可以简单地用矩阵乘法表示，而且保留了内积和叉积的性质，能够代表刚体转换。

$SE(3)$上的指数坐标（映射）

  在前面，我们研究了旋转矩阵$R\in SO(3)$的指数坐标。 对于完全刚体运动$g\in SE(3)$的齐次表示也存在类似的形式。

  考虑连续移动的刚体对象的运动由$R$到$SE(3)$的曲线描述：$g(t)=(R(t),T(t))$，或者是齐次表示：

$$g(t)=\left[\begin{array}{cc}R(t)& T(t)\\ 0& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

  这里，为了简化表示法，我们将从符号中移除“bar”以进行齐次表示【即去掉字母上面横线】，并简单地将$g$表示相同的矩阵。我们将对点使用相同的约定：$\mathrm{X}$表示$\bar{\mathrm{X}}$和对于向量： 只要从上下文中清楚它们的维度，就可以用$v$表示$\bar{v}$。与纯旋转情况类似，让我们首先看一下矩阵$\dot{g}(t)g^{-1}(t)$的结构：

$$\dot{g}(t)g^{-1}(t)=\left[\begin{array}{cc}\dot{R}(t)R^T(t)& \dot{T}(t)-\dot{R}(t)R^T(t)T(t)\\ 0& 0\end{array}\right]$$

顺带一提，$\dot{g}(t)$是$g(t)$对$t$求导

  根据我们对旋转矩阵的研究，我们知道$\dot{R}(t)R^T(t)$是一个斜对称矩阵，即存在$\hat{\omega }(t)\in \mathfrak{s}\mathfrak{o}(3)$使得$\hat{\omega }(t)=\dot{R}(t)R^T(t)$。

  定义一个向量$v(t)\in {\mathbb{R}}^3$使得$v(t)=\dot{T}(t)-\hat{\omega }(t)T(t)$。所以上面的式子变成了：

$$\dot{g}(t)g^{-1}(t)=\left[\begin{array}{cc}\hat{\omega }(t)& v(t)\\ 0& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

  如果我们进一步定义矩阵 $\hat{\xi }(t)\in {\mathbb{R}}^{4\times 4}$是：

$$\hat{\xi }(t)=\left[\begin{array}{cc}\hat{\omega }(t)& v(t)\\ 0& 0\end{array}\right]$$

  接着我们有：

$$\dot{g}(t)=(\dot{g}(t)g^{-1}(t))g(t)=\hat{\xi }(t)g(t)\text{\hspace{1em}(6)}$$

  $\hat{\xi }$ 可以看作沿着$g(t)$曲线的"切向量"并用于局部近似$g(t)$：

$$g(t+dt)\approx g(t)+\hat{\xi }(t)g(t)dt=(I+\hat{\xi }(t)dt)g(t)$$

  在机器人学中形式为$\hat{\xi }$的$4\times 4$矩阵为被称为运动旋量(twist)。 然后将所有运动旋量(twist)的集合表示为：

$$\mathfrak{s}\mathfrak{e}(3)\equiv \left\{\hat{\xi }=\left(\begin{array}{cc}\hat{w}& v\\ 0& 0\end{array}\right)|\hat{w}\in \mathfrak{s}\mathfrak{o}(3),v\in {\mathbb{R}}^3\right\}\subset {\mathbb{R}}^{4\times 4}$$

  $\mathfrak{s}\mathfrak{e}(3)$被称为矩阵群$SE(3)$的切空间（或李代数）。 我们还定义了两个运算符$\vee$和$\wedge$用于$\hat{\xi }\in \mathfrak{s}\mathfrak{e}(3)$及其运动旋量$\xi \in {\mathbb{R}}^6$之间的转换,如下所示：

$${\left[\begin{array}{cc}\hat{\omega }& v\\ 0& 0\end{array}\right]}^{\vee }\dot{=}\left[\begin{array}{c}v\\ \omega \end{array}\right]\in {\mathbb{R}}^6$$

$$\left[\begin{array}{c}v\\ \omega \end{array}\right]\dot{=}{\left[\begin{array}{cc}\hat{\omega }& v\\ 0& 0\end{array}\right]}^{\wedge }\in {\mathbb{R}}^{4\times 4}$$

  在运动旋量坐标$\xi$中，我们将$v$称为线速度和$\omega$作为角速度，表示它们与整个运动的平移或旋转部分相关。 现在让我们考虑等式(6)一个特殊情况，当运动旋量坐标$\hat{\xi }$是一个常数矩阵：

$$\dot{g}(t)=\hat{\xi }g(t)$$

  因此，我们再次得到一个时不变的线性常微分方程，它可以求解得到：

$$g(t)=e^{\hat{\xi }t}g(0)$$

  假设初始条件$g(0)=I$，我们可以得出结论：

$$g(t)=e^{\hat{\xi }t}$$

  运动旋量指数进行泰勒展开是：

$$e^{\hat{\xi }t}=I+\hat{\xi }t+\frac{(\hat{\xi }t{)}^2}{2!}+\cdots +\frac{(\hat{\xi }t{)}^n}{n!}+\cdots$$

  使用前面介绍的罗德里格斯公式，可以直接得到：

$$e^{\hat{\xi }t}=\left[\begin{array}{cc}{e}^{\hat{\omega }t}& (I-e^{\hat{\omega }t})\hat{\omega }v+\omega {\omega }^{T}vt\\ 0& 1\end{array}\right]\text{\hspace{1em}(7)}$$

后期可能给出完整过程，目前先给出结论。

  从这个表达式中可以清楚地看出，指数$\hat{\xi }t$确实是$SE(3)$中的刚体变换矩阵。 因此，指数映射定义了从空间$\mathfrak{s}\mathfrak{e}(3)$到$SE(3)$的映射：

$$\exp :\mathfrak{s}\mathfrak{e}(3)\to SE(3)$$

$$\hat{\xi }\in \mathfrak{s}\mathfrak{e}(3)\mapsto e^{\hat{\xi }}\in SE(3)$$

  运动旋量$\hat{\xi }\in \mathfrak{s}\mathfrak{e}(3)$也称为$SE(3)$的指数坐标，正如$\hat{\omega }\in \mathfrak{s}\mathfrak{o}(3)$对$SO(3)$一样。

  那么每个刚体运动$g\in SE(3)$是否总能以这种指数形式表示？ 答案是肯定的。

SE(3)上指数映射的满射性(Surjectivity)

  对于$\forall g\in SE(3)$，存在运动旋量（可能并不唯一）坐标$\xi =(v,\omega )$和$t\in \mathbb{R}$以至于$g=e^{\hat{\xi }t}$

证明

  假设$g=(R,T)$。 对于旋转矩阵$R\in SO(3)$，我们总是找到$(\omega ,t)$，其中$||\omega ||=1$，使得$e^{\hat{\omega }t}=R$。如果$t=0$，从等式(7)，我们可以求解线性方程式中的$v\in {\mathbb{R}}^3$:

$$(I-e^{\hat{\omega }t})\hat{\omega }v+\omega {\omega }^{T}vt=T\Rightarrow v=[(I-e^{\hat{\omega }t})\hat{\omega }+\omega {\omega }^{T}t{]}^{-1}T$$

  如果$t=0$，则$R=I$。我们可能简单的选择$\omega =0,v=T/||T||$而且$t=||T||$。

---

  类似于旋转矩阵的指数坐标，从$\mathfrak{s}\mathfrak{e}(3)$到$SE(3)$的指数映射不是单射的，因此不是一对一的。 通常有很多指数坐标（或运动旋量）对应于每个$g\in SE(3)$。 与纯旋转情况类似，$\mathfrak{s}\mathfrak{e}(3)$的线性结构与李括号操作下的封闭：

$$[{\hat{\xi }}_1,{\hat{\xi }}_2]={\hat{\xi }}_1{\hat{\xi }}_2-{\hat{\xi }}_2{\hat{\xi }}_1=\left[\begin{array}{cc}\widehat{{\omega }_1\times {\omega }_2}& {\omega }_1\times v_2-{\omega }_2\times v_1\\ 0& 0\end{array}\right]\in \mathfrak{s}\mathfrak{e}(3)$$

  使$\mathfrak{s}\mathfrak{e}(3)$成为$SE(3)$的李代数。 两个刚体运动$g_1=e^{{\hat{\xi }}_1}$和$g_2=e^{{\hat{\xi }}_2}$彼此可交换：$f_1{g}_2=g_2{g}_1$，仅当$[{\hat{\xi }}_1,{\hat{\xi }}_2]=0$。

坐标和速度转换(伴随映射的引出)

  在上面描述的刚体运动中，我们描述了三维点相对于相机坐标系的移动方式。在计算机视觉中，我们通常需要知道点的坐标和它们的速度相对于不同位置的相机坐标系是如何变化的。

  这主要是因为选择相机坐标系作为参考坐标系来描述相机运动和相对于它的3D点通常更方便和自然。由于相机可能正在移动，我们需要知道从一个相机坐标系转换到另一个相机坐标系的变化量（例如坐标和速度）有多少。特别是，如何根据移动摄像机的正确表达（特征）点的位置和速度。

坐标变换规则

  我们把时间$t\in \mathbb{R}$始终用作注册摄像机运动的索引。即使在给出一些快照的离散情况下，我们也会按照一些时间索引对它们进行排序，就好像它们就是这个顺序一样！我们发现时间对于离散情况和连续情况都是一个很好的统一索引。所以我们将用$t$作为相机位置和响应图像的索引。因此，我们将使用$g(t)=(R(t),T(t))$或：

$$g(t)=\left[\begin{array}{cc}R(t)& T(t)\\ 0& 1\end{array}\right]\in SE(3)$$

  来表示在时间$t\in \mathbb{R}$处某个固定世界坐标系$W$和相机坐标系$C$之间的相对位移。这里我们将忽略来自所谓的$g_{cw}(t)$的下标$cw$，只要从上下文中清楚相关性即可。 默认情况下，我们假设$g(0)=I$，即在时间$t=0$，相机坐标系与世界坐标系重合。 因此，如果点$p\in {\mathbb{E}}^3$相对于世界坐标系的坐标是${\mathrm{X}}_0=\mathrm{X}(0)$，则其在时间t相对于相机的坐标是：

$$\mathrm{X}(t)=R(t){\mathrm{X}}_0+T(t)$$

  或以齐次的形式表示：

$$\mathrm{X}(t)=g(t){\mathrm{X}}_0$$

  如果相机位于$g(t_1),...,g(t_m)$的位置的对应时间索引分别$t_1,...,t_m$，则相同点$p$的坐标相应为$\mathrm{X}(t_i)=g(t_i){\mathrm{X}}_0,i=1,...,m$。 如果重要的只是位置而不是时间，我们将经常使用$g_i$作为$g(t_i)$的简写，并且类似地使用${\mathrm{X}}_i$作为$X(t_i)$。

  当开始时间不是$t=0$时，在时间$t_2$处的相机相对于在时间$t_1$处的相机之间的相对运动将表示为$g(t_2,t_1)\in SE(3)$。 然后我们在同一点$p$的坐标之间有以下关系：

$$\mathrm{X}(t_2)=g(t_2,t_1)\mathrm{X}(t_1),\forall t_1,t_2\in \mathbb{R}$$

  现在考虑在$t=t_3\in {\mathbb{R}}^3$处摄像机的第三个位置，如图6所示。

图6。刚体运动的组成。 $X(t_1),X(t_2),X(t_3)$分别是在时间$t=t_1,t_2,t_3$时相对于三个相机坐标系的点$p$的坐标。

  摄像机在$t_3$和$t_2$之间的相对运动是$g(t_3,t_2)$，而$t_3$和$t_1$之间的相对运动是$g(t_3,t_1)$。 然后我们在坐标之间有以下关系:

$$\mathrm{X}(t_3)=g(t_3,t_2)\mathrm{X}(t_2)=g(t_3,t_2)\mathrm{X}(t_1)$$

  与$t_3$和$t_1$坐标之间的直接关系进行比较：

$$\mathrm{X}(t_3)=g(t_3,t_1)\mathrm{X}(t_1)$$

  必须遵守以下连续运动的组合规则：

$$g(t_3,t_1)=g(t_3,t_2)g(t_2,t_1)$$

  组合规则描述了点$p$相对于任何相机位置的坐标$X$，如果它们相对于特定的一个已知相机。 相同的组合规则意味着反向规则：

$$g^{-1}(t_2,t_1)=g(t_1,t_2)$$

  因为$g(t_2,t_1)g(t_1,t_2)=g(t_2,t_2)=I$。如果时间对于特定问题没有物理意义，我们可以使用$g_{ij}$作为$g(t_i,t_j)$的简写 ,其中$t_1,t_2\in \mathbb{R}$。

  了解了坐标的变换后，我们现在研究速度会发生什么。 我们知道点$p\in {\mathbb{E}}^3$相对于移动摄像机的坐标$X(t)$是时间$t$的函数：

$$\mathrm{X}(t)=g_{cw}(t){\mathrm{X}}_0$$

  然后，点p相对于（瞬时）相机坐标系的速度为：

$$\dot{\mathrm{X}}(t)={\dot{g}}_{cw}(t){\mathrm{X}}_0$$

  按照在移动坐标系中的量表示为$\dot{\mathrm{X}}(t)$，我们用${\mathrm{X}}_0=g_{cw}^{-1}(t){\mathrm{X}}_0$代替并使用运动旋量的概念：

$${\hat{V}}_{cw}^c(t)={\dot{g}}_{cw}(t)g_{cw}^{-1}(t)\in \mathfrak{s}\mathfrak{e}(3)$$

  在前面我们已经了解到$\dot{g}(t)g^{-1}(t)$是下式的等式：

$$\dot{g}(t)g^{-1}(t)=\left[\begin{array}{cc}\dot{R}(t)R^T(t)& \dot{T}(t)-\dot{R}(t)R^T(t)T(t)\\ 0& 0\end{array}\right]$$

  所以对于${\hat{V}}_{cw}^c(t)={\dot{g}}_{cw}(t)g_{cw}^{-1}(t)$可以改写为：

$$\dot{\mathrm{X}}(t)={\hat{V}}_{cw}^c\mathrm{X}(t)$$

  由于${\hat{V}}_{cw}^c$是如下的形式：

$${\hat{V}}_{cw}^c=\left[\begin{array}{cc}\hat{\omega }(t)& v(t)\\ 0& 0\end{array}\right]$$

  我们也可以用3D坐标（而不是齐次坐标）写出点的速度：

$$\dot{\mathrm{X}}(t)=\hat{\omega }(t)\mathrm{X}(t)+v(t)$$

  符号${\hat{V}}_{cw}^c$的物理解释是世界坐标系相对于相机坐标系移动的速度，如相机坐标系中${\hat{V}}_{cw}^c$所示的下标和上标表示。 通常，为了清楚地指定速度的物理意义，我们需要指定：它是哪个坐标系相对移动到哪一个坐标系的速度，以及它在哪个坐标系中被观察。

  如果我们改变观察速度的位置，表达式将相应地改变。 例如，假设观察者处于通过刚体变换$g\in SE(3)$相对于相机坐标系移位的另一坐标系中。 然后，相对于该坐标系的相同点$p$的坐标是$Y(t)=g\mathrm{X}(t)$。 计算我们所在的新坐标系中的速度：

$$\dot{\mathrm{Y}}(t)=g{\dot{g}}_{cw}(t)g_{cw}^{-1}(t)g^{-1}\mathrm{Y}(t)=g{\hat{V}}_{cw}^c{g}^{-1}\mathrm{Y}(t)$$

  所以新的速度（或运动旋量）是：

$$\hat{V}=g{\hat{V}}_{cw}^c{g}^{-1}$$

  这是简单相同的物理量，但从不同的有利位置观察。我们看到两个速度通过相对运动$g$定义的映射相关，特别是：

$$\begin{array}{rcr}{\mathrm{a}\mathrm{d}}_g:\mathfrak{s}\mathfrak{e}(3)& \to & \mathfrak{s}\mathfrak{e}(3)\\ \hat{\xi }& \mapsto & g\hat{\xi }{g}^{-1}\end{array}$$

  这是空间$\mathfrak{s}\mathfrak{e}(3)$上的所谓伴随映射。 在前面的例子中使用这种表示法我们有$\hat{V}={\mathrm{a}\mathrm{d}}_g({\hat{V}}_{cw}^c)$。 显然，伴随映射将速度从一个坐标系转换为另一坐标系。 使用$g_{cw}(t)g_{wc}(t)=I$的事实，可以直接验证：

$${\hat{V}}_{cw}^c={\dot{g}}_{cw}{g}_{cw}^{-1}=-g_{cw}^{-1}{\dot{g}}_{wc}=-g_{cw}({\dot{g}}_{wc}{g}_{wc}^{-1})g_{cw}^{-1}={\mathrm{a}\mathrm{d}}_{g_{cw}}(-{\hat{V}}_{wc}^w)$$

  因此，在（瞬时）相机坐标系中观察，${\hat{V}}_{cw}^c$也可以被解释为相对于世界坐标系移动的相机的负速度（negated velocity）。

总结

  $g\in SE(3)$的两个最常用的元素表示是：

• 齐次表示：

$$\bar{g}=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right],R\in SO(3),T\in {\mathbb{R}}^3$$

• 运动旋量表达：

  $$g(t)=e^{\hat{\xi }t},\hat{\xi }=(v,\omega )\in {\mathbb{R}}^6,t\in \mathbb{R}$$

  在瞬时情况下，点相对于（瞬时）相机坐标系的速度为：

$$\dot{\mathrm{X}}(t)={\hat{V}}_{cw}^c\mathrm{X}(t)，{\hat{V}}_{cw}^c={\dot{g}}_{cw}{g}_{cw}^{-1}$$

  $g_{cw}(t)$是相机相对于世界坐标系的配置。 使用实际的3D坐标，3D点的速度产生熟悉的关系：

$$\dot{\mathrm{X}}(t)=\hat{\omega }(t)\mathrm{X}(t)+v(t)$$

	旋转$SO(3)$	刚体运动$SE(3)$
矩阵表示	$R\in GL(3)$ $R^{T}R=I$ $\det (R)=1$	$g=\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)$
三维坐标	$x=R{x}_0$	$x=R{x}_0+T$
逆	$R^{-1}=R^T$	$g^{-1}=\left(\begin{array}{cc}{R}^T& -R^{T}T\\ 0& 1\end{array}\right)$
指数表示	$R=\exp (\hat{w})$	$g=\exp (\hat{\xi })$
速度	$\dot{x}=\hat{w}x$	$\dot{x}=\hat{w}x+v$
伴随映射	$\hat{w}\mapsto R\hat{w}{R}^T$	$\hat{\xi }\mapsto g\hat{\xi }{g}^{-1}$