CN110375765B - 基于直接法的视觉里程计方法、系统及存储介质 - Google Patents

基于直接法的视觉里程计方法、系统及存储介质 Download PDF

Info

Publication number
CN110375765B
CN110375765B CN201910575743.4A CN201910575743A CN110375765B CN 110375765 B CN110375765 B CN 110375765B CN 201910575743 A CN201910575743 A CN 201910575743A CN 110375765 B CN110375765 B CN 110375765B
Authority
CN
China
Prior art keywords
image
camera
points
point
matrix
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910575743.4A
Other languages
English (en)
Other versions
CN110375765A (zh
Inventor
缪瑞航
刘佩林
应忍冬
龚正
薛午阳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN201910575743.4A priority Critical patent/CN110375765B/zh
Publication of CN110375765A publication Critical patent/CN110375765A/zh
Application granted granted Critical
Publication of CN110375765B publication Critical patent/CN110375765B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C3/00—Measuring distances in line of sight; Optical rangefinders
    • G01C3/10—Measuring distances in line of sight; Optical rangefinders using a parallactic triangle with variable angles and a base of fixed length in the observation station, e.g. in the instrument
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Electromagnetism (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Processing (AREA)

Abstract

本发明提供了一种基于直接法的视觉里程计方法、系统及介质，包括：亮度稳定步骤：在高动态亮度变换环境下，使用对数图像增强方法使得场景中的亮度趋于稳定，提高对梯度信息的提取；加快优化收敛步骤：在快速旋转以及高速运动情况下，使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛。本发明能够克服传统直接法在亮度巨变和快速旋转时无法正常工作的问题，可以用于无人机、无人车等自主导航机器人的导航算法上，可以用于在自主导航、探索、侦查等任务中提供自身的位置信息。

Description

基于直接法的视觉里程计方法、系统及存储介质
技术领域
本发明涉及计算机视觉技术领域，具体地，涉及鲁棒的基于直接法的视觉里程计方法、系统及存储介质。
背景技术
目前自助机器人在复杂环境下(如室内、丛林里、洞穴等等)的导航是极其困难的，特别是是在机器人高速运动和周围环境变化的情况下(光线变化、物体移动等)。在这种情况下一般的视觉里程计很容易由于环境的突变而中断，使用特征点匹配方式的视觉里程计对纹理信息要求较高，一旦进入走廊、楼梯一类的无丰富纹理环境，该类视觉里程计就会失去工作能力，而另外一种直接使用像素灰度之进行匹配的直接法视觉里程计则对光照要求较高，并且这类视觉里程计一旦中断掉一次，由于没有特征匹配，很难恢复到中断掉前的状态，因此本发明改进了直接法视觉里程计，并结合特征点法视觉里程计的重定位功能，提出一个鲁棒的视觉里程计方法。
专利文献CN108489482B(申请号：201810150612.7)公开了一种视觉惯性里程计的实现方法，包括以下步骤：第一步，通过相机实时采集图像数据；通过IMU采集陀螺仪和加速度的数据；第二步，场景初始化模块建立视觉惯性里程计系统的初始空间三维地图；第三步，地图扩展模块对空间三维地图进行实时更新；第四步，姿态估计模块接收计算出每一帧图像对应的设备的位置和姿态。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于直接法的视觉里程计方法、系统及存储介质。
根据本发明提供的一种基于直接法的视觉里程计方法，包括：
亮度稳定步骤：在高动态亮度变换环境下，使用对数图像增强方法使得场景中的亮度趋于稳定，提高对梯度信息的提取；
加快优化收敛步骤：在快速旋转以及高速运动情况下，使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛；
位置姿态修正步骤：同时在降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，在视觉里程计系统跟踪点云失败的时候，复原当前状态，并给出一个姿态预测值。
优选地，所述显著的梯度点指的是：图像中梯度大小大于预设值的点；
所述对数图像增强方法：
通过对原始图像进行对数变换，将图像低灰度部分进行扩展，并且将高灰度值部分进行压缩，增强低亮度光照下的图像纹理和抑制过曝光情况，所述对原始图像进行对数变换如下：
s＝c×logv+1(1+v×r) (1)
其中，
s表示对数变换后的图像像素亮度值；
c表示线性变换系数，为一个常数；
v表示对数变换的底数；
r表示袁术图像中像素的亮度值。
优选地，所述三角剖分算法生成深度预测面：
在低分辨率的图像上提取出角点，并且利用双目三角化计算角点的深度，之后使用三角剖分算法，将角点连接成一个网络，相邻的三个点会组成一个三角形，形成一个平面，根据三角形的三个顶点的深度，计算出三角形内任意一点的深度，预测三角形内的梯度点深度，计算过程如下：
设获得的双目图像已经是去畸变的图像，并设世界坐标系为
Figure BDA0002112057410000021
相机坐标系表示为
Figure BDA0002112057410000022
相机的位姿可以表示成矩阵
Figure BDA0002112057410000023
该
Figure BDA0002112057410000024
矩阵包含了相机的旋转和平移，同时使用矩阵K来表示相机的内参，该K矩阵包括了相机的焦距和相机的中心点，三维世界的点投影在相平面，可以表达成如下表达式：
Figure BDA0002112057410000025
Figure BDA0002112057410000026
Figure BDA0002112057410000027
其中，
x＝[x,y,1]T,X＝[X,Y,Z,1]T分别表示二维坐标点和三维坐标点；
r表示将投影结果转换为齐次坐标的比例系数；
Kl和Kr分别表示双目相机中左相机和右相机的内参矩阵；
Figure BDA0002112057410000031
表示双目相机中左相机的外参矩阵；
Figure BDA0002112057410000032
表示双目相机中右相机的外参矩阵；
xl和xr分别表示双目相机系统的左右两幅不同图像中相对应的二维点；
通过双目相机系统的两幅图像，获得空间中的三维点坐标，表达式如下：
Figure BDA0002112057410000033
其中，
τ表示对双目图像的三角化函数；
使用已知的顶点深度和三角剖分结果来计算三角形中任意一点的深度，设三个点的坐标分别为pi＝(Xi,Yi,Zi),i＝1,2,3,根据三维空间中的平面方程：
π1X+π2Y+π3Z+1＝0 (6)
其中，
X表示三角形顶点pi的x方向坐标；
Y表示三角形顶点pi的y方向坐标；
Z表示三角形顶点pi的z方向坐标；
将三角形的三个点的坐标带入该方程中，可以计算出平面方程的系数参数π1,π2,π3，从而确定整个平面，最后带入位置深度信息的梯度点在图像中的平面坐标u和v，u和v直接在图像中测量得到，分别和x＝[x,y,1]T中的x,y等价，根据相机成像的原理和已知的相机参数fx,fy,cx,cy，可以计算出：
X＝Z(u-cx)/fx (7)
Y＝Z(v-cy)/fy (8)
其中，
fx表示相机内参矩阵中的x方向焦距参数；
fy表示相机内参矩阵中的y方向焦距参数；
cx表示相机内参矩阵中的x方向图像中心偏移参数；
cy表示相机内参矩阵中的y方向图像中心偏移参数；
将公式(7)、(8)带入公式(6)式即可计算出深度信息Z，即图像上某个平面点所对应的三维空间中点的深度，进而获得三维坐标点。
优选地，所述Lucas-Kanade光流法：
根据获得的三维坐标点，通过不同时刻相机所拍摄的图像来获取相机两个不同时刻的相对位姿，从而达到给出位置姿态预测值的效果，由于相机和载体是固定连接的，可以获得载体在这个时刻的位姿，相对位姿使用Ti(t)∈SE(3)，SE(3)指的是三阶的特殊欧式群，是一种李群，下标i表示这是第i个图像帧，t表示时刻；
Ti(t)给出了
Figure BDA0002112057410000041
因此当3D点云和图像上的2D点的对应关系被给出之后，求Ti(t)等效于一个优化问题，这个优化问题可以被表达成：
Figure BDA0002112057410000042
其中，
Ti(t)表示在t时刻第i个图像所对应的位置和姿态矩阵；
T表示优化变量，为图像的位置姿态矩阵；
ρ表示欧氏距离的平方，即x和π(TPWC,X)两个点的距离的平方，x是观测出的二维点，π(TPWC,X)是投影的二维点；
Figure BDA0002112057410000043
表示3D点和2D图像点的对应关系集合，
Figure BDA0002112057410000044
并且V是当前图像帧里面的特征点集合。
优选地，所述位置姿态修正步骤包括：
保存参数步骤：保存双目相机的内参参数、外参参数、畸变参数，并且将左相机的中心设置为双目相机的中心点；
抽取特征点步骤：利用Shi-Tomas角点提取算法找出左相机图像中的角点xl，即左图像上检测出来的角点，使用Lucas-Kanade算法找出右相机图像中的对应点xr，即右图像上检测出来的角点，并计算出这些角点的特征描述子；
剔除双目匹配中的异常点对步骤：根据获得的点对，所述点对指xl和对应的xr，使用RANSAC算法计算单应性矩阵Hlr和基础矩阵Flr，并且使用下列公式剔除异常匹配点对：
xl＝Hlrxr，xlFlrxr＝0 (10)
当双目相机的图像场景是平面或者接近平面的状况下使用单应矩阵Hlr，但是当双目相机图像场景比较复杂，远近特征点都比较多的时候采用基础矩阵Flr来剔除异常点；
位置姿态的恢复步骤：当直接法视觉里程计丢失追踪的时，使用计算出的特征描述子匹配当前左相机图像和上个时刻的左相机图像中的特征点，根据公式(9)计算出当前的相对位置和姿态，恢复当前的位置姿态状态。
根据本发明提供的一种基于直接法的视觉里程计系统，包括：
亮度稳定模块：在高动态亮度变换环境下，使用对数图像增强方法使得场景中的亮度趋于稳定，提高对梯度信息的提取；
加快优化收敛模块：在快速旋转以及高速运动情况下，使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛；
位置姿态修正模块：同时在降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，在视觉里程计系统跟踪点云失败的时候，复原当前状态，并给出一个姿态预测值。
优选地，所述显著的梯度点指的是：图像中梯度大小大于预设值的点；
所述对数图像增强方法：
通过对原始图像进行对数变换，将图像低灰度部分进行扩展，并且将高灰度值部分进行压缩，增强低亮度光照下的图像纹理和抑制过曝光情况，所述对原始图像进行对数变换如下：
s＝c×logv+1(1+v×r) (1)
其中，
s表示对数变换后的图像像素亮度值；
c表示线性变换系数，为一个常数；
v表示对数变换的底数；
r表示袁术图像中像素的亮度值。
优选地，所述三角剖分算法生成深度预测面：
在低分辨率的图像上提取出角点，并且利用双目三角化计算角点的深度，之后使用三角剖分算法，将角点连接成一个网络，相邻的三个点会组成一个三角形，形成一个平面，根据三角形的三个顶点的深度，计算出三角形内任意一点的深度，预测三角形内的梯度点深度，计算过程如下：
设获得的双目图像已经是去畸变的图像，并设世界坐标系为
Figure BDA0002112057410000051
相机坐标系表示为
Figure BDA0002112057410000052
相机的位姿可以表示成矩阵
Figure BDA0002112057410000053
该
Figure BDA0002112057410000054
矩阵包含了相机的旋转和平移，同时使用矩阵K来表示相机的内参，该K矩阵包括了相机的焦距和相机的中心点，三维世界的点投影在相平面，可以表达成如下表达式：
Figure BDA0002112057410000055
Figure BDA0002112057410000056
Figure BDA0002112057410000061
其中，
x＝[x,y,1]T,X＝[X,Y,Z,1]T分别表示二维坐标点和三维坐标点；
r表示将投影结果转换为齐次坐标的比例系数；
Kl和Kr分别表示双目相机中左相机和右相机的内参矩阵；
Figure BDA0002112057410000062
表示双目相机中左相机的外参矩阵；
Figure BDA0002112057410000063
表示双目相机中右相机的外参矩阵；
xl和xr分别表示双目相机系统的左右两幅不同图像中相对应的二维点；
通过双目相机系统的两幅图像，获得空间中的三维点坐标，表达式如下：
Figure BDA0002112057410000064
其中，
τ表示对双目图像的三角化函数；
使用已知的顶点深度和三角剖分结果来计算三角形中任意一点的深度，设三个点的坐标分别为pi＝(Xi,Yi,Zi),i＝1,2,3,根据三维空间中的平面方程：
π1X+π2Y+π3Z+1＝0 (6)
其中，
X表示三角形顶点pi的x方向坐标；
Y表示三角形顶点pi的y方向坐标；
Z表示三角形顶点pi的z方向坐标；
将三角形的三个点的坐标带入该方程中，可以计算出平面方程的系数参数π1,π2,π3，从而确定整个平面，最后带入位置深度信息的梯度点在图像中的平面坐标u和v，u和v直接在图像中测量得到，分别和x＝[x,y,1]T中的x,y等价，根据相机成像的原理和已知的相机参数fx,fy,cx,cy，可以计算出：
X＝Z(u-cx)/fx (7)
Y＝Z(v-cy)/fy (8)
其中，
fx表示相机内参矩阵中的x方向焦距参数；
fy表示相机内参矩阵中的y方向焦距参数；
cx表示相机内参矩阵中的x方向图像中心偏移参数；
cy表示相机内参矩阵中的y方向图像中心偏移参数；
将公式(7)、(8)带入公式(6)式即可计算出深度信息Z，即图像上某个平面点所对应的三维空间中点的深度，进而获得三维坐标点；
所述Lucas-Kanade光流法：
根据获得的三维坐标点，通过不同时刻相机所拍摄的图像来获取相机两个不同时刻的相对位姿，从而达到给出位置姿态预测值的效果，由于相机和载体是固定连接的，可以获得载体在这个时刻的位姿，相对位姿使用Ti(t)∈SE(3)，SE(3)指的是三阶的特殊欧式群，是一种李群，下标i表示这是第i个图像帧，t表示时刻；
Ti(t)给出了
Figure BDA0002112057410000071
因此当3D点云和图像上的2D点的对应关系被给出之后，求Ti(t)等效于一个优化问题，这个优化问题可以被表达成：
Figure BDA0002112057410000072
其中，
Ti(t)表示在t时刻第i个图像所对应的位置和姿态矩阵；
T表示优化变量，为图像的位置姿态矩阵；
ρ表示欧氏距离的平方，即x和π(TPWC,X)两个点的距离的平方，x是观测出的二维点，π(TPWC,X)是投影的二维点；
Figure BDA0002112057410000073
表示3D点和2D图像点的对应关系集合，
Figure BDA0002112057410000074
并且V是当前图像帧里面的特征点集合。
优选地，所述位置姿态修正模块包括：
保存参数模块：保存双目相机的内参参数、外参参数、畸变参数，并且将左相机的中心设置为双目相机的中心点；
抽取特征点模块：利用Shi-Tomas角点提取算法找出左相机图像中的角点xl，即左图像上检测出来的角点，使用Lucas-Kanade算法找出右相机图像中的对应点xr，即右图像上检测出来的角点，并计算出这些角点的特征描述子；
剔除双目匹配中的异常点对模块：根据获得的点对，所述点对指xl和对应的xr，使用RANSAC算法计算单应性矩阵Hlr和基础矩阵Flr，并且使用下列公式剔除异常匹配点对：
xl＝Hlrxr，xlFlrxr＝0 (10)
当双目相机的图像场景是平面或者接近平面的状况下使用单应矩阵Hlr，但是当双目相机图像场景比较复杂，远近特征点都比较多的时候采用基础矩阵Flr来剔除异常点；
位置姿态的恢复模块：当直接法视觉里程计丢失追踪的时，使用计算出的特征描述子匹配当前左相机图像和上个时刻的左相机图像中的特征点，根据公式(9)计算出当前的相对位置和姿态，恢复当前的位置姿态状态。
根据本发明提供的一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现上述中任一项所述的基于直接法的视觉里程计方法的步骤。
与现有技术相比，本发明具有如下的有益效果：
本发明能够克服传统直接法在亮度巨变和快速旋转时无法正常工作的问题，可以用于无人机、无人车等自主导航机器人的导航算法上，可以用于在自主导航、探索、侦查等任务中提供自身的位置信息。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明提供的图像增强前后梯度点的示意图。
图2为本发明提供的中三角剖分的效果示意图。
图3为本发明提供的中双目相机坐标系的示意图。
图4为本发明提供的一实例中关键帧图像和当前相机图像帧的特征点匹配以及使用本发明方法中的转换生成的临时图像示意图。
图5为本发明提供的两个场景实例中视觉里程计轨迹示意图。
图6为本发明提供的鲁棒的基于直接法的视觉里程计方法的整体算法流程示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
根据本发明提供的一种基于直接法的视觉里程计方法，包括：
亮度稳定步骤：在高动态亮度变换环境下，使用对数图像增强方法使得场景中的亮度趋于稳定，提高对梯度信息的提取；
加快优化收敛步骤：在快速旋转以及高速运动情况下，使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛；
位置姿态修正步骤：同时在降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，在视觉里程计系统跟踪点云失败的时候，复原当前状态，并给出一个姿态预测值。
具体地，所述显著的梯度点指的是：图像中梯度大小大于预设值的点；
所述对数图像增强方法：
通过对原始图像进行对数变换，将图像低灰度部分进行扩展，并且将高灰度值部分进行压缩，增强低亮度光照下的图像纹理和抑制过曝光情况，所述对原始图像进行对数变换如下：
s＝c×logv+1(1+v×r) (1)
其中，
s表示对数变换后的图像像素亮度值；
c表示线性变换系数，为一个常数；
v表示对数变换的底数；
r表示袁术图像中像素的亮度值。
具体地，所述三角剖分算法生成深度预测面：
在低分辨率的图像上提取出角点，并且利用双目三角化计算角点的深度，之后使用三角剖分算法，将角点连接成一个网络，相邻的三个点会组成一个三角形，形成一个平面，根据三角形的三个顶点的深度，计算出三角形内任意一点的深度，预测三角形内的梯度点深度，计算过程如下：
设获得的双目图像已经是去畸变的图像，并设世界坐标系为
Figure BDA0002112057410000091
相机坐标系表示为
Figure BDA0002112057410000092
相机的位姿可以表示成矩阵
Figure BDA0002112057410000093
该
Figure BDA0002112057410000094
矩阵包含了相机的旋转和平移，同时使用矩阵K来表示相机的内参，该K矩阵包括了相机的焦距和相机的中心点，三维世界的点投影在相平面，可以表达成如下表达式：
Figure BDA0002112057410000095
Figure BDA0002112057410000096
Figure BDA0002112057410000097
其中，
x＝[x,y,1]T,X＝[X,Y,Z,1]T分别表示二维坐标点和三维坐标点；
r表示将投影结果转换为齐次坐标的比例系数；
Kl和Kr分别表示双目相机中左相机和右相机的内参矩阵；
Figure BDA0002112057410000101
表示双目相机中左相机的外参矩阵；
Figure BDA0002112057410000102
表示双目相机中右相机的外参矩阵；
xl和xr分别表示双目相机系统的左右两幅不同图像中相对应的二维点；
通过双目相机系统的两幅图像，获得空间中的三维点坐标，表达式如下：
Figure BDA0002112057410000103
其中，
τ表示对双目图像的三角化函数；
使用已知的顶点深度和三角剖分结果来计算三角形中任意一点的深度，设三个点的坐标分别为pi＝(Xi,Yi,Zi),i＝1,2,3,根据三维空间中的平面方程：
π1X+π2Y+π3Z+1＝0 (6)
其中，
X表示三角形顶点pi的x方向坐标；
Y表示三角形顶点pi的y方向坐标；
Z表示三角形顶点pi的z方向坐标；
将三角形的三个点的坐标带入该方程中，可以计算出平面方程的系数参数π1,π2,π3，从而确定整个平面，最后带入位置深度信息的梯度点在图像中的平面坐标u和v，u和v直接在图像中测量得到，分别和x＝[x,y,1]T中的x,y等价，根据相机成像的原理和已知的相机参数fx,fy,cx,cy，可以计算出：
X＝Z(u-cx)/fx (7)
Y＝Z(v-cy)/fy (8)
其中，
fx表示相机内参矩阵中的x方向焦距参数；
fy表示相机内参矩阵中的y方向焦距参数；
cx表示相机内参矩阵中的x方向图像中心偏移参数；
cy表示相机内参矩阵中的y方向图像中心偏移参数；
将公式(7)、(8)带入公式(6)式即可计算出深度信息Z，即图像上某个平面点所对应的三维空间中点的深度，进而获得三维坐标点。
具体地，所述Lucas-Kanade光流法：
根据获得的三维坐标点，通过不同时刻相机所拍摄的图像来获取相机两个不同时刻的相对位姿，从而达到给出位置姿态预测值的效果，由于相机和载体是固定连接的，可以获得载体在这个时刻的位姿，相对位姿使用Ti(t)∈SE(3)，SE(3)指的是三阶的特殊欧式群，是一种李群，下标i表示这是第i个图像帧，t表示时刻；
Ti(t)给出了
Figure BDA0002112057410000111
因此当3D点云和图像上的2D点的对应关系被给出之后，求Ti(t)等效于一个优化问题，这个优化问题可以被表达成：
Figure BDA0002112057410000112
其中，
Ti(t)表示在t时刻第i个图像所对应的位置和姿态矩阵；
T表示优化变量，为图像的位置姿态矩阵；
ρ表示欧氏距离的平方，即x和π(TPWC,X)两个点的距离的平方，x是观测出的二维点，π(TPWC,X)是投影的二维点；
Figure BDA0002112057410000113
表示3D点和2D图像点的对应关系集合，
Figure BDA0002112057410000114
并且V是当前图像帧里面的特征点集合。
具体地，所述位置姿态修正步骤包括：
保存参数步骤：保存双目相机的内参参数、外参参数、畸变参数，并且将左相机的中心设置为双目相机的中心点；
抽取特征点步骤：利用Shi-Tomas角点提取算法找出左相机图像中的角点xl，即左图像上检测出来的角点，使用Lucas-Kanade算法找出右相机图像中的对应点xr，即右图像上检测出来的角点，并计算出这些角点的特征描述子；
剔除双目匹配中的异常点对步骤：根据获得的点对，所述点对指xl和对应的xr，使用RANSAC算法计算单应性矩阵Hlr和基础矩阵Flr，并且使用下列公式剔除异常匹配点对：
xl＝Hlrxr，xlFlrxr＝0 (10)
当双目相机的图像场景是平面或者接近平面的状况下使用单应矩阵Hlr，但是当双目相机图像场景比较复杂，远近特征点都比较多的时候采用基础矩阵Flr来剔除异常点；
位置姿态的恢复步骤：当直接法视觉里程计丢失追踪的时，使用计算出的特征描述子匹配当前左相机图像和上个时刻的左相机图像中的特征点，根据公式(9)计算出当前的相对位置和姿态，恢复当前的位置姿态状态。
本发明提供的基于直接法的视觉里程计系统，可以通过本发明给的基于直接法的视觉里程计方法的步骤流程实现。本领域技术人员可以将所述基于直接法的视觉里程计方法，理解为所述基于直接法的视觉里程计系统的一个优选例。
根据本发明提供的一种基于直接法的视觉里程计系统，包括：
亮度稳定模块：在高动态亮度变换环境下，使用对数图像增强方法使得场景中的亮度趋于稳定，提高对梯度信息的提取；
加快优化收敛模块：在快速旋转以及高速运动情况下，使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛；
位置姿态修正模块：同时在降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，在视觉里程计系统跟踪点云失败的时候，复原当前状态，并给出一个姿态预测值。
具体地，所述显著的梯度点指的是：图像中梯度大小大于预设值的点；
所述对数图像增强方法：
通过对原始图像进行对数变换，将图像低灰度部分进行扩展，并且将高灰度值部分进行压缩，增强低亮度光照下的图像纹理和抑制过曝光情况，所述对原始图像进行对数变换如下：
s＝c×logv+1(1+v×r) (1)
其中，
s表示对数变换后的图像像素亮度值；
c表示线性变换系数，为一个常数；
v表示对数变换的底数；
r表示袁术图像中像素的亮度值。
具体地，所述三角剖分算法生成深度预测面：
在低分辨率的图像上提取出角点，并且利用双目三角化计算角点的深度，之后使用三角剖分算法，将角点连接成一个网络，相邻的三个点会组成一个三角形，形成一个平面，根据三角形的三个顶点的深度，计算出三角形内任意一点的深度，预测三角形内的梯度点深度，计算过程如下：
设获得的双目图像已经是去畸变的图像，并设世界坐标系为
Figure BDA0002112057410000121
相机坐标系表示为
Figure BDA0002112057410000122
相机的位姿可以表示成矩阵
Figure BDA0002112057410000123
该
Figure BDA0002112057410000124
矩阵包含了相机的旋转和平移，同时使用矩阵K来表示相机的内参，该K矩阵包括了相机的焦距和相机的中心点，三维世界的点投影在相平面，可以表达成如下表达式：
Figure BDA0002112057410000131
Figure BDA0002112057410000132
Figure BDA0002112057410000133
其中，
x＝[x,y,1]T,X＝[X,Y,Z,1]T分别表示二维坐标点和三维坐标点；
r表示将投影结果转换为齐次坐标的比例系数；
Kl和Kr分别表示双目相机中左相机和右相机的内参矩阵；
Figure BDA0002112057410000134
表示双目相机中左相机的外参矩阵；
Figure BDA0002112057410000135
表示双目相机中右相机的外参矩阵；
xl和xr分别表示双目相机系统的左右两幅不同图像中相对应的二维点；
通过双目相机系统的两幅图像，获得空间中的三维点坐标，表达式如下：
Figure BDA0002112057410000136
其中，
τ表示对双目图像的三角化函数；
使用已知的顶点深度和三角剖分结果来计算三角形中任意一点的深度，设三个点的坐标分别为pi＝(Xi,Yi,Zi),i＝1,2,3,根据三维空间中的平面方程：
π1X+π2Y+π3Z+1＝0 (6)
其中，
X表示三角形顶点pi的x方向坐标；
Y表示三角形顶点pi的y方向坐标；
Z表示三角形顶点pi的z方向坐标；
将三角形的三个点的坐标带入该方程中，可以计算出平面方程的系数参数π1,π2,π3，从而确定整个平面，最后带入位置深度信息的梯度点在图像中的平面坐标u和v，u和v直接在图像中测量得到，分别和x＝[x,y,1]T中的x,y等价，根据相机成像的原理和已知的相机参数fx,fy,cx,cy，可以计算出：
X＝Z(u-cx)/fx (7)
Y＝Z(v-cy)/fy (8)
其中，
fx表示相机内参矩阵中的x方向焦距参数；
fy表示相机内参矩阵中的y方向焦距参数；
cx表示相机内参矩阵中的x方向图像中心偏移参数；
cy表示相机内参矩阵中的y方向图像中心偏移参数；
将公式(7)、(8)带入公式(6)式即可计算出深度信息Z，即图像上某个平面点所对应的三维空间中点的深度，进而获得三维坐标点；
所述Lucas-Kanade光流法：
根据获得的三维坐标点，通过不同时刻相机所拍摄的图像来获取相机两个不同时刻的相对位姿，从而达到给出位置姿态预测值的效果，由于相机和载体是固定连接的，可以获得载体在这个时刻的位姿，相对位姿使用Ti(t)∈SE(3)，SE(3)指的是三阶的特殊欧式群，是一种李群，下标i表示这是第i个图像帧，t表示时刻；
Ti(t)给出了
Figure BDA0002112057410000141
因此当3D点云和图像上的2D点的对应关系被给出之后，求Ti(t)等效于一个优化问题，这个优化问题可以被表达成：
Figure BDA0002112057410000142
其中，
Ti(t)表示在t时刻第i个图像所对应的位置和姿态矩阵；
T表示优化变量，为图像的位置姿态矩阵；
ρ表示欧氏距离的平方，即x和π(TPWC,X)两个点的距离的平方，x是观测出的二维点，π(TPWC,X)是投影的二维点；
Figure BDA0002112057410000143
表示3D点和2D图像点的对应关系集合，
Figure BDA0002112057410000144
并且V是当前图像帧里面的特征点集合。
具体地，所述位置姿态修正模块包括：
保存参数模块：保存双目相机的内参参数、外参参数、畸变参数，并且将左相机的中心设置为双目相机的中心点；
抽取特征点模块：利用Shi-Tomas角点提取算法找出左相机图像中的角点xl，即左图像上检测出来的角点，使用Lucas-Kanade算法找出右相机图像中的对应点xr，即右图像上检测出来的角点，并计算出这些角点的特征描述子；
剔除双目匹配中的异常点对模块：根据获得的点对，所述点对指xl和对应的xr，使用RANSAC算法计算单应性矩阵Hlr和基础矩阵Flr，并且使用下列公式剔除异常匹配点对：
xl＝Hlrxr，xlFlrxr＝0 (10)
当双目相机的图像场景是平面或者接近平面的状况下使用单应矩阵Hlr，但是当双目相机图像场景比较复杂，远近特征点都比较多的时候采用基础矩阵Flr来剔除异常点；
位置姿态的恢复模块：当直接法视觉里程计丢失追踪的时，使用计算出的特征描述子匹配当前左相机图像和上个时刻的左相机图像中的特征点，根据公式(9)计算出当前的相对位置和姿态，恢复当前的位置姿态状态。
根据本发明提供的一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现上述中任一项所述的基于直接法的视觉里程计方法的步骤。
下面通过优选例，对本发明进行更为具体地说明。
优选例1：
本发明主要解决的技术问题为如何在高动态光照环境下实现鲁棒的视觉里程计和在视觉里程计中断掉后如何恢复到中断前状态。使得在高动态亮度变换环境下，使用对数图像增强的方法使得场景中的亮度趋于稳定，极大的提高了对梯度信息的提取，并且克服了难以快速跟踪的问题；在快速旋转以及高速运动情况下，使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛；同时在降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，在视觉里程计系统跟踪点云失败的时候，复原当前状态，并给出一个姿态预测值。最后可以实现一种鲁棒的，能够克服高动态亮度变换和克服快速旋转的视觉里程计。
为了解决这一技术问题本发明中发明了高动态光照环境下实现鲁棒的视觉里程计的方法。本发明主要针对视觉里程计的前端输入图像进行预处理。
在本发明中使用的与处理方法为对数图像增强方法，由于在低光照的情况下或者在有强烈光源的情况下，图像中的大部分将会变得昏暗，从而导致无法获取有效的梯度信息。利用对数函数的特殊性质，可以将低灰度值进行扩展，同时压缩高灰度值，最后实现整幅图像的梯度信息提取和抑制相机自动曝光导致的图像亮度变化。该对数变化方法如下：
s＝c×logv+1(1+v×r) (1)
其中，
s表示对数变换后的图像像素亮度值
c表示线性变换系数，为一个常数
v表示对数变换的底数
r表示袁术图像中像素的亮度值
进一步的，在计算机资源足够的时候，可以参考论文《Adaptive LogarithmicMapping For Displaying High Contrast Scenes》改用自适应对数映射的高对比图像显示技术(Tone Mapping)来实现更加鲁棒的图像预处理，使得输入到视觉里程计的图像具有高对比度和较为恒定的亮度。
本发明还另外提出了一个基于三角剖分的深度初始化算法，该算法的目的是给出点云的初始化深度，降低深度优化次数，加快优化收敛。该方法使用开源的Delaunay三角剖分算法，对低分辨率图像中提出来的角点进行处理。该算法具有以下特点：将输入的最接近的三个点形成三角形，并且所有三角形的边不会相互交叉；该算法具有唯一性，对于同一组输入点，只会有同一个输出；当删除或者增加点的时候，只会对该点临近的三角形有所影响。
当三角剖分算法完成后，会在原有的图像上生成大大小小的三角形，每个三角形的顶点深度可以通过双目的三角化来确定，但是在后续的直接法视觉里程计算中，将会有许多梯度点出现在三角形内部或者边上。
现在假设获得的双目图像已经是去畸变的图像，并设世界坐标系为
Figure BDA0002112057410000161
相机坐标系表示为
Figure BDA0002112057410000162
那么左相机的位姿(位置和姿态)可以表示成一个3×4的矩阵
Figure BDA0002112057410000163
该
Figure BDA0002112057410000164
矩阵包含了相机的旋转和平移，同时使用矩阵K来表示左相机的内参，该K矩阵包括了相机的焦距和相机的中心点。因此三维世界的点投影在向平面，可以表达成如下表达式
Figure BDA0002112057410000165
这里的r表示将投影结果转换为齐次坐标的比例系数，x＝[x,y,1]T,X＝[X,Y,Z,1]T分别表示二维坐标点和三维坐标点。通过双目相机系统的两幅图像，我们可以获得空间中的三维点坐标，表达式如下
Figure BDA0002112057410000166
其中，
Figure BDA0002112057410000167
表示双目相机中左相机的外参矩阵
Figure BDA0002112057410000168
表示双目相机中右相机的外参矩阵
τ表示对双目图像的三角化函数
表达式中xl和xr分别指左右两幅不同图像中相对应的二维点，通过结合左右两个图像的(2)表达式可以求出对应三维空间中该点的坐标。
我们需要使用已知的顶点深度和三角剖分结果来计算三角形中任意一点的深度。设三个点的坐标分别为pi＝(xi,yi,zi),i＝1,2,3,根据三维空间中的平面方程
π1X+π2Y+π3Z+1＝0 (4)
将三角形的三个顶点带入该方程中，可以计算出参数π1,π2,π3(平面方程的系数)，从而确定整个平面，最后带入位置深度信息的梯度点在图像中的平面坐标u和v，根据相机成像的原理和已知的相机参数fx,fy,cx,cy，可以计算出
X＝Z(u-cx)/fx (5)
Y＝Z(v-cy)/fy (6)
将(5-6)表达式带入(4)式即可计算出深度信息Z(图像上某个平面点对应的3维空间中点的深度)。
此外，本发明还提出一种使用Lucas-Kanade光流法复原当前状态，并给出一个姿态预测值的方法，该方法的目的是在视觉里程计系统直接法跟踪失败的时候，因为机器人(相机载体)已经运动了一段时间，不能直接使用跟踪失败时候的状态，需要进行一定预测来复原当前状态，从而实现视觉里程计的鲁棒运行。
在三维坐标点被给出之后，我们通过不同时刻左边相机所拍摄的图像来获取左边相机两个不同时刻的相对位姿，从而达到给出位置姿态预测值的效果。由于相机和载体是固定连接的，那么我们由此可以获得载体在这个时刻的位姿。相对位姿使用Ti(t)∈SE(3)，SE(3)指的是三阶的特殊欧式群，是一种李群，下标i表示这是第i个图像帧，t表示时刻。Ti(t)就给出了
Figure BDA0002112057410000171
因此当3D点云和图像上的2D点的对应关系被给出之后，求Ti(t)等效于一个优化问题，这个优化问题可以被表达成
Figure BDA0002112057410000172
其中，
Ti(t)表示在t时刻第i个图像所对应的位置和姿态矩阵
T表示优化变量，为图像的位置姿态矩阵
这里的ρ指的是欧氏距离的平方(指的是x和π(TPWC,X)两个点的距离，第一个是观测出的二维点，第二个是投影的二维点)，
Figure BDA0002112057410000175
是3D点和2D图像点的对应关系集合，
Figure BDA0002112057410000173
Figure BDA0002112057410000174
并且V是当前图像帧里面的特征点集合。本发明中的位置姿态修正算法流程为：
1)保存参数：保存双目相机的内参参数、外参参数、畸变参数，并且将左相机的中心设置为双目相机的中心点
2)抽取特征点：利用Shi-Tomas角点提取算法找出左相机图像中的角点xl(左图像上检测出来的角点)，使用Lucas-Kanade算法找出右相机图像中的对应点xr(右图像上检测出来的角点)，并且计算出这些角点的特征描述子
3)剔除双目匹配中的异常点对：计算出点对后，使用RANSAC算法计算单应性矩阵Hlr和基础矩阵Flr，并且使用关系式
xl＝Hlrxr，xlFlrxr＝0 (8)
剔除异常匹配点。当双目相机的图像场景是平面或者接近平面的状况下使用单应矩阵，但是当双目相机图像场景比较复杂，远近特征点都比较多的时候采用基础矩阵来剔除异常点。
4)位置姿态的恢复：使用Lucas-Kanade算法匹配当前左相机图像和上个时刻(直接法视觉里程计丢失追踪的时候)的左相机图像中的特征点，根据(7)表达式计算出当前的相对位置和姿态，恢复当前的位置姿态状态。
本发明使用对数图像增强的方法使得场景中的亮度趋于稳定，极大的提高了对梯度信息的提取；使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度；在降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，给出一个姿态预测值。综合上面的三种技术方法可以实现一种鲁棒的，能够克服高动态亮度变换和克服快速旋转的视觉里程计。
如图1所示，为本发明提供的图像增强前后梯度点的示意图，上图为图像增强前，下图为图像增强后；
如图2所示，为本发明提供的中三角剖分的效果示意图，左边为三角剖分结果，右图为对应的场景。
如图3所示，为本发明提供的中双目相机坐标系的示意图。
如图4所示，为本发明提供的一实例中关键帧图像和当前相机图像帧的特征点匹配以及使用本发明方法中的转换生成的临时图像示意图。
如图5所示，为本发明提供的两个场景实例(办公室大厅和工厂)中视觉里程计轨迹示意图。
优选例2：
本发明中的对数图像增强流程如下，对所有图像做如下处理：
1)从传感器读入图像数据，进行去畸变处理，并将图像灰度化
2)选择一个合适的对数底数，对灰度化图像进行对数变换
3)将处理后的图像像素进行归一化，归一化到0-255的像素值之间，方便后续处理和显示
在本发明的一个实例中取对数底数取10，比例系数取1。
本发明中的三角剖分的深度初始化算法具体流程如下：
1)将双目图像进行降采样，利用Shi-Tomas角点检测算法检测存在的角点，并进行左右目的对应点对。
2)三角化计算角点的深度
3)利用Bowyer-Watson算法，将已有的深度点作为输入，进行三角剖分，生成三角网格
4)对每一个三角形，根据方程(4)计算平面方程
5)在直接法中计算出来的每一个梯度点，查询包含该梯度点的三角形，并根据俄对应的平面方程计算该梯度点的粗糙深度信息，作为深度舒适化
本发明中的使用Lucas-Kanade光流法复原位置姿态的具体流程如下：
1)保存参数：保存双目相机的内参参数、外参参数、畸变参数，并且将左相机的中心设置为双目相机的中心点
2)抽取特征点：利用Shi-Tomas角点提取算法找出左相机图像中的角点xl，使用Lucas-Kanade算法找出右相机图像中的对应点xr，并且计算出这些角点的特征描述子
3)剔除双目匹配中的异常点对：计算出点对后，使用RANSAC算法计算单应性矩阵Hlr和基础矩阵Flr，并且使用关系式(8)剔除异常匹配点。当双目相机的图像场景是平面或者接近平面的状况下使用单应矩阵，但是当双目相机图像场景比较复杂，远近特征点都比较多的时候采用基础矩阵来剔除异常点。
4)位置姿态的恢复：使用Lucas-Kanade算法匹配当前左相机图像和上个时刻(直接法视觉里程计丢失追踪的时候)的左相机图像中的特征点，根据(7)表达式计算出当前的相对位置和姿态，恢复当前的位置姿态状态。
优选例3：
一种鲁棒的基于直接法的视觉里程计方法，能够克服传统直接法在亮度巨变和快速旋转时无法正常工作的问题。
该方法主要内容有：
在高动态亮度变换环境下，该方法使用对数图像增强的方法使得场景中的亮度趋于稳定，极大的提高了对梯度信息的提取，并且克服了难以快速跟踪的问题；
在快速旋转以及高速运动情况下，本方法使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛；
同时在降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，在视觉里程计系统跟踪点云失败的时候，复原当前状态，并给出一个姿态预测值。最后可以实现一种鲁棒地，能够克服高动态亮度变换和克服快速旋转的视觉里程计。
在高动态亮度变换环境下，该方法使用对数图像增强的方法使得场景中的亮度趋于稳定，极大的提高了对梯度信息的提取：
其中的对数图像增强方式通过对原始图像进行对数变换，将图像低灰度部分进行扩展，并且将高灰度值部分进行压缩，能够有效的增强低亮度光照下的图像纹理和抑制过曝光情况。对于光照明显变化的环境，传统直接法将会受到很大的影响，使用对数图像增强后能够有效的抑制光照变化带来的图像灰度值变化。
在快速旋转以及高速运动情况下，本方法使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛：
其中三角剖分算法生成深度预测面指的是在低分辨率的图像上提取出角点，并且利用双目三角化计算这些角点的深度，之后使用三角剖分的算法，把这些角点连接成一个网络，相邻的三个点会组成一个三角形，形成一个平面。由于三角形的三个顶点的深度已知，我们可以近似的计算出三角形内任意一点的深度，由此可以来预测三角形内的梯度点深度。
所描述的降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，在视觉里程计系统跟踪点云失败的时候，复原当前状态，并给出一个姿态预测值：
其中使用Lucas-Kanade光流法对若干显著梯度点进行匹配指的是当时用直接法跟踪点云丢失的时候，使用Lucas-Kanade光流法重新跟踪点云，并利用上一帧图像点云的深度信息计算当前图像帧的位置，从而恢复到直接法跟踪状态。
整体算法流程图如下图6所示。
在本申请的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (6)

