SLAM学习笔记-------------（七）视觉里程计

第七讲和第八讲主要介绍两种视觉里程计的常用方法：特征点法和光流法

本讲将介绍：什么是特征点、如何提取和匹配特征点、如何根据配对的特征点估计相机运动。

7.1 特征点法

第二讲我们说过，SLAM系统分为前端和后端。前端又叫做视觉里程计。视觉里程计根据相邻图像的信息估计出粗略的相机运动。给后端提供较好的初始值。

视觉里程计算法主要分为两大类：特征点法和直接法。

视觉里程计的核心问题就是根据图像估计相机运动。

我们从图像中选取有代表性的点，这些点在相机视角发生少量变化的情况下保持不变。

我们在各个图像中找到相同的点，称这些点为路标。在视觉SLAM中，路标就是图像特征。

一些图像特征，如SIFT（尺度不变特征变换），很精确，但是计算量太大。

还有一些，适当降低精度和鲁棒性，提升了计算速度。如ORB。

ORB是质量与性能之间较好的折中。所以我们就讲一下ORB的过程

关键点 Oriented FAST

FAST认为，如果一个像素与邻域的像素差别较大，那么他更可能是角点。流程如下：

此外还有预检测提高速度和防止扎堆问题，都比较简单易懂。看看就行

FAST特征点存在尺度问题：它取固定半径为3的点，有的点远处看着像角点，接近看可能就不是了。

而且FAST特征点没有方向信息。

针对这两个问题，ORB添加了尺度和旋转的描述。

尺度不变性：构建图像金字塔，在金字塔的每一层上检测角点

特征的旋转：灰度质心法。

计算特征点附近的图像灰度质心：（质心是指以图像块灰度值作为权重的中心）

1、在一个小的图像块B中，定义图像块的矩为

通过这两个方法，FAST的角点就有了尺度和旋转的描述。这种改进之后的FAST我们将其称为Oriented FAST。

BRIEF 描述子

特征匹配

特征匹配是视觉SLAM中极为关键的一步，宽泛地说，特征匹配解决了SLAM 中的数据关联问题( data association )

即确定当前看到的路标与之前看到的路标之间的对应关系。通过对图像与图像或者图像与地图之间的描述子进行准确匹配，我们可以为后续的姿态估计、优化等操作减轻大量负担。

然而，由于图像特征的局部特性，误匹配的情况广泛存在,而且长期以来一直没有得到有效解决，目前已经成为视觉SLAM中制约性能提升的一大瓶颈。部分原因是场景中经常存在大量的重复纹理，使得特征描述非常相似。在这种情况下，仅利用局部特征解决误匹配是非常困难的。

当然这个做的的问题也很明显，暴力匹配如果点很多那就会很慢。

当匹配点数量极多的时候，我们可以用快速近似最近邻（FLANN）。OpenCV上已经集成了他的实现，这里不再描述了。

7.2 实践：特征提取和匹配

7.2.1 使用OpenCV来提取和匹配ORB

这个代码编译的时候，报了个错误

‘index_sequence’ is not a member of ‘std’

修改CMakeLists文件，把11改成14就编译成功了

代码分析：

orb_cv.cpp

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <chrono>using namespace std;
using namespace cv;int main(int argc, char **argv) {if (argc != 3) {cout << "usage: feature_extraction img1 img2" << endl;return 1;}//-- 读取图像Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);assert(img_1.data != nullptr && img_2.data != nullptr);//-- 初始化std::vector<KeyPoint> keypoints_1, keypoints_2;Mat descriptors_1, descriptors_2;Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> descriptor = ORB::create();Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//-- 第一步:检测 Oriented FAST 角点位置chrono::steady_clock::time_point t1 = chrono::steady_clock::now();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);chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;Mat outimg1;drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);imshow("ORB features", outimg1);//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离vector<DMatch> matches;t1 = chrono::steady_clock::now();matcher->match(descriptors_1, descriptors_2, matches);t2 = chrono::steady_clock::now();time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;//-- 第四步:匹配点对筛选// 计算最小距离和最大距离auto min_max = minmax_element(matches.begin(), matches.end(),[](const DMatch &m1, const DMatch &m2) { return m1.distance < m2.distance; });double min_dist = min_max.first->distance;double max_dist = min_max.second->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("all matches", img_match);imshow("good matches", img_goodmatch);waitKey(0);return 0;
}

运行结果：

ORB关键点

未筛选的匹配：

筛选后的匹配：

运行结果：

手写ORB特征：

运行结果

特征点匹配好了，下面就是计算相机的运动。

分情况计算：

7.3 2D-2D 对极几何

对极几何，嗯。就是要解决一个问题。估计相机的运动。

我们知道两幅图，然后提取出来特征点，然后根据特征点，使用对极几何约束，就可以求出来本质矩阵，然后可以求出R t。估计相机的运动

看这个图，空间中的一个点P。

两帧图像I1 I2，从第一帧到第二帧经过了旋转R+平移t

通过前面，我们学了特征点的匹配，假设在这两帧图像中，p1,p2匹配上了，那就说明他们是同一个空间点P在两个成像平面上的投影。

