CN108151713A - 一种单目vo快速位姿估计方法 - Google Patents
一种单目vo快速位姿估计方法 Download PDFInfo
- 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
- monocular
- pose estimation
- equation
- homogeneous
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 239000011159 matrix material Substances 0.000 claims abstract description 73
- 239000013598 vector Substances 0.000 claims abstract description 20
- 238000000354 decomposition reaction Methods 0.000 claims abstract description 9
- 239000000243 solution Substances 0.000 claims description 14
- 239000002585 base Substances 0.000 claims description 4
- 239000003637 basic solution Substances 0.000 claims description 3
- 239000012456 homogeneous solution Substances 0.000 claims 1
- 230000000007 visual effect Effects 0.000 abstract description 4
- 238000010586 diagram Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
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快速位姿估计方法，属于视觉导航技术领域。
背景技术
无人驾驶车辆一般是通过在车上安装多种感应设备，主要包括各种车载传感器、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即为正确的。
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 (10)
| 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 | 杭州萤石软件有限公司 | 一种目标区域的监控方法、系统 |
| CN111739072B (zh) * | 2020-06-22 | 2024-10-29 | 浙江大华技术股份有限公司 | 像素点的匹配方法及装置、存储介质和电子装置 |
Citations (3)
| 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 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
-
2017
- 2017-12-13 CN CN201711325142.5A patent/CN108151713A/zh active Pending
Patent Citations (3)
| 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)
| Title |
|---|
| 曾庆喜: ""无人驾驶车辆单目视觉里程计快速位姿估计"", 《河北科技大学学报》 * |
Cited By (16)
| 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和动力学模型的水下机器人定位系统及方法 |
| CN110689577B (zh) * | 2019-09-30 | 2022-04-01 | 深圳市瑞立视多媒体科技有限公司 | 单相机环境中主动式刚体的位姿定位方法及相关设备 |
| CN110689577A (zh) * | 2019-09-30 | 2020-01-14 | 深圳市瑞立视多媒体科技有限公司 | 单相机环境中主动式刚体的位姿定位方法及相关设备 |
| CN110728245A (zh) * | 2019-10-17 | 2020-01-24 | 珠海格力电器股份有限公司 | 用于vslam前端处理的优化方法、装置、电子设备及存储介质 |
| CN110910453B (zh) * | 2019-11-28 | 2023-03-24 | 魔视智能科技(上海)有限公司 | 基于无重叠视域多相机系统的车辆位姿估计方法及其系统 |
| CN110910453A (zh) * | 2019-11-28 | 2020-03-24 | 魔视智能科技(上海)有限公司 | 基于无重叠视域多相机系统的车辆位姿估计方法及其系统 |
| CN113676696A (zh) * | 2020-05-14 | 2021-11-19 | 杭州萤石软件有限公司 | 一种目标区域的监控方法、系统 |
| CN111739072A (zh) * | 2020-06-22 | 2020-10-02 | 浙江大华技术股份有限公司 | 像素点的匹配方法及装置、存储介质和电子装置 |
| CN111739072B (zh) * | 2020-06-22 | 2024-10-29 | 浙江大华技术股份有限公司 | 像素点的匹配方法及装置、存储介质和电子装置 |
| 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快速位姿估计方法 | |
| US11988781B2 (en) | Extrinsic calibration method of multiple 3D LiDAR sensors for autonomous navigation system | |
| CN104299244B (zh) | 基于单目相机的障碍物检测方法及装置 | |
| CN106056643B (zh) | 一种基于点云的室内动态场景slam方法及系统 | |
| CN105856230A (zh) | 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法 | |
| Muñoz-Bañón et al. | Targetless camera-LiDAR calibration in unstructured environments | |
| Parra et al. | Robust visual odometry for vehicle localization in urban environments | |
| CN104732518A (zh) | 一种基于智能机器人地面特征的ptam改进方法 | |
| CN112183171A (zh) | 一种基于视觉信标建立信标地图方法、装置 | |
| CN108917753B (zh) | 基于从运动恢复结构的飞行器位置确定方法 | |
| CN108229416A (zh) | 基于语义分割技术的机器人slam方法 | |
| CN111738032B (zh) | 一种车辆行驶信息确定方法及装置、车载终端 | |
| CN113570662B (zh) | 3d定位来自真实世界图像中地标的系统和方法 | |
| Fan et al. | Dynamicfilter: an online dynamic objects removal framework for highly dynamic environments | |
| CN114549549B (zh) | 一种动态环境下基于实例分割的动态目标建模跟踪方法 | |
| Han et al. | Robust ego-motion estimation and map matching technique for autonomous vehicle localization with high definition digital map | |
| Akai | Mobile robot localization considering uncertainty of depth regression from camera images | |
| CN112435294B (zh) | 目标物体的六自由度姿态跟踪方法及终端设备 | |
| CN116681733B (zh) | 一种空间非合作目标近距离实时位姿跟踪方法 | |
| Suzuki et al. | SLAM using ICP and graph optimization considering physical properties of environment | |
| CN114323038B (zh) | 融合双目视觉和2d激光雷达的室外定位方法 | |
| Ye et al. | Robust and efficient vehicles motion estimation with low-cost multi-camera and odometer-gyroscope | |
| CN114462545A (zh) | 一种基于语义slam的地图构建方法及装置 | |
| Zhao et al. | L-VIWO: Visual-Inertial-Wheel Odometry based on Lane Lines | |
| Kang et al. | A Vision-based Forward Driving Vehicle Velocity Estimation Algorithm for Autonomous Vehicles |
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 |