李群数学推导及Opencv代码实现

本文主要是一些李群基本原理的整理和基于Opencv实现了一个李代数的接口，主要用于学习和不想配置Eigen的CVer。

1. 李群与李代数

1.1 李群的定义

1. 三维空间中的旋转矩阵构成了特殊正交群（SO(3)）；而变换矩阵构成了特殊欧式群（SE(3)）。通常会被表示成如下形式：
   S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } S E ( 3 ) = C = { [ R T 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , T ∈ R 3 } SO(3)=\{R \in \mathbb{R^{3 \times 3}}|RR^T=I,det(R)=1\} \\ SE(3)=C=\{\begin{bmatrix}R & T \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R^{4 \times 4}}|R \in SO(3),T \in \mathbb{R}^3\} SO(3)={R∈R3×3∣RRT=I,det(R)=1}SE(3)=C={[R0T​T1​]∈R4×4∣R∈SO(3),T∈R3}
2. 群（Group）是一种运算+一种集合的代数结构。我们把运算记为 $\cdot$：
   (1) 封闭性：集合中任意两个元素做运算得到的结果还属于这个集合。
   (2) 结合律：对于集合中任意三个元素A 、B、C；均满足$A\cdot (B\cdot C)=(A\cdot B)\cdot C$。
   (3) 幺元：集合中存在一个元素，任何元素跟他做运算都等于他本身（SE(3)和SO(3)的幺元就是单位矩阵）。
   (4) 可逆：集合中的元素和他的逆做运算等于幺元。
   上述可以记为 “凤姐咬你”。
3. 进一步的具有连续（光滑）性质的群就叫做李群，像整数群那样离散没有连续性质的群就不是李群。而SO(n)和SE(n)在实数空间上是连续的（可以直观的理解为一个刚体在空间上可以连续运动），所以它们都是李群。

1.2 李代数

1.2.1 李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质，准确地说是单位元附近的正切空间。李代数由一个集合$\mathbb{V}$，一个数域$\mathbb{F}$和一个二元运算组成$[,]$。如果他们满足一下几条性质，则称$(\mathbb{V},\mathbb{F},[,])$为一个李代数。

1. 封闭性：$\forall X，Y\in \mathbb{V};[X,Y]\in \mathbb{V}$

2. 双线性：$\forall X,Y,Z\in \mathbb{V}，a,b\in \mathbb{F}$ 有:
   $[aX+bY,Z]=a[X,Z]+b[Y,Z]$, $[Z,aX+bY]=a[Z,X]+b[Z,Y]$

3. 自反性: $\forall X,[X,X]=0$

4. 雅可比等价:$\forall X,Y,Z\in \mathbb{V},[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$
   其中的二元运算被称作李括号。它不要求结合律，而要求元素和自己做运算为零的性质。一个简单的例子，三维向量${\mathbb{R}}^3$和运算叉积构成了一李代数。

1.2.2 SO(3)对应的李代数

SO(3)对应的李代数是定义在${\mathbb{R}}^3$上的向量$w$，每一个$w$都可以生成一个反对称矩阵$W$（其本质是向量叉乘得矩阵表达形式）。
$$W=w^{\wedge }=\left[\begin{array}{ccc}0& -w_3& w_2\\ w_3& 0& -w_1\\ -w_2& w_1& 0\end{array}\right]$$
在此种定义下，两个向量$w_1$，$w_2$的李括号为（$-\wedge$代表$\wedge$逆运算）：
$$[w_1,w_2]=(W_1{W}_2-W_2{W}_1{)}^{-\wedge }$$
他跟SO(3)的对应关系由指数映射（Exponential Map）给出:
$$R=\exp (w^{\wedge })$$

1.2.3 SE(3)对应的李代数

对于SE(3)，其同样也有对应的李代数，其是一个六维向量$\rho$（其在机器人学领域被称作旋量（twist））：
$$\rho =\left[\begin{array}{c}v\\ w\end{array}\right]$$
它的前三维是平移（但与SE(3)上对应的平移意义不一样），记为$v$；后三维是旋转，记为$w$，本质就是SO(3)所对应的李代数。同样我们使用$\wedge$表示为将六维向量$\rho$转成4维矩阵${\rho }^m$，用$-\wedge$表示逆运算：
$${\rho }^m={\rho }^{\wedge }=\left[\begin{array}{cc}{w}^{\wedge }& v\\ 0^T& 0\end{array}\right]$$
对应的李括号可以定义为：
$$[{\rho }_1^m,{\rho }_2^m]=({\rho }_1^m{\rho }_2^m-{\rho }_2^m{\rho }_1^m{)}^{-\wedge }$$
他跟SE(3)的对应关系也可以由指数映射给出:
$$\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]=\exp ({\rho }^{\wedge })$$

1.3 SO(3)上的指数映射