1.一种基于直接法的视觉里程计方法，其特征在于，包括：
亮度稳定步骤：在高动态亮度变换环境下，使用对数图像增强方法使得场景中的亮度趋于稳定，提高对梯度信息的提取；
加快优化收敛步骤：在快速旋转以及高速运动情况下，使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛；
位置姿态修正步骤：同时在降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，在视觉里程计系统跟踪点云失败的时候，复原当前状态，并给出一个姿态预测值；
所述显著的梯度点指的是：图像中梯度大小大于预设值的点；
所述对数图像增强方法：
通过对原始图像进行对数变换，将图像低灰度部分进行扩展，并且将高灰度值部分进行压缩，增强低亮度光照下的图像纹理和抑制过曝光情况，所述对原始图像进行对数变换如下：
s＝c×logv+1(1+v×r) (1)
其中，
s表示对数变换后的图像像素亮度值；
c表示线性变换系数，为一个常数；
v表示对数变换的底数；
r表示原始图像中像素的亮度值；
所述三角剖分算法生成深度预测面：
在低分辨率的图像上提取出角点，并且利用双目三角化计算角点的深度，之后使用三角剖分算法，将角点连接成一个网络，相邻的三个点会组成一个三角形，形成一个平面，根据三角形的三个顶点的深度，计算出三角形内任意一点的深度，预测三角形内的梯度点深度，计算过程如下：
设获得的双目图像已经是去畸变的图像，并设世界坐标系为
Figure FDA0002892082600000011
相机坐标系表示为
Figure FDA0002892082600000012
相机的位姿可以表示成矩阵
Figure FDA0002892082600000013
该
Figure FDA0002892082600000014
矩阵包含了相机的旋转和平移，同时使用K矩阵来表示相机的内参，所述K矩阵包括了相机的焦距和相机的中心点，三维世界的点投影在相平面表达成如下表达式：
Figure FDA0002892082600000021
Figure FDA0002892082600000022
Figure FDA0002892082600000023
其中，
x＝[x，y，1]T，X＝[X，Y，Z，1]T分别表示二维坐标点和三维坐标点；
r表示将投影结果转换为齐次坐标的比例系数；
Kl和Kr分别表示双目相机中左相机和右相机的内参矩阵；
Figure FDA0002892082600000024
表示双目相机中左相机的外参矩阵；
Figure FDA0002892082600000025
表示双目相机中右相机的外参矩阵；
xl和xr分别表示双目相机系统的左右两幅不同图像中相对应的二维点；
通过双目相机系统的两幅图像，获得空间中的三维点坐标，表达式如下：
Figure FDA0002892082600000026
其中，
τ表示对双目图像的三角化函数；
使用已知的顶点深度和三角剖分结果来计算三角形中任意一点的深度，设三个点的坐标分别为pi＝(Xi，Yi，Zi)，i＝1，2，3，根据三维空间中的平面方程：
π1X+π2Y+π3Z+1＝0 (6)
其中，
X表示三角形顶点pi的x方向坐标；
Y表示三角形顶点pi的y方向坐标；
Z表示三角形顶点pi的z方向坐标；
将三角形的三个点的坐标带入该方程中，计算出平面方程的系数参数π1，π2，π3，从而确定整个平面，最后带入位置深度信息的梯度点在图像中的平面坐标u和v，u和v直接在图像中测量得到，分别和x＝[x，y，1]T中的x，y等价，根据相机成像的原理和已知的相机参数fx，fy，cx，cy，计算出：
X＝Z(u-cx)/fx (7)
Y＝Z(v-cy)/fy (8)
其中，
fx表示相机内参矩阵中的x方向焦距参数；
fy表示相机内参矩阵中的y方向焦距参数；
cx表示相机内参矩阵中的x方向图像中心偏移参数；
cy表示相机内参矩阵中的y方向图像中心偏移参数；
将公式(7)、(8)带入公式(6)式即可计算出深度信息Z，即图像上某个平面点所对应的三维空间中点的深度，进而获得三维坐标点。
2.根据权利要求1所述的基于直接法的视觉里程计方法，其特征在于，所述Lucas-Kanade光流法：
根据获得的三维坐标点，通过不同时刻相机所拍摄的图像来获取相机两个不同时刻的相对位姿，从而达到给出位置姿态预测值的效果，由于相机和载体是固定连接的，获得载体在这个时刻的位姿，相对位姿使用Ti(t)∈SE(3)，SE(3)指的是三阶的特殊欧式群，是一种李群，下标i表示这是第i个图像帧，t表示时刻；
Ti(t)给出了
Figure FDA0002892082600000031
因此当3D点云和图像上的2D点的对应关系被给出之后，求Ti(t)等效于一个优化问题，这个优化问题被表达成：
Figure FDA0002892082600000032
其中，
Ti(t)表示在t时刻第i个图像所对应的位置和姿态矩阵；
T表示优化变量，为图像的位置姿态矩阵；
ρ表示欧氏距离的平方，即x和π(TPWC，X)两个点的距离的平方，x是观测出的二维点，π(TPWC，X)是投影的二维点；
Figure FDA0002892082600000033
表示3D点和2D图像点的对应关系集合，
Figure FDA0002892082600000034
并且V是当前图像帧里面的特征点集合。
3.根据权利要求2所述的基于直接法的视觉里程计方法，其特征在于，所述位置姿态修正步骤包括：
保存参数步骤：保存双目相机的内参参数、外参参数、畸变参数，并且将左相机的中心设置为双目相机的中心点；
抽取特征点步骤：利用Shi-Tomas角点提取算法找出左相机图像中的角点xl，即左图像上检测出来的角点，使用Lucas-Kanade算法找出右相机图像中的对应点xr，即右图像上检测出来的角点，并计算出这些角点的特征描述子；
剔除双目匹配中的异常点对步骤：根据获得的点对，所述点对指xl和对应的xr，使用RANSAC算法计算单应性矩阵Hlr和基础矩阵Flr，并且使用下列公式剔除异常匹配点对：
xl＝Hlrxr，xlFlrxr＝0 (10)
当双目相机的图像场景是平面或者接近平面的状况下使用单应矩阵Hlr，但是当双目相机图像场景比较复杂，远近特征点都比较多的时候采用基础矩阵Flr来剔除异常点；
位置姿态的恢复步骤：当直接法视觉里程计丢失追踪的时，使用计算出的特征描述子匹配当前左相机图像和上个时刻的左相机图像中的特征点，根据公式(9)计算出当前的相对位置和姿态，恢复当前的位置姿态状态。
4.一种基于直接法的视觉里程计系统，其特征在于，包括：
亮度稳定模块：在高动态亮度变换环境下，使用对数图像增强方法使得场景中的亮度趋于稳定，提高对梯度信息的提取；
加快优化收敛模块：在快速旋转以及高速运动情况下，使用若干显著的梯度点和三角剖分算法生成深度预测面，有效给出点云的初始化深度，降低深度优化次数，加快优化收敛；
位置姿态修正模块：同时在降采样的图像上使用Lucas-Kanade光流法对若干显著梯度点进行匹配，计算相机姿态的初始值，在视觉里程计系统跟踪点云失败的时候，复原当前状态，并给出一个姿态预测值；
所述显著的梯度点指的是：图像中梯度大小大于预设值的点；
所述对数图像增强方法：
通过对原始图像进行对数变换，将图像低灰度部分进行扩展，并且将高灰度值部分进行压缩，增强低亮度光照下的图像纹理和抑制过曝光情况，所述对原始图像进行对数变换如下：
s＝c×logv+1(1+v×r) (1)
其中，
s表示对数变换后的图像像素亮度值；
c表示线性变换系数，为一个常数；
v表示对数变换的底数；
r表示原始图像中像素的亮度值；
所述三角剖分算法生成深度预测面：
在低分辨率的图像上提取出角点，并且利用双目三角化计算角点的深度，之后使用三角剖分算法，将角点连接成一个网络，相邻的三个点会组成一个三角形，形成一个平面，根据三角形的三个顶点的深度，计算出三角形内任意一点的深度，预测三角形内的梯度点深度，计算过程如下：
设获得的双目图像已经是去畸变的图像，并设世界坐标系为
Figure FDA0002892082600000051
相机坐标系表示为
Figure FDA0002892082600000052
相机的位姿表示成矩陌
Figure FDA0002892082600000053
该
Figure FDA0002892082600000054
矩阵包含了相机的旋转和平移，同时使用矩阵K来表示相机的内参，该K矩阵包括了相机的焦距和相机的中心点，三维世界的点投影在相平面，表达成如下表达式：
Figure FDA0002892082600000055
Figure FDA0002892082600000056
Figure FDA0002892082600000057
其中，
x＝[x，y，1]T，X＝[X，Y，Z，1]T分别表示二维坐标点和三维坐标点；
r表示将投影结果转换为齐次坐标的比例系数；
Kl和Kr分别表示双目相机中左相机和右相机的内参矩阵；
Figure FDA0002892082600000058
表示双目相机中左相机的外参矩阵；
Figure FDA0002892082600000059
表示双目相机中右相机的外参矩阵；
xl和xr分别表示双目相机系统的左右两幅不同图像中相对应的二维点；
通过双目相机系统的两幅图像，获得空间中的三维点坐标，表达式如下：
Figure FDA00028920826000000510
其中，
τ表示对双目图像的三角化函数；
使用已知的顶点深度和三角剖分结果来计算三角形中任意一点的深度，设三个点的坐标分别为pi＝(Xi，Yi，Zi)，i＝1，2，3，根据三维空间中的平面方程：
π1X+π2Y+π3Z+1＝0 (6)
其中，
X表示三角形顶点pi的x方向坐标；
Y表示三角形顶点pi的y方向坐标；
Z表示三角形顶点pi的z方向坐标；
将三角形的三个点的坐标带入该方程中，计算出平面方程的系数参数π1，π2，π3，从而确定整个平面，最后带入位置深度信息的梯度点在图像中的平面坐标u和v，u和v直接在图像中测量得到，分别和x＝[x，y，1]T中的x，y等价，根据相机成像的原理和已知的相机参数fx，fy，cx，cy，计算出：
X＝Z(u-cx)/fx (7)
Y＝Z(v-cy)/fy (8)
其中，
fx表示相机内参矩阵中的x方向焦距参数；
fy表示相机内参矩阵中的y方向焦距参数；
cx表示相机内参矩阵中的x方向图像中心偏移参数；
cy表示相机内参矩阵中的y方向图像中心偏移参数；
将公式(7)、(8)带入公式(6)式即可计算出深度信息Z，即图像上某个平面点所对应的三维空间中点的深度，进而获得三维坐标点；
所述Lucas-Kanade光流法：
根据获得的三维坐标点，通过不同时刻相机所拍摄的图像来获取相机两个不同时刻的相对位姿，从而达到给出位置姿态预测值的效果，由于相机和载体是固定连接的，获得载体在这个时刻的位姿，相对位姿使用Ti(t)∈SE(3)，SE(3)指的是三阶的特殊欧式群，是一种李群，下标i表示这是第i个图像帧，t表示时刻；
Ti(t)给出了
Figure FDA0002892082600000061
因此当3D点云和图像上的2D点的对应关系被给出之后，求Ti(t)等效于一个优化问题，这个优化问题被表达成：
Figure FDA0002892082600000062
其中，
Ti(t)表示在t时刻第i个图像所对应的位置和姿态矩阵；
T表示优化变量，为图像的位置姿态矩阵；
ρ表示欧氏距离的平方，即x和π(TPWC，X)两个点的距离的平方，x是观测出的二维点，π(TPWC，X)是投影的二维点；
Figure FDA0002892082600000063
表示3D点和2D图像点的对应关系集合，
Figure FDA0002892082600000064
并且V是当前图像帧里面的特征点集合。
5.根据权利要求4所述的基于直接法的视觉里程计系统，其特征在于，所述位置姿态修正模块包括：
保存参数模块：保存双目相机的内参参数、外参参数、畸变参数，并且将左相机的中心设置为双目相机的中心点；
抽取特征点模块：利用Shi-Tomas角点提取算法找出左相机图像中的角点xl，即左图像上检测出来的角点，使用Lucas-Kanade算法找出右相机图像中的对应点xr，即右图像上检测出来的角点，并计算出这些角点的特征描述子；
剔除双目匹配中的异常点对模块：根据获得的点对，所述点对指xl和对应的xr，使用RANSAC算法计算单应性矩阵Hlr和基础矩阵Flr，并且使用下列公式剔除异常匹配点对：
xl＝Hlrxr，xlFlrxr＝0 (10)
当双目相机的图像场景是平面或者接近平面的状况下使用单应矩阵Hlr，但是当双目相机图像场景比较复杂，远近特征点都比较多的时候采用基础矩阵Flr来剔除异常点；
位置姿态的恢复模块：当直接法视觉里程计丢失追踪的时，使用计算出的特征描述子匹配当前左相机图像和上个时刻的左相机图像中的特征点，根据公式(9)计算出当前的相对位置和姿态，恢复当前的位置姿态状态。
6.一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现权利要求1至3中任一项所述的基于直接法的视觉里程计方法的步骤。
CN201910575743.4A 2019-06-28 2019-06-28 基于直接法的视觉里程计方法、系统及存储介质 Active CN110375765B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910575743.4A CN110375765B (zh) 2019-06-28 2019-06-28 基于直接法的视觉里程计方法、系统及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910575743.4A CN110375765B (zh) 2019-06-28 2019-06-28 基于直接法的视觉里程计方法、系统及存储介质

