CN110807809A - 基于点线特征和深度滤波器的轻量级单目视觉定位方法 - Google Patents

Abstract

本发明涉及一种基于点线特征和深度滤波器的轻量级单目视觉定位方法。包括：采集标定图像信息对相机进行标定，得到相机的畸变参数和内参矩阵；采集图像数据，进行畸变校正，得到校正后的图像数据；检测校正后图像的特征点；选取关键帧初始化系统；提取图像线段特征和特征点，跟踪每一帧得到图像的位姿；三维特征加入时使用CNN初始化深度滤波器；对种子进行迭代更新直到收敛。本发明适用于计算性能受限的设备，可用于纹理比较缺乏的场景，不计算、匹配描述子，具有更高的运行速度；加入了线段特征，能够得到更多的特征点；使用了CNN估计的深度值初始化深度滤波器，大大降低迭代的次数。

Description

基于点线特征和深度滤波器的轻量级单目视觉定位方法

技术领域

本发明属于视觉实时定位与建图技术领域，更具体地，涉及一种基于点线特征和深度滤波器的轻量级单目视觉定位方法。

背景技术

当今在机器人实时定位与建图领域中，通常采用激光雷达作为定位的核心传感器，以激光雷达为核心的各种定位技术，经过长期发展，已经走向了成熟，甚至广泛应用到了市场。视觉传感器相比于激光雷达，采样频率更高，跟重要的是，价格低廉，以视觉作为主要传感器的定位技术，越来越受到重视。视觉里程计计算效率更高，硬件成本得到显著降低。

目前视觉传感器可以分类三类：单目、双目和深度相机，深度相机采用结光的原理获取场景的深度，在室内可以得到测量精度比较高的值，但在室外大场景下，深度相机几乎失效；双目相机利用双目视觉和已知基线可以获取共视点的深度，可用于室外，但深度计算本生就是计算量比较大的一部分；单目视觉传感器成本更低，也不受基线影响，可以利用三角化来初始化场景的深度，室内室外均适用。

当前的视觉实时定位与建图多采用匹配特征点的方式来跟踪位姿，该做涉及计算特征点、为所有特征点计算描述子、匹配描述子，在匹配描述子的过程中，往往采用暴力匹配的方式，比较耗时，同时在特征非常缺乏的场景无法工作。基于直接法的匹配方式，通过最小化灰度值误差来迭代产生相机位姿，速度远远高于特征点法，在获得比较好的初始位姿时，迭代将会快速收敛，但直接法的缺点显而易见：对光度敏感、容易跟丢。

使用单目相机作为传感器，成本低、速度快，但无法引入尺度信息，通过三角化测量可以恢复二维点的深度，但具有尺度不确定性，随着时间推移，尺度误差将会累积，极大影响了定位效果。

LSD是在2012年被提出来的一种线段检测算法，相比于常规的线段检测算法，LSD能够在线性级别的时间内，得到亚像素精度的检测结果。LSD算法是检测图像中的局部直线边缘，边缘被定义为图像灰度从黑到白变化明显的区域。LSD因其较高的检测速率，可以满足SLAM对实时性的要求。

深度滤波器被广泛用于单目稠密重建中，深度滤波器通过三角化测量，得到二维点的初始深度，每得到一次观测就对种子进行更新，直到种子的不确定性小于某种程度停止更新，并将种子加入地图。实验证明，深度滤波器因为初始的不确定性过大，往往在SLAM结束的时候，绝大多数种子依然没有收敛，对定位产生了比较大的误差影响。

发明内容

本发明为克服上述现有技术中的缺陷，提供一种基于点线特征和深度滤波器的轻量级单目视觉定位方法。

为解决上述技术问题，本发明采用的技术方案是：一种基于点线特征和深度滤波器的轻量级单目视觉定位方法，包括以下步骤，

S1.使用待标定单目相机采集含有标定板信息的图像数据，使用标定得到相机模型的内参和畸变参数；

S2.将载有单目相机的设备放置至目标环境中，记录传感器数据，对采集到的每一帧图片进行畸变校正，并计算ORB特征和LSD线段特征；

S3.跟踪步骤S2得到的特征点，确定第一个关键帧和第二个关键帧，计算单应矩阵和基础矩阵，并确定最佳的参数，作为单目初始化全局的尺度；

S4.将采集线段特征得到的线段进行等距采样得到新的点，对所有ORB特征点和LSD特征上的点，确定一块特征块，使用直接法对所有特征块的位姿参数迭代，通过最小化特征块灰度差值，得到相机的初始位姿；利用得到的初始位姿和当前帧具有共视关系的关键帧，计算仿射变换并最小化光度误差，来对特征块的位置进行优化；最后利用得到的位姿和特征位置计算重投影误差，通过高斯牛顿法优化位姿和三维点的位置；