什么意思呢？

其实很好理解，你看图，在第一帧图像上，O1p1这条射线上的所有点都会被投影到p1上，但是射线有无数个点，所以没办法确定这个点究竟在哪（这也就是所谓的成像丢失了深度信息）。那么再来一个第二帧图像，同样的射线O2p2上的点都有可能投影到p2，如果我们能够通过匹配证明，p1和p2成像的是同一个点。那么这两个射线的交点就是我们真实点的位置啊。所以，匹配很重要！

关于一些数学推导，可以看书。我这里放一个汇总的ppt。

ps：解释一个东西

这个P[X,Y,Z]T，他虽然写的是世界坐标

下面又写了一句，说以第一个图为参考系。实际上这里讨论的第一个图就是这里的世界坐标系。

也就是说，这个P的世界坐标[X,Y,Z]就是图一的坐标系下的坐标。

所以这里KP直接就等于像素坐标了，不需要把P做平移旋转。 // s1 s2是距离。

这两种形式的式子都称为对极约束，它以形式简洁著名。它的几何意义是O1,P,O2三者共面。

对极约束中同时包含了平移和旋转。我们把中间部分记作两个矩阵:基础矩阵 F 和本质矩阵 E ，于是可以进一步简化对极约束

对极约束简洁地给出了两个匹配点的空间位置关系。于是,相机位姿估计问题变为以下两步:

1．根据配对点的像素位置求出E或者F。
2．根据E或者F求出R,t。

由于E和F只相差了相机内参，而内参在SLAM中通常是已知的，所以实践中E和F知道一个就行了。

一般我们用E就够了（E的形式更加简单一些）

7.3.2 本质矩阵

下面我们介绍一下本质矩阵

介绍一下八点法：

八点法就不写了，直接看书吧。

最终的结论就是：如果八对匹配点组成的矩阵满足秩为8得条件，那么E的各个元素可以由这个方程解出来

接下来的问题就是根据解出来的E，恢复相机的运动R，t

这个过程是由奇异值分解(SVD)得到的，比较麻烦，记着结论就算了

就看最后结论，有四个可能的解

看图形象地展示了分解本质矩阵得到的4个解。

我们已知空间点在相机（蓝色线)上的投影（红色点)，想要求解相机的运动。在保持红色点不变的情况下，可以画出4种可能的情况。

不过幸运的是，只有第一种解中P在两个相机中都具有正的深度。因此，只要把任意一点代入4种解中,检测该点在两个相机下的深度，就可以确定哪个解是正确的了。

还有最后一个问题

7.3.3 单应矩阵

除了基本矩阵和本质矩阵,二视图几何中还存在另一种常见的矩阵:单应矩阵(Homography)H，它描述了两个平面之间的映射关系。

若场景中的特征点都落在同一平面上（比如墙、地面等)，则可以通过单应性进行运动估计。

这种情况在无人机携带的俯视相机或扫地机携带的顶视相机中比较常见。

单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系。

这个我没太看懂，但是结论就是：自由度为8的单应矩阵可以通过4对匹配特征点算出（在非退化的情况下，即这些特征点不能有三点共线的情况)

即求解上图那个最大的线性方程组（类似八点法）

7.4 实践对极约束求解相机运动

代码：

运行结果：

讨论：

就是这个2D-2D的对极约束有两个缺点：

1、尺度不确定性：就是它这个可以乘上任意比例常数，都不变的。所以没办法确定距离
2、纯旋转问题：这个单目初始化，不能纯旋转，必须要有平移。所以单目相机需要左右平移才能初始化

（具体看书上的解释，这是我自己瞎解释的）

7.5 三角测量

前面我们用对极几何约束估计了相机的运动。（根据对极几何求出E，再根据E求出R t 就知道了相机的运动）

三角测量呢是已知相机的运动，然后估计观测点的深度。

就是他这个由于有噪音，所以可能两条线汇聚不到一个点上。

这样没法求深度。

假如我们已经知道了R，t。也就是知道了相机的运动。那么可以估计出来深度。

原理就是，找到O1p1这个射线，在这个射线上面的点，拿去往I2上映射。看映射出来哪个点最接近p2.那就是了。

数学描述的话

如果我们要求s1。那就两边同时左乘x2的反对称。左边就等于0了

右边就是个关于s1的方程，解出来就好了。

也可以用最小二乘

7.6 实践三角测量

代码不说了，看书就好。

还需要强调几点：

三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈得上三角测量。

因此,纯旋转是无法使用三角测量的，因为在平移为零时，对极约束一直为零。

当然，实际数据往往不会完全等于零。在平移存在的情况下，我们还要关心三角测量的不确定性，这会引出一个三角测量的矛盾。

看这个图就明白了，当平移很小时,像素上的不确定性将导致较大的深度不确定性。

因此，要提高三角化的精度，一种方式是提高特征点的提取精度，也就是提高图像分辨率——但这会导致图像变大，增加计算成本。

另一种方式是使平移量增大。但是，这会导致图像的外观发生明显的变化,比如箱子原先被挡住的侧面显示出来,或者物体的光照发生变化,等等。

