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

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

算法数学基础

`相机模型`

视觉SLAM的数据主要来源为图像，因此必须建立从真实世界三维空间点 pw∈R3 到图像坐标点pw∈R3 的映射：f:R3→R2。相机模型就被用来描述这个映射。
LSD SLAM算法分析（一）：算法数学基础
在相机模型中针孔相机是相对简单而常用的模型。的来说就是将相机简化成小孔成像。如上图所示，相机所在位置为针孔，即相机所在空间坐标系的原点。值得注意的是相平面以针孔Oc为中心得到虚像平面Oi。简单起见一般都对虚像平面进行分析。所有一般看到的针孔映射模型都没有图片被旋转180°的情况出现。根据简单的几何推导可以得出：

p i = ⎛ ⎝ ⎜ x i y i 1 ⎞ ⎠ ⎟ = ⎛ ⎝ ⎜ a x f 00 0 a y f 0 c x c y 1 ⎞ ⎠ ⎟ p w [p w] 3 = K p w [p w] 3

其中K就是通常说的相机内参。普通相机镜头有着视场角小的特点，而相机在运动过程中有较大的图像变化，所以需要宽视场的镜头。而鱼眼镜头由于其宽视场且长景深的特点恰好符合SLAM应用的需求。所以在SLAM系统中为了寻求好的结果一般采用鱼眼镜头。而鱼眼镜头有着自己的成像模型，但一般采用畸变校正来处理鱼眼镜头成像模型，最终使用针孔成像模型进行算法过程。

`相机标定`

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

`李群与李代数`

坐标变换中由旋转矩阵R3×3和平移向量t3×1经过其次变换后组成位姿变换矩阵

T 4 \times 4 = (R 0 t 1)

为李群中的特殊欧几里得群SE(3)。一个SE(3)有其最小化表示即李代数se3，它们之间满足指数对数映射关系（SO(3)与so3是真正的矩阵指数映射，而SE(3)到se3的指数映射是人为这么叫的，其转换过程很复杂，参考[6]）。对于一个ξ∈se3和对应的T∈SE(3)，T=expse3(ξ)，ξ=logSE(3)(T)。使用李代数的一个关键原因是直接法的SLAM中一般采用迭代优化算法求出图像位姿变换，此时需要定义误差函数及寻求误差函数对位姿变换的导数。通过链式求导，导数式最后一项就是经过3D变换后的一个空间点p相对于位姿变换T的求导。因为旋转矩阵的空间对加法是不闭包的，三维空间点对位姿变换中旋转的导数没有一个良好的增量的模型去定义它，由此引入李代数。直接用摇动后的点对ξ求导非常复杂且没有实用性，但如果在T上加一个微小的se3扰动δξ，那么可以构建李代数的扰动求导模型，扰动后的点对于扰动的偏导为[2]：

\partial T p \partial δ ξ = [I 3 \times 3 0 T - (R p + t)^0]

其中a^⋅p=a×p。

`对极几何`

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

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

对极约束与本质矩阵

LSD SLAM算法分析（一）：算法数学基础
如上图所示，O1，O2为两相机的相机坐标系。I1，I2为相应的像平面。p1和p2分别为世界坐标点P在两像平面上的投影像素坐标。e1和e2被称为极点，分别为两相机在另一相机拍摄的图像上的投影坐标。l1=p1e1，l1=p2e2分别为图像上的极线。由图可以看出：P,p1,p2,e1,e2,l1,l2,O1,O2全部共面，此即为对极约束。

设两个相机到相机间的运动为R,t。这里R,t分别表示从相机1到相机2的旋转和平移。一个3D空间点P在两个相机坐标系下的坐标分别为P1,P2，注意这两个坐标都是三维向量，表示一个空间坐标。且：

P 2 = R P 1 + t

由上式P在两个像平面上投影下的坐标为p1=KP1[P1]3,p2=KP2[P2]3。那么根据对极约束，我们可以得到：

p T 2 K - T t^R K - 1 p 1 = 0 p T 2 K - T E K - 1 p 1 = 0 p T 2 F p 1 = 0

