CN108151713A - 一种单目vo快速位姿估计方法 - Google Patents

一种单目vo快速位姿估计方法 Download PDF

Info

Publication number
CN108151713A
CN108151713A CN201711325142.5A CN201711325142A CN108151713A CN 108151713 A CN108151713 A CN 108151713A CN 201711325142 A CN201711325142 A CN 201711325142A CN 108151713 A CN108151713 A CN 108151713A
Authority
CN
China
Prior art keywords
matrix
characteristic
monocular
solution
estimation methods
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.)
Pending
Application number
CN201711325142.5A
Other languages
English (en)
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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201711325142.5A priority Critical patent/CN108151713A/zh
Publication of CN108151713A publication Critical patent/CN108151713A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/04—Interpretation of pictures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种单目VO快速位姿估计方法，其特征是，包括如下步骤：1)设置基础矩阵；2)利用相邻两帧得到的特征匹配集合中任意一对特征点的图像坐标的齐次坐标与上式的矩阵F；3)从相邻两帧特征匹配集合中任意选取7对特征点；4)求解步骤3)得到的非齐次线性方程组的解；5)求解基础矩阵F；6)采集摄像机的内参数，计算本质矩阵；7)利用奇异值分解从本质矩阵中得到旋转矩阵R和平移向量t。通过优化视觉里程计位姿估计模块基础矩阵的求解过程，在保证位姿估计精度的前提下，有效的提高算法的运行速度。

Description

