在SLAM后端优化过程中关于李群与李代数的的理解与总结

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

---

参考资料《视觉SLAM十四讲》

首先为什么要引入李群与李代数？ 两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动。对于一个向量 $\alpha$，假设他在两个坐标系下的坐标分别为 $[\alpha_1,\alpha_2,\alpha_3]^T$和 $[\alpha_1',\alpha_2',\alpha_3']^T$，因为向量本身并没有改变，因此可以得到：

$[e_1,e_2,e_3] \begin{bmatrix} \alpha_1 \\ \alpha_2\\\alpha_3\\ \end{bmatrix} = [e_1',e_2',e_3'] \begin{bmatrix} \alpha_1' \\ \alpha_2'\\\alpha_3'\\ \end{bmatrix}$

为了将等式左边系数简化为单位阵，将上述等式左右两边左乘 $[e_1,e_2,e_3]^T$，即：

$\begin{bmatrix} \alpha_1 \\ \alpha_2\\\alpha_3\\ \end{bmatrix} = \begin{bmatrix} e_1^Te_1'&e_1^Te_2' &e_1^Te_3' \\ e_21^Te_2'&e_2^Te_2' &e_1^Te_3'\\ e_3^Te_3'&e_3^Te_2' &e_3^Te_3'\\ \end{bmatrix} \begin{bmatrix} \alpha_1' \\ \alpha_2'\\\alpha_3'\\ \end{bmatrix} = Ra'$

上式中中间的矩阵定义为旋转矩阵，我们可以从定义看出，旋转矩阵为一个行列式为1的正交矩阵，即逆为自身的矩阵。

在使用旋转矩阵表达三维世界中刚体的运动方式时，我们需要对其进行估计和优化。例如在优化位姿T时，就需要构建一个残差方程，也就是估计值与观测值之间的误差，而此时需要求解残差方程J对变化矩阵T的求导，变成了矩阵求导问题，而且对于变换矩阵是并不封闭的，所谓的不封闭就是两个变换矩阵相加得到的并不是变换矩阵，而不像是实数1加上实数2得到的3仍然是一个实数。

除此之外，由上面的变换矩阵的定义我们知道，旋转矩阵本身是带有约束的矩阵，也就是旋转矩阵为一个行列式为1的正交矩阵，额外的约束会增加优化的困难，为了简化求解的方式，引入了李群。

简单的说群就是一种集合加上一种运算的代数结构。虽然旋转矩阵对于加法是不封闭的，但是对于乘法是封闭的，两个旋转矩阵相乘代表做了两次旋转。 对于旋转矩阵和变换矩阵的群定义如下：

$SO(3)=\{R \in \Bbb R^{3\times 3}|RR^T=I,det(R)=1\}$ $SE(3)=\lbrace {T =\begin{bmatrix} R&t\\ 0^T & 1 \\ \end{bmatrix} \in \Bbb R^{4\times 4} | R \in SO(3), T \in \Bbb R^3 }\rbrace$

所谓的李群是指具有连续（光滑）性质的群，S0(3)和SE(3)在实数空间上都是连续的，所以他们都是李群。到此为止李群的引入解决了变换矩阵额外约束的问题，

接下来解决矩阵的求导问题。首先对于任意矩阵R，假设R是某个刚体的旋转，它会随着时间连续变换即为时间的函数R(t)，因此： $R(t)R(t)^T = I$

对时间t进行求导，其中 $\hat{R(t)}$代表函数R(t)的导数 $\hat{R(t)}R(t)^T +R(t)\hat{R(t)}^T = 0$

通过移项可以得到：

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

显然， $\hat{R(t)}R(t)^T$是一个反对称函数。其特征是主对角线上的元素是0，关于主对角线对称的元素互为相反数。因此对于任意反对称矩阵，总能找到一个唯一与之对应的向量。

$a^∧= A=\begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix}$

因此对于反对称矩阵 $\hat{R(t)}R(t)^T$，同样也可以找到一个三维向量 $\phi(t) \in \Bbb R^3$，即： $\hat{R(t)}R(t)^T = \phi(t) ^∧$

等式两边右乘 $R(t)$，且R为正交矩阵，所以得到： $\hat{R(t)} = \phi(t) ^∧R(t)= \begin{bmatrix} 0 & - \phi_3 & \phi_2 \\ \phi_3 & 0 & - \phi_1 \\ - \phi_2 & \phi_1 & 0 \\ \end{bmatrix} R(t)$

至此我们就得到了矩阵的导数，每次对旋转矩阵求导只需要左乘 $\phi(t)$即可。李代数描述了旋转矩阵R局部的导数关系，也叫正切空间。

下面给出旋转矩阵和变换矩阵的群所对应的李代数： $so(3)=\{\phi \in \Bbb R^3,\Phi = \phi ^∧\in \Bbb R^{3\times 3}\}$ $se(3)=\lbrace {\xi =\begin{bmatrix} \rho \\ \phi \end{bmatrix} \in \Bbb R^6 , \rho \in \Bbb R^3 ,\phi \in so(3), \xi ^∧ =\begin{bmatrix} \phi^∧ & \rho \\0^T & 0 \end{bmatrix} \in \Bbb R^{4\times 4} }\rbrace$

注： $\xi$是一个六维向量（3+3），且在se(3)中符号∧表示将六维向量转成六维矩阵 ，并不是代表反对称。

因此，引入李群的目的是解决旋转矩阵本身的约束问题，简化后面的优化过程。引入李代数是为了解决旋转矩阵的求导问题，当然李群和李代数之间也存在指数映射和对数映射的相互转换。

关于Vins-Mono学习的过程中问题记录

1.关于参数 max_solver_time 最大求解时间 在VINS中默认为0.04，迭代次数最多为8次，在实际使用中该参数需要根据性能调试，确保迭代次数足够，否则很难有一个很好的优化结果！

2.在IMU参数中，噪声和随机游走会直接影响到IMU的协方差矩阵，关于IMU的噪声，在标定的情况下是静止的，但是在运动时使用，因此一般将此扩大10~15倍去使用。

3.关于IMU和Cam的时间戳同步，理想的情况下是实现时间戳的硬同步。

4.在IMU、图像、后端三个线程中，后端需要处理IMU和图像，找到合适的图像和图像之间的IMU数据。因此该三个线程共用一个 m_buf 线程互斥锁。在后端process()中通过调用wait()自动调用m_buf.lock()来加锁，然后去寻找数据。如果未得到则线程被阻塞，自动调用m_buf.unlock()释放锁，使得IMU和图像的线程可以继续向BUF中添加数据，每当添加数据后都会调用con.notify_one唤醒process()线程。

5.在readImage对图像处理中，关于直方图均衡化，可增强图像的对比度。

cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
复制代码

在VINS中默认为3.0，如果将其增大，对比度会增加，同时噪声点也会增加。 6.在VINS中的光流跟踪也是直接调用opencv中的函数，其中两个关键参数，一个为maxLevel为金字塔层数，还有一个则为flags，设置下一帧特征点的初始位置，如果为空则表示与上一帧相同。

cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
复制代码

在VINS-MONO中设置为空。

7.协方差矩阵即不确定性，随着IMU帧数的增加，噪声的不确定性也增加。在IMU和视觉之间，通过对比协方差矩阵来判断更相信谁，其两者为一个相对的关系。

---

8.对于每一帧得到的图像，将当前帧看到的特征点image放到特征点管理器f_manager中，然后进行滑窗优化。image表示当前帧跟踪到上一帧的特征点集合。放入特征点管理器中，将当前帧与地图中的其他帧建立数据关联，若为已知点，则加到共视关系中，若为新点，则新建立一个特征点。

9.在SLAM后端中，首先确定所有的约束关系，然后针对每个约束确定三个要素：误差项、优化变量、协方差。然后计算Jacobian，调用GN、LM等求解BA。

10.在更新滑动窗口的过程中，要不去掉最老帧，要不去掉次新帧。在去掉最老帧的时候，该帧所看到的路标点需要更新该路标点的第一帧，IMU预积分信息舍弃掉。而在去掉次新帧时，次新帧上一帧到下一帧的两段预积分合并。