CN114004900A - 一种基于点线面特征的室内双目视觉里程计方法 - Google Patents

一种基于点线面特征的室内双目视觉里程计方法 Download PDF

Info

Publication number
CN114004900A
CN114004900A CN202111360538.XA CN202111360538A CN114004900A CN 114004900 A CN114004900 A CN 114004900A CN 202111360538 A CN202111360538 A CN 202111360538A CN 114004900 A CN114004900 A CN 114004900A
Authority
CN
China
Prior art keywords
line
formula
representing
frame image
manhattan
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
CN202111360538.XA
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.)
Hefei Institutes of Physical Science of CAS
Original Assignee
Hefei Institutes of Physical Science of CAS
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 Hefei Institutes of Physical Science of CAS filed Critical Hefei Institutes of Physical Science of CAS
Priority to CN202111360538.XA priority Critical patent/CN114004900A/zh
Publication of CN114004900A publication Critical patent/CN114004900A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G06T7/85—Stereo camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G06F18/232—Non-hierarchical techniques
    • G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
    • G06F18/23213—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions with fixed number of clusters, e.g. K-means clustering
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/253—Fusion techniques of extracted features
    • 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

Abstract

本发明公开了一种基于点线面特征的双目视觉里程计方法，针对于室内结构化环境做出曼哈顿世界假设，利用相交线对的参数计算面特征和高斯球面均值漂移聚类算法优化曼哈顿世界坐标系，以此基于线面特征实现无漂移旋转估计，基于点线特征的重投影函数估计平移矩阵以此获取最优相机位姿。本发明针对于室内结构化环境，解决低纹理、低语义和光曝强的人造结构化环境下视觉SLAM系统信息利用率低、计算代价高昂和重定位失准和等问题，从而能提高系统鲁棒性、实时性和定位精度。

Description

