简单的V-SLAM介绍
【导读】slam是“simultaneous localization and mapping”的缩写，可译为同步定位与建图。最早，slam 主要用在机器人领域，是为了在没有任何先验知识的情况下，根据传感器数据实时构建周围环境地图，同时根据这个地图推测自身的定位。因此本文以简单清晰的文字为大家介绍了视觉v-slam。
简单的v-slam介绍，就当入门：）。
首先，从双目立体几何（stereo geometry）原理开始来定义外极（epipolar）约束：两个摄像头光心分别是 c0 和 c1，3-d 空间点 p 在两个图像平面的投影点分别是 x0 和 x1，那么直线 c0c1 和两个图像平面的交点即外极点（epipole）e0 和 e1，pc0c1 平面称为外极平面（epipolar plane），它和两个图像平面的交线 l0 和 l1 即外极线（epipolar line）；可以看到两个摄像头坐标系之间的转换满足（r，t），同时说明摄像头 c0 的图像点 x0 在摄像头 c1 的图像对应点 x1 一定落在其外极线 l1，反之依然；
（a）对应一条光线的外极线 （b）对应的外极线集合和外极平面 外极线约束
这里需要定义一个本质矩阵（essential matrix）e 和一个基础矩阵（fundamental matrix）f：e = [t]×r，其中 [t]× 是反对称矩阵，即定义向量
那么
而 f = a−tea−1，其中 a 是摄像头内参数矩阵；对矩阵 e 和矩阵 f 来说，均满足外极约束方程x1tex0 =0，x1tfx0 =0
前者是摄像头已标定情况下图像特征点的对应关系，后者是摄像头未标定情况下图像特征点的对应关系；
其次，得到外极线约束（以f矩阵为例，e矩阵同样）如下
l1=fx0，l0=x1f，
以及，外极点约束（以f矩阵为例，e矩阵同样）如下
fe0=0，fte1=0；
根据上面图像特征点的外极约束方程，有8 点算法求解 f 和 e，以 f 为例，给定一组匹配特征点（n> 7）
定义f矩阵元素fij（i=1～3，j=1～3），那么线性齐次方程组为
简记为
qf = 0
做svd得到q = usvt，而最终f的解是v最右的奇异向量（singular vector）；
另外，因为e矩阵完全由摄像头外参数（r和t共6个未知参数，但有一个不确定平移尺度量）决定，所以存在5 点算法求解e矩阵；
可以分解e得到摄像头外参数，其步骤如下：
i. 同样对e矩阵做奇异值分解（svd）：
ii. 那么第一个摄像头投影矩阵简单设为
而第二个摄像头矩阵p2有四种可能情况，如图（a）-（d）所示：
其中
从e矩阵分解得到r和t
根据视图方向与摄像头中心到3-d点的方向之间夹角可以发现，四个可能中只有情况（a）是合理的解；
确定两个视角的姿态之后，匹配的特征点 x，x’可以重建其 3-d 坐标x，即三角化（triangulation）理论；首先存在一个线性解：设两个摄像头投影矩阵为 p 和 p’，相应的它们列向量为pi，pi’，i=1～3，则有方程组：ax= 0
其中
但一些误差干扰的存在，上述线性解是不存在的；所以需要一个非线性的解，这里采用f 矩阵定义的外极约束方程xtfx’= 0，得到最小化误差函数为
下面采用外极线 l, l’的来约束定义误差，如图所示，将目标函数重写为
外极线定义的误差
另外，在已知重建的 3-d 点集，如何和新视角的 2-d 图像特征点匹配呢？这个问题解法称为pnp（perspective n points），算法如下：
i. 首先，3d 点表示为 4 个控制点的加权和；
ii. 控制点坐标是求解的（12）未知数；
iii. 3d点投影到图像上以控制点坐标建立线性方程；
iv. 控制点坐标表达为零特征向量（null eigenvectors）线性组合；
v. 上面组合的权重（bi）是新未知数（<4）；
vi. 增加刚体（rigidity）约束以得到bi二次方程；
vii. 根据bi数目（无论线性化，或重新线性化）求解。
（注：有时候3-d-2-d匹配比3-d之间匹配的精度高）
pnp求解的示意图
这里需要补充两个概念，一是鲁棒估计的随机样本共识法（ransac，random sample consensus），另一个是全局优化的集束修正法（ba，bundle adjustment）：
i. ransac的目的是在包含异常点（outlier）的数据集上鲁棒地拟合一个模型，如图 2-12 所示：
1. 随机选择（最小）数据点子集并实例化（instantiate）模型;
2. 基于此模型，将所有数据点分类为内点（inlier）或异常点；
3. 迭代重复 1-2 步；
4. 选择最大的内地集，以此重新估计最终模型。
ransac示意图
ii. ba的目的是优化全局参数估计，对 slam 来说，结构重建的 3-d 点x和其对应的 2-d 特征点x，还有估计的视角变换参数（甚至包括摄像头内参数）p，位于一个重投影（reprojection）误差函数 d 最小化框架下（如图所示），即
ba示意图
这里集束（bundle）指2-d点和3-d点之间的光线集，而修正（adjustment）是指全局优化过程；其解法是非线性迭代的梯度下降法，如gauss-newton 方法和其修正 levenberg-marquardt 方法，因为问题自身的特性，这里的雅可比矩阵是非常稀疏的；另外，只取重建 3-d 点修正的话，称为 structure only ba，而只取视角变换修正的话，称为 motion-only ba；
在以上基础之上，再重新审视slam过程：
i. 首先要提到概念 vo，即视觉里程计（visual odometry）；vo 是 slam 的一部分，vo 主要是估计视角之间的变换，或者运动参数，它不需要输出制图（mapping）的结果，而且 ba 就是 motion-only 的模式；
ii. slam 方法分两种途径，一种是递推滤波器方法，另一种是批处理估计方法；
a)滤波器方法，比如卡尔曼滤波递推估计，实际上建立一个状态空间的观测模型和状态转换（运动）模型；观察模型描述当摄像头姿态和地标位置（来自于地图）已知时观测（地标）的概率; 运动模型是系统状态（摄像头姿态）转换的概率分布，即马尔可夫过程; 那么在递归贝叶斯估计中，同时更新系统状态和建立的地图，其中融合来自不同视角的观测来完成制图，而估计系统状态可计算摄像头的姿态，即定位问题;
b)批处理估计方法，也称“关键帧”方法，其步骤是：
1）首先通过选择的头两（关键）帧，采用双目几何的特征点匹配得到初始的3-d点云重建；
2）正常模式：假设 3d 地图可用，并且估计出摄像头增量运动，跟踪特征点并使用 pnp 进行摄像头姿态估计；
3）恢复模式：假设 3d 地图可用，但跟踪失败故没有增量运动，可相对于先前重建的地图重新定位（re-localize）摄像头姿势；
4）关键帧 ba：保持一个“关键帧”子集，状态向量是所有关键帧的 3d 地标和对应摄像头姿势，ba 可以在与跟踪模块并列的线程中调整状态估计；
（注意：关键帧的选择策略是算法性能很重要的一个因素）
slam 中的闭环（loop closure）问题：当摄像头又回到原来位置，称为闭环，需要检测闭环，并以此在 ba 框架下优化结构和运动估计；闭环检测和重定位是类似的，可以基于图像特征的匹配实现，俗称“基于图像的重定位（image-based re-localization），当关键帧子集较大的时候，需要对特征匹配进行压缩和加速，比如词包（bag of words）法和k维-树（kd-tree）数据结构等等；
slam 中的传感器可以是单目、双目、深度传感器（rgb-d）甚至激光雷达，也可以和 imu 融合，称为 vins（visual inertial navigation system）。
附录：g-n 和 l-m 的非线性最小二乘算法
假设有观测向量 zi’，其预测模型为 zi = zi(x)，其中x为模型参数；那么最小二乘（ls）法就是最小化如下代价函数：平方误差加权和（weighted sum of squared error，sse)
其中 wi 是一个任意对称正定（symmetric positive definite，spd) 矩阵，特征误差函数为
雅可比矩阵j和黑森（hessian）矩阵h的计算为
那么 h 近似为
h≈j⊤wj
作为梯度下降法，其 g-n 迭代的步进量即 z → z + delta z，由下面方程组计算
对于上面 g-n 的迭代步进量计算，可能左边的矩阵不可逆。为此，一种改进的方法是在对角线元素中加入阻尼因子（damped factor），即 l-m 迭代的步进量计算变成如下方程组
数字滤波器的MATLAB与DSP上设计实现
将string转换成int/long的步骤分享
STN-LCD彩屏模块功能分析
红魔5G电竞手机曝光将会搭载骁龙865旗舰平台和三摄像头
关于新能源汽车充电桩市场新发展
简单的V-SLAM介绍
灵活的单四极杆 ICP 质谱仪安捷伦7900介绍
边缘AI的“灵魂”，能否跟上Ta飞快脚步
环网柜是一种髙压开关设备，它的优点有哪些
小米和华为同时发布新机，你更期待哪一款？
智能电瓶车云平台的应用措施及优势
EA的PSB 10000 30kW电池测试系统
阿里全面应用IPv6 淘宝优酷高德已接入 阿里云完成IPv6改造
虹科新闻 | ATTO CEO官宣和虹科建立合作伙伴关系！！！
基于LTC6804的锂离子电池阻抗测量系统设计
一种3D打印支架集成的“三明治”结构微流控装置
研华物联网设备运营管理应用程序，可远程发送软件和固件更新
测量CAN总线网络阻抗的原理及方法
OPPOR17什么时候上市？OPPOR17卖多少钱？oppor17发布时间就在今晚
智能锁B端火热C端局面冷清 巨头或将鏖战千元智能锁
简单的v-slam介绍，就当入门：）。
首先，从双目立体几何（stereo geometry）原理开始来定义外极（epipolar）约束：两个摄像头光心分别是 c0 和 c1，3-d 空间点 p 在两个图像平面的投影点分别是 x0 和 x1，那么直线 c0c1 和两个图像平面的交点即外极点（epipole）e0 和 e1，pc0c1 平面称为外极平面（epipolar plane），它和两个图像平面的交线 l0 和 l1 即外极线（epipolar line）；可以看到两个摄像头坐标系之间的转换满足（r，t），同时说明摄像头 c0 的图像点 x0 在摄像头 c1 的图像对应点 x1 一定落在其外极线 l1，反之依然；
（a）对应一条光线的外极线 （b）对应的外极线集合和外极平面 外极线约束
这里需要定义一个本质矩阵（essential matrix）e 和一个基础矩阵（fundamental matrix）f：e = [t]×r，其中 [t]× 是反对称矩阵，即定义向量
那么
而 f = a−tea−1，其中 a 是摄像头内参数矩阵；对矩阵 e 和矩阵 f 来说，均满足外极约束方程x1tex0 =0，x1tfx0 =0
前者是摄像头已标定情况下图像特征点的对应关系，后者是摄像头未标定情况下图像特征点的对应关系；
其次，得到外极线约束（以f矩阵为例，e矩阵同样）如下
l1=fx0，l0=x1f，
以及，外极点约束（以f矩阵为例，e矩阵同样）如下
fe0=0，fte1=0；
根据上面图像特征点的外极约束方程，有8 点算法求解 f 和 e，以 f 为例，给定一组匹配特征点（n> 7）
定义f矩阵元素fij（i=1～3，j=1～3），那么线性齐次方程组为
简记为
qf = 0
做svd得到q = usvt，而最终f的解是v最右的奇异向量（singular vector）；
另外，因为e矩阵完全由摄像头外参数（r和t共6个未知参数，但有一个不确定平移尺度量）决定，所以存在5 点算法求解e矩阵；
可以分解e得到摄像头外参数，其步骤如下：
i. 同样对e矩阵做奇异值分解（svd）：
ii. 那么第一个摄像头投影矩阵简单设为
而第二个摄像头矩阵p2有四种可能情况，如图（a）-（d）所示：
其中
从e矩阵分解得到r和t
根据视图方向与摄像头中心到3-d点的方向之间夹角可以发现，四个可能中只有情况（a）是合理的解；
确定两个视角的姿态之后，匹配的特征点 x，x’可以重建其 3-d 坐标x，即三角化（triangulation）理论；首先存在一个线性解：设两个摄像头投影矩阵为 p 和 p’，相应的它们列向量为pi，pi’，i=1～3，则有方程组：ax= 0
其中
但一些误差干扰的存在，上述线性解是不存在的；所以需要一个非线性的解，这里采用f 矩阵定义的外极约束方程xtfx’= 0，得到最小化误差函数为
下面采用外极线 l, l’的来约束定义误差，如图所示，将目标函数重写为
外极线定义的误差
另外，在已知重建的 3-d 点集，如何和新视角的 2-d 图像特征点匹配呢？这个问题解法称为pnp（perspective n points），算法如下：
i. 首先，3d 点表示为 4 个控制点的加权和；
ii. 控制点坐标是求解的（12）未知数；
iii. 3d点投影到图像上以控制点坐标建立线性方程；
iv. 控制点坐标表达为零特征向量（null eigenvectors）线性组合；
v. 上面组合的权重（bi）是新未知数（<4）；
vi. 增加刚体（rigidity）约束以得到bi二次方程；
vii. 根据bi数目（无论线性化，或重新线性化）求解。
（注：有时候3-d-2-d匹配比3-d之间匹配的精度高）
pnp求解的示意图
这里需要补充两个概念，一是鲁棒估计的随机样本共识法（ransac，random sample consensus），另一个是全局优化的集束修正法（ba，bundle adjustment）：
i. ransac的目的是在包含异常点（outlier）的数据集上鲁棒地拟合一个模型，如图 2-12 所示：
1. 随机选择（最小）数据点子集并实例化（instantiate）模型;
2. 基于此模型，将所有数据点分类为内点（inlier）或异常点；
3. 迭代重复 1-2 步；
4. 选择最大的内地集，以此重新估计最终模型。
ransac示意图
ii. ba的目的是优化全局参数估计，对 slam 来说，结构重建的 3-d 点x和其对应的 2-d 特征点x，还有估计的视角变换参数（甚至包括摄像头内参数）p，位于一个重投影（reprojection）误差函数 d 最小化框架下（如图所示），即
ba示意图
这里集束（bundle）指2-d点和3-d点之间的光线集，而修正（adjustment）是指全局优化过程；其解法是非线性迭代的梯度下降法，如gauss-newton 方法和其修正 levenberg-marquardt 方法，因为问题自身的特性，这里的雅可比矩阵是非常稀疏的；另外，只取重建 3-d 点修正的话，称为 structure only ba，而只取视角变换修正的话，称为 motion-only ba；
在以上基础之上，再重新审视slam过程：
i. 首先要提到概念 vo，即视觉里程计（visual odometry）；vo 是 slam 的一部分，vo 主要是估计视角之间的变换，或者运动参数，它不需要输出制图（mapping）的结果，而且 ba 就是 motion-only 的模式；
ii. slam 方法分两种途径，一种是递推滤波器方法，另一种是批处理估计方法；
a)滤波器方法，比如卡尔曼滤波递推估计，实际上建立一个状态空间的观测模型和状态转换（运动）模型；观察模型描述当摄像头姿态和地标位置（来自于地图）已知时观测（地标）的概率; 运动模型是系统状态（摄像头姿态）转换的概率分布，即马尔可夫过程; 那么在递归贝叶斯估计中，同时更新系统状态和建立的地图，其中融合来自不同视角的观测来完成制图，而估计系统状态可计算摄像头的姿态，即定位问题;
b)批处理估计方法，也称“关键帧”方法，其步骤是：
1）首先通过选择的头两（关键）帧，采用双目几何的特征点匹配得到初始的3-d点云重建；
2）正常模式：假设 3d 地图可用，并且估计出摄像头增量运动，跟踪特征点并使用 pnp 进行摄像头姿态估计；
3）恢复模式：假设 3d 地图可用，但跟踪失败故没有增量运动，可相对于先前重建的地图重新定位（re-localize）摄像头姿势；
4）关键帧 ba：保持一个“关键帧”子集，状态向量是所有关键帧的 3d 地标和对应摄像头姿势，ba 可以在与跟踪模块并列的线程中调整状态估计；
（注意：关键帧的选择策略是算法性能很重要的一个因素）
slam 中的闭环（loop closure）问题：当摄像头又回到原来位置，称为闭环，需要检测闭环，并以此在 ba 框架下优化结构和运动估计；闭环检测和重定位是类似的，可以基于图像特征的匹配实现，俗称“基于图像的重定位（image-based re-localization），当关键帧子集较大的时候，需要对特征匹配进行压缩和加速，比如词包（bag of words）法和k维-树（kd-tree）数据结构等等；
slam 中的传感器可以是单目、双目、深度传感器（rgb-d）甚至激光雷达，也可以和 imu 融合，称为 vins（visual inertial navigation system）。
附录：g-n 和 l-m 的非线性最小二乘算法
假设有观测向量 zi’，其预测模型为 zi = zi(x)，其中x为模型参数；那么最小二乘（ls）法就是最小化如下代价函数：平方误差加权和（weighted sum of squared error，sse)
其中 wi 是一个任意对称正定（symmetric positive definite，spd) 矩阵，特征误差函数为
雅可比矩阵j和黑森（hessian）矩阵h的计算为
那么 h 近似为
h≈j⊤wj
作为梯度下降法，其 g-n 迭代的步进量即 z → z + delta z，由下面方程组计算
对于上面 g-n 的迭代步进量计算，可能左边的矩阵不可逆。为此，一种改进的方法是在对角线元素中加入阻尼因子（damped factor），即 l-m 迭代的步进量计算变成如下方程组
数字滤波器的MATLAB与DSP上设计实现
将string转换成int/long的步骤分享
STN-LCD彩屏模块功能分析
红魔5G电竞手机曝光将会搭载骁龙865旗舰平台和三摄像头
关于新能源汽车充电桩市场新发展
简单的V-SLAM介绍
灵活的单四极杆 ICP 质谱仪安捷伦7900介绍
边缘AI的“灵魂”，能否跟上Ta飞快脚步
环网柜是一种髙压开关设备，它的优点有哪些
小米和华为同时发布新机，你更期待哪一款？
智能电瓶车云平台的应用措施及优势
EA的PSB 10000 30kW电池测试系统
阿里全面应用IPv6 淘宝优酷高德已接入 阿里云完成IPv6改造
虹科新闻 | ATTO CEO官宣和虹科建立合作伙伴关系！！！
基于LTC6804的锂离子电池阻抗测量系统设计
一种3D打印支架集成的“三明治”结构微流控装置
研华物联网设备运营管理应用程序，可远程发送软件和固件更新
测量CAN总线网络阻抗的原理及方法
OPPOR17什么时候上市？OPPOR17卖多少钱？oppor17发布时间就在今晚
智能锁B端火热C端局面冷清 巨头或将鏖战千元智能锁