视觉里程计 特征点法 的基本过程

转自：http://blog.csdn.net/danmeng8068/article/details/76818885

视觉里程计的任务是估计相邻图像间相机的运动，分为特征点法和直接法。

1. 特征点法

1.1 概念

定义：特征点是图像当中具有代表性的部分，这些点在相机视角发生微小变化后依然保持不变

组成：由关键点和描述子两部分组成

关键点——位置、大小、方向

描述子——关键点周围的像素信息

1.2 ORB特征

组成：

关键点——Oriented FAST

描述子——BRIEF

1.2.1 FAST关键点

原理：主要检测局部像素灰度变化明显的地方，速度快，思想是若某像素点与其周围领域内足够多的像素点处于不同的区域，则该像素点可能为角点。也就是某些属性与众不同，考虑灰度图像，即若该点的灰度值比其周围领域内足够多的像素点的灰度值大或者小，则该点可能为角点。

算法步骤：

a.从图片中选取一个像素 P，它的亮度值设为Ip；

b.设定一个合适的阈值 t（比如Ip的20%）；

c.以像素 P为中心，选取半径为3的圆上16个像素点；

d.如果圆上有 n个连续的像素点的像素值大于Ip+t或小于Ip−t，那么p被认为是角点，n的值可以设置为12或者9，实验证明选择9可能会有更好的效果。

e.循环以上4步，对每一个像素执行相同的操作

上面的算法中，对于图像中的每一个点，我们都要去遍历其邻域圆上的16个点的像素，效率较低。进行改进来快速排除一大部分非角点的像素，只有当这4个像素中有3个大于Ip+t或者小于Ip−t时，当前像素 p才有可能是一个角点，否则排除。原始的FAST角点检测之后会出现角点集中的问题，采用非极大值抑制，在一定区域内仅保留响应值最大的角点。

•  OrientedFAST 改进的FAST

a.指定提取的角点数量

对原始FAST角点分别计算Harris响应值，然后选取前N个具有最大响应值的角点作为最终的角点集合

b.方向不变性和尺度不变性

添加了尺度和旋转的描述，尺度不变性通过构建高斯金字塔，然后在每一层金字塔图像上检测角点来实现，旋转利用灰度质心法来解决。灰度质心法假设角点的灰度与质心之间存在一个偏移，这个向量可以用于表示一个方向。对于任意一个特征点 p来说，我们定义 p的邻域像素的矩为：

其中 为点 处的灰度值。那么我们可以得到图像的质心为：

那么特征点与质心的夹角定义为FAST特征点的方向：

1.2.2 BRIEF描述子

是一种二进制描述子，在特征点附近通过一种选定的方法来挑选出来n个点对。然后对于每一个点对 (p,q)，我们来比较这两个点的亮度值，如果I(p)>I(q)则这个点对生成了二值串中一个的值为1，如果I(p)<I(q)，则对应为0。原始的BRIEF描述子不具有旋转不变性，在图像发生旋转时容易丢失，ORB在FAST特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算旋转之后的“SteerBRIEF”特征使ORB描述子具有较好的旋转不变性。

1.2.3 特征匹配

特征匹配解决了SLAM中的数据关联问题，即确定当前帧的图像特征与前一帧的图像对应关系，通过描述子的差异判断哪些特征为同一个点。

方法：暴力匹配，即比较图1中每个特征和图2特征的距离，改进：快速最近邻（FLANN）

1.2.4 实践：特征点的提取与匹配

调用opencv函数，提取图像ORB特征并进行匹配。

算法步骤：

a.     检测Oriented FAST关键点位置（detect函数）

b.     根据关键点位置计算BRIEF描述子（compute函数）

c.      对两幅图像中的BRIEF描述子进行匹配（match函数）

d.     匹配点对删选

e.      绘制匹配结果

运行结果：

提取的特征点：

所有的匹配点对：

筛选后的匹配点对：

2. 估计相机运动

特征匹配之后，得到了特征点之间的对应关系，根据匹配的点对来估计相机的运动，即求旋转矩阵R和平移矩阵t。分三种情况：

•  已知两幅图像的匹配点，求R,t ——对极几何（2D-2D）

•  已知特征点的世界坐标和相机上的投影点，求R,T——PnP（3D-2D）

•  已知两幅图像的匹配点及特征点的深度——ICP

2.1.1对极几何

P为空间的路标，为P在图像中的投影点，并且两幅图像较好的匹配到了，两个相机之间的变换为，已知，求

求解过程：

设P点的世界坐标：

以第一个图为参考系，投影方程：

归一化坐标（去掉内参）：

齐次关系：

两侧左乘：

再一步左乘：

推导出，对极约束公式：

约定：

E为基础矩阵，F为本质矩阵，对极约束给出了两个匹配点之间的空间关系，于是，问题简化为：由匹配点计算E或者F，由E或F求R,t。

2.1.2 本质矩阵

本质矩阵由对极约束定义，由于对极约束是等式为零的约束，所以E乘以乘任意非零常数依然满足，E共五个自由度，当成普通矩阵的话，有八个自由度，可用八点法求解。

八点法构成的方程组：

由上述方程可解出E，已知E的奇异值满足，由奇异值分解，，对于任意的E，存在两个可能的R,t

但只有一个深度为正，因此只要检测该点在在两个相机下的深度就可以确定哪个解是正确的。

八点法的总结：

a.用于单目视觉的初始化

b.尺度不确定性：归一化 t 或特征点的平均深度

c.纯旋转问题：t=0 时无法求解

d.多于八对点时：最小二乘或随机采样一致性（RANSAC）

2.1.3 三角测量

已知了相机的运动R,t，需要估计特征点P的空间位置，即求解两个特征点的深度,，在单目中，仅通过单张图像无法获得像素的深度信息，需要通过三角测量的方法来估计特征点的空间位置。

若匹配正确，理论上会相交于P点，但由于噪声的影响，会有偏差，所以可以通过最小二乘法求解。

按照对极几何的定义，,为两个特征点的归一化坐标，满足：

先求, 两侧乘

由上式可求出

2.1.4 实践

（1）对极约束求解相机运动2D-2D

算法步骤：

a.     读取图像，提取特征和匹配点对

b.     计算基础矩阵

c.      从基础矩阵求得R,t

运行结果：

（2）三角测量

利用对极几何求解得相机位姿R,t，通过三角化求出特征点在空间中的位置。

算法步骤：

a.     读入R,t以及图像匹配点对信息

b.     将匹配点像素坐标转换为相机坐标（相机内参K已知）

c.      调用pencv函数triangulatePoint函数重构出空间点坐标

d.     得到的值进行归一化，即得到特征点在空间中的位置

运行结果：

因有79对匹配点，故恢复的空间特征点有79个，以下只展示了前12个。

程序还验证了三角点与重投影关系：

2.2 PnP

已知特征点的世界坐标及对应的相机上的投影点，求R,t

方法：代数解法（DLT、P3P、EpnP等），优化方法（BundleAdjustment）

对极几何方法需要8个以上的点对，存在着初始化、纯旋转和尺度问题，如果两张图像中其中一张的特征点的3D位置已知，那么最少只需要3个点对就可以估计相机运动了，特征点的3D位置可以由三角化或者RGB-D相机的深度图确定，在单目视觉里，必须先初始化，然后才能使用PnP。

2.2.1 直接线性变换DLT

设空间点，投影点为：，投影关系：，展开：

解得：

,,X,Y,Z已知，t待求，由于t一共有12维，故最少需要6对匹配点即可实现矩阵T的线性求解。

2.2.2 P3P

利用三对点求相机外参

已知空间点A,B,C世界坐标系，对应的相机投影点a,b,c，一旦3D点在相机坐标系下的坐标能够算出，就将问题转化为ICP问题。

根据三角形相似和余弦定理，可得

令

令，，，得

消去v，得到关于x,y的二次方程，用吴氏消元法解析解得

得到x,y后，可求出v，从而得到OC的长度，进而得到OA,OB的长度，即得到A,B,C在相机坐标系下的坐标，转化为已知带有匹配信息的3D-3D的位姿求解问题（ICP）。

P3P只利用三个点的信息，当给定点大于3组时难以处理，出现误匹配时算法失效。

2.2.3  PnP的优化解法BundleAdjustment

线性方法往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化，可以对PnP或ICP给出的结果进行优化，BundleAdjustment是一个最小化投影误差的问题。

已知n个三维空间点P及其投影p，求相机的位姿R,t（李代数表示为）

设：空间点，投影点为

投影关系：

由于相机位姿未知及观测点噪声，该等式存在误差，将误差求和，构建最小二乘问题，寻找最好的位姿，使其最小化

考虑单个投影点误差：

线性化            

P点变换到相机坐标系下的坐标为

对进行投影

得：          

链式求导法则：

第一项为误差关于投影点的导数，第二项为变换后的点关于李代数的导数，根据李群-李代数关系，得

非齐次形式：         

得

这个雅克比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系，负号为观测值减去误差值，也可以反过来，这个表示平移在前旋转在后，若旋转在前平移在后，则把这个矩阵的前三列和后三列对调即可。

同理，也可以优化特征点空间位置。

2.2.4 实践：求解PnP

利用RGB-D相机拍摄得图片，同时输出的是彩色图和深度图，即直接可获得路标点的世界坐标和像素坐标。

用opencv提供得solverPnP函数求解PnP问题，得到R,t,将此结果作为初始值进行优化。

运行结果：

2.3 ICP

给定配对好的两组3D点，求其旋转和平移。

方法：线性代数法（SVD方法），非线性优化方法（类似于BundleAdjustment）

2.3.1 SVD方法

设：

运动关系：

定义误差项：

最小二乘问题，求使误差平方和最小达到极小的R，t

推导过程如下：

定义质心

目标函数变形为

交叉项部分求和为零，目标函数化简为

第一项只和R有关，第二项和R,t都有关，故最小化第一项，求得R，令第二项为零求得t。

展开关于R的误差项，得

观察第一项与R无关，第二项故也与R无关，所以优化目标函数变为

令

利用SVD分解：

故解得R，继而得t

2.3.2 非线性优化方法

用李代数表示位姿，目标函数可以写成

单个误差项关于位姿的导数

已知匹配时，ICP问题存在唯一解或无穷多解的情况。在唯一解的情况下，只要能找到极小值解，那么这个极小值就是全局最优值。

2.6.3 实践：求解ICP

图像数据是具有深度图RGB-D图。

（1）代数方法

算法步骤：

a.读入图像，并求解匹配点对信息

b.将匹配点对的像素坐标转换为相机坐标，根据深度图信息，进而转化为世界坐标

c.求两组点的质心，每个点去质心后的坐标，计算

e.对w进行SVD分解，得到U,V

f.，求出R，进而求出t

运行结果：

（2）优化方法

将前一步的估计值作为初始值，利用g2o库实现迭代更新，求解极值。

参考：高翔《视觉slam十四讲》