接下来首先推导一下SO(3)所对应的指数映射，考虑一个旋转矩阵（SO(3)）R，由于其为正交矩阵，则其满足如下：
$$R^{T}R=I$$
现在我们说R是某个相机的旋转，它会随时间而变化，即为时间的函数:$R(t)$，由于其仍然为旋转矩阵，则依然满足：
$$R(t{)}^{T}R(t)=I$$
在等式两边同时对时间求导：
$$\dot{R}(t{)}^{T}R(t)+R(t{)}^T\dot{R}(t)=0$$
整理得：
$$R(t{)}^T\dot{R}(t)=-(R(t{)}^T\dot{R}(t){)}^T$$
可以看到$\dot{R}(t{)}^{T}R(t)$是一个反对称矩阵（反对称满足$A=-A^T$），根据前文的定义不妨取$R(t{)}^T\dot{R}(t)=w^{\wedge }$，则：
$$\dot{R}(t)=R(t)w^{\wedge }$$
注意：也可以从$R{R}^T=I$推导出$\dot{R}(t)=w^{\wedge }R(t)$的结果，这两没有本质的区别，只是形式不同，左乘和由乘分别是全局坐标系和局部坐标系的不同表示（因为左乘是外旋，右乘是内旋？）。
根据高数中的定义，$\dot{R}(t)=R(t)w^{\wedge }$是一个关于$R$的一阶常微分方程，其可以写成如下形式：
$$\frac{dR(t)}{dt}=R(t)w^{\wedge }$$
分离变量法：
$$\frac{1}{R(t)}dR(t)=w^{\wedge }dt$$
对两侧进行积分：
$$\ln (R(t))=w^{\wedge }t$$
最后得：
$$R(t)=\exp (w^{\wedge }t)$$
公式$R(t)=\exp (w^{\wedge }t)$便是指数映射。现在考虑下一个问题一个矩阵的指数$\exp (w^{\wedge }t)$如何计算？

首先我们可以对矩阵形式的指数$\exp (w^{\wedge }t)$进行泰勒展开：
$$\exp (w^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}(w^{\wedge }{)}^n$$
上述公式没法直接计算，因为我们无法直接计算矩阵的无穷次幂。由于w是一个三维向量，因此我们定义它的模长和方向，分别记作$\theta$和$a$。这里$a$是一个长度为1的方向向量，即$||a||=1$。首先对于$a^{\wedge }$有如下两条性质：
$$a^{\wedge }{a}^{\wedge }=\left[\begin{array}{ccc}-a_2^2-a_3^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& -a_1^2-a_3^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& -a_1^2-a_2^2\end{array}\right]=a{a}^T-I$$
以及：
$$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=a^{\wedge }(a{a}^T-I)=-a^{\wedge }$$
注：回顾一下之前的定义$a^{\wedge }$是向量叉乘的矩阵形式。因此$a^{\wedge }a$相当于向量和自己做叉乘，结果为0。

以上两个公式提供了处理$a^{\wedge }$高阶项的方法。基于此，我们可以把指数映射写成如下形式：
$$\begin{gathered} \exp (w^{\wedge })=\exp (\theta a^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}(\theta a^{\wedge }{)}^n \\ =I+\theta a^{\wedge }+\frac{1}{2!}(\theta a^{\wedge }{)}^2+\frac{1}{3!}(\theta a^{\wedge }{)}^3+\frac{1}{4!}(\theta a^{\wedge }{)}^4+...... \\ =a{a}^T-a^{\wedge }{a}^{\wedge }+\theta a^{\wedge }+\frac{1}{2!}(\theta a^{\wedge }{)}^2-\frac{1}{3!}{\theta }^3{a}^{\wedge }-\frac{1}{4!}{\theta }^4(a^{\wedge }{)}^2+...... \\ =a{a}^T+(\theta -\frac{1}{3!}{\theta }^3+......)a^{\wedge }-(1-\frac{1}{2!}{\theta }^2+......)a^{\wedge }{a}^{\wedge } \\ =a^{\wedge }{a}^{\wedge }+I+\sin (\theta )a^{\wedge }-\cos (\theta )a^{\wedge }{a}^{\wedge } \\ =(1-\cos (\theta ))a^{\wedge }{a}^{\wedge }+\sin (\theta )a^{\wedge }+I \\ =\cos (\theta )I+(1-\cos (\theta ))a{a}^T+\sin (\theta )a^{\wedge } \end{gathered}$$
注：$(\theta -\frac{1}{3!}{\theta }^3+......)$和$(1-\frac{1}{2!}{\theta }^2+......)$分别为$\cos (\theta )$和$\sin (\theta )$的泰勒展开式。

最后我们得到了如下公式：
$$exp(w^{\wedge })=\cos (\theta )I+(1-\cos (\theta ))a{a}^T+\sin (\theta )a^{\wedge }$$
这便是大名鼎鼎的罗德里格斯公式（Rodrigues），其用于将旋转向量转化为旋转矩阵。这表明SO(3)所对应的李代数的物理意义就是旋转向量，而指数映射就说罗德里格斯公式。 反之如果我们定义对数映射，也可以把李代数映射到李群。指数映射是一个满射而不是单射，这意味着每个SO(3)都有与之对应的李代数；但是可能存在多个李代数的元素对应同一个SO(3)。但是，如果我们把$\theta$的范围定位为$\pm \pi$的范围内，那么李群和李代数就是一一对应的。李群和李代数的关系与旋转矩阵和旋转向量的关系是相似的，他们的对应关系都由罗德里格斯公式给出，旋转矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵间进行微积分运算。

1.3 SE(3)上的指数映射