Publications (2)

Publication Number Publication Date
CN110375765A CN110375765A (zh) 2019-10-25
CN110375765B true CN110375765B (zh) 2021-04-13

Family

ID=68251228

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910575743.4A Active CN110375765B (zh) 2019-06-28 2019-06-28 基于直接法的视觉里程计方法、系统及存储介质

Country Status (1)

Country Link
CN (1) CN110375765B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111080627B (zh) * 2019-12-20 2021-01-05 南京航空航天大学 一种基于深度学习的2d+3d大飞机外形缺陷检测与分析方法
CN111210463B (zh) * 2020-01-15 2022-07-15 上海交通大学 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及系统
CN112197766B (zh) * 2020-09-29 2023-04-28 西安应用光学研究所 一种针对系留旋翼平台的视觉测姿装置
CN113203407B (zh) * 2021-05-20 2024-01-02 南昌大学 基于关键平面的视觉惯性里程计方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104574293A (zh) * 2014-11-28 2015-04-29 中国科学院长春光学精密机械与物理研究所 基于有界运算的多尺度Retinex图像清晰化算法
CN107527366A (zh) * 2017-08-23 2017-12-29 上海视智电子科技有限公司 一种面向深度相机的相机跟踪方法
CN107564061A (zh) * 2017-08-11 2018-01-09 浙江大学 一种基于图像梯度联合优化的双目视觉里程计算方法
WO2018133119A1 (zh) * 2017-01-23 2018-07-26 中国科学院自动化研究所 基于深度相机进行室内完整场景三维重建的方法及系统
CN108615246A (zh) * 2018-04-19 2018-10-02 浙江大承机器人科技有限公司 提高视觉里程计系统鲁棒性和降低算法计算消耗的方法
CN109523589A (zh) * 2018-11-13 2019-03-26 浙江工业大学 一种更鲁棒的视觉里程计的设计方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104574293A (zh) * 2014-11-28 2015-04-29 中国科学院长春光学精密机械与物理研究所 基于有界运算的多尺度Retinex图像清晰化算法
WO2018133119A1 (zh) * 2017-01-23 2018-07-26 中国科学院自动化研究所 基于深度相机进行室内完整场景三维重建的方法及系统
CN107564061A (zh) * 2017-08-11 2018-01-09 浙江大学 一种基于图像梯度联合优化的双目视觉里程计算方法
CN107527366A (zh) * 2017-08-23 2017-12-29 上海视智电子科技有限公司 一种面向深度相机的相机跟踪方法
CN108615246A (zh) * 2018-04-19 2018-10-02 浙江大承机器人科技有限公司 提高视觉里程计系统鲁棒性和降低算法计算消耗的方法
CN109523589A (zh) * 2018-11-13 2019-03-26 浙江工业大学 一种更鲁棒的视觉里程计的设计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
StructSLAM: Visual SLAM With Building Structure Lines;Huizhong Zhou;《IEEE》;20150106;第64卷(第4期);1364-1375 *

