从零开始学视觉里程计——一个初学者教程

从零开始学视觉里程计——一个初学者教程

什么是里程计

你见过汽车仪表盘上那个能告诉你汽车行驶了多少距离的小装置吗？它叫做里程计。它(可能)测量车轮的旋转圈数，并将其乘以周长，从而得到汽车行驶的距离的估计值。在机器人技术中，里程计是一个更通用的术语，通常不仅指所走的距离，还指移动机器人的整个轨迹。因此，对于每一个时刻t都有一个向量[x^t y^t z^t α^t β^t γ^t] 来描述完整的位姿。注意，这里的α^t β^t γ^t 是欧拉角，而x^t y^t z^t是机器人的笛卡尔坐标。

什么是视觉里程计

有不止一种方法来确定移动机器人的轨迹，但我们将在这个文章中关注的是视觉上的运动。在这种方法中，我们将一个相机(或多个相机)放置到一个移动的物体(比如汽车或机器人)，我们的工作是使用从这个相机(或多个相机)的视频流来构造一个6自由度的轨迹。当我们只用一个相机的时候,它被称为单目视觉里程计（Monocular Visual Odometry），当我们使用两个(或更多)摄像头时,它被认为是立体视觉里程计（Stereo Visual Odometry）。

为什么使用立体相机，或者为什么使用单目相机？

立体视觉和单目视觉各有优缺点，这里我将简要介绍其中一些主要优点。(请注意，这篇文章目前只关注立体视觉，但我可能也会记录并发布单目示例)。立体视觉的优点是你可以估计准确的轨迹，而在单目视觉中你只能估计轨迹的比例因子。在单目视觉里程计中，你只能说x移动了一个单位，y移动了两个单位，以此类推；而在立体视觉中，你可以说x移动了1米，y移动了2米，以此类推。而且，立体里程计通常更加健壮(因为有更多的数据可用)。但是，如果物体与摄像机之间的距离太远(与立体相机系统的两个摄像头之间的距离相比)，立体里程计退化为单目里程计。所以，让我们假设你有一个非常小的机器人(比如robobees)，那么拥有一个立体系统就没有用了，你只能使用单目视觉里程计算法，比如SVO。而且，无人机越来越小是一个普遍的趋势，所以像Davide Scaramuzza这样的团队现在更多地关注单目视觉里程计方法(至少在我参加的一次演讲中他是这么说的)。

理论足够了，现在讨论算法

问题描述

输入

我们有来自一对相机的(灰度/彩色)图像。设t时刻和t+1时刻捕获的左右帧分别为
我们已经预先知道了立体图像的所有内外标定参数，获得了许多立体标定算法中的任何一个。

输出

对于每一对立体图像，我们需要找到旋转矩阵R和平移向量t，它描述了移动机器人在两帧之间的运动。

算法

提纲：

如果您不理解上面看到的一些术语，如视差图或FAST特性，请不要担心。它们中的大多数将在后面的文本中更详细地解释，以及在MATLAB编程实现。

去畸变、校正

在计算视差图之前，我们必须执行一些预处理步骤。

去畸变:这一步补偿镜头失真。利用标定过程中得到的畸变参数来实现。

校正:这一步是为了缓解视差图的计算问题。在此步骤之后，所有的极线都与水平线平行，视差计算步骤只需在一个方向上搜索匹配块。

这两种操作都是在MATLAB中实现的，由于我使用的KITTI视觉里程计数据集已经实现了这些操作，所以您不会在我的实现中找到它们的代码。你可以在这里和这里看到如何使用这些函数。注意，您需要计算机视觉工具箱(Computer Vision Toolbox)和MATLAB R2014a或更新版本来实现这些功能。

视差图计算

给定一个立体相机图像，我们可以计算视差图。假定一个三维物理世界的F在左图中被定为在(x,y)，并且相同的特征在右图中坐标为(x+d,y)，然后在视差图上的(x,y)位置会保存d的值。注意在图像校准之后左右图像中y坐标的值相同。因此，我们可以将像平面上每一点的视差定义为：

Block-Matching算法

使用滑动窗口计算每个点的视差。对于左侧图像中的每个像素，将在其周围生成一个15x15像素宽的窗口，并存储窗口中所有像素的值。然后在右侧图像的相同坐标上构造该窗口，并水平滑动，直到绝对偏差和(SAD)最小。在我们的实现中使用的算法是这种块匹配技术的高级版本，称为半全局块匹配算法( Semi-Global Block Matching algorithm)。一个函数直接在MATLAB中实现了该算法：

disparityMap1 = disparity(I1_l,I1_r, 'DistanceThreshold', 5);

特征检测

我的方法是使用FAST角点检测。现在我将简要地解释检测器是如何工作的，不过如果您想真正理解它是如何工作的，那么您必须查看原始论文和源代码。假设有一个点P我们想检验它是否是一个角。我们在这个点周围画一个16px的圆，如下图所示。每个像素位于这个圆的圆周上，我们看到如果存在一组连续的像素的强度在可信度I下超过原始像素的强度，另一组相邻像素强度在相同可信度I下小于原始像素的强度。如果是，那么我们把这个点标记为一个角点。使用一种探索性的方式剔除大多数非角点，首先检查1、9、5、13号像素点，至少在可信度I下至少三个像素有更高的强度，或更低的强度，才可以将该点作为一个角点。之所以选择这种特殊的方法，是因为与其他流行的感兴趣点检测器(如SIFT)相比，其计算效率较高。

我们在这个方法中做的另一件事叫做bucketing。如果我们只是对整幅图像运行一个特征检测，很有可能大多数特征会集中在图像的某些富含特征的区域，而其他一些区域则没有任何特征。这对我们的算法来说并不好，因为该算法假设是一个静态场景，而要找到真正的静态场景，我们必须查看图像的所有区域，而不仅仅是它的某些区域。为了解决这个问题，我们将图像划分为网格(大约100x100px)，并从每个网格中提取最多20个特征，从而保持图像的均匀分布。
在代码中，你可以看到下面一行：

points1_l = bucketFeatures(I1_l, h, b, h_break, b_break, numCorners);

这一行使用的是下面的函数：

function points = bucketFeatures(I, h, b, h_break, b_break, numCorners)
% input image I should be grayscale

y = floor(linspace(1, h - h/h_break, h_break));
x = floor(linspace(1, b - b/b_break, b_break));

final_points = [];
for i=1:length(y)
    for j=1:length(x)
    roi =   [x(j),y(i),floor(b/b_break),floor(h/h_break)];
    corners = detectFASTFeatures(I, 'MinQuality', 0.00, 'MinContrast', 0.1, 'ROI',roi );
    corners = corners.selectStrongest(numCorners);
    final_points = vertcat(final_points, corners.Location);
    end
end
points = cornerPoints(final_points);

正如您所看到的，图像被划分为网格，每个网格中最强的角点被选择用于后续步骤。

特征描述和特征匹配

上一步中检测到的FAST角点在下一步使用，它使用了一个KLT跟踪器。KLT跟踪器基本上是在每一个要跟踪的角点寻找，然后利用这个局部信息在下一张图像中找到角点。欢迎浏览KLT网页，了解更多详情。
在MATLAB中，这也是非常容易做到的，下面三行代码使跟踪器初始化，并运行一次。

tracker = vision.PointTracker('MaxBidirectionalError', 1);
initialize(tracker, points1_l.Location, I1_l);
[points2_l, validity] = step(tracker, I2_l);

注意，在我目前的实现中我只是从一帧到下一帧跟踪特征点,然后再做检测的部分,但更好的实现方式是只要点的数量不低于特定的阈值，就可以跟踪这些点。

3D点云三角测量

F^t和F^t+1中所有点的真实世界3D坐标都是通过使用视差图中与这些特征相对应的视差值和两个摄像机的已知投影矩阵P1,P2来计算的。我们首先使用P1,P2计算重投影矩阵Q

cx=x-左相机的光学中心坐标(以像素为单位)
cy=y-左相机的光学中心坐标(以像素为单位)
f=左相机的焦距
Tx=右相机相对于左相机的x坐标(单位:米)
我们使用以下关系来获得F^t和F^t+1中每个特征的三维坐标

令得到的点云集合为W^t,W^t+1。为了更好地理解上述方程中的几何，你可以看一看视觉几何圣经，即Hartley和Zisserman的Multiple View Geometry。

内部检测

与大多数视觉测程算法相比，该算法不具有离群值检测步骤，但具有内值检测步骤。我们假设场景是刚性的，即它在时间t和t+1之间不会改变。因此，点云W^t中任意两个特征之间的距离必须与W^t+1中对应点之间的距离相同。如果任何这样的距离是不相同的，那么要么在两个特征中至少有一个的三维三角测量中存在误差，要么我们已经对一个移动物体进行了三角测量，我们不能在下一步使用它。为了得到一致匹配的最大集合，我们形成了一致性矩阵M，使

从原始点云,我们现在希望选择最大的子集,这样他们所有的点在这个子集是相互一致的(约简后的一致性矩阵中的每个元素都为1)。这个问题等价于最大团问题，以M为一个邻接矩阵。一个团基本上是一个图的子集，它只包含所有相互连接的节点。一种直观的方法是把一个图表想象成一个社交网络，然后试着找出最大的相互认识的人群。

这个问题被认为是非完全多项式，在任何实际情况下都无法找到最优解。因此，我们使用贪食试探给出一个接近最优解的团:

1. 选择等级最大的节点，并初始化团以包含该节点。
2. 从现有的团中，确定与团中所有节点相连的节点v的子集。
3. 从集合v中选择一个与v中其他节点数量最大的节点，重复步骤2，直到不再有节点加入到这个团中。

以上算法在我的代码中通过以下两个函数实现:

function cl = updateClique(potentialNodes, clique, M)


maxNumMatches = 0;
curr_max = 0;
for i = 1:length(potentialNodes)
    if(potentialNodes(i)==1)
        numMatches = 0;
        for j = 1:length(potentialNodes)
            if (potentialNodes(j) & M(i,j))
                numMatches = numMatches + 1;
            end
        end
        if (numMatches>=maxNumMatches)
            curr_max = i;
            maxNumMatches = numMatches;
        end
    end
end

if (maxNumMatches~=0)
    clique(length(clique)+1) = curr_max;
end

cl = clique;


function newSet = findPotentialNodes(clique, M)

newSet = M(:,clique(1));
if (size(clique)>1)  
    for i=2:length(clique)
        newSet = newSet & M(:,clique(i));
    end
end

for i=1:length(clique)
    newSet(clique(i)) = 0;
end

计算R和t

为了确定旋转矩阵R和平移向量t，我们使用拟牛顿法（Levenberg-Marquardt）非线性最小二乘极小化来最小化下面的和:


MATLAB中的优化工具箱直接在函数lsqnonlin中实现了Levenberg-Marquardt算法，需要提供一个需要最小化的向量目标函数和一组可更改的参数。
这就是要最小化的函数在MATLAB中的表示方式。算法的这一部分，在计算上是最昂贵的。

function F = minimize(PAR, F1, F2, W1, W2, P1)
r = PAR(1:3);
t = PAR(4:6);
%F1, F2 -> 2d coordinates of features in I1_l, I2_l
%W1, W2 -> 3d coordinates of the features that have been triangulated
%P1, P2 -> Projection matrices for the two cameras
%r, t -> 3x1 vectors, need to be varied for the minimization
F = zeros(2*size(F1,1), 3);
reproj1 = zeros(size(F1,1), 3);
reproj2 = zeros(size(F1,1), 3);

dcm = angle2dcm( r(1), r(2), r(3), 'ZXZ' );
tran = [ horzcat(dcm, t); [0 0 0 1]];

for k = 1:size(F1,1)
    f1 = F1(k, :)';
    f1(3) = 1;
    w2 = W2(k, :)';
    w2(4) = 1;
    
    f2 = F2(k, :)';
    f2(3) = 1;
    w1 = W1(k, :)';
    w1(4) = 1;
    
    f1_repr = P1*(tran)*w2;
    f1_repr = f1_repr/f1_repr(3);
    f2_repr = P1*pinv(tran)*w1;
    f2_repr = f2_repr/f2_repr(3);
    
    reproj1(k, :) = (f1 - f1_repr);
    reproj2(k, :) = (f2 - f2_repr);    
end

结果验证

一个特定的R和t集合，如果它满足以下条件，则称它是有效的

1. 如果在团中特征的数量至少是8
2. 再投影误差ϵ低于某个阈值

上述约束有助于处理噪声数据。

一个重要的“程序”

如果您在真实序列上运行上述算法，您将遇到一个相当大的问题。当大型车辆如卡车或货车占据相机的大部分视场时，场景刚性的假设不成立。为了处理这样的数据，我们引入了一个简单的程序:只接受一个过渡/旋转矩阵，当主导运动是在向前的方向。这可以显著提高KITTI数据集的结果，尽管你不会在大多数发表的论文中发现这一点。

备注

此文章介绍双目视觉里程计的实现方式，原文为英文，翻译自https://avisingh599.github.io/vision/visual-odometry-full/