一种基于点线面特征的室内双目视觉里程计方法
技术领域
本发明属于移动机器人同步定位与地图构建技术领域，涉及一种多特征融合的双目视觉里程计方法。
背景技术
视觉SLAM通过相机作为环境信息读取设备，使机器人在运动过程中进行自身定位并且创建环境地图，同时对地图进行实时修正消除误差。机器人运动时通过SLAM所构建地图的质量直接对此之后的路径规划、导航定位以及避障等应用起着决定性的作用。此项技术已被证明在广泛的应用中起到决定性作用，例如移动机器人、自动驾驶汽车和增强/虚拟现实，其中准确的相机位姿估计使汽车、机器人等移动设备能够自身定位，构建地图提供环境信息用于机器人与环境和机器人与人的交互。
在现有的研究成果中，主流的视觉SLAM方法之一为特征点法，通过环境提供的特征点信息进行匹配和数据关联估计相机位姿。近些年的研究中逐渐加入线特征信息以提升系统稳定性。而大多数SLAM方法仅只是使用点线特征进行定位追踪，即使两种特征已经能承载大多人造结构化场景中的结构信息，但是低纹理、低语义环境仍存在特征分布不均导致的定位精度低、实时性差和系统鲁棒性低等问题。针对于此，文献“Yanyan Li等人，Structure-SLAM:Low-Drift Monocular SLAM in Indoor Environments.IEEE Roboticsand Automation Letters(RAL),2020”充分利用结构化场景现有的几何信息，在点线特征的基础上加入面特征以实现相机跟踪过程的无漂移旋转估计。但输入图像帧的平面法线估计计算依靠卷积神经网络实现，一定程度上需要付出高昂的计算代价，影响系统轻量化程度。文献“Xiaoyu Zhang等人，Stereo Plane SLAM Based on Intersecting Lines.IEEEConference on Computer Vision and Pattern Recognition(CVPR),2020”提出通过双目相机在图像帧中基于相交线特征端点和方向向量参数化面特征的新方法，计算过程剔除估计错误的面特征使得漂移误差减小，系统性能有所提升。但此系统相机位姿估计仍基于传统方法，存在帧间平面特征数据关联计算量大，稳定性差的问题。
发明内容
本发明是为了解决上述现有技术存在的不足之处，提出基于点线面特征的室内双目视觉里程计方法，以期针对于室内结构化环境，能解决低纹理、低语义和光曝强的人造结构化环境下视觉SLAM系统信息利用率低、计算代价高昂和重定位失准和等问题，从而能提高系统鲁棒性、实时性和定位精度。
为达到上述目的，本发明提供如下技术方案：
本发明一种基于点线面特征的室内双目视觉里程计方法的特点在于，包括以下步骤：
步骤1、以双目相机为信息读取设备，标定所述双目相机的内参后对所采集的图像帧进行预处理，得到去噪后的第k帧图像；
步骤2、对于所述去噪后的第k帧图像提取ORB点特征；再采用改进的LSD算法提取所述去噪后的第k帧图像中的线特征，筛选出线特征中的相交线对；
步骤3、计算相交线对的端点3D位置(Pstart,Pend)和方向向量nl；
步骤4、根据所述相交线对的端点3D位置和方向向量获取面特征的参数；
步骤5、对双目相机所处的室内环境做出曼哈顿世界假设，以利用位姿估计解耦算法将所述双目相机的运动过程中的旋转和平移矩阵分别解算：
步骤5.1、基于曼哈顿世界假设估计旋转矩阵；
步骤5.2、构建基于ORB点特征和线特征的重投影误差函数，用于估计平移矩阵，从而估计双目相机帧间的运动位姿；
步骤6、判定经过跟踪线程预处理的第k帧图像是否为关键帧，若是，则用于构建局部地图，否则，返回步骤1。
本发明所述的一种基于点线面特征的室内双目视觉里程计方法的特点也在于，所述步骤4是按如下过程获取面特征的参数：
步骤4.1、利用式(1)得到相交线对的两线夹角θ，判定两线夹角θ是否大于阈值α；若是，则执行步骤4.2；否则返回步骤3；
Figure BDA0003359076270000021
式(1)中，nl1表示相交线对中线段1的方向向量，nl2表示相交线对中线段2的方向向量；
步骤4.2、判断相交线对中两线中心点的距离是否小于相交线对中任一线段的长度；若是，则执行步骤4.3；否则返回步骤3；
步骤4.3、判断相交线对中两线的四个端点是否在同一平面：
步骤4.3.1、利用式(2)得到所述相交线对所构成平面的法向量nπ：
nπ＝nl1×nl2 (2)
步骤4.3.2、根据平面法向量nπ和相交线对中的e个端点
Figure BDA0003359076270000022
利用式(3)计算e个平面系数
Figure BDA0003359076270000031
de＝-nπ(xe,ye,ze)T (3)
式(3)中，de表示第e个平面系数；(xe,ye,ze)表示第e个端点Pe的3D空间位置；
步骤4.3.3、利用式(4)计算双目相机到相交线对所构成平面的深度估计值
Figure BDA0003359076270000032
Figure BDA0003359076270000033
步骤4.3.4、利用式(5)得到最大平面系数差D：
D＝max(de)-min(de) (5)
步骤4.3.5、若最大平面系数差D小于阈值N，则判断两线的四个端点
Figure BDA0003359076270000034
存在于同一平面，并利用式(6)表示面特征的参数π，否则，表示四个端点
Figure BDA0003359076270000035
不在同一平面上；
Figure BDA0003359076270000036
所述步骤5.1包括：
步骤5.1.1、建立曼哈顿世界坐标系M，通过高斯映射将所述平面法向量nπ投影至单位高斯球上；
步骤5.1.2、将所述法向量nπ再投影到曼哈顿世界坐标系M坐标轴的2D切平面上，所得到的投影向量记为nπ·project；
步骤5.1.3、在2D切平面上对投影向量nπ·project进行mean-shift聚类，从而利用式(7)得到聚类均值向量n′mean：
Figure BDA0003359076270000037
式(7)中，n表示第k帧图像中的平面法向量总数，g()表示高斯核函数；
步骤5.1.4、将聚类均值向量n′mean终点重投影至高斯球面上，记为s；s与所述高斯球球心连接的向量称为重投影向量，记为nmean；
步骤5.1.5、利用式(8)得到所述双目相机的当前旋转矩阵Rcurrent：
Figure BDA0003359076270000041
式(8)中，nmean,r1表示曼哈顿世界坐标系M的第1个坐标轴r1，
Figure BDA0003359076270000042
表示曼哈顿世界坐标系M的第2个坐标轴r2，
Figure BDA0003359076270000043
表示曼哈顿世界坐标系M的第3个坐标轴r3；
步骤5.1.6、根据所述双目相机的当前旋转矩阵Rcurrent和重投影向量nmean，利用式(9)来更新曼哈顿坐标系M的三个坐标轴
Figure BDA0003359076270000044
Figure BDA0003359076270000045
式(9)中，ra表示更新后的曼哈顿坐标系M的第a个坐标轴；
Figure BDA0003359076270000046
表示更新前的曼哈顿世界坐标系M的第a个坐标轴；
步骤5.1.7、按照步骤5.1.3-步骤5.1.6的过程迭代计算重投影向量nmean，直到式(9)收敛，从而利用式(10)得到所述双目相机的第k帧图像与曼哈顿世界坐标系M之间的最优旋转矩阵
Figure BDA0003359076270000047
Figure BDA0003359076270000048
式(10)中，r1 *表示最终更新的曼哈顿坐标系M的第1个坐标轴，
Figure BDA0003359076270000049
表示最终更新的曼哈顿坐标系M的第2个坐标轴，
Figure BDA00033590762700000410
表示最终更新的曼哈顿坐标系M的第3个坐标轴；
步骤5.1.8、按照步骤1-步骤5.1.7的过程求得第k+1帧图像与曼哈顿坐标系M的最优旋转矩阵
Figure BDA00033590762700000411
步骤5.1.9、利用式(11)计算相邻两帧的旋转矩阵
Figure BDA00033590762700000412
Figure BDA00033590762700000413
所述步骤5.2包括：
步骤5.2.1、利用式(12)定义第k帧图像中ORB点特征集合的重投影误差函数ek,o：
ek,o＝pk,o-Π(Rk,k-1Pk,o+tk,k-1) (12)
式(12)中，Π()表示重投影函数，pk,o表示第k帧图像中ORB点特征的投影位置，Pk,o表示第k帧图像中ORB点特征的3D观测位置，Rk,k-1表示所述双目相机从第k-1帧图像运动至第k帧图像的旋转矩阵，tk,k-1表示所述双目相机从第k-1帧图像运动至第k帧图像的平移矩阵；
步骤5.2.2、根据第k帧图像中线特征的端点3D位置，利用式(13)得到归一化的线特征函数L：
Figure BDA0003359076270000051
式(13)中，Pstart表示线特征起点3D位置，Pend表示线特征终点3D位置；
步骤5.2.3、利用式(14)得到线特征端点Px的误差函数ek,l：
ek,l＝L·Π(Rk,k-1Pk,x+tk,k-1) (14)
式(14)中，Pk,x表示第k帧图像中线特征端点x的3D观测位置；
步骤5.2.4、假设观测值服从高斯分布，则利用式(15)构建目标函数ψ*：
Figure BDA0003359076270000052
式(15)中，
Figure BDA0003359076270000053
表示局部关键帧集，
Figure BDA0003359076270000054
表示第k帧图像中ORB点特征集，
Figure BDA0003359076270000055
表示第k帧图像中线特征集，
Figure BDA0003359076270000056
表示第k帧图像中ORB点特征的协方差矩阵，
Figure BDA0003359076270000057
表示第k帧图像中线特征的协方差矩阵；
步骤5.2.5、利用Levenberg-Marquardt优化方法对所述目标函数ψ*进行迭代更新，从而得到最优平移矩阵，用于估计双目相机帧间的运动位姿。
与现有技术相比，本发明的有益效果在于：
1)本发明充分利用室内结构化环境中的点、线、面几何特征集，并合理做出曼哈顿世界假设，通过线面特征的数据关联，理论上实现无漂移旋转矩阵估计，解决累积误差主要来自旋转漂移的问题，同时优化相机平移矩阵的估计；有效保证在低纹理、低语义和动态曝光环境下相机运动过程中跟踪稳定，重定位准确快速；
2)基于本发明可以很好的在公开室内实验数据集上达到高效准确的建图效果，得利于多种特征的检测与提取，不仅反映机器人运动过程中周边环境的空间结构特征，丰富其所构建环境地图的特征信息，经其可协助处理完成各种评价、预测、规划与决策。
3)本发明在特征参数化方面，直接利用必要的线特征参数来计算参数化面特征，避免了通过卷积神经网络估计平面法向量，降低了实际处理过程中高昂的计算成本，保证了本发明的实时性、系统的轻量化和小型化。
附图说明
图1为本发明室内双目视觉里程计方法的流程示意图；
图2为本发明平面特征示意图；
图3为本发明曼哈顿世界坐标系示意图；
图4为本发明平面法向量投影示意图；
图5为本发明mean-shift聚类示意图；
图6为本发明双目相机运动模型示意图。
具体实施方式
本实施例中，如图1所示，一种基于点线面特征的室内双目视觉里程计方法，在Planar-SLAM系统的结构基础上进行改进，采用原系统相同的主体结构，主要针对系统前端跟踪线程采用位姿估计解耦算法，对室内结构化环境做出曼哈顿世界假设，利用相交线对的参数计算面特征和高斯球面均值漂移聚类算法优化曼哈顿世界坐标系，以此基于线面特征实现无漂移旋转估计，基于点线特征的重投影函数估计平移矩阵以此获取最优相机位姿。具体的说，包括以下步骤：
步骤1、以双目相机为信息读取设备，标定双目相机的内参后对所采集的图像帧进行预处理，得到去噪后的第k帧图像；
步骤2、对于去噪后的第k帧图像提取ORB点特征；再采用改进的LSD算法提取去噪后的第k帧图像中的线特征，筛选出线特征中的相交线对；
步骤3、计算相交线对的端点3D位置(Pstart,Pend)和方向向量nl；
步骤4、根据相交线对的端点3D位置和方向向量获取面特征的参数的过程如图2所示，涉及参数主要包括两线夹角θ，两线中心点距离dc1·c2，平面系数
Figure BDA0003359076270000061
具体获取面特征参数过程按如下：
步骤4.1、通过相交线对的方向向量nl，利用式(1)求得相交线对的两线夹角θ，判定两线夹角θ是否大于阈值α，若是，则执行步骤4.2；否则返回步骤3；
Figure BDA0003359076270000062
式(1)中，nl1表示相交线对中线段1的方向向量，nl2表示相交线对中线段2的方向向量；
步骤4.2、判断相交线对中两线中心点的距离是否小于相交线对中任一线段的长度，如图2所示，相交线对中心点pc1，pc2的距离L必须大于任一线段的长度；若是，则执行步骤4.3；否则返回步骤3；
步骤4.3、判断相交线对中两线的四个端点是否在同一平面，主要通过计算四个平面系数
Figure BDA0003359076270000071
和其最大平面系数差D做出判断：
步骤4.3.1、通过相交线对方向向量的叉积，利用式(2)得到相交线对所构成平面的法向量nπ：
nπ＝nl1×nl2 (2)
步骤4.3.2、根据平面法向量nπ和相交线对中的e个端点
Figure BDA0003359076270000072
利用式(3)计算e个平面系数
Figure BDA0003359076270000073
de＝-nπ(xe,ye,ze)T (3)
式(3)中，de表示第e个平面系数；(xe,ye,ze)表示第e个端点Pe的3D空间位置；
步骤4.3.3、利用式(4)计算双目相机到相交线对所构成平面的深度估计值
Figure BDA0003359076270000074
Figure BDA0003359076270000075
步骤4.3.4、利用式(5)得到最大平面系数差D：
D＝max(de)-min(de) (5)
步骤4.3.5、若最大平面系数差D小于阈值N，则判断两线的四个端点
Figure BDA0003359076270000076
存在于同一平面，并利用式(6)表示面特征的参数π；否则，表示四个端点不在同一平面上；
Figure BDA0003359076270000077
步骤5、充分利用室内人造结构化环境中存在的点线面特征，对双目相机所处的室内人造结构化环境做出曼哈顿世界假设，以利用位姿估计解耦算法将双目相机的运动过程中旋转和平移矩阵分别解算：
步骤5.1、基于曼哈顿世界假设估计双目相机运动过程中的旋转矩阵，具体实现过程按如下：
步骤5.1.1、建立曼哈顿世界坐标系M，通过高斯映射将平面法向量nπ投影至单位高斯球上，具体映射过程如图3所示，即将第k帧图像中所获得的所有相交线对构成的平面法向量起点平移至曼哈顿世界坐标系M坐标原点；
步骤5.1.2、将法向量nπ再投影到曼哈顿世界坐标系M坐标轴的2D切平面上，所得到的投影向量记为nπ·project，投影结果如图4所示；在理想状态下所有平面法向量呈正交、平行、垂直状态，其投影基本上聚焦于平面上同一点，但由于平面法向量的估计误差，存在分散于球面上的稀疏法向量，故投影向量呈分散状态；
步骤5.1.3、为了减小误差，在2D切平面上对投影向量nπ·project进行mean-shift聚类，mean-shift聚类过程如图5所示，利用式(7)使得聚类结果达到收敛即可得聚类均值向量n′mean：
Figure BDA0003359076270000081
式(7)中，n表示第k帧图像中的平面法向量总数，g()表示高斯核函数；
步骤5.1.4、将聚类均值向量n′mean终点重投影至高斯球面上，记为s；s与高斯球球心连接的向量称为重投影向量，记为nmean。
步骤5.1.5、利用式(8)得到双目相机的当前旋转矩阵Rcurrent：
Figure BDA0003359076270000082
式(8)中，nmean,r1表示曼哈顿世界坐标系M的第1个坐标轴r1，
Figure BDA0003359076270000083
表示曼哈顿世界坐标系M的第2个坐标轴r2，
Figure BDA0003359076270000084
表示曼哈顿世界坐标系M的第3个坐标轴r3；
步骤5.1.6、根据双目相机的当前旋转矩阵Rcurrent和重投影向量nmean，利用式(9)来更新曼哈顿坐标系M的三个坐标轴
Figure BDA0003359076270000085
Figure BDA0003359076270000086
式(9)中，ra表示更新后的曼哈顿坐标系M的第a个坐标轴；
Figure BDA0003359076270000087
表示更新前的曼哈顿世界坐标系M的第a个坐标轴；
步骤5.1.7、按照步骤5.1.3-步骤5.1.6的过程迭代计算重投影向量nmean，直到式(9)收敛，从而利用式(10)得到双目相机的第k帧图像与曼哈顿世界坐标系M之间的最优旋转矩阵
Figure BDA0003359076270000088
Figure BDA0003359076270000091
式(10)中，r1 *表示最终更新的曼哈顿坐标系M的第1个坐标轴，
Figure BDA0003359076270000092
表示最终更新的曼哈顿坐标系M的第2个坐标轴，
Figure BDA0003359076270000093
表示最终更新的曼哈顿坐标系M的第3个坐标轴；
步骤5.1.8、按照步骤1-步骤5.1.7的过程求得第k+1帧图像与曼哈顿坐标系M的最优旋转矩阵
Figure BDA0003359076270000094
步骤5.1.9、在获得前后两帧图像与曼哈顿坐标系M的最优旋转矩阵之后，利用式(11)计算的相邻两帧旋转矩阵
Figure BDA0003359076270000095
理论上可实现无漂移旋转估计：
Figure BDA0003359076270000096
步骤5.2、构建基于ORB点特征和线特征的重投影误差函数，用于估计平移矩阵从而估计双目相机帧间的运动位姿。本实施例中的位姿估计解耦算法，平移矩阵的解算不仅取决于优化方法的选取，也取决于旋转矩阵的估计是否达到最优，其运动过程中观测点线特征的运动模型如图6所示，具体实施过程按如下所示：
步骤5.2.1、利用式(12)定义第k帧图像中ORB点特征集合的重投影误差函数ek,o：
ek,o＝pk,o-Π(Rk,k-1Pk,o+tk,k-1) (12)
式(12)中，Π()表示重投影函数，pk,o表示第k帧图像中ORB点特征的投影位置，Pk,o表示第k帧图像中ORB点特征的3D观测位置，Rk,k-1表示双目相机从第k-1帧图像运动至第k帧图像的旋转矩阵，tk,k-1表示双目相机从第k-1帧图像运动至第k帧图像的平移矩阵；
步骤5.2.2、根据第k帧图像中线特征的端点3D位置，利用式(13)得到归一化的线特征函数L：
Figure BDA0003359076270000097
式(13)中，Pstart表示线特征起点3D位置，Pend表示线特征终点3D位置；
步骤5.2.3、利用式(14)得到线特征端点Px的误差函数ek,l：
ek,l＝L·Π(Rk,k-1Pk,x+tk,k-1) (14)
式(14)中，Pk,x表示第k帧图像中线特征端点x的3D观测位置；
步骤5.2.4、假设双目相机运动过程中所有观测值服从高斯分布，则利用式(15)构建目标函数ψ*：
Figure BDA0003359076270000101
式(15)中，
Figure BDA0003359076270000102
表示局部关键帧集，
Figure BDA0003359076270000103
表示第k帧图像中ORB点特征集，
Figure BDA0003359076270000104
表示第k帧图像中线特征集，
Figure BDA0003359076270000105
表示第k帧图像中ORB点特征的协方差矩阵，
Figure BDA0003359076270000106
表示第k帧图像中线特征的协方差矩阵；
步骤5.2.5、利用Levenberg-Marquardt优化方法对目标函数ψ*进行迭代更新，从而得到最优平移矩阵，结合最优旋转矩阵，即可对双目相机帧间的运动位姿达到最优估计。
步骤6、判定经过跟踪线程预处理的第k帧图像是否为关键帧，若是，则用于构建局部地图，否则，返回步骤1。
综上所述，本方法能充分利用室内结构化环境的几何特征，解决机器人在低纹理和动态曝光等环境下鲁棒性差，无法准确构建室内环境地图的问题，更为精确地解算搭载双目相机的机器人本体在真实世界的运动状态，提高系统鲁棒性、重定位精度及速度；从而优化SLAM系统在室内环境的定位与建图效果，提升视觉SLAM技术在室内人造结构化场景中的普适性。
实践证明在室内人造结构化场景充分利用环境的几何信息且运用多特征融合技术能够解决机器人在探测陌生环境时运动跟踪精度低、累积误差大和构建地图信息不丰富等问题，增强系统生存能力，提高系统鲁棒性、实时性和信息利用率，扩展整个SLAM系统时间、空间覆盖率。

