slam十四讲第二版 pdf_SLAM十四讲重要公式定理整理（上）

SLAM十四讲重要公式定理整理（上）

本文整理高翔张涛等著“视觉SLAM十四讲（第一版）”中出现的重要公式与定理推导，按照十四讲中的章节顺序进行整理，选取的公式内容有作者强烈的主观倾向，多次出现的公式分配至第一次详细解释处（例如SO(3)放入ch4李群部分）,并且不包含单目相关内容。此为上篇，包含ch2~ch7的内容。

• CH2 初识SLAM
  • ch2-1 slam问题基本方程
• CH3 三维空间刚体运动
  • ch3-1 向量外积转化矩阵乘法
  • ch3-2 齐次坐标描述的变换矩阵
  • ch3-3 罗德里格斯公式及推导
  • ch3-4 旋转的四元数表示
  • ch3-5 四元数和旋转矩阵的相互转化
• CH4 李群与李代数
  • ch4-1 SO(3)与SE(3)
  • ch4-2 李代数的引出
  • ch4-3 李代数so(3)和se(3)定义
  • ch4-4 SO(3)上得指数与对数映射
  • ch4-4 SE(3)上得指数与对数映射
  • ch4-5 BCH公式线性近似表达
  • ch4-6 李代数求导
• CH5 相机与图像
  • ch5-1 针孔相机模型
  • ch5-2 畸变纠正
  • ch5-3 双目相机模型
• CH6 非线性优化
  • ch6-1 最大后验与最大似然
  • 状态估计问题与最小二乘的转化
  • ch6-3 非线性最小二乘一阶和二阶梯度法
  • ch6-4 Gauss-Newton
  • ch6-5 Levenberg-Marquadt
• CH7视觉里程计1
  • ch7-1 对极约束
  • ch7-2 本质矩阵求解（八点法）
  • ch7-3 单应矩阵(Homography)定义及求解
  • ch7-4 三角测量
  • ch7-5 3D-2D: PnP 直接线性变换求解
  • ch7-6 P3P
  • ch7-7 Bundle Adjustment
  • ch7-8 3D-3D ICP(Iterative Closest Point)

CH2 初识SLAM

ch2-1 slam问题基本方程

说明：当我们知道运动测量的读数u，以及传感器的读数z时，如何求解定位问题（估计x）和建图问题（估计y）。

CH3 三维空间刚体运动

ch3-1 向量外积转化矩阵乘法

ch3-2 齐次坐标描述的变换矩阵

ch3-3 罗德里格斯公式及推导

Rodrigues's Formula:

推导：

ch3-4 旋转的四元数表示

给定转轴（单位向量）及旋转角度，求旋转四元数表示：

从单位四元数中计算出对应的旋转轴与夹角：

用q表示这个旋转：

那么旋转后的点p'即可表示为这样的乘积：

ch3-5 四元数和旋转矩阵的相互转化

设四元数为：

对应的旋转矩阵为：

反之，假设矩阵为

对应的四元数q由下式给出：

CH4 李群与李代数

ch4-1 SO(3)与SE(3)

ch4-2 李代数的引出

定义反对称矩阵

（R是正交阵） 把R(t)在0附近一阶泰勒展开：

上式为关于R的微分方程，初值R(0)=I，解得：

ch4-3 李代数so(3)和se(3)定义

ch4-4 SO(3)上得指数与对数映射

• 指数映射 定义

并运用性质：

可以把指数映射写成：

得到：

• 对数映射

ch4-4 SE(3)上得指数与对数映射

• 指数映射

ch4-5 BCH公式线性近似表达

• SO(3)

左乘：

右乘：

李群左乘微小旋转，转化李代数：

李代数上加法，近似为李群上带左右雅可比乘法：

• SE(3)

ch4-6 李代数求导

• SO(3) 直接求导法

扰动模型（左乘）

• SE(3)上李代数求导(p为齐次坐标)

CH5 相机与图像

ch5-1 针孔相机模型

K为相机内参数矩阵(Camera Intrinsics)，P为相机坐标系下的坐标

归一化处理：

ch5-2 畸变纠正

• 径向畸变（透镜自身形状影响，图像边缘失真）

• 切向畸变（透镜和成像平面不平行）

• 二者结合

纠正后通过内参数矩阵投影到像素平面：

ch5-3 双目相机模型

根据几何模型的相似关系：

整理得

d为视差Disparity

CH6 非线性优化

ch6-1 最大后验与最大似然

根据贝叶斯法则，有：

左侧P(x|z)为后验概率，右侧P(z|x)为似然，P(x)为先验 后验概率最大化(Maximize a Posterior, MAP)：

无先验，求解最大似然估计(Maximize Likelihood Estimation, MLE):

状态估计问题与最小二乘的转化

噪声项服从高斯分布

对右侧的高斯分布，采用最小化负对数的方法求解最大似然。 对于任意高斯分布

求负对数的最小值即为式中二次型的最小值，所以待求的最小似然，转化为求：

即为最小化噪声项的平方，定义数据与估计值间误差：

加上第一步的状态方程，得到总体意义下的最小二乘问题(Least Square Problem)

ch6-3 非线性最小二乘一阶和二阶梯度法

问题模型：

x附近泰勒展开

• 保留一阶梯度：

取步长λ，即为最速下降法（贪心）

• 保留二阶梯度：

等式右侧求增量倒数，并令它等于零，得到增量解：

此即牛顿法（需计算Hessian Matrix）

ch6-4 Gauss-Newton

求增量dx的导数，并令其等于0：

此为高斯牛顿方程（Gauss Newton equations）或者正规方程（Normal equations）

其中H即为牛顿法中Hessian Matrix的近似

ch6-5 Levenberg-Marquadt

思想：给dx添加一个信赖区域(Trust Region)，使其满足泰勒近似效果 评价标准：

迭代中需要求解：

使用Lagrange乘子转化为无约束优化问题：

展开后 化为计算增量的线性方程：

取D=I:

CH7视觉里程计1

ch7-1 对极约束

• 对极约束推导

使用齐次坐标：

观察到左侧等于0，于是

上述两个式子即为对极约束

E为本质矩阵(Essential Matrix)，F为基础矩阵(Fundamental Matrix)

ch7-2 本质矩阵求解（八点法）

考虑一对匹配点

接下来用SVD来分解E求解位姿

对E进行SVD后，得到奇异值矩阵

或者直接将奇异值矩阵取成diag(1,1,0)

• 当给定的点数多于八对时，计算最小二乘解，记左侧系数矩阵为A

(实际求解时更多运用随机采样一致性RANSAC求解)

ch7-3 单应矩阵(Homography)定义及求解

用处：特征点都落在同一平面上。

自由度为8的H可以通过四组匹配点求得

ch7-4 三角测量

两处观察同一个点的夹角，确定该点的距离

ch7-5 3D-2D: PnP 直接线性变换求解

定义T的行向量

每对特征点提供了两个关于t的线性约束，一共N对特征点，可以列出线性方程组：

t一共12维，于是最少六对匹配点，可以求解T

ch7-6 P3P

cos<a,b>,cos<b,c>,cos<a,c>是已知的（可以用IAC来求）

ch7-7 Bundle Adjustment

最小化重投影误差(Reprojection error)问题

重投影误差优化问题即：

• 优化位姿 下面求解误差项关于优化变量的导数

利用第三行消去s：

运用链式法则：

其中第一项，由前边相机投影关系式可得：

链式法则中第二项，可由李代数求导部分(本文ch4-6)得到：

P'为空间点的非齐次坐标，于是取出上式的前三维，得：

由此可得误差关于优化变量导数的雅可比矩阵：

• 优化特征点的空间位置

其中第一项在位姿优化中已经求得，下求链式法则中第二项

ch7-8 3D-3D ICP(Iterative Closest Point)

问题描述： 对于配对好的3D点：

找一个欧式变换R,t,使得：

• SVD方法

构建最小二乘问题：

定义两组点质心：

化简后可得：

优先优化只包含R的左边，得到R以后，令第二项为零，得到t，下求左边的优化：

定义矩阵：

对W进行SVD分解：

W满秩时，R为：

• 非线性优化方法 目标函数为：

ICP在唯一解的情况下，只要能找到极小值解，这个极小值就是全局最优值

更多精彩内容欢迎关注公众号“机器学习炼丹师”

http://weixin.qq.com/r/uTvu9kHEOxxErQWV926r (二维码自动识别)