一种单目VO快速位姿估计方法
技术领域
本发明涉及一种单目VO快速位姿估计方法，属于视觉导航技术领域。
背景技术
无人驾驶车辆一般是通过在车上安装多种感应设备，主要包括各种车载传感器、GPS、雷达以及摄像机等，来感知周围的环境，并根据所获取的信息，自动规划路径实现车辆的自主驾驶，安全可靠地到达目的地。实时准确的定位方法是实现无人驾驶技术的基础，是保障无人驾驶车辆完成自主行为的前提。随着视觉技术不断发展，视觉里程计技术已被广泛应用，比如移动机器人、无人机、卫星或水下探测器以及工厂AGV(Automated GuidedVehicle)，同时也越来越多的被应用于无人驾驶车辆的自主定位和运动估计。
发明内容
为解决现有技术的不足，本发明的目的在于提供一种单目VO快速位姿估计方法，在保证定位精度的前提下，可以有效的提高位姿估计算法的运行速度。
为了实现上述目标，本发明采用如下的技术方案：
一种单目VO快速位姿估计方法，其特征是，包括如下步骤：
1)设置基础矩阵
2)利用相邻两帧得到的特征匹配集合中任意一对特征点的图像坐标的齐次坐标与上式的矩阵F，得到
3)从相邻两帧特征匹配集合中任意选取7对特征点，叠加7对特征点得到如下方程组：其中，矩阵Q7×8中左上标(i),i＝1,2，…，7表示第i对匹配特征点，
4)求解步骤3)得到的非齐次线性方程组的解η*，且有ξ为非齐次方程的基础解系中的解向量；
5)将步骤4)中得到的解写成步骤1)中所示的矩阵形式有由基础矩阵是奇异矩阵的性质可知，矩阵F 的秩为2，即其行列式det(F)＝0，对其求解可以计算得到x的值以及基础矩阵F，
6)采集摄像机的内参数，由步骤1)中所示的矩阵计算本质矩阵 E＝Min T·F·Min；
7)利用奇异值分解从本质矩阵E中得到旋转矩阵R和平移向量t。
前述的一种单目VO快速位姿估计方法，其特征是，所述步骤2)中将等式展开后重写为如下的线性非齐次方程为：其中，
前述的一种单目VO快速位姿估计方法，其特征是，所述步骤7)中利用本质矩阵E的两个非零奇异值相等的性质进行奇异值分解，并对得到的若干组旋转矩阵R和平移向量t进行判定是否是真实。
前述的一种单目VO快速位姿估计方法，其特征是，所述本质矩阵E的奇异值分解为E＝Udiag(1,1,0)VT，得到四组不同的旋转矩阵R和平移向量t：其中，U3表示矩阵U的第三行。
前述的一种单目VO快速位姿估计方法，其特征是，所述步骤7)中判定准则是：判断该交点是否在摄像机的前方，若交点在两摄像机的前方，则得到的旋转矩阵R和平移向量t即为正确的。
本发明所达到的有益效果：本方法通过对基础矩阵的优化，在保证位姿估计精度的前提下，有效的提高算法的运行速度。
附图说明
图1是本发明的位姿估计示意图；
图2(a)(b)(c)(d)是步骤7)中本质矩阵中得到的位姿矩阵的四组解的示意图；
图3(a)是本方法与现有算法的旋转误差均值对比图；
图3(b)是本方法与现有算法的平移误差均值对比图；
图3(c)是本方法与现有算法的旋转误差中值对比图；
图3(d)是本方法与现有算法的平移误差中值对比图。
具体实施方式
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
本发明通过优化视觉里程计位姿估计模块基础矩阵的求解过程，在保证定位精度的前提下，可以有效的提高位姿估计算法的运行速度。
如图1所示，具体的步骤如下：
1)设置基础矩阵由于基础矩阵F在相差任意一个非零常数因子的条件下是唯一的，故可以令矩阵H中的元素f9＝1，则变形有
2)利用相邻两帧得到的特征匹配集合中任意一对特征点的图像坐标的齐次坐标与上式的矩阵F，得到将等式展开后重写为如下的线性非齐次方程为：其中，
其中，特征集合是两幅图像提取特征后进行特征关联得到的，有跟踪和匹配两种方式，特征匹配集合采用的就是匹配方式。特征匹配是在每帧图像中进行特征点提取，然后利用合适的相似度度量在两帧图像的所有特征点中进行比较，找到最佳匹配特征点对。
3)约束方程式中有8个未知参数(f1～f8)，从相邻两帧特征匹配集合中任意选取7对特征点，叠加7对特征点得到如下方程组：其中，矩阵Q7×8中左上标(i),i＝1,2，…，7表示第i对匹配特征点。
4)对于有7对匹配特征点形成的系数矩阵Q7×8的秩的7，所以其对应的非齐次方程的基础解系中只包含一个解向量，设为ξ，求解步骤3)得到的非齐次线性方程组的解η*，且有
5)将步骤4)中得到的解写成步骤1)中所示的矩阵形式有由基础矩阵是奇异矩阵的性质可知，矩阵F 的秩为2，即其行列式det(F)＝0，对其求解可以计算得到x的值以及基础矩阵F；
6)采集摄像机的内参数，由步骤1)中所示的矩阵计算本质矩阵 E＝Min T·F·Min；
7)利用奇异值分解从本质矩阵E中得到旋转矩阵R和平移向量t。但是，从矩阵E中得到的旋转矩阵R和平移向量t都存在投影歧义，故将从一个本质矩阵中得到四组不同的旋转矩阵R和平移向量t。如图2所示。
利用本质矩阵E的两个非零奇异值相等的性质进行奇异值分解，分解为E＝U diag(1,1,0)VT，得到四组不同的旋转矩阵R和平移向量t：其中，U3表示矩阵U的第三行，并对得到的若干组旋转矩阵R和平移向量t进行判定是否是真实。摄像机的内参数已经提前标定得知，利用摄像机内参数和得到的旋转矩阵R和平移向量t，可以得到成像点在三维空间中的射线。通过特征匹配集合中任意一对特征点可以求得两条射线交点的三维坐标，然后判断该交点是否在摄像机的前方。若交点在两摄像机的前方，则得到的旋转矩阵R和平移向量t即为正确的。从图2中可以看出，只有图2(a)所示的两摄像机的位置才是视觉里程计***所求的真实结果。
下面通过将现有的集中算法与本发明的方法进行比较，以突显本发明的进步性：
Nistér的5点算法、基于车辆运动学模型的位姿估计单点算法、MYP算法以及本文中提出算法分别的迭代次数如表1所示。
表1各种算法的迭代次数
由于RANSAC算法是一种概率算法，具有不确定性，它只是有一定的概率可以得到正确的模型假设。为了提高这个概率就需要增加迭代次数。
所以，处于算法鲁棒性的考虑，需要将其最小迭代次数放大一定的倍数，如表1所示。
表2四种单目视觉位姿估计算法运行时间比较
通过表1和表2的内容，能够看出本方法通过对基础矩阵的优化，在保证位姿估计精度的前提下，有效的提高算法的运行速度，与现有的算法相比，其运行速度的提升是巨大的，而且本方法在同样的条件下，迭代次数更高，精度更精确。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (5)