接下来，我们推导一下SE(3)上的指数映射，首先根据前文可知，有如下公式：
$$\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]=\exp ({\rho }^{\wedge })$$
我们同样对上述公式进行泰勒展开：
$$\begin{gathered} \exp ({\rho }^{\wedge })=\exp (\left[\begin{array}{cc}{w}^{\wedge }& v\\ 0^T& 0\end{array}\right])=\sum _{n=0}^{\infty }\frac{1}{n!}(\left[\begin{array}{cc}{w}^{\wedge }& v\\ 0^T& 0\end{array}\right]{)}^n \\ =I+\left[\begin{array}{cc}{w}^{\wedge }& v\\ 0^T& 0\end{array}\right]+\frac{1}{2!}{\left[\begin{array}{cc}{w}^{\wedge }& v\\ 0^T& 0\end{array}\right]}^2+\frac{1}{3!}{\left[\begin{array}{cc}{w}^{\wedge }& v\\ 0^T& 0\end{array}\right]}^3+...... \\ =I+\left[\begin{array}{cc}{w}^{\wedge }& v\\ 0^T& 0\end{array}\right]+\frac{1}{2!}\left[\begin{array}{cc}{w^{\wedge }}^2& w^{\wedge }v\\ 0^T& 0\end{array}\right]+\frac{1}{3!}\left[\begin{array}{cc}{w^{\wedge }}^3& {w^{\wedge }}^{2}v\\ 0^T& 0\end{array}\right]+...... \end{gathered}$$
其可以被简化为如下形式：
$$\begin{gathered} \exp (\left[\begin{array}{cc}{w}^{\wedge }& v\\ 0^T& 0\end{array}\right])=\left[\begin{array}{cc}\exp (w^{\wedge })& Jv\\ 0^T& 1\end{array}\right] \\ J=I+\frac{1}{2!}{w}^{\wedge }+\frac{1}{3!}(w^{\wedge }{)}^2+...... \end{gathered}$$
从上述公式看SE(3)的指数映射的旋转部分与SO(3)保持一致，而平移部分多了个系数$J$。我们可使用前文推导SO(3)指数映射所用的几条性质（$a^{\wedge }{a}^{\wedge }=a{a}^T-I，a^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }$），继续化简J。首先同样将$w^{\wedge }$表达成一个模长乘一个单位向量的形式$\theta a$：
$$\begin{gathered} J=I+\frac{1}{2!}\theta a+\frac{1}{3!}(\theta a{)}^2+...... \\ =I+\frac{1}{\theta }(\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+...)a^{\wedge }+\frac{1}{\theta }(\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+...)a^{\wedge }{a}^{\wedge } \\ =\frac{1}{\theta }(1-\cos (\theta ))a^{\wedge }+\frac{\theta -\sin (\theta )}{\theta }(a{a}^T-I)+I \\ =\frac{\sin (\theta )}{\theta }I+(1-\frac{\sin (\theta )}{\theta })a{a}^T+\frac{1-\cos (\theta )}{\theta }{a}^{\wedge } \end{gathered}$$
不同的其他文献中也有采用$w^{\wedge }$，采用如下表达式表示$J$：
$$J=I+\frac{1-\cos (\theta )}{{\theta }^2}{w}^{\wedge }+\frac{\theta -\sin (\theta )}{{\theta }^3}{w}^{\wedge }{w}^{\wedge }$$
以上就是SE(3)上完整的指数映射推导过程，同样该过程也有逆运算对数映射。不过我们通常不采用对数映射求解李代数，SE(3)的旋转部分，可以通过计算他的旋转向量得到；而平移部分由于$t=Jv$，且$J$可以由旋转向量得到，我们只需要直接求解$t=Jv$这个线性方程就能够得到$v$。并且SE(3)上的李代数同样可以表征SE(3)导数性质（瞬时状态）

2. 李群下的伴随性质(Adjoint)