其中E=t^R为本质矩阵，F=K−Tt^RK−1加入相机内参信息为基本矩阵。其中，E,F各有所用，E单纯地包含了两相机间的位姿变换。使用F可以更便捷地在图像上求极线。其中由点在线上的条件可知Fp1为图像I2上的极线，(pT2F)T为图像I1上的极线。
对于同一对图像，其本质矩阵显然是唯一的。如果知道多对空间点在两图像上的投影点，可以通过联立方程组求解E，考虑到E只由平移和旋转组成遂由6个自由度（旋转3个，平移3个），但由于E内存在的非线性性质，一般使用八点法求解E[5]。求得E之后通过SVD分解就可以得到旋转矩阵和平移向量R,t。

沿极线搜索得深度

得到旋转矩阵R和平移向量t后，通过沿极线搜索就可以得到关键帧上某一点的深度。深度即世界点P在相机坐标系O2下的深度坐标[P2]3。
如上图所示，P在相机坐标系1和2下的坐标分别为P1和P2。在他们相平面上的投影分别为p1和p2。设深度d=[P2]3。如果在相平面I2上找到任意一点p2，那么由对极约束可以在相平面I1上找到一条极线l1=pT2F。且由针孔模型得

p 2 = d K - 1 p 2

上式中P2为反投影到3D空间中的坐标。根据空间坐标系变换：

P 1 = R - 1 (P 2 - t) = R' P 2 + t' = d R' K - 1 p 2 + t'

上式中P1为P2经过坐标系变换后在相平面I1上的坐标，记

[P 1] 1 = d [R'] T 1 p 2 + [t'] 1 [P 1] 2 = d [R'] T 2 p 2 + [t'] 2 [P 1] 3 = d [R'] T 3 p 2 + [t'] 3

式中R,t,p2已知，d,p1未知。如果通过极线搜索匹配找到了一个确定的p1，使用p1的横纵坐标均可求得深度d：

d = [t'] 1 - [P 1] 1 [P 1] 3 [t'] 3 [P 1] 1 [P 1] 3 [K - 1 p 2] 3 - [K - 1 p 2] 1 [t'] 2 - [P 1] 2 [P 1] 3 [t'] 3 [P 1] 2 [P 1] 3 [K - 1 p 2] 3 - [K - 1 p 2] 2

上式中[P1]1[P1]3=[K−1p1]1，[P1]2[P1]3=[K−1p1]2可通过匹配到的点p1求得。由于SLAM领域经过长期实践发现取深度倒数在计算机中表示并对其进行估计有着更高的健壮性[7]，所以如今的SLAM系统一般使用反深度对三维世界进行表示。

`优化算法`

直接法的SLAM中一般采用迭代优化算法求出图像位姿变换，此时需要定义误差函数及寻求误差函数对位姿变换的导数。而求解这个问题的方法就是高斯牛顿迭代法的各种变种。本文先只简单总结具体使用方法（而非推导）。

高斯牛顿法

高斯牛顿法解决了这样一个问题：对于一个函数模型及一系列观测数据：

y = f (x, β) β = (β 1, β 2, . . ., β n) (x 1, y 1), (x 2, y 2), . . . (x m, y m)

上式描述了一个待优化的问题，此优化问题的目的是去求一个最优的β∗使得模型最贴合观测数据。高斯牛顿法要求给定一组好的参数估计初值β0，将其带入模型求得估计的残差：

Δ y = ⎛ ⎝ ⎜ ⎜ ⎜ ⎜ y 1 - f (x 1, β 0) y 2 - f (x 2, β 0) . . . y m - f (x m, β 0) ⎞ ⎠ ⎟ ⎟ ⎟ ⎟

上式几位求得的估计参数带入模型后的估计值与真实观察值的差，称为残差。同时求得模型f对参数β0的雅可比：

J = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ \partial f (x 1, β 0) \partial [β 0] 1 \partial f (x 1, β 0) \partial [β 0] 2 . . . \partial f (x 1, β 0) \partial [β 0] n \partial f (x 2, β 0) \partial [β 0] 1 \partial f (x 2, β 0) \partial [β 0] 2 . . . \partial f (x 2, β 0) \partial [β 0] n . . . . . . . . . . . . \partial f (x m, β 0) \partial [β 0] 1 \partial f (x m, β 0) \partial [β 0] 2 . . . \partial f (x m, β 0) \partial [β 0] n ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

上式中J即为模型f对参数β0的雅可比。那么高斯牛顿法会给出β0距离最符合观测值的参数β∗的偏移量Δβ，由下式给出：

(J T J) Δ β = J T Δ y

上式中Δβ为偏移量。此时通过Δβ与β0可以通过迭代的方法求出β1：

β k + 1 = β k + Δ β

上式中给出了迭代方法，以此类推直到βk收敛，即可得到最优的β∗。

列文伯格—马夸尔特法（LM法）

高斯牛顿法的内在原理是对模型y=f(x,β)在β附近进行泰勒展开并取一阶近似。通过逐步迭代的方式求解一个无约束非线性优化模型：

f (x, β + Δ β) \approx f (x, β) + J Δ β Δ β * = a r g min Δ β \sum i = 1 m ∣ ∣ ∣ ∣ f' (x, β) + J Δ β 2 ∣ ∣ ∣ ∣

为表达简洁，上式中f′(x,β)=f(x,β)−yi。
而实际的SLAM系统是强非线性的，过大的Δβ会使得近似误差不可忽略。为此此列文伯格—马夸尔特算法（LM算法）[8]提出将优化半径限制在一个范围内。当泰勒近似好时扩大优化半径，反之缩小半径。泰勒近似的好坏程度使用下式来定义：

ρ = f (x, β + Δ β) - f (x, β) J Δ β

式中ρ越接近1说明泰勒近似效果越好。而对优化半径使用||DΔβ||2来表示，其中D是一个系数矩阵，当D=I时，优化半径表示为一个球，否则表示一个椭球形的优化半径，这对β各维有不同实际含义的情况有更大的扩展性。
规定优化半径μ于是原先的无约束优化问题转变为带约束优化问题：

Δ β * = a r g min Δ β \sum i = 1 m ∣ ∣ ∣ ∣ f' (x, β) + J Δ β 2 ∣ ∣ ∣ ∣ s . t . | | D Δ β | | 2 \leq μ

通过构造拉格朗日乘子可以将上式中的问题再次转换为无约束优化问题从而实现迭代求解最优值β∗。

`贝叶斯滤波`

贝叶斯滤波在SLAM中一般被用来融合先验知识和当前的观测。对于一个需要估计的k时刻的系统状态xk，如果已知该时刻的观测值yk和一个经验值xˇ。
根据贝叶斯公式可得：

p (x k | y k) = p (y k | x k) p (k ˇ) p y k = η p (y k | x k) p (x ˇ k)

其中yk已知，且p(yk)与系统参数xk无关，所以被当成一个系数提出。p(yk|xk)就是当前的观测概率分布，实际情况中，此项可以通过传感器模型得到。p(xˇk)为先验状态分布，可使用观测概率分布去更新此值。

[1] Zhang Z. A Flexible New Technique for Camera Calibration[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 2000, 22(11):1330-1334
[2] Hall B. Matrix Lie Groups[J]. 2015.
[3] Hartley R, Zisserman A. Multiple View Geometry in Computer Vision[M]. Cambridge University Press, 2003,239-240.
[4] Hartley R, Zisserman A. Multiple View Geometry in Computer Vision[M]. Cambridge University Press, 2003,239-240, 257-260.
[5] Hartley R I. In defense of the eight-point algorithm[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 1997, 19(6):580-593.
[6] http://pan.baidu.com/s/1qY4ZKwk 《视觉SLAM中的李群基础-王京》
[7] ivera J, Davison A J, Montiel J. Inverse Depth Parametrization for Monocular SLAM[J]. IEEE Transactions on Robotics, 2008, 24(5):932-945.
[8] Moré J J. The Levenberg-Marquardt algorithm: Implementation and theory[J]. Lecture Notes in Mathematics, 1978, 630:105-116