Claims (4)

1.一种基于点线面特征的室内双目视觉里程计方法，其特征在于，包括以下步骤：
步骤1、以双目相机为信息读取设备，标定所述双目相机的内参后对所采集的图像帧进行预处理，得到去噪后的第k帧图像；
步骤2、对于所述去噪后的第k帧图像提取ORB点特征；再采用改进的LSD算法提取所述去噪后的第k帧图像中的线特征，筛选出线特征中的相交线对；
步骤3、计算相交线对的端点3D位置(Pstart,Pend)和方向向量nl；
步骤4、根据所述相交线对的端点3D位置和方向向量获取面特征的参数；
步骤5、对双目相机所处的室内环境做出曼哈顿世界假设，以利用位姿估计解耦算法将所述双目相机的运动过程中的旋转和平移矩阵分别解算：
步骤5.1、基于曼哈顿世界假设估计旋转矩阵；
步骤5.2、构建基于ORB点特征和线特征的重投影误差函数，用于估计平移矩阵，从而估计双目相机帧间的运动位姿；
步骤6、判定经过跟踪线程预处理的第k帧图像是否为关键帧，若是，则用于构建局部地图，否则，返回步骤1。
2.如权利要求1中所述的一种基于点线面特征的室内双目视觉里程计方法，其特征在于，所述步骤4是按如下过程获取面特征的参数：
步骤4.1、利用式(1)得到相交线对的两线夹角θ，判定两线夹角θ是否大于阈值α；若是，则执行步骤4.2；否则返回步骤3；
Figure FDA0003359076260000011
式(1)中，nl1表示相交线对中线段1的方向向量，nl2表示相交线对中线段2的方向向量；
步骤4.2、判断相交线对中两线中心点的距离是否小于相交线对中任一线段的长度；若是，则执行步骤4.3；否则返回步骤3；
步骤4.3、判断相交线对中两线的四个端点是否在同一平面：
步骤4.3.1、利用式(2)得到所述相交线对所构成平面的法向量nπ：
nπ＝nl1×nl2 (2)
步骤4.3.2、根据平面法向量nπ和相交线对中的e个端点
Figure FDA0003359076260000012
利用式(3)计算e个平面系数
Figure FDA0003359076260000013
Figure FDA0003359076260000021
式(3)中，de表示第e个平面系数；(xe,ye,ze)表示第e个端点Pe的3D空间位置；
步骤4.3.3、利用式(4)计算双目相机到相交线对所构成平面的深度估计值
Figure FDA0003359076260000022
Figure FDA0003359076260000023
步骤4.3.4、利用式(5)得到最大平面系数差D：
D＝max(de)-min(de) (5)
步骤4.3.5、若最大平面系数差D小于阈值N，则判断两线的四个端点
Figure FDA0003359076260000024
存在于同一平面，并利用式(6)表示面特征的参数π，否则，表示四个端点
Figure FDA0003359076260000025
不在同一平面上；
Figure FDA0003359076260000026
3.如权利要求2中所述的一种基于点线面特征的室内双目视觉里程计方法，其特征在于，所述步骤5.1包括：
步骤5.1.1、建立曼哈顿世界坐标系M，通过高斯映射将所述平面法向量nπ投影至单位高斯球上；
步骤5.1.2、将所述法向量nπ再投影到曼哈顿世界坐标系M坐标轴的2D切平面上，所得到的投影向量记为nπ·project；
步骤5.1.3、在2D切平面上对投影向量nπ·project进行mean-shift聚类，从而利用式(7)得到聚类均值向量n′mean：
Figure FDA0003359076260000027
式(7)中，n表示第k帧图像中的平面法向量总数，g()表示高斯核函数；
步骤5.1.4、将聚类均值向量n′mean终点重投影至高斯球面上，记为s；s与所述高斯球球心连接的向量称为重投影向量，记为nmean；
步骤5.1.5、利用式(8)得到所述双目相机的当前旋转矩阵Rcurrent：
Figure FDA0003359076260000028
式(8)中，nmean,r1表示曼哈顿世界坐标系M的第1个坐标轴r1，
Figure FDA0003359076260000031
表示曼哈顿世界坐标系M的第2个坐标轴r2，
Figure FDA0003359076260000032
表示曼哈顿世界坐标系M的第3个坐标轴r3；
步骤5.1.6、根据所述双目相机的当前旋转矩阵Rcurrent和重投影向量nmean，利用式(9)来更新曼哈顿坐标系M的三个坐标轴
Figure FDA0003359076260000033
Figure FDA0003359076260000034
式(9)中，ra表示更新后的曼哈顿坐标系M的第a个坐标轴；
Figure FDA0003359076260000035
表示更新前的曼哈顿世界坐标系M的第a个坐标轴；
步骤5.1.7、按照步骤5.1.3-步骤5.1.6的过程迭代计算重投影向量nmean，直到式(9)收敛，从而利用式(10)得到所述双目相机的第k帧图像与曼哈顿世界坐标系M之间的最优旋转矩阵
Figure FDA0003359076260000036
Figure FDA0003359076260000037
式(10)中，r1 *表示最终更新的曼哈顿坐标系M的第1个坐标轴，
Figure FDA0003359076260000038
表示最终更新的曼哈顿坐标系M的第2个坐标轴，
Figure FDA0003359076260000039
表示最终更新的曼哈顿坐标系M的第3个坐标轴；
步骤5.1.8、按照步骤1-步骤5.1.7的过程求得第k+1帧图像与曼哈顿坐标系M的最优旋转矩阵
Figure FDA00033590762600000310
步骤5.1.9、利用式(11)计算相邻两帧的旋转矩阵
Figure FDA00033590762600000311
Figure FDA00033590762600000312
4.如权利要求3中所述的一种基于点线面特征的室内双目视觉里程计方法，其特征在于，所述步骤5.2包括：
步骤5.2.1、利用式(12)定义第k帧图像中ORB点特征集合的重投影误差函数ek,o：
ek,o＝pk,o-Π(Rk,k-1Pk,o+tk,k-1) (12)
式(12)中，Π()表示重投影函数，pk,o表示第k帧图像中ORB点特征的投影位置，Pk,o表示第k帧图像中ORB点特征的3D观测位置，Rk,k-1表示所述双目相机从第k-1帧图像运动至第k帧图像的旋转矩阵，tk,k-1表示所述双目相机从第k-1帧图像运动至第k帧图像的平移矩阵；
步骤5.2.2、根据第k帧图像中线特征的端点3D位置，利用式(13)得到归一化的线特征函数L：
Figure FDA0003359076260000041
式(13)中，Pstart表示线特征起点3D位置，Pend表示线特征终点3D位置；
步骤5.2.3、利用式(14)得到线特征端点Px的误差函数ek,l：
ek,l＝L·Π(Rk,k-1Pk,x+tk,k-1) (14)
式(14)中，Pk,x表示第k帧图像中线特征端点x的3D观测位置；
步骤5.2.4、假设观测值服从高斯分布，则利用式(15)构建目标函数ψ*：
Figure FDA0003359076260000042
式(15)中，
Figure FDA0003359076260000043
表示局部关键帧集，
Figure FDA0003359076260000044
表示第k帧图像中ORB点特征集，
Figure FDA0003359076260000045
表示第k帧图像中线特征集，
Figure FDA0003359076260000046
表示第k帧图像中ORB点特征的协方差矩阵，
Figure FDA0003359076260000047
表示第k帧图像中线特征的协方差矩阵；
步骤5.2.5、利用Levenberg-Marquardt优化方法对所述目标函数ψ*进行迭代更新，从而得到最优平移矩阵，用于估计双目相机帧间的运动位姿。
CN202111360538.XA 2021-11-17 2021-11-17 一种基于点线面特征的室内双目视觉里程计方法 Pending CN114004900A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111360538.XA CN114004900A (zh) 2021-11-17 2021-11-17 一种基于点线面特征的室内双目视觉里程计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111360538.XA CN114004900A (zh) 2021-11-17 2021-11-17 一种基于点线面特征的室内双目视觉里程计方法