外观变化会使得特征提取与匹配变得困难。总而言之，增大平移，可能导致匹配失效;而平移太小，则三角化精度不够——这就是三角化的矛盾。我们把这个问题称为“视差”( parallax )。

在单目视觉中，由于单目图像没有深度信息，我们要等待特征点被追踪几帧之后，产生了足够的视角，再用三角化来确定新增特征点的深度值。这有时也被称为延迟三角化。

但是，如果相机发生了原地旋转，导致视差很小，就不好估计新观测到的特征点的深度。

这种情况在机器人场合下更常见，因为原地旋转往往是一个机器人常见的指令。在这种情况下，单目视觉就可能出现追踪失败、尺度不正确等情况。

7.7 3D-2D PNP

已知n个3D空间点及其投影位置时，如何估计相机位姿。

如果两张图像中的一张特征点的3D位置已知，那么最少只需3个点对（以及至少一个额外点验证结果）就可以估计相机运动。

介绍几种PnP求解方法

直接线性变换（DLT）

空间点：P=(X,Y,Z,1)T

投影点：x=(u,v,1) 归一化坐标

投影关系：sx=[R|t]p

展开：

一对匹配可以提供两个方程，（第一行看作t1 第二行t2 第三行t3）

一共十二个未知量，所以，用六个点可以求解。这就是DLT。

算出来的矩阵还需要做一个投影，投影到SO(3)上。（因为本来是按照普通矩阵算的，没有加约束，算出来的也是普通矩阵，但是他是正交阵，是有约束的，所以要做一个投影）

P3P

P3P需要利用给定的3个点的几何关系。它的输入数据为3对3D-2D匹配点

此外，P3P还需要使用一对验证点，以从可能的解中选出正确的那一个(类似于对极几何情形)。

3D-2D à 3D-3D 把 PnP问题转换为了ICP问题。

化简

最终得到一个二元二次方程，求解这个方程也很麻烦（吴消元法）。

算出来A,B,C在相机坐标系下3D坐标，世界坐标系下也知道了。

问题就转化为3D-3D了，就用ICP。

最小化重投影误差求解PnP

定义重投影误差并取最小化

最小二乘问题，前面讲了，用高斯牛顿，列文伯格方法。

我们需要知道每个误差项关于优化变量的导数，也就是线性化？？？

7.9 3D-3D ICP

3D-3D就是，就是我们已经有了一组匹配好的3D点，求解R,t。这就要用最近迭代点（ICP）求解

ICP求解也分两种方式：

代数方法：SVD
非线性优化：迭代

总结：

这里简单做个总结：

这一章讲视觉里程计，开头说了，视觉里程计就是根据相邻图像之间的信息，估计相机运动。

有两种方法：特征点法和直接法

这一章讲特征点法，下一章讲直接法。

特征点法：1、提取特征点 2、特征匹配（比较描述子）

匹配好了之后，我们就可以估计相机运动了

如果是单目相机：那只知道2D像素坐标，那就是2D-2D，用对极几何

如果是双目相机/RGB-D：那知道深度信息，那就是3D-3D，用ICP

如果是3D-2D：那用PnP （把2D转化成3D，就可以用ICP了

SLAM学习笔记-------------（七）视觉里程计相关推荐

slam学习笔记五----视觉里程计的学习1
一,什么是视觉里程计 SLAM系统分为前端和后端,前端也称为是视觉里程计,主要功能是根据相邻图像的信息粗略的估计出相机的运动,为后端提供较好的初始值. 视觉里程计的算法有两大类特征点法和直接法.特征点 ...
视觉SLAM十四讲学习笔记1——视觉里程计
一.基础概念 1.按照相机的工作方式,我们把相机分为单目(Monocular).双目(Stereo)和深度相机(RGB-D)三个大类. 2.照片,本质上是拍照时的场景(Scene),在相机的成像平面 ...
SLAM前端中的视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
slam学习笔记七----IMU传感器
一,IMU 惯性测量单元惯性测量单元(IMU)包含了三个单轴的加速度计和三个单轴的陀螺仪,加速度计检测物体在载体坐标系统独立三轴的加速度信号,而陀螺仪检测载体相对于导航坐标系的角速度信号,测量物体在 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
SLAM 05.视觉里程计-2-特征法
相机模型是理解视觉里程计之前的基础.本文主要是对高翔博士的<SLAM十四讲>的总结. 我们希望测量一个运动物体的轨迹,这可以通过许多不同的手段来实现.例如,我们在汽车轮胎上安装轮式计数码盘 ...
什么是视觉里程计（Visual Odometry）？
1.概念:什么是里程计? 在里程计问题中,我们希望测量一个运动物体的轨迹.这可以通过许多不同的手段来实现.例如,我们在汽车轮胎上安装计数码盘,就可以得到轮胎转动的距离,从而得到汽车的估计.或者,也可以 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-三角测量和实践
专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-对极几何和对极约束、本质矩阵、基础矩阵
专栏系列文章如下: 专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLA ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-PnP和实践
专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记- ...

SLAM学习笔记-------------（七）视觉里程计