接下来通过一个简单的机器人学应用问题，引出李群下的伴随性质。考虑一个经典的眼在手上的手眼标定问题，其可以被定义为如下公式：
$$AX=XB$$
其中A的物理意义是机器人工具坐标系下单位时间内的$\Delta tool$；$X$的物理意义是相机在机器人工具坐标系下的位置和姿态；$B$的物理意义是相机坐标系下单位时间内的$\Delta camera$（手眼标定问题不是本文的重点，因此不对上述公式做详细推导）。因此上述公式可以写成如下的形式：
$$\frac{d{T}_{tool}}{dt}X=X\frac{d{T}_{camera}}{dt}$$
根据前文的介绍我们可以知道SE(3)上的李代数$\rho$可以表示SE(3)的导数性质，因此上述公式可以被进一步的写成如下形式：
$$\begin{gathered} {\rho }_{tool}^{\wedge }X=X{\rho }_{camera}^{\wedge } \\ {\rho }_{tool}^{\wedge }=X{\rho }_{camera}^{\wedge }{X}^{-1} \end{gathered}$$
将上述公式进一步的抽象为如下形式：
$${\rho }_2^{\wedge }=C_{12}{\rho }_1^{\wedge }{C}_{12}^{-1}$$
其中$C_{12}$代表坐标系1到坐标系2的刚体变换，${\rho }_1^{\wedge }$和${\rho }_2^{\wedge }$分别代表坐标系1和坐标系2下的6D速度（旋量）。这组公式也被称作李群上的共轭变换。
根据前文对刚体变换$C$和李代数$\rho$的定义，共轭变换可以进行如下化简：
$$\begin{gathered} \left[\begin{array}{cc}{w}_2^{\wedge }& v_2\\ 0^T& 0\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{w}_1^{\wedge }& v_1\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{R}^{-1}& -R^{-1}T\\ 0^T& 1\end{array}\right] \\ =\left[\begin{array}{cc}R{w}_1^{\wedge }& R{v}_1\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{R}^{-1}& -R^{-1}T\\ 0^T& 1\end{array}\right] \\ =\left[\begin{array}{cc}R{w}_1^{\wedge }{R}^{-1}& -R{w}_1^{\wedge }{R}^{-1}T+R{v}_1\\ 0^T& 0\end{array}\right] \end{gathered}$$
为了进一步化简上述公式，先推导一个小结论$R{w}^{\wedge }=R{w}^{\wedge }{R}^{-1}$，考虑有一个向量$V$，有：
$$\begin{gathered} R{w}^{\wedge }v=R{w}^{\wedge }R{R}^{-1}v（正交矩阵满足R{R}^{-1}=I） \\ =(Rw)\times (R{R}^{-1}v)（w^{\wedge }的本质为向量叉乘的矩阵表达方式） \\ =R(w\times R^{-1}v)（叉乘的分配律） \\ =Rw\times R^{-1}v（结合律） \\ =R{w}^{\wedge }{R}^{-1}v \end{gathered}$$
因此：
$$\left[\begin{array}{cc}{w}_2^{\wedge }& v_2\\ 0^T& 0\end{array}\right]=\left[\begin{array}{cc}R{w}_1^{\wedge }& -R{w}_1^{\wedge }T+R{v}_1\\ 0^T& 0\end{array}\right]$$
由于有$-R{w}_1^{\wedge }T=-(R{w}_1^{\wedge })T=-(R{w}_1)\times T=T\times (R{w}_1)$，将上述矩阵变换写成6x6的矩阵形式：
$$\left[\begin{array}{c}{v}_2\\ w_2\end{array}\right]=\left[\begin{array}{cc}R& T^{\wedge }R\\ 0& R\end{array}\right]\left[\begin{array}{c}{v}_1\\ w_1\end{array}\right]$$
上述6x6的矩阵就算是SE(3)上的伴随矩阵，可以用于将一个坐标系下的6D速度转换到另一个坐标系下。 并且从SE(3)的伴随矩阵可以很容易的得出如下结论:
$$w_2=R{w}_1$$
这里面的$R$就是SO(3)的伴随矩阵。

3. 李代数求导

3.1 BCH公式与近似形式