Publications (1)

Publication Number Publication Date
CN114004900A true CN114004900A (zh) 2022-02-01

Family

ID=79929480

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111360538.XA Pending CN114004900A (zh) 2021-11-17 2021-11-17 一种基于点线面特征的室内双目视觉里程计方法

Country Status (1)

Country Link
CN (1) CN114004900A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114332232A (zh) * 2022-03-11 2022-04-12 中国人民解放军国防科技大学 基于空间点线面特征混合建模的智能手机室内定位方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114332232A (zh) * 2022-03-11 2022-04-12 中国人民解放军国防科技大学 基于空间点线面特征混合建模的智能手机室内定位方法
CN114332232B (zh) * 2022-03-11 2022-05-31 中国人民解放军国防科技大学 基于空间点线面特征混合建模的智能手机室内定位方法

Similar Documents

Publication Publication Date Title
CN109166149B (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN108520554B (zh) 一种基于orb-slam2的双目三维稠密建图方法
Pomerleau et al. Tracking a depth camera: Parameter exploration for fast ICP
CN107590827A (zh) 一种基于Kinect的室内移动机器人视觉SLAM方法
CN107588721A (zh) 一种基于双目视觉的零件多尺寸的测量方法及系统
CN107358629B (zh) 一种基于目标识别的室内建图与定位方法
CN108225319B (zh) 基于目标特征的单目视觉快速相对位姿估计系统及方法
CN101082988A (zh) 自动的深度图像配准方法
Li et al. Leveraging planar regularities for point line visual-inertial odometry
CN107462897B (zh) 基于激光雷达的三维建图的方法
CN110487286B (zh) 基于点特征投影与激光点云融合的机器人位姿判断方法
Huang et al. Gmmloc: Structure consistent visual localization with gaussian mixture models
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN111899280A (zh) 采用深度学习和混合型位姿估计的单目视觉里程计方法
Agrawal et al. PCE-SLAM: A real-time simultaneous localization and mapping using LiDAR data
CN114066773B (zh) 一种基于点云特征与蒙特卡洛扩展法的动态物体去除
Tian et al. Research on multi-sensor fusion SLAM algorithm based on improved gmapping
KR102361133B1 (ko) 이동체의 전방향에 위치한 적어도 하나의 물체에 대한 거리를 획득하는 방법 및 이를 이용한 비전 장치
KR102372298B1 (ko) 이동체의 전방향에 위치한 적어도 하나의 물체에 대한 거리를 획득하는 방법 및 이를 이용한 비전 장치
CN113160401B (zh) 一种面向物体的视觉slam轻量化语义地图创建方法
CN114004900A (zh) 一种基于点线面特征的室内双目视觉里程计方法
CN113487631A (zh) 基于lego-loam的可调式大角度探测感知及控制方法
CN113393524A (zh) 一种结合深度学习和轮廓点云重建的目标位姿估计方法
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
Cigla et al. Gaussian mixture models for temporal depth fusion

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