---

声明：本文是深蓝学院 高翔博士主讲的《SLAM理论与实践》的学习笔记。

针孔相机模型与图像

照片记录了真实世界在成像平面上的投影。但这个过程丢弃了“距离”维度上的信息。普通相机可以用针孔模型很好地近似。

单目

相机坐标系内的小孔成像模型:

原始形式：

成像是倒立的一般相机会帮我们做翻转，这等价于　把成像平面从-f移动到f。得到更好的数学关系：

整理之后得到：

成像平面到像素坐标:（１．缩放到像素平面上　２ 由中心为原点的坐标转换为左上角为原点的坐标）

带入得到：

用fx,fy,cx,cy$f_x,f_y,c_x,c_y$描述投影过程，它们称为相机内参。

矩阵形式(像素坐标为齐次坐标)：

传统习惯（像素坐标为非齐次坐标）：

中间矩阵Ｋ称为内参数,内参通常在相机生产之后就已固定

丢失深度信息：同一直线上的投影点仍是同一个，k[X;Y;Z]=[kX,kY,kZ]⇒kXkZ=XZ,kYkZ=YZ$k[X;Y;Z]=[kX,kY,kZ]\Rightarrow \frac{kX}{kZ}=\frac{X}{Z},\frac{kY}{kZ}=\frac{Y}{Z}$

相机坐标系与世界坐标系

先把P从世界坐标变到相机坐标系,再映射到像素坐标:

这里 R, t 或 T 称为外参.外参是SLAM估计的目标.

上面的公式就是观察方程g:

(注：右侧式子隐含了一次非齐次到齐次的变换,见书).

投影顺序：世界(Pw$P_w$)——相机(TP_w)——归一化平面(1ZTPw$\frac{1}{Z}T{P}_w$)——像素(K1ZTPw$K\frac{1}{Z}T{P}_w$)

针孔前的镜头会引入畸变.

双目

双目模型:

• 左右相机中心距离称为基线b
• 左右像素根据相似三角形存在几何关系:
• 整理得: d称为视差（disparity），描述同一个点在左右目上成像的距离
  d最小为1个像素，因此双目能测量的z有最大值：fb
  虽然距离公式简单，但d不容易计算

RGB-D

RGB-D相机：物理手段测量深度

• ToF或结构光两种主要原理
• 通常能得到与RGB图对应的深度图

相机成像后，生成了图像
图像在计算机中以矩阵形式存储（二维数组）
需要对感光度量化成数值，例如0~255之间的整数（彩色图像还有通道）

实践：OpenCV/RGBD图像拼接

批量状态估计问题

SLAM的目的就是根据观测数据z和输入量u(例如IMU)来估计状态变量: x(就是定位,相机的外参), y(就是建图, 地图的观测点)

处理方法有很多种:

批量式（batch）

• 一次给定所有的数据，估计所有的变量
• 状态变量：
• 先不考虑运动方程，仅看观测方程（类似于SfM) ,根据贝叶斯法则有

与之对应还有 增量式(incremental)

P(x|z)条件分布很难求解，但可以求：

• 最大后验估计（Maximize a Posterior，MAP）
• 最大似然估计（Maximize Likelihood Estimation, MLE）

“在哪种状态下，最容易产生当前的观测”

从最大似然到最小二乘:

现在要求x,y的最大似然估计，怎么求？

一般的高斯分布:

负对数形式为:

最大化原问题相当于最小化它的负对数(最小化x时，只和最后一项有关)

因此,最大化似然
等价于最小化它的负对数(这货就是所谓的最小二乘):

我们把状态最大似然估计变成了最小二乘问题.

注意以上没有考虑运动方程.如果单独考虑运动方程则有P(xk|xk−1,uk)=N(f(xk−1,uk),Rk)$P(x_k|x_{k-1},u_k)=N(f(x_{k-1},u_k),R_k)$

对于原问题(同时考虑运动方程和观测方程)：
定义误差:

根据前面的推导可知,需要最小化误差的二范数：
其中Q,R是信息矩阵用来调节权重.

直观解释：
由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程中时，它们并不会完美的成立。此时就调整状态的估计，使得误差最小化

该问题有何结构？
由许多个误差的平方和（或Sigma范数和）组成。
虽然总体维度高，但每个项很简单，只关联2个变量。
如果用李代数表达位姿，那么是无约束优化问题。

如何求解？
下面先来介绍通用的非线性最小二乘问题。

非线性最小二乘法

先考虑简单的问题：
其中x∈Rn$x\in R^n$ f为任意函数

当 f 很简单时，可以对f进行求导(dfdx=0$\frac{df}{dx}=0$) 将得到极值点或鞍点，比较这些解即可

当f复杂时：df/dx$df/dx$难求或df/dx=0$df/dx=0$很难求解。可以实用迭代方式求解。

问题：如何确定这个增量？
答：确定增量的方法（即梯度下降策略）：一阶的或二阶的

泰勒展开：

若只保留一阶梯度（称为最速下降法（Steepest Method））：

若保留二阶梯度（称为牛顿法）：
令上式关于Δx$\mathrm{\Delta }x$的导数为零可得到 HΔx=−JT$H\mathrm{\Delta }x=-J^T$

最速下降法和牛顿法虽然直观，但实用当中存在一些缺点

• 最速下降法会碰到zigzag问题（过于贪婪 总是走曲线 导致迭代次数过多）
• 牛顿法迭代次数少，但需要计算复杂的Hessian矩阵

能否回避Hessian的计算 的同时减少迭代次数？ (记住目的是求解增量Δx$\mathrm{\Delta }x$)

• Gauss-Newton （属于线搜索方法：先找到方向，再确定长度）： 一阶近似 f(x) 平方误差变为： 令关于Δx$\mathrm{\Delta }x$导数为零： G-N用J的表达式近似了H
  Gauss-Newton简单实用但 Δxk=H−1g$\mathrm{\Delta }{x}_k=H^{-1}g$无法保证H可逆。Levenberg-Marquadt 方法一定程度上改善了它

• Levenberg-Marquadt（L-M属于信赖区域方法Trust Region，认为近似只在区域内可靠 会判断近似的好坏）：考虑近似程度的描述（实际下降值 比上 理论下降值）
  若太小，则减小近似范围，若太大，则增加近似范围
  它可以看作改进版本的G-N。Trust Region内的优化，利用Lagrange乘子转化为无约束 仍参照G-N展开，可得到增量方程为 在Levenberg方法中，取D=I，则 LM相比于GN，能够保证增量方程的正定性，即认为近似只在一定范围内成立，如果近似不好则缩小范围。从增量方程上来看，可以看成一阶和二阶的混合，参数λ$\lambda$控制着两边的权重

小结

• 非线性优化是个很大的主题，研究者们为之奋斗多年,主要方法：最速下降、牛顿、G-N、L-M、DogLeg等
• 与线性规划不同，非线性需要针对具体问题具体分析
• 问题非凸时，对初值敏感，会陷入局部最优
  
  • 目前没有非凸问题的通用最优值的寻找办法
  • 问题凸时，二阶方法通常一两步就能收敛