根据前文的推导我们已经清楚了SO(3)和SE(3)上李群与李代数关系。但是，当在SO(3)中完成两个矩阵乘法时，其对应的李代数发生了什么改变呢？反过来说，当两个李代数做加法时，SO(3)上是否对应这两个矩阵的乘积？换而言之，我们在研究下式是否成立：
$$\ln (\exp (A)\exp (B))=A+B?$$
很遗憾，上述公式在对于李代数的时候并不成立。两个李代数指数映射乘积的完整形式，由 Baker-Campbell-Hausdorff（BCH） 公式（完整推导很复杂，当个结论记住就行）给出：
$$\ln (\exp (A)\exp (B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+......$$
其中$[,]$为李括号，定义与前文相同（$[A,B]=AB-BA$）。BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。当将其用于SO(3)上的李代数$\ln (\exp (w_1^{\wedge })\exp (w_2^{\wedge }){)}^{-\wedge }$，当$w_1$或$w_2$为一个小量的时候，小量二次以上的项都可以被忽略。此时用于SO(3)的BCH公式有近似表达：
$$\begin{gathered} \ln (\exp (w_1^{\wedge })\exp (w_2^{\wedge }){)}^{-\wedge }\approx J_l(w_2{)}^{-1}{w}_1+w_2（左乘近似w_1为小量） \\ \ln (\exp (w_1^{\wedge })\exp (w_2^{\wedge }){)}^{-\wedge }\approx J_r(w_1{)}^{-1}{w}_2+w_1（右乘近似w_2为小量） \end{gathered}$$
以第一个公式为例，该式告诉我们，当对一个旋转矩阵$R_2$（李代数为$w_2$）左乘一个微小旋转矩阵$R_1$（李代数为$w_1$）时，可以近似地看作，在原有的李代数$w_2$上加上了一项$J_l(w_2{)}^{-1}{w}_1$，第二个也是同理（其中的$J_l$就是SE(3)所对应的李代数中的雅可比矩阵，右乘只需要将自变量取一个负号就行$J_r(w)=J_l(-w)$）。最后左乘近似也可以简单的写成如下形式：
$$\exp (\Delta w^{\wedge })\exp (w^{\wedge })=\exp ((w+J_l(w{)}^{-1}\Delta w{)}^{\wedge })$$
根据上述理论，使用李代数解决求导问题的思路分为两种：

1. 用李代数表示姿态，然后根据李代数加法对李代数求导。
2. 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

第一种方法涉及到求解$J$，较为复杂，且不常用。因此主要介绍第二种求导方法。

3.2 so(3)上的李代数求导

3.2.1 扰动模型（左乘）

首先考虑SO(3)上的情况，假设我们对一个空间点$l$进行旋转，得到了$Rp$。现在要计算旋转之后点的坐标相对于旋转矩阵的导数。简单的表达为如下形式：
$$\frac{\partial (Rp)}{\partial R}$$
设有一个旋转矩阵$R$的李代数为$w$，现在对其施加一个左乘的扰动，看结果对于这个扰动的变化率。设左扰动$\Delta R$对应的李代数为$\Delta w$。然后对$w$求导，即：
$$\begin{gathered} \frac{\partial (Rp)}{\partial w}=\underset{w\to 0}{\lim }\frac{\exp (\Delta w^{\wedge })\exp (w^{\wedge })p-\exp (w^{\wedge })p}{\Delta w} \\ =\underset{w\to 0}{\lim }\frac{(I+\Delta w^{\wedge })\exp (w^{\wedge })p-\exp (w^{\wedge })p}{\Delta w}(1阶泰勒展开) \\ =\underset{w\to 0}{\lim }\frac{\Delta w^{\wedge }\exp (w^{\wedge })p+\exp (w^{\wedge })p-\exp (w^{\wedge })p}{\Delta w} \\ =\underset{w\to 0}{\lim }\frac{\Delta w^{\wedge }\exp (w^{\wedge })p}{\Delta w} \\ =\underset{w\to 0}{\lim }\frac{\Delta w^{\wedge }(Rp)}{\Delta w} \\ =\underset{w\to 0}{\lim }\frac{-(Rp{)}^{\wedge }\Delta w}{\Delta w}(向量叉乘)=-(Rp{)}^{\wedge } \end{gathered}$$
最后得到基于左乘扰动模型的雅克比矩阵就是$-(Rp{)}^{\wedge }$

3.2.2 扰动模型（右乘）

右乘同理，这里直接给出推导过程
$$\begin{gathered} \frac{\partial (Rp)}{\partial w}=\underset{w\to 0}{\lim }\frac{\exp (w^{\wedge })\exp (\Delta w^{\wedge })p-\exp (w^{\wedge })p}{\Delta w} \\ =\underset{w\to 0}{\lim }\frac{\exp (w^{\wedge })(I+\Delta w^{\wedge })p-\exp (w^{\wedge })p}{\Delta w} \\ =\underset{w\to 0}{\lim }\frac{\exp (w^{\wedge })\Delta w^{\wedge }p+\exp (w^{\wedge })p-\exp (w^{\wedge })p}{\Delta w} \\ =\underset{w\to 0}{\lim }\frac{\exp (w^{\wedge })\Delta w^{\wedge }p}{\Delta w} \\ =\underset{w\to 0}{\lim }\frac{R\Delta w^{\wedge }p}{\Delta w} \\ =\underset{w\to 0}{\lim }\frac{-R{p}^{\wedge }\Delta w}{\Delta w}=-R{p}^{\wedge } \end{gathered}$$

3.3 se(3)上的李代数求导

3.3.1 扰动模型（左乘）

之后给出SE(3)上的扰动模型。假设某空间点$p$（$p_h$为$p$的齐次坐标）经过一次变换$C$（对应的李代数为$\rho$）,得到$Cp$。现在给$C$左乘一个扰动$\Delta {\rho }^{\wedge }=[\Delta v,\Delta w{]}^T$，那么：
$$\begin{gathered} \frac{\partial Cp}{\partial \Delta \rho }=\underset{w\to 0}{\lim }\frac{\exp (\Delta {\rho }^{\wedge })\exp ({\rho }^{\wedge })p_h-\exp ({\rho }^{\wedge })p_h}{\Delta \rho } \\ =\underset{w\to 0}{\lim }\frac{(I+\Delta {\rho }^{\wedge })\exp ({\rho }^{\wedge })p_h-\exp ({\rho }^{\wedge })p_h}{\Delta \rho }(1阶泰勒展开) \\ =\underset{w\to 0}{\lim }\frac{\Delta {\rho }^{\wedge }\exp ({\rho }^{\wedge })p_h+\exp ({\rho }^{\wedge })p_h-\exp ({\rho }^{\wedge })p_h}{\Delta \rho } \\ =\underset{w\to 0}{\lim }\frac{\Delta {\rho }^{\wedge }\exp ({\rho }^{\wedge })p_h}{\Delta \rho } \\ =\underset{w\to 0}{\lim }\frac{\left[\begin{array}{cc}\Delta w^{\wedge }& \Delta v\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}Rp+T\\ 1\end{array}\right]}{\Delta \rho } \\ =\underset{w\to 0}{\lim }\frac{\left[\begin{array}{c}\Delta w^{\wedge }(Rp+T)+\Delta v\\ 0^T\end{array}\right]}{{\left[\begin{array}{cc}\Delta v& \Delta w\end{array}\right]}^T} \\ =\underset{w\to 0}{\lim }\frac{\left[\begin{array}{c}-(Rp+T{)}^{\wedge }\Delta w+\Delta v\\ 0^T\end{array}\right]}{{\left[\begin{array}{cc}\Delta v& \Delta w\end{array}\right]}^T}(向量叉乘) \\ =\underset{w\to 0}{\lim }\frac{\left[\begin{array}{cc}I& -(Rp+T{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\left[\begin{array}{c}\Delta v\\ \Delta w\end{array}\right]}{{\left[\begin{array}{cc}\Delta v& \Delta w\end{array}\right]}^T} \\ =\left[\begin{array}{cc}I& -(Rp+T{)}^{\wedge }\\ 0^T& 0^T\end{array}\right] \end{gathered}$$
最后我们得到了一个4x6的雅克比矩阵。

3.3.2 扰动模型（右乘）

同理给出SE(3)右乘扰动模型的雅克比矩阵推导过程：
$$\begin{gathered} \frac{\partial Cp}{\partial \Delta \rho }=\underset{w\to 0}{\lim }\frac{\exp ({\rho }^{\wedge })\exp (\Delta {\rho }^{\wedge })p_h-\exp ({\rho }^{\wedge })p_h}{\Delta \rho } \\ =\underset{w\to 0}{\lim }\frac{\exp ({\rho }^{\wedge })(I+\Delta {\rho }^{\wedge })p_h-\exp ({\rho }^{\wedge })p_h}{\Delta \rho }(1阶泰勒展开) \\ =\underset{w\to 0}{\lim }\frac{\exp ({\rho }^{\wedge })\Delta {\rho }^{\wedge }{p}_h+\exp ({\rho }^{\wedge })p_h-\exp ({\rho }^{\wedge })p_h}{\Delta \rho } \\ =\underset{w\to 0}{\lim }\frac{\exp ({\rho }^{\wedge })\Delta {\rho }^{\wedge }{p}_h}{\Delta \rho } \\ =\underset{w\to 0}{\lim }\frac{\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}\Delta w^{\wedge }& \Delta v\\ 0^T& 0\end{array}\right]p_h}{\Delta \rho } \\ =\underset{w\to 0}{\lim }\frac{\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}\Delta w^{\wedge }p+\Delta v\\ 0\end{array}\right]}{\Delta \rho } \\ =\underset{w\to 0}{\lim }\frac{\left[\begin{array}{c}R(\Delta w^{\wedge }p+\Delta v)\\ 0^T\end{array}\right]}{{\left[\begin{array}{cc}\Delta v& \Delta w\end{array}\right]}^T} \\ =\underset{w\to 0}{\lim }\frac{\left[\begin{array}{c}R(\Delta v-p^{\wedge }\Delta w)\\ 0^T\end{array}\right]}{{\left[\begin{array}{cc}\Delta v& \Delta w\end{array}\right]}^T}(向量叉乘) \\ =\underset{w\to 0}{\lim }\frac{\left[\begin{array}{cc}R& -R{p}^{\wedge }\\ 0^T& 0^T\end{array}\right]\left[\begin{array}{c}\Delta v\\ \Delta w\end{array}\right]}{{\left[\begin{array}{cc}\Delta v& \Delta w\end{array}\right]}^T} \\ =\left[\begin{array}{cc}R& -R{p}^{\wedge }\\ 0^T& 0^T\end{array}\right] \end{gathered}$$
注：求解雅克比和更新雅克比用的扰动模型一定要一致

