LSD SLAM算法分析（一）：算法数学基础

本文为记录本科期间毕设论文。文中总结了LSD SLAM所用到的详细的数学基础知识。时间关系本篇文章介绍算法基础知识。预计下一篇总结算法的详细过程即代码架构。本博客提取了我毕设论文的主要知识点。错误之处敬请提出（热烈欢迎）。

---

算法数学基础

相机模型

视觉SLAM的数据主要来源为图像，因此必须建立从真实世界三维空间点 $\boldsymbol{ p}_w \in \mathbb{R}^3$ 到图像坐标点$\boldsymbol{ p}_w \in \mathbb{R}^3$ 的映射：$\boldsymbol{f}:\mathbb{R}^3\rightarrow\mathbb{R}^2$。相机模型就被用来描述这个映射。

在相机模型中针孔相机是相对简单而常用的模型。的来说就是将相机简化成小孔成像。如上图所示，相机所在位置为针孔，即相机所在空间坐标系的原点。值得注意的是相平面以针孔$\boldsymbol{O}_c$为中心得到虚像平面$\boldsymbol{O}_i$。简单起见一般都对虚像平面进行分析。所有一般看到的针孔映射模型都没有图片被旋转180°的情况出现。根据简单的几何推导可以得出：

$$\boldsymbol{p}_i= \left( \begin{array}{ccc} x_i\\y_i\\1 \end{array}\right)= \left( \begin{array}{ccc} a_xf & 0 & c_x\\ 0 & a_yf & c_y\\ 0 & 0 & 1 \end{array} \right)\frac{\boldsymbol{p}_w}{[\boldsymbol{p}_w]_3}=\frac{\boldsymbol{Kp}_w}{[\boldsymbol{p}_w]_3}$$
其中 $\boldsymbol{K}$就是通常说的相机内参。普通相机镜头有着视场角小的特点，而相机在运动过程中有较大的图像变化，所以需要宽视场的镜头。而鱼眼镜头由于其宽视场且长景深的特点恰好符合SLAM应用的需求。所以在SLAM系统中为了寻求好的结果一般采用鱼眼镜头。而鱼眼镜头有着自己的成像模型，但一般采用畸变校正来处理鱼眼镜头成像模型，最终使用针孔成像模型进行算法过程。

相机标定

正常情况下，除了专业的高精度相机，市场上销售的相机的内参矩阵 是未知的。为了进行下一步工作，通常需要一个被称为相机标定的过程。考虑到成本和精度的关系，一般使用基于二维平面的标定法进行相机标定。典型做法就是使用相机对一个固定大小的方格棋盘在不同距离，角度上进行观测，用过建模优化得到相机内参。如今有很多基于张正友标定法[1]实现的开源软件可以很快速的对相机进行精度较高的标定。使用软件时只需要将需要进行标定的相机对着指定大小的黑白棋盘网格拍摄图像作为程序的输入。软件即可给出标定结果，一般包含相机内参以及内参在给定图像上的误差。

李群与李代数

坐标变换中由旋转矩阵$\boldsymbol{R}_{3\times3}$和平移向量$\boldsymbol{t}_{3\times1}$经过其次变换后组成位姿变换矩阵

$$\boldsymbol{T}_{4\times4}= \left( \begin{array}{ccc} \boldsymbol{R} & \boldsymbol{t}\\ \boldsymbol{0} & 1 \end{array} \right)$$
为李群中的特殊欧几里得群 $SE(3)$。一个 $SE(3)$有其最小化表示即李代数 $se3$，它们之间满足指数对数映射关系（ $SO(3)$与 $so3$是真正的矩阵指数映射，而 $SE(3)$到 $se3$的指数映射是人为这么叫的，其转换过程很复杂，参考[6]）。对于一个 $\boldsymbol{\xi}\in se3$和对应的 $\boldsymbol{T}\in SE(3)$， $\boldsymbol{T}=exp_{se3}(\boldsymbol{\xi})$， $\boldsymbol{\xi}=log_{SE(3)}(\boldsymbol{T})$。使用李代数的一个关键原因是直接法的 $SLAM$中一般采用迭代优化算法求出图像位姿变换，此时需要定义误差函数及寻求误差函数对位姿变换的导数。通过链式求导，导数式最后一项就是经过3D变换后的一个空间点 $\boldsymbol{p}$相对于位姿变换 $\boldsymbol{T}$的求导。因为旋转矩阵的空间对加法是不闭包的，三维空间点对位姿变换中旋转的导数没有一个良好的增量的模型去定义它，由此引入李代数。直接用摇动后的点对 $\boldsymbol{\xi}$求导非常复杂且没有实用性，但如果在 $\boldsymbol{T}$上加一个微小的 $se3$扰动 $\delta\boldsymbol{\xi}$，那么可以构建李代数的扰动求导模型，扰动后的点对于扰动的偏导为[2]：
$$\frac{\partial \boldsymbol{Tp}}{\partial \delta \boldsymbol{\xi}} = \left[ \begin{array}{ccc} \boldsymbol{I}_{3\times3}&\hat{ -(\boldsymbol{Rp}+\boldsymbol{t})}\\ \boldsymbol{0}^T & 0 \end{array} \right]$$
其中 $\hat{\boldsymbol{a}}\cdot\boldsymbol{p}=\boldsymbol{a}\times{\boldsymbol{p}}$。

对极几何

对极几何也成为多视角几何[3]。通过在多个视角上（一般两个）对同一个物体进行观测。通过比对图像差异得到相点在真实空间中的坐标。对于同一个$3D$空间点，它和两视角下的相机共面这个特性，通过数学分析可以解决诸多问题。通常使用多视角几何解决以下两个问题

• 已知一些匹配好的点，求两相机视角的位姿变换，解本质矩阵，$SVD$分解矩阵得相机位姿变换[4]。
• 已知两视角的变换，求某相点的空间位置—沿极线搜索匹配点，反投影得空间坐标深度。

对极约束与本质矩阵

如上图所示，$\boldsymbol{O}_1$，$\boldsymbol{O}_2$为两相机的相机坐标系。$\boldsymbol{I}_1$，$\boldsymbol{I}_2$为相应的像平面。$\boldsymbol{p}_1$和$\boldsymbol{p}_2$分别为世界坐标点$\boldsymbol{P}$在两像平面上的投影像素坐标。$\boldsymbol{e}_1$和$\boldsymbol{e}_2$被称为极点，分别为两相机在另一相机拍摄的图像上的投影坐标。 l1=p1e