1.一种单目VO快速位姿估计方法，其特征是，包括如下步骤：
1)设置三阶基础矩阵
2)利用相邻两帧得到的特征匹配集合中任意一对特征点的图像坐标的齐次坐标与上式的矩阵F，得到表示k时刻得到的特征匹配集合中特征点的图像坐标的齐次坐标左乘摄像机内参数矩阵Min的逆矩阵得到的图像点的球面归一化坐标，表示k-1时刻得到的特征匹配集合中特征点图像坐标的齐次坐标左乘摄像机内参数矩阵的逆矩阵得到的图像点的球面归一化坐标；
3)从相邻两帧特征匹配集合中任意选取7对特征点，叠加7对特征点得到如下方程组：其中，矩阵Q7×8中左上标(i)，i＝1，2，…，7，表示第i对匹配特征点，
4)求解步骤3)得到的非齐次线性方程组的解η*，且有ξ为非齐次方程的基础解系中的解向量；
5)将步骤4)中得到的解写成步骤1)中所示的矩阵形式有由基础矩阵是奇异矩阵的性质可知，矩阵F的秩为2，即其行列式det(F)＝0，对其求解可以计算得到x的值以及基础矩阵F，下标i指的是第i对匹配特征点，一共有7对，η*为非齐次线性方程组的解；ξ为非齐次方程的基础解系中的解向量；
6)采集摄像机的内参数，由步骤1)中所示的矩阵计算本质矩阵E＝Min T·F·Min，Min为相机的内参数矩阵；
7)利用奇异值分解从本质矩阵E中得到旋转矩阵R和平移向量t。
2.根据权利要求1所述的一种单目VO快速位姿估计方法，其特征是，所述步骤2)中将等式展开后重写为如下的线性非齐次方程为：其中，
3.根据权利要求1所述的一种单目VO快速位姿估计方法，其特征是，所述步骤7)中利用本质矩阵E的两个非零奇异值相等的性质进行奇异值分解，并对得到的若干组旋转矩阵R和平移向量t进行判定是否是真实。
4.根据权利要求3所述的一种单目VO快速位姿估计方法，其特征是，所述本质矩阵E的奇异值分解为E＝Udiag(1,1,0)VT，得到四组不同的旋转矩阵R和平移向量t：其中，U3表示矩阵U的第三行。
5.根据权利要求3所述的一种单目VO快速位姿估计方法，其特征是，所述步骤7)中判定准则是：判断该交点是否在摄像机的前方，若交点在两摄像机的前方，则得到的旋转矩阵R和平移向量t即为正确的。
CN201711325142.5A 2017-12-13 2017-12-13 一种单目vo快速位姿估计方法 Pending CN108151713A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711325142.5A CN108151713A (zh) 2017-12-13 2017-12-13 一种单目vo快速位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711325142.5A CN108151713A (zh) 2017-12-13 2017-12-13 一种单目vo快速位姿估计方法

Publications (1)

Publication Number Publication Date
CN108151713A true CN108151713A (zh) 2018-06-12

Family

ID=62466204

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711325142.5A Pending CN108151713A (zh) 2017-12-13 2017-12-13 一种单目vo快速位姿估计方法

Country Status (1)