4. 代码实现

本文基于上述原理结合opencv的cv::Matx、完成对cv::Affine3和cv::Quat的李代数功能扩展.。

4.1 SO3

/// @file
/// Special Euclidean group SO(3) - rotation and translation in 3d.

#pragma once

#include <opencv2/opencv.hpp>
#include <opencv2/core/quaternion.hpp>

namespace cvSophus
{
    template <typename T>
    class SO3
    {
    public:
        /// @brief non-parametric constructor
        SO3() = default;

        /// @brief constructor from quaternion
        /// @param q : quaternion
        SO3(const cv::Quat<T> &q) : data_(q) {}

        /// @brief constructor from rotation matrix
        /// @param R : rotation matrix
        SO3(const cv::Matx<T, 3, 3> &R) { data_ = data_.createFromRotMat(R); }


        /// @brief constructor from rotation vector
        /// @param rvec : rotation vector
        SO3(const cv::Vec<T, 3> &rvec)
        {
            // if Quaternions are directly converted rotation vector, rotation vector will be singular.
            cv::Matx<T, 3, 3> R;
            cv::Rodrigues(rvec, R);
            data_ = data_.createFromRotMat(R);
        }


        /// @brief constructor from euler angle
        /// @param euler : euler angle
        /// @param euler_type : euler angel type
        SO3(const cv::Vec<T, 3> &euler, const cv::QuatEnum::EulerAnglesType &euler_type)
        {
            data_ = data_.createFromEulerAngles(euler, euler_type);
        }

        SO3<T> operator*(const SO3<T> &rhs)
        {
            return SO3<T>(data_.toRotMat3x3() * rhs.matrix());
        }

        /// @brief generate the adjoint matrix of SO(3)
        /// @return adjoint matrix 
        cv::Matx<T, 3, 3> adjoint() { return data_; }


        /// @brief generate the Derivative by left perturbation
        /// @param p : the point of 3D space
        /// @return jacobian matrix
        cv::Matx<T, 3, 3> left_jacobian(const cv::Vec<T, 3> &p)
        {
            return -skew(data_.toRotMat3x3() * p);
        }

        /// @brief generate the Derivative by right perturbation
        /// @param p : the point of 3D space
        /// @return jacobian matrix
        cv::Matx<T, 3, 3> right_jacobian(const cv::Vec<T, 3> &p)
        {
            return -data_.toRotMat3x3() * skew(p);
        }

        /// @brief log map(to rotation vector)
        /// @return the Lie Algebra of SO(3) (rotation vector)
        cv::Vec<T, 3> log() const
        {
            cv::Matx<T, 3, 3> R = matrix();

            cv::Vec<T, 3> rvec;
            cv::Rodrigues(R, rvec);

            return rvec;
        }

        /// @brief get rotation matrix
        /// @return rotation matrix
        cv::Matx<T, 3, 3> matrix() const { return data_.toRotMat3x3(); }


        /// @brief get euler angle
        /// @param euler_type : euler angle type
        /// @return euler angle by rad
        cv::Vec<T, 3> to_euler_angle(const cv::QuatEnum::EulerAnglesType &euler_type) const
        {
            return data_.toEulerAngles(euler_type);
        }

        /// @brief get quterntion
        /// @return quterntion
        cv::Quat<T> to_querntion() const { return data_ };

        /// @brief Exponential Map
        /// @param w : the Lie Algebra of SO(3) (rotation vector)
        /// @return SO(3)
        static SO3 exp(const cv::Vec<T, 3> &w) { return SO3(w); }

        /// @brief get skew matrix
        /// @param w : the Lie Algebra of SO(3) (rotation vector)
        /// @return skew matrix
        static cv::Matx<T, 3, 3> skew(const cv::Vec<T, 3> &w)
        {
            cv::Matx<T, 3, 3> skew_matrix;
            skew_matrix << 0, -w(2), w(1),
                w(2), 0, -w(0),
                -w(1), w(0), 0;

            return skew_matrix;
        }

    private:
        cv::Quat<T> data_;
    };

    template<typename T>
    std::ostream& operator<<(std::ostream& out, const SO3<T> &data)
    {
        out << data.matrix();
        return out;
    }

    using SO3d = SO3<double>;
    using SO3f = SO3<float>;
}

4.2 SE(3)

/// @file
/// Special Euclidean group SE(3) - rotation and translation in 3d.

#pragma once

#include <opencv2/opencv.hpp>
#include "so3.hpp"


namespace cvSophus
{
    template <typename datatype>
    class SE3
    {
    public:
        /// @brief non-parametric constructor
        SE3() = default;

        /// @brief constructor from rotation matrix and translation
        /// @param R : rotation matrix
        /// @param T : translation
        SE3(const cv::Matx<datatype, 3, 3> &R, const cv::Vec<datatype, 3> &T) { data_ = cv::Affine3<datatype>(R, T); }

        /// @brief constructor from SO(3) and translation
        /// @param R : SO(3)
        /// @param T : translation
        SE3(const SO3<datatype> &R, const cv::Vec<datatype, 3> &T) { data_ = cv::Affine3<datatype>(R.log(), T); }

        /// @brief constructor from rotation vector and translation
        /// @param rvec : rotation vector
        /// @param T : translation
        SE3(const cv::Vec<datatype, 3> &rvec, const cv::Vec<datatype, 3> &T) { data_ = cv::Affine3<datatype>(rvec, T); }


        /// @brief constructor from rotation vector and translation
        /// @param t_rvec : the std::vector of translation and rotation vector
        SE3(const std::vector<datatype> &t_rvec)
        {
            assert(t_rvec.size() == 6);
            cv::Vec<datatype, 3> t(t_rvec[0], t_rvec[1], t_rvec[2]);
            cv::Vec<datatype, 3> rvec(t_rvec[3], t_rvec[4], t_rvec[5]);

            data_ = cv::Affine3<datatype>(rvec, t);
        }

        /// @brief  constructor from euler angle and translation
        /// @param euler : euler angle
        /// @param t : translation
        /// @param euler_type : the type of euler angle
        SE3(const cv::Vec<datatype, 3> &euler, const cv::Vec<datatype, 3> &t,
            const cv::QuatEnum::EulerAnglesType &euler_type)
        {
            SO3<datatype> tmp(euler, euler_type);
            data_ = cv::Affine3<datatype>(tmp.log(), t);
        }


        /// @brief constructor from the euler angle and translation of std::vector
        /// @param t_euler : the euler angle and translation of std::vector
        /// @param euler_type : the type fo euler angle
        SE3(const std::vector<datatype> &t_euler, const cv::QuatEnum::EulerAnglesType &euler_type)
        {
            cv::Vec<datatype, 3> t(t_euler[0], t_euler[1], t_euler[2]);
            cv::Vec<datatype, 3> euler(t_euler[3], t_euler[4], t_euler[5]);

            SO3<datatype> tmp(euler, euler_type);
            data_ = cv::Affine3<datatype>(tmp.log(), t);
        }

        SE3 operator*(const SE3 &rhs)
        {
            cv::Affine3<datatype> rhs_affine(rhs.rvec(), rhs.translation());
            cv::Affine3<datatype> result = data_ * rhs_affine;

            return SE3<datatype>(result.rvec(), result.translation());
        }


        /// @brief get translaton
        /// @return translaton
        cv::Vec<datatype, 3> translation() const { return data_.translation(); }


        /// @brief get rotation vector
        /// @return  rotation vector
        cv::Vec<datatype, 3> rvec() const { return data_.rvec(); }


