李代数求导

1） SLAM的过程就是不断的估计相机的位姿和建立地图，在此过程中会出现一定的误差，所以目的是寻找一个最佳位姿（通过优化方法，一般采用迭代优化方法，每次迭代都更新一个位姿的增量delta，使得目标函数最小。这个delta就是通过误差函数对T微分得到的。也就是说我们需要对变换矩阵T求微分），使得整体误差最小。

为此，可以列出关于变换矩阵T的目标函数，要求解方程需要对变换矩阵求导。

2） 就旋转矩阵而言，其矩阵加法不封闭，为此不满足导数定义中“加一个极小量”。所以，要使得矩阵满足求导运算，引出李群和李代数。

李群是具有连续性质的群，群上定义了某种矩阵的集合。对于旋转矩阵、变换矩阵都是满足李群的定义，例如旋转矩阵在旋转矩阵的集合中关于乘法运算满足李群。李群运算不满足求导，但与李群对应的李代数满足加法后保持性质。至此，将变换矩阵求导的问题转化为了求李代数的导数问题。

在讨论李代数的物理意义时，so(3)就代表了旋转向量组成的空间。实际上，so(3)是三维向量phi的集合，每个向量phi^表达了这个李代数对应的李群SO(3)上旋转矩阵R的导数。其中，R与phi是一个指数映射关系。

关于李代数和李群在几何意义上的关系：李代数对应李群的正切空间，描述了李群的局部导数。

李代数求导

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]]+...$$

其中“[ , ]”为李括号。

考虑SO(3)上的李代数：

$$\ln(\exp(\phi_1^\wedge)\exp(\phi_2^\wedge))^\vee=\left\{\begin{matrix} {J_l(\phi_2)}^{-1}\phi_1+\phi_2\;\;当\phi_1为小量\\ {J_r(\phi_1)}^{-1}\phi_2+\phi_1\;\;当\phi_2为小量 \end{matrix}\right.\tag{*}$$

第一个近似描述了：对一个旋转矩阵$R2$（李代数$\phi_2$），左乘一个微小旋转$R_1$，近似看作：在李代数$\phi_2$上加了一项${J_l(\phi_2)}^{-1}\phi_1$.

$$J_l=J=\frac{\sin\theta}{\theta}I+(1-\frac{\sin\theta}{\theta})\alpha\alpha^T+\frac{1-\cos\theta}{\theta}\alpha^\wedge\tag{**}$$ $$J^{-1}=\frac{\theta}{2}\cot\frac{\theta}{2}I+(1-\frac{\theta}{2}\cot\frac{\theta}{2})\alpha\alpha^T-\frac{\theta}{2}\alpha^\wedge\tag{**}$$

右乘：$J_r(\phi)=J_l(-\phi)$

至此，得到李群乘法和李代数加法的关系

BCH近似

$旋转矩阵R，其李代数\phi，微小旋转\Delta R，其李代数\Delta\phi$，李群乘法$\rightarrow$李代数加法

$$\exp(\Delta\phi^\wedge)\exp(\phi^\wedge)=\exp((\phi+J_l^{-1}(\phi)\Delta\phi)^\wedge)\tag{***}$$

李代数加法$\rightarrow$李群乘法

$$\exp((\phi+\Delta\phi)^\wedge)=\exp((J_l\Delta\phi)^\wedge)\exp(\phi^\wedge)=\exp(\phi^\wedge)\exp((J_r\Delta\phi)^\wedge)\tag{***}$$

李代数求导

两种思路：

1 导数模型 ， 2 扰动模型

SO(3)上的李代数求导

$位姿T，观察点的世界坐标p，产生的观测数据z，随即噪声w$，对于N个观测误差$e=z-Tp$，（寻找一个最优T，使误差最小）

$$\underset{T}{\min}J(T)=\sum_{i=1}^N||z_i-Tp_i||_2^2$$

李代数求导

1）按照导数模型，（根据李代数的加法结合导数的定义来求旋转后点的坐标对于旋转的导数，或者说是求旋转很小时偏移的变化率），可以求得：

$$\begin{align} & \frac{\partial (\exp(\phi^\wedge)p)}{\partial \phi}\approx-(Rp)^\wedge J_l\\&\\ \Rightarrow &\frac{\partial (Rp)}{\partial \phi}=(-Rp)^\wedge J_l \tag{旋转后的点对于李代数的导数} \end{align}$$

2）扰动模型，对旋转矩阵R进行一次扰动$\Delta R$（对应的李代数为$\varphi $），看结果相对于扰动的变化率。

$$\frac{\partial (Rp)}{\partial \phi}=(-Rp)^\wedge$$

SE(3)上的李代数求导

空间点p，变换T（对应的李代数为$\xi$），扰动变换$\Delta T=\exp(\delta\xi^\wedge)$，其李代数为$\delta\xi=[\delta\rho,\delta\phi]^T$：

$$\frac{\partial (Tp)}{\partial \delta\xi}=\begin{bmatrix}I & -(Rp+t)^\wedge\\ 0^T & 0^T\end{bmatrix}git$$

---

参考资料：

• 视觉SLAM十四讲：从理论到实践（第2版）. 高翔.