S5.计算当前帧与所有关联关键帧平移量，如果平移足够大则确定当前帧是关键帧；

S6.对于关键帧，使用CNN计算特征点的深度值初始化深度滤波器；对于普通帧，融合观测数据和预测数据，得到新的关于深度估计的高斯分布。

进一步的，所述的S1步骤中得到相机模型的内参的方法，包括：

在包含标定板的图像数据中，标定板所在的平面为Π，成像平面为π，已知成像平面π和标定板平面Π的信息，可以求得两个平面的单应矩阵Η，可以得到如下的等式：

Η＝KT

K为内参矩阵，Τ为变换矩阵，由于标定板上特征点可以被检测到，所以标定板的信息已知，通过对应的点对解得Η后，可以通过上面的等式得到内参矩阵K。

进一步的，所述的步骤S2中畸变校正的方法包括：

畸变可以分为径向畸变和切向畸变，这里只考虑径向畸变，采用张氏标定法即可获取畸变参数，设径向畸变参数为k1,k2,k3，则校正后的归一化坐标可以表示为：

x_true＝x(1+k_ir²+k₂r⁴+k₃r⁶)

y_true＝y(1+k₁r²+k₂r⁴+k₃r⁶)

其中x，y为归一化平面的坐标；由归一化的坐标根据标定得到的内参可以得到像素平面的坐标u，v。

进一步的，所述的步骤S3中单目相机初始化的方法，包括：

当传感器开始采集数据后，则开始检测每一帧图像的特征点数量，当检测到某一帧的特征点的数量大于100的时候，则选取该帧作为此次定位观察到的第一帧，并且确定为关键帧；

使用LK光流法计算稀疏光流来进行跟踪特征点，如果接下来的某一帧跟踪成功到的特征点数量大于50，并且像素差均值大于50个像素点，则认为该帧是关键帧，作为第二的关键帧，用来初始化，像素差均值是所有跟踪到的特征点与其在第一帧中二维位置的像素差总和除以跟踪成功的像素点数量得来，用来体现是否移动了足够的距离；

确定成功前两个关键帧后，计算匹配到的二维点对的之间单应矩阵Η和基础矩阵F，并通过RANSAC抽样，获取最优的单应矩阵Η和基础矩阵F，然后根据情况选择选用其中一个矩阵；在初始化场景是平面的情况下，使用单应矩阵在非平面场景下，使用基础矩阵；

三角化二维点对集来得到特征点的三维坐标，三维点全部作为地图点加入地图中，并使用这些地图点的深度初始化尺度。

进一步的，所述的S4步骤具体包括：

S41.判断当前帧检测到的特征点数量是否大于设置的最大特征点数量，如果没有，则将从线段特征中提取点，提取点的方式采取等距采样，采样的步长由需要补充的特征点数量决定；

S42.对于每一个点，取固定像素长度的矩形块，特征点为矩形块的中心，设初始的位姿为单位矩阵，以该特征块为单位投影到上一帧的所在像素位置，使用直接法最小化灰度误差来优化位姿，得到一个比较准确的初始位姿；

S43.寻找与当前帧具有共视关系的关键帧序列，利用步骤S42已经求得的位姿将关键帧上的地图点投影到当前帧，通过最小化光度误差来迭代优化当前帧的像素位置，直到收敛；

S44.使用BundleAdjustment来优化位姿和三维坐标。

进一步的，所述的步骤S6具体包括：

S61.对于得到的每一个关键帧，使用CNN得到关键帧的深度值，得到的深度值作为深度滤波器初始的均值，对于线段特征，只取线段的两个端点加入深度滤波器，估计两个端点的三维坐标；

S62.对于每一个非关键帧，计算所有种子在当前帧上的极线，计算种子所在帧与当前帧之间的仿射矩阵，在当前帧上进行极线搜索，得到匹配的位置，将当前测量深度与原先的分布进行深度融合得到新的均值和不确定性，对种子队列重复更新直到收敛。

与现有技术相比，有益效果是：

1.本发明适用于在计算性能受限的设备实现实时定位，采取直接法和光流法跟踪图像的特征点，只计算图像的ORB特征点，并不计算和匹配描述子，极大节省了前端跟踪的时间；

2.本发明采用的是单目传感器，成本相对于双目更低，对于单目的深度不确定性本发明采用了深度滤波器来解决；