Also Published As

Publication number Publication date
CN110375765A (zh) 2019-10-25

Similar Documents

Publication Publication Date Title
CN110375765B (zh) 基于直接法的视觉里程计方法、系统及存储介质
CN111210463B (zh) 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及系统
JP6768156B2 (ja) 仮想的に拡張された視覚的同時位置特定及びマッピングのシステム及び方法
CN112902953B (zh) 一种基于slam技术的自主位姿测量方法
CN106940704B (zh) 一种基于栅格地图的定位方法及装置
CN108629843B (zh) 一种实现增强现实的方法及设备
EP2656309B1 (en) Method for determining a parameter set designed for determining the pose of a camera and for determining a three-dimensional structure of the at least one real object
JP2023175052A (ja) ３次元空間内の姿勢推定
US20190026943A1 (en) Dense visual slam with probabilistic surfel map
CN108492316A (zh) 一种终端的定位方法和装置
CN108564652B (zh) 高效利用内存的高精度三维重建方法与系统及设备
US9940725B2 (en) Method for estimating the speed of movement of a camera
KR102206108B1 (ko) 체적형 객체 촬영을 위한 다중 rgb-d 카메라 기반의 포인트 클라우드 정합 방법
CN111462207A (zh) 一种融合直接法与特征法的rgb-d同时定位与地图创建方法
US11082633B2 (en) Method of estimating the speed of displacement of a camera
CN108776989A (zh) 基于稀疏slam框架的低纹理平面场景重建方法
CN109785373B (zh) 一种基于散斑的六自由度位姿估计系统及方法
CN113052907B (zh) 一种动态环境移动机器人的定位方法
CN114494150A (zh) 一种基于半直接法的单目视觉里程计的设计方法
CN112541423A (zh) 一种同步定位与地图构建方法和系统
CN112967340A (zh) 同时定位和地图构建方法、装置、电子设备及存储介质
CN110119189A (zh) Slam系统的初始化、ar控制方法、装置和系统
CN113345032B (zh) 一种基于广角相机大畸变图的初始化建图方法及系统
CN112200917A (zh) 一种高精度增强现实方法及系统
CN113011212B (zh) 图像识别方法、装置及车辆

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant