视觉SLAM十四讲 第五讲 相机和图像共21页
视觉SLAM：从理论到实践特征点法视觉里程计
2. 2D-2D 对极几何
• 小结 • 2D-2D情况下，只知道图像坐标之间的对应关系
• 当特征点在平面上时（例如俯视或仰视），使用H恢复R,t • 否则，使用E或F恢复R,t • t 没有尺度
• 求得R,t后：
• 利用三角化计算特征点的3D位置（即深度）
• 实际中用于单目SLAM的初始化部分
3. 3D-2D PnP
• 利用非线性优化可以将ICP与PnP结合在一起求解。
5. 三角化与深度估计
5. 三角化与深度估计
• 已知运动时，求解特征点的3D位置 • 几何关系： • 求 ������2 时，两侧乘 ������1∧
• 反之亦然
• 或者同时解 ������1, ������2 ：
•求
−������������2, ������1
• 雅可比的形式？
P’为P在相机坐标系下的坐标：
第一项：
对P’进行投影：
3. 3D-2D PnP
• 第二项： • 非齐次形式： • 两项相乘：
3. 3D-2D PnP
• 也可以对3D点求导：
1 ������������ = ������������ − ������������ ������exp
������1 ������2
= ������
的最小二乘解 ������ = ������������������ −1������������������
5. 三角化与深度估计
• 三角化中的问题：
• 解得深度的质量与平移相关
• 但是平移大时特征匹配可能不成功
• 方程
−������������2, ������1
• 最少可使用五个点计算R,t，称为五点法
视觉SLAM
用形式更简单的 E 。
三角测量确定深度
• 在得到运动之后，下一步我们需要用相机的运动估计特征点的空间位置。在单目 SLAM 中，仅通过单张图 像无法获得像素的深度信息，我们需要通过三角测量（ Triangulation ）（或三角化）的方法来估计地图点
回环检测
• 回环检测，又称闭环检测（ Loop Closure Detection ），主要解决位置 估计随时间漂移的问题。 • 前端提供特征点的提取和轨迹、地图的初值，而后端负责对这所有的数据
进行优化。然而，如果像 VO 那样仅考虑相邻时间上的关联，那么，之前
产生的误差将不可避免地累计到下一个时刻，使得整个 SLAM 会出现累积 误差。长期估计的结果将不可靠，或者说，我们无法构建全局一致的轨迹
用对极几何恢复出在两帧之间摄像机的运动
• 现在，假设我们从两张图像中，得到了一对配对好的特征点，像图 7-7 里显示的那样。如果我们有若 干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出在两帧之间摄像机的运动。
• 1. 根据配对点的像素位置，求出 E 或者 F ；
• 2. 根据 E 或者 F ，求出 R, t 。由于 E 和 F 只相差了相机
视觉 SLAM
蔺志强 苏 敬
• SLAM 是 Simultaneous Lo“同时定位与地图构
建”。它是指搭载特定传感器的主体，在没有环 境先验信息的情况下，于运动过程中建立环境的 模型，同时估计自己的运动。如果这里的传感器 主要为相机，那就称为“视觉 SLAM ”。
和地图。
核心问题是如何计算图像间的相似性
• 最简单的方式就是对任意两张图像都做一遍特征匹配，根据正确匹配的数量确定哪两个图像存在关联。但 计算量大，效率低下。 • 基于里程计的几何关系（ Odometry based ）， • 基于外观（ Appearanc e based ）仅根据两张图像的相似性确定回环检测关系，这种做法摆脱了累计误差， 使回环检测模块成为 SLAM 系统中一个相对独立的模块。
【视觉SLAM十四讲】直接法视觉里程计
2 LK 光流(5分，约3小时)2.1光流文献综述(1分)我们课上演示了Lucas-Kanade 稀疏光流，用OpenCV 函数实现了光流法追踪特征点。
实际上，光流法有很长时间的研究历史，直到现在人们还在尝试用Deeplearning 等方法对光流进行改进[1,2]。
本题将指导你完成基于Gauss-Newton 的金字塔光流法。
首先，请阅读文献[3]（paper 目录下提供了pdf ），回答下列问题。
问题：1. 按此文的分类，光流法可分为哪几类？答：作者在文中对光流法按照两种不同的方法进行分类。
➢ 按照估计的是参数的叠加增量还是增量Warp 将光流法分为叠加(additional)和组合(compositional)算法➢ 按照Warp 更新规则可以将光流法分为前向（forward ）和逆向/反向(inverse)两种算法综上：可以分4类，分别是 FA(Forward Additional), FC(Forward Composition), (Inverse Additional) 和 IC(Inverse Compositional)。
2. 在compositional 中，为什么有时候需要做原始图像的wrap ？该wrap 有何物理意义？答: 与Lucas-Kanade 算法中那样简单地将迭代的更新 p ∆添加到当前估计的参数p 不同，组合（compositional ）算法中，对扭曲();W x p ∆的增量更新必须由Warp 的当前估计组成() ;Wx p 。
这两个 warp 的组合可能更复杂。
因此，我们对 warp 集合有两个要求: 1）warp 集合必须包含 identity warp 。
2）warp 集合必须在组合运算中闭合。
需要在当前位姿估计之前引入增量式 warp （incremental warp ）以建立半群约束要求（the semi-group requirement ）。
机器人SLAM技术及其ROS系统应用教学课件第五章
n
R
R
t
状态更新方程为：
fv
rW new
qWR new
vW new
R new
rW vW V W t
qWR q ( R R )t
vW V W
R R
通过雅可比矩阵计算得到过程噪声协方差 Qv ：
Qv
fv n
Pn
fv n
T
5、主动特征测量与地图更新
相机的位置： 图像中特征的位置：
2、局部建图线程
（1）关键帧插入 （2）地图点云筛选 （3）新地图点云创建 （4）局部BA （5）局部关键帧筛选
3、闭环检测
（1）候选关键帧 （2）计算相似变换 （3）回环融合 （4）本征图优化
ORB-SLAM2主要模块
4、单目、双目近处和双目远处特征点
▪ORB-SLAM2直接对输入图像进行预处理，提取一些关键位置上的特征，输入图像直接被弃用 ▪ ORB-SLAM2能处理单目或者双目特征点，进一步分成远处特征点和近处特征点两类。
3、系统初始化
在单目相机SLAM算法中，初始化时通常在相机前面放置一个已知物体作为先验信息 ●在单目相机SLAM中，没有直接的方法来测量特征深度或里程信息 ●初始化时，已知特征信息可有助于直接进入预测、测量、更新的模式
4、运动模型和预测
MonoSLAM采用恒定速度、恒定角速度模型
噪声为：
V W aW t
y pi
riW hˆiW
7、地图管理
●对地图中特征数量的管理，需要动态地决定何时应该识别和初始化新特征，何时需要删除某个特征。 ●使从任何相机位置所可见的可靠特征的数量接近预定值 ●只有当相机经过的区域中可见的特征数量小于阈值时，才会将特征添加到地图中
SLAM入门之视觉里程计（6）：相机标定张正友经典标定法详解
SLAM ⼊门之视觉⾥程计（6）：相机标定张正友经典标定法详解想要从⼆维图像中获取到场景的三维信息，相机的内参数是必须的，在SLAM 中，相机通常是提前标定好的。
张正友于1998年在论⽂："A Flexible New Technique fro Camera Calibration"提出了基于单平⾯棋盘格的相机标定⽅法。
该⽅法介于传统的标定⽅法和⾃标定⽅法之间，使⽤简单实⽤性强，有以下优点：不需要额外的器材，⼀张打印的棋盘格即可。
标定简单，相机和标定板可以任意放置。
标定的精度⾼。
相机的内参数设P =(X ,Y ,Z )为场景中的⼀点，在针孔相机模型中，其要经过以下⼏个变换，最终变为⼆维图像上的像点p =(µ,ν):1. 将P 从世界坐标系通过刚体变换(旋转和平移)变换到相机坐标系，这个变换过程使⽤的是相机间的相对位姿，也就是相机的外参数。
2. 从相机坐标系，通过透视投影变换到相机的成像平⾯上的像点p =(x ,y )。
3. 将像点p 从成像坐标系，通过缩放和平移变换到像素坐标系上点p =(µ,ν)。
相机将场景中的三维点变换为图像中的⼆维点，也就是各个坐标系变换的组合，可将上⾯的变换过程整理为矩阵相乘的形式：s µν1=α0c x 0βc y1f 0000f 0001R t 0T1X Y Z1=f x 0c x 00f yc y 001Rt 0T1X Y Z1将矩阵K 称为相机的内参数,K =f x0c x 0f yc y 001其中，α,β表⽰图像上单位距离上像素的个数，则f x =αf ,f y =βf 将相机的焦距f 变换为在x,y ⽅向上像素度量表⽰。
另外，为了不失⼀般性，可以在相机的内参矩阵上添加⼀个扭曲参数γ，该参数⽤来表⽰像素坐标系两个坐标轴的扭曲。
则内参数K 变为K =f xγc x 0f yc y 01对于⼤多数标准相机来说，可将扭曲参数γ设为0. Multiple View Geometry in Computer Vision张⽒标定法在上⼀篇博⽂，介绍的单应矩阵表⽰两个平⾯间的映射。
视觉SLAM十四讲——特征提取和匹配
视觉SLAM⼗四讲——特征提取和匹配主要内容1. 常⽤的特征提取的⽅法：SIFT SURF ORB 其中，SIFT(尺度不变特征变换，Scale-Invariant Feature Transform)最为经典，充分考虑了相机的运动，光照变化，以及尺度和旋转变化，但需要较⼤的计算量， 在普通pc cpu上⽆法实时计算SIFT特征进⾏定位与建图 考虑适当减低精度和健壮性，减⼩计算量，如下两种： FAST关键点：没有描述⼦ ORB (Oriented FAST and Rotated BRIEF): 改进了FAST检测⼦不具有⽅向性的问题，并采⽤速度极快的⼆进制描述⼦BRIEF。
2. ORB特征——FAST关键点 1）选取周围半径为3的圆上的像素点，检测连续超过本像素点正负p%门限的像素点个数，根据个数分别为FAST-9, FAST-11, FAST-12。
2）预处理，排除⾮⾓点像素，增加处理速度 3）避免⾓点集中，采⽤⾮极⼤值抑制，在⼀定区域仅保留响应极⼤值的⾓点 4）提取最终的⾓点数量N,选取前N个最⼤响应值5）尺度不变性：构建图像⾦字塔 6）旋转：灰度质⼼法3. ORB特征——BRIEF描述⼦ 4. 特征匹配 计算所有特征点描述⼦距离，表征了特征点的相似程度 （采⽤不同的距离度量范数，浮点型描述⼦-欧⽒距离，⼆进制描述⼦-汉明距离） 匹配算法: 快速近似最近邻（FLANN）5. 正确匹配筛选的依据：汉明距离⼩于最⼩距离的两倍，⼯程上的经验⽅法 在后⾯的运动估计中，还需要使⽤去除误匹配的算法参考链接代码#include <iostream>#include <opencv2/core/core.hpp>#include <opencv2/features2d/features2d.hpp>#include <opencv2/highgui/highgui.hpp>using namespace std;using namespace cv;int main ( int argc, char** argv ){if ( argc != 3 ){cout<<"usage: feature_extraction img1 img2"<<endl;return1;}//-- 读取图像Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );//-- 初始化std::vector<KeyPoint> keypoints_1, keypoints_2;Mat descriptors_1, descriptors_2;Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> descriptor = ORB::create();// Ptr<FeatureDetector> detector = FeatureDetector::create(detector_name);// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create(descriptor_name);Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" );//-- 第⼀步:检测 Oriented FAST ⾓点位置detector->detect ( img_1,keypoints_1 );detector->detect ( img_2,keypoints_2 );//-- 第⼆步:根据⾓点位置计算 BRIEF 描述⼦descriptor->compute ( img_1, keypoints_1, descriptors_1 );descriptor->compute ( img_2, keypoints_2, descriptors_2 );Mat outimg1;drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );imshow("ORB特征点",outimg1); imwrite ("ORB_Feature.png", outimg1);//-- 第三步:对两幅图像中的BRIEF描述⼦进⾏匹配，使⽤ Hamming 距离vector<DMatch> matches;//BFMatcher matcher ( NORM_HAMMING );matcher->match ( descriptors_1, descriptors_2, matches );//-- 第四步:匹配点对筛选double min_dist=10000, max_dist=0;//找出所有匹配之间的最⼩距离和最⼤距离, 即是最相似的和最不相似的两组点之间的距离for ( int i = 0; i < descriptors_1.rows; i++ ){double dist = matches[i].distance;if ( dist < min_dist ) min_dist = dist;if ( dist > max_dist ) max_dist = dist;}// 仅供娱乐的写法min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance; max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance; printf ( "-- Max dist : %f \n", max_dist );printf ( "-- Min dist : %f \n", min_dist );//当描述⼦之间的距离⼤于两倍的最⼩距离时,即认为匹配有误.但有时候最⼩距离会⾮常⼩,设置⼀个经验值30作为下限.std::vector< DMatch > good_matches;for ( int i = 0; i < descriptors_1.rows; i++ ){if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) ){good_matches.push_back ( matches[i] );}}//-- 第五步:绘制匹配结果Mat img_match;Mat img_goodmatch;drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch );imshow ( "所有匹配点对", img_match );imshow ( "优化后匹配点对", img_goodmatch ); // 保存图像imwrite ("all_feature_matching.png", img_match);imwrite ("good_feature_matching.png", img_goodmatch);waitKey(0);return0;}结果输出。
视觉SLAM综述
通过基于光束平差法 ( Bundle Adjustment，BA) 的图优化理论估计相机的 位姿和地图点坐标，大大 提高了 SLAM 的精度。
目前，基于关键帧的VSLAM成为主流算法。最具代表性的有 两种: 一种是 LSD-SLAM [6] ，属于半稠密的单目的 VSLAM; 一 种是 ORB-SLAM [7,8] ，属于稀疏的 VSLAM。
3、稠密VSLAM
为了降低稠密地图的计算量，Engel J[12,13]提出了半稠 密地图（semi-direct visual odometry，SVO），通过结 合关键点的思想和直接配准的方法，大大降低了计算复 杂度。目前最先进的SVO就是Engel J等提出的LSDSLAM[13]，已经能在CPU上实现了实时性。
什么是SLAM？
SLAM问题可以描述为: 机器人在未知环境中从一个未知 位置开始移动,在移动过程中根据位置估计和地图进行 自身定位,同时在自身定位的基础上建造增量式地图， 实现机器人的自主定位和导航。
（1）定位(localization)：机器人必须知道自己在环境中位 置。 （2）建图(mapping)：机器人必须记录环境中特征的位置 （如果知道自己的位置）
后端图优化流程
2.3 后端优化
2.4 建图
在经典的VSLAM模型中，所谓的建图，即找到所有路标 点，这些路标点的集合即是最终的地图，一旦确定了路 标点的位置，即可以说完成了建图。地图类型分为栅格 地图，拓扑地图，特征地图和混合地图。根据建图需求 不同，可以将建图用途分为两类：
定位：把地图下来，机器人下次开机后仍然能在地图上 定位，不需要再次建模，此类地图，稀疏地图即可完成。
1.2 地图的稠密程度
移动机器人SLAM技术【ch07】视觉SLAM 教学课件
0 2 对极几何
假设两张图像中得到了一对配对好的特征点，对极几何如图7.8所示，若能够得到多 个匹配好的点，则可实现运动轨迹的推测。
如果没有特征点匹配，我们就没法确定p?到底在极线的哪个位置了。那时， 就必须在极线上搜索以获得正确的匹配。
0 2 对极几何
现在，我们从代数角度来看一下这里出现的几何关系。在第一帧的坐标系下，设P的空间位置为 两个像素点p、p?的像素位置为
除了使用线性方法，我们还可以把PnP问题构建成一个定义于李代数上的非线性最 小二乘问题。前面说的线性方法，往往要先求相机位姿，再求空间点位置，而非 线性优化则把它们都看成优化变量，放在一起优化。
02 3D-2D·PnP
考虑n个三维空间点P和它们的投影p,我们希望计算相机的位姿R、t,其李代数表 示为。假设某空间点的坐标为F=[X,,Y,Z,],其投影的像素坐标为μ=[x,,y]],那么 可得像素位置与空间点位置的关系如下：
02
PART ONE
视觉前端——视觉里程计
Hale Waihona Puke 02视觉里程计 7.2.1视觉里程计的数学模型
视觉里程计关心相邻图像之间的相机运动，最简单的情况当然是两张图像 之间的运动 关系。在计算机视觉领域，人类在直觉上看来十分自然的事情，在计算机 视觉中却非常困 难。图像在计算机里只是一个数值矩阵，而在视觉SLAM中，我们只能看到 一个个像素， 知道它们是某些空间点在相机的成像平面上投影的结果。所以，为了定量 地估计相机运动， 必须在了解相机与空间点的几何关系之后进行。
第七章 视觉SLAM
高等院校公共课系列精品教材
大学生礼仪
01
PART ONE
视觉SLAM概述
01视觉SLAM的糖念与框架