3.本发明实现了准确的相机初始化功能，使用光流法跟踪图像帧，确定初始的关键帧，计算单应矩阵和基础矩阵，并且在没有外部设置的情况下，能够自适应的选择单应矩阵和基础矩阵，得到最好的初始化效果；

4.本发明使用深度滤波器来得到地图点，并且使用CNN估计的深度来作为初始化种子，利用非关键帧来更新种子的深度和不确定性，直到种子收敛并加入地图。

附图说明

图1是本发明的总流程图。

图2是本发明深度融合的流程图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。

实施例1：

如图1和图2所示，一种基于点线特征和深度滤波器的轻量级单目视觉定位方法，基本的步骤如下：

S1.使用待标定单目相机采集含有标定板信息的图像数据，使用标定得到相机模型的内参和畸变参数；

S2.将载有单目相机的设备放置至目标环境中，记录传感器数据，对采集到的每一帧图片进行畸变校正，并计算ORB特征和LSD线段特征；

S3.跟踪S2得到的特征点，确定第一个关键帧和第二个关键帧，计算单应矩阵和基础矩阵，并确定最佳的参数，为单目初始化全局的尺度；

S4.将采集线段特征得到的线段等距采样得到新的点，对所有ORB特征点和LSD特征上的点，确定一块特征块，使用直接法对所有特征块的位姿参数迭代，通过最小化特征块灰度差值，得到相机的初始位姿；利用得到的初始位姿和当前帧具有共视关系的关键帧，计算仿射变换并最小化光度误差，来对特征块的位置进行优化；最后利用得到的位姿和特征位置计算重投影误差，通过高斯牛顿法优化位姿和三维点的位置；

S5.计算当前帧与所有关联关键帧平移量，如果平移足够大则确定当前帧是关键帧；

S6.对于关键帧，使用CNN计算特征点的深度值初始化深度滤波器；对于普通帧，融合观测数据和预测数据，得到新的关于深度估计的高斯分布。

对于步骤S1所述的实施标定内参的过程中，因为标定是一种基于平面棋盘格的标定，所以标定平面到成像平面具有单应性，在包含标定板的图像数据中，标定板所在的平面为Π，成像平面为π，已知成像平面π和标定板平面Π，可以求得两个平面的单应矩阵Η，可以得到如下的等式：

Η＝KΤ

K为内参矩阵，Τ为变换矩阵，由于标定板上特征点可以被检测到，标定板的信息已知，通过对应的点对解得Η后，可以通过上面的等式得到内参矩阵K。

对于步骤S2所述的畸变校正方法，畸变可以分为径向畸变和切向畸变，这里只考虑径向畸变，采用传统方式如张氏标定法即可获取畸变参数，设径向畸变参数为k₁,k₂,k₃，则校正后的归一化坐标可以表示为：

x_true＝x(1+k₁r²+k₂r⁴+k₃r⁶)

y_true＝y(1+k₁r²+k₂r⁴+k₃r⁶)

其中x，y为归一化平面的的坐标。由归一化的坐标根据标定得到的内参可以得到像素平面的坐标u，v，像素坐标如下：

u＝f_x*x_true+C_x

v＝f_y*y_true+C_y

其中，f_x为相机x轴方向上的焦距，f_y为相机y轴上的焦距，c_x、c_y分别是x轴和y轴方向上的偏移。

对于步骤S3所述的特征检测，在实施中，首先提取ORB点特征，从图像中选取一点P，以P为中心取一个半径为3像素的圆，圆周上如果有连续N个像素点的灰度值比P点的灰度值大或小，则认为P为特征点。提取特征点前，可以快速剔除非特征点，首先检测特征点上下左右四个位置上的灰度值，如果P是特征点，那么这四个位置上有3个或3个以上的像素值都大于或者小于P点的灰度值。

其中，线段特征采用LSD算法来检测，LSD是在2012年被提出来的一种线段检测算法，相比于常规的线段检测算法，LSD能够在线性级别的时间内，得到亚像素精度的检测结果，可以满足SLAM对实时性的要求。线段特征往往在环境中纹理信息不是很充足的情况下使用，采用等距采样即可从线段中提取特征点，供跟踪线程迭代。

对于步骤S3所述的相机初始化过程，包括如下步骤：

(1)当传感器开始采集数据后，即开始检测每一帧图像的特征点数量，当检测到某一帧的特征点的数量大于100的时候，即选取该帧作为此次定位观察到的第一帧，并且确定为关键帧。