Country Link
CN (1) CN108151713A (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109737950A (zh) * 2019-01-28 2019-05-10 黑龙江迈普斯企业管理中心(有限合伙) 基于vio和动力学模型的水下机器人定位***及方法
CN110689577A (zh) * 2019-09-30 2020-01-14 深圳市瑞立视多媒体科技有限公司 单相机环境中主动式刚体的位姿定位方法及相关设备
CN110728245A (zh) * 2019-10-17 2020-01-24 珠海格力电器股份有限公司 用于vslam前端处理的优化方法、装置、电子设备及存储介质
CN110910453A (zh) * 2019-11-28 2020-03-24 魔视智能科技(上海)有限公司 基于无重叠视域多相机***的车辆位姿估计方法及其***
CN111739072A (zh) * 2020-06-22 2020-10-02 浙江大华技术股份有限公司 像素点的匹配方法及装置、存储介质和电子装置
CN112798812A (zh) * 2020-12-30 2021-05-14 中山联合汽车技术有限公司 基于单目视觉的目标测速方法
CN113029128A (zh) * 2021-03-25 2021-06-25 浙江商汤科技开发有限公司 视觉导航方法及相关装置、移动终端、存储介质
CN113379840A (zh) * 2021-06-10 2021-09-10 北京航空航天大学 一种基于共面目标的单目视觉位姿估计方法
CN113676696A (zh) * 2020-05-14 2021-11-19 杭州萤石软件有限公司 一种目标区域的监控方法、***

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102435188A (zh) * 2011-09-15 2012-05-02 南京航空航天大学 一种用于室内环境的单目视觉/惯性全自主导航方法
CN104180818A (zh) * 2014-08-12 2014-12-03 北京理工大学 一种单目视觉里程计算装置
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102435188A (zh) * 2011-09-15 2012-05-02 南京航空航天大学 一种用于室内环境的单目视觉/惯性全自主导航方法
CN104180818A (zh) * 2014-08-12 2014-12-03 北京理工大学 一种单目视觉里程计算装置
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
曾庆喜: ""无人驾驶车辆单目视觉里程计快速位姿估计"", 《河北科技大学学报》 *

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109737950A (zh) * 2019-01-28 2019-05-10 黑龙江迈普斯企业管理中心(有限合伙) 基于vio和动力学模型的水下机器人定位***及方法
CN109737950B (zh) * 2019-01-28 2020-04-17 智真海洋科技(威海)有限公司 基于vio和动力学模型的水下机器人定位***及方法
CN110689577A (zh) * 2019-09-30 2020-01-14 深圳市瑞立视多媒体科技有限公司 单相机环境中主动式刚体的位姿定位方法及相关设备
CN110689577B (zh) * 2019-09-30 2022-04-01 深圳市瑞立视多媒体科技有限公司 单相机环境中主动式刚体的位姿定位方法及相关设备
CN110728245A (zh) * 2019-10-17 2020-01-24 珠海格力电器股份有限公司 用于vslam前端处理的优化方法、装置、电子设备及存储介质
CN110910453A (zh) * 2019-11-28 2020-03-24 魔视智能科技(上海)有限公司 基于无重叠视域多相机***的车辆位姿估计方法及其***
CN110910453B (zh) * 2019-11-28 2023-03-24 魔视智能科技(上海)有限公司 基于无重叠视域多相机***的车辆位姿估计方法及其***
CN113676696A (zh) * 2020-05-14 2021-11-19 杭州萤石软件有限公司 一种目标区域的监控方法、***
CN111739072A (zh) * 2020-06-22 2020-10-02 浙江大华技术股份有限公司 像素点的匹配方法及装置、存储介质和电子装置
CN112798812A (zh) * 2020-12-30 2021-05-14 中山联合汽车技术有限公司 基于单目视觉的目标测速方法
CN112798812B (zh) * 2020-12-30 2023-09-26 中山联合汽车技术有限公司 基于单目视觉的目标测速方法
CN113029128A (zh) * 2021-03-25 2021-06-25 浙江商汤科技开发有限公司 视觉导航方法及相关装置、移动终端、存储介质
CN113029128B (zh) * 2021-03-25 2023-08-25 浙江商汤科技开发有限公司 视觉导航方法及相关装置、移动终端、存储介质
CN113379840A (zh) * 2021-06-10 2021-09-10 北京航空航天大学 一种基于共面目标的单目视觉位姿估计方法
CN113379840B (zh) * 2021-06-10 2023-02-28 北京航空航天大学 一种基于共面目标的单目视觉位姿估计方法

Similar Documents

Publication Publication Date Title
CN108151713A (zh) 一种单目vo快速位姿估计方法
CN108717712B (zh) 一种基于地平面假设的视觉惯导slam方法
US11988781B2 (en) Extrinsic calibration method of multiple 3D LiDAR sensors for autonomous navigation system
CN108303099B (zh) 基于三维视觉slam的无人机室内自主导航方法
CN108051002B (zh) 基于惯性测量辅助视觉的运输车空间定位方法及***
CN105809687B (zh) 一种基于图像中边沿点信息的单目视觉测程方法
Levinson et al. Automatic online calibration of cameras and lasers.
CN104732518B (zh) 一种基于智能机器人地面特征的ptam改进方法
CN104204721B (zh) 单个相机距离估计
CN108225327B (zh) 一种顶标地图的构建与定位方法
Zhang et al. Robust appearance based visual route following for navigation in large-scale outdoor environments
CN105469405A (zh) 基于视觉测程的同时定位与地图构建方法
Muñoz-Bañón et al. Targetless camera-LiDAR calibration in unstructured environments
CN108932477A (zh) 一种巡检机器人充电房视觉定位方法
CN113570662B (zh) 3d定位来自真实世界图像中地标的***和方法
CN105096341A (zh) 基于三焦张量和关键帧策略的移动机器人位姿估计方法
Rehder et al. Submap-based SLAM for road markings
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
Hakeem et al. Estimating geospatial trajectory of a moving camera
CN116128966A (zh) 一种基于环境物体的语义定位方法
CN109764864B (zh) 一种基于颜色识别的室内无人机位姿获取方法及***
CN117115271A (zh) 无人机飞行过程中的双目相机外参数自标定方法及***
CN111862146B (zh) 一种目标对象的定位方法及装置
CN113673462B (zh) 一种基于车道线的物流agv定位方法
Ye et al. Robust and efficient vehicles motion estimation with low-cost multi-camera and odometer-gyroscope

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20180612