        /// @brief generate the adjoint matrix of SE(3)
        /// @return adjoint matrix
        cv::Matx<datatype, 6, 6> adjoint() const
        {
            cv::Matx<datatype, 3, 3> R = data_.rotation();
            cv::Matx<datatype, 3, 3> zero = cv::Matx<T, 3, 3>::zeros();

            cv::Matx<datatype, 3, 6> upper;
            cv::hconcat(R, SO3<T>::skew(data_.translation() * R), upper);
            cv::Matx<datatype, 3, 6> lower;
            cv::hconcat(zero, R, lower);

            cv::Matx<datatype, 6, 6> result;
            cv::vconcat(upper, lower, result);

            return result;
        }

        /// @brief generate the Derivative by left perturbation
        /// @param p : the point of 3D space
        /// @return jacobian matrix
        cv::Matx<datatype, 4, 6> left_jacobian(const cv::Vec<datatype, 3> &p)
        {
            cv::Matx<datatype, 3, 3> left = cv::Matx<datatype, 3, 3>::eye();
            cv::Matx<datatype, 3, 3> right = -SO3<datatype>::skew(data_.rotation() * p + data_.translation());

            cv::Matx<datatype, 3, 6> upper;
            cv::hconcat(left, right, upper);

            cv::Matx<datatype, 1, 6> lower = cv::Matx<datatype, 1, 6>::zeros();

            cv::Matx<datatype, 4, 6> result;
            cv::vconcat(upper, lower, result);

            return result;
        }

        /// @brief generate the Derivative by right perturbation
        /// @param p : the point of 3D space
        /// @return jacobian matrix
        cv::Matx<datatype, 4, 6> right_jacobian(const cv::Vec<datatype, 3> &p)
        {
            cv::Matx<datatype, 3, 3> left = data_.rotation();
            cv::Matx<datatype, 3, 3> right = -left * SO3<datatype>::skew(p);

            cv::Matx<datatype, 3, 6> upper;
            cv::hconcat(left, right, upper);

            cv::Matx<datatype, 1, 6> lower = cv::Matx<datatype, 1, 6>::zeros();

            cv::Matx<datatype, 4, 6> result;
            cv::vconcat(upper, lower, result);

            return result;
        }

        /// @brief log map(to twist)
        /// @return the Lie Algebra of SE(3) (twist)
        cv::Vec<datatype, 6> log() const
        {
            cv::Vec<datatype, 3> rvec = data_.rvec();

            cv::Vec<datatype, 3> t = data_.translation();
            cv::Matx<datatype, 3, 3> J = jacobian(rvec, 1e-8);
            cv::Vec<datatype, 3> v = J.solve(t, cv::DECOMP_SVD);

            return {v(0), v(1), v(2), rvec(0), rvec(1), rvec(2)};
        }

        cv::Matx<datatype, 4, 4> matrix() const
        {
            return data_.matrix();
        }

        /// @brief to the std::vector of rotation vector and translation
        /// @return the std::vector of rotation vector and translation
        std::vector<datatype> to_stdvector_t_rvec()
        {
            cv::Vec<datatype, 3> rvec = data_.rvec();
            cv::Vec<datatype, 3> t = data_.translation();

            return {t(0), t(1), t(2), rvec(0), rvec(1), rvec(2)};
        }

        /// @brief Exponential Map
        /// @param w : the Lie Algebra of SE(3) (twist)
        /// @return SE(3)
        static SE3 exp(const cv::Vec<datatype, 6> &rho)
        {
            cv::Vec<datatype, 3> v(rho(0), rho(1), rho(2));
            cv::Vec<datatype, 3> w(rho(3), rho(4), rho(5));

            cv::Vec<datatype, 3> t = jacobian(w, 1e-8) * v;

            return SE3<T>(w, t);
        }

        /// @brief generate jacobian matrix(to generate the Lie Algerba of SE(3))
        /// @param w : the Lie Algebra of SO(3) (rotaton vector)
        /// @return jacobian matrix
        static cv::Matx<datatype, 3, 3> jacobian(const cv::Vec<datatype, 3> &w, const double &eps)
        {
            double theta_ = cv::norm(w);

            cv::Matx<datatype, 3, 3> J = cv::Matx<datatype, 3, 3>::eye() + ((1 - std::cos(theta_)) / (theta_ * theta_)) * SO3<datatype>::skew(w) +
                                  ((theta_ - std::sin(theta_)) / (theta_ * theta_ * theta_)) * SO3<datatype>::skew(w) * SO3<datatype>::skew(w);

            // To numerical stability
            if (theta_ < eps)
            {
                J = cv::Matx<datatype, 3, 3>::eye() + 0.5 * SO3<datatype>::skew(w) +
                    (1.f + 1.f / 6.0) * SO3<datatype>::skew(w) * SO3<datatype>::skew(w);
            }

            return J;
        }

    private:
        cv::Affine3<datatype> data_;
    };

    template <typename datatype>
    std::ostream& operator<<(std::ostream& out, const SE3<datatype> &data)
    {
        out << data.matrix();
        return out;
    }

    using SE3f = SE3<float>;
    using SE3d = SE3<double>;
}

• 参考文献：《视觉slam14讲》《机器人学当中的状态估计》《Lie Groups for 2D and 3D Transformations》
  所有代码及参考书籍及文件见github