(2)使用LK光流法计算稀疏光流来进行跟踪特征点，如果接下来的某一帧跟踪成功到的特征点数量大于50，并且像素差均值大于50个像素点，则认为该帧是关键帧，作为第二的关键帧，用来初始化，像素差均值是所有跟踪到的特征点与其在第一帧中二维位置的像素差总和除以跟踪成功的像素点数量得来，用来体现是否移动了足够的距离。

(3)确定成功前两个关键帧后，计算匹配到的二维点对的之间单应矩阵Η和基础矩阵F，并通过RANSAC抽样，获取最优的单应矩阵Η和基础矩阵F，然后根据情况使用外部配置，指定选用其中一个矩阵，一般在初始化场景是平面的情况下，使用单应矩阵在非平面场景下，使用基础矩阵。也可以在不使用外部配置的情况自适应选择，步骤如下：

i.分别使用单应矩阵Η和基础矩阵F投影将当前帧投影到参考帧上，计算出两种情况下的内点和外点。

ii.对于两种情况下的所有内点，计算深度小于0的点的数量，作为误差点数。

iii.选取误差点数更小的模型作为变换矩阵。

(4)三角化二维点对集来得到特征点的三维坐标，计算这些三维点平均深度，并作为初始的尺度。

实施步骤S4具体过程如下：

S41.判断当前帧检测到的特征点数量是否大于设置的最大特征点数量，如果没有，则将从线段特征中提取点，提取点的方式采取等距采样，采样的步长由需要补充的特征点数量决定。

S42.对于每一个点，取固定像素长度的矩形块，这里采用4×4的矩形块，特征点为矩形块的中心，设初始的位姿为单位矩阵，以该特征块为单位投影到上一帧的所在像素位置，使用直接法最小化灰度误差来优化位姿，得到一个比较准确的初始位姿。将上述过程转化为如下公式：

利用高斯牛顿法迭代最小化光度误差，得到相对位姿T_k,k-1的最优估计，其中u_i为像素位置，δI表示光度误差。

S43.步骤S42中采取相邻帧间的匹配来得到初始位姿，随着匹配的帧数增多，会带来累积误差，所以需要通过关键帧来做进一步的约束。寻找与当前帧具有共视关系的关键帧序列，利用S42已经求得的位姿计算仿射矩阵A_i，将关键帧上的地图点投影到当前帧，通过最小化光度误差块来迭代优化当前帧的像素位置u_i，公式表示如下：

k表示当前帧的编号，r表示关键帧的编号，I_r表示矩阵块在参考帧的灰度值。

S44.得到准确的位姿后，则可以使用经典的BundleAdjustment来优化位姿和三维坐标，在这里使用高斯牛顿法最小化重投影误差即可。

对于步骤S6所述的深度融合，主要包括以下过程：

S61.对于得到每一个关键帧，使用CNN估计关键帧的深度值，将所有的特征点加入深度滤波器，得到的深度值作为深度滤波器初始的均值，针对线段特征，只取线段的两个端点加入深度滤波器，估计两个端点的三维坐标。

S62.对于每一个非关键帧，将会被用来更新种子，计算所有种子在当前帧上的极线，因为采用的不是单个像素特征，而是使用一个矩形窗口来描述像素特征，矩阵的形状可能发生改变，所以需要计算种子所在帧与当前帧之间的仿射矩阵，在当前帧上进行极线搜索，得到匹配的位置。

其中，深度滤波器假设像素深度服从高斯分布，对于新的观测数据，假设依然服从高斯分布，将当前测量深度与原先的分布进行深度融合得到新的高斯分布。每当有新的普通帧被读入，就对种子进行更新，直到种子收敛。使用深度滤波器进行深度融合的流程如附图2所示。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (6)

1.一种基于点线特征和深度滤波器的轻量级单目视觉定位方法，其特征在于，包括以下步骤：

S1.使用待标定单目相机采集含有标定板信息的图像数据，使用标定得到相机模型的内参和畸变参数；

S2.将载有单目相机的设备放置至目标环境中，记录传感器数据，对采集到的每一帧图片进行畸变校正，并计算ORB特征和LSD线段特征；

S3.跟踪步骤S2得到的特征点，确定第一个关键帧和第二个关键帧，计算单应矩阵和基础矩阵，并确定最佳的参数，作为单目初始化全局的尺度；

S4.将采集线段特征得到的线段进行等距采样得到新的点，对所有ORB特征点和LSD特征上的点，确定一块特征块，使用直接法对所有特征块的位姿参数迭代，通过最小化特征块灰度差值，得到相机的初始位姿；利用得到的初始位姿和当前帧具有共视关系的关键帧，计算仿射变换并最小化光度误差，来对特征块的位置进行优化；最后利用得到的位姿和特征位置计算重投影误差，通过高斯牛顿法优化位姿和三维点的位置；

S5.计算当前帧与所有关联关键帧平移量，如果平移足够大则确定当前帧是关键帧；

S6.对于关键帧，使用CNN计算特征点的深度值初始化深度滤波器；对于普通帧，融合观测数据和预测数据，得到新的关于深度估计的高斯分布。

2.根据权利要求1所述的基于点线特征和深度滤波器的轻量级单目视觉定位方法，其特征在于，所述的S1步骤中得到相机模型的内参的方法，包括：

在包含标定板的图像数据中，标定板所在的平面为Π，成像平面为π，已知成像平面π和标定板平面Π的信息可以求得两个平面的单应矩阵Η，可以得到如下的等式：

Η＝KT

K为内参矩阵，Τ为变换矩阵，由于标定板上特征点可以被检测到，所以标定板的信息已知，通过对应的点对解得Η后，可以通过上面的等式得到内参矩阵K。

3.根据权利要求1所述的基于点线特征和深度滤波器的轻量级单目视觉定位方法，其特征在于，所述的步骤S2中畸变校正的方法包括：

畸变可以分为径向畸变和切向畸变，这里只考虑径向畸变，采用张氏标定法即可获取畸变参数，设径向畸变参数为k₁,k₂,k₃，则校正后的归一化坐标可以表示为：

x_true＝x(1+k₁r²+k₂r⁴+k₃r⁶)

y_true＝y(1+k₁r²+k₂r⁴+k₃r⁶)

其中x，y为归一化平面的坐标；由归一化的坐标根据标定得到的内参可以得到像素平面的坐标u，v。

4.根据权利要求1所述的基于点线特征和深度滤波器的轻量级单目视觉定位方法，其特征在于，所述的步骤S3中单目相机初始化的方法，包括：

当传感器开始采集数据后，则开始检测每一帧图像的特征点数量，当检测到某一帧的特征点的数量大于100的时候，则选取该帧作为此次定位观察到的第一帧，并且确定为关键帧；

使用LK光流法计算稀疏光流来进行跟踪特征点，如果接下来的某一帧跟踪成功到的特征点数量大于50，并且像素差均值大于50个像素点，则认为该帧是关键帧，作为第二的关键帧，用来初始化，像素差均值是所有跟踪到的特征点与其在第一帧中二维位置的像素差总和除以跟踪成功的像素点数量得来，用来体现是否移动了足够的距离；

确定成功前两个关键帧后，计算匹配到的二维点对的之间单应矩阵Η和基础矩阵F，并通过RANSAC抽样，获取最优的单应矩阵Η和基础矩阵F，然后根据情况选择选用其中一个矩阵；在初始化场景是平面的情况下，使用单应矩阵在非平面场景下，使用基础矩阵；

三角化二维点对集来得到特征点的三维坐标，三维点全部作为地图点加入地图中，并使用这些地图点的深度初始化尺度。

5.根据权利要求1所述的基于点线特征和深度滤波器的轻量级单目视觉定位方法，其特征在于，所述的S4步骤具体包括：

S41.判断当前帧检测到的特征点数量是否大于设置的最大特征点数量，如果没有，则将从线段特征中提取点，提取点的方式采取等距采样，采样的步长由需要补充的特征点数量决定；

S42.对于每一个点，取固定像素长度的矩形块，特征点为矩形块的中心，设初始的位姿为单位矩阵，以该特征块为单位投影到上一帧的所在像素位置，使用直接法最小化灰度误差来优化位姿，得到一个比较准确的初始位姿；

S43.寻找与当前帧具有共视关系的关键帧序列，利用步骤S42已经求得的位姿将关键帧上的地图点投影到当前帧，通过最小化光度误差来迭代优化当前帧的像素位置，直到收敛；

S44.使用BundleAdjustment来优化位姿和三维坐标。

6.根据权利要求1所述的基于点线特征和深度滤波器的轻量级单目视觉定位方法，其特征在于，所述的步骤S6具体包括：

S61.对于得到的每一个关键帧，使用CNN得到关键帧的深度值，得到的深度值作为深度滤波器初始的均值，对于线段特征，只取线段的两个端点加入深度滤波器，估计两个端点的三维坐标；

S62.对于每一个非关键帧，计算所有种子在当前帧上的极线，计算种子所在帧与当前帧之间的仿射矩阵，在当前帧上进行极线搜索，得到匹配的位置，将当前测量深度与原先的分布进行深度融合得到新的均值和不确定性，对种子队列重复更新直到收敛。

Images