CN115170847A - 基于先验地图约束的煤矿井下单目视觉定位方法及装置 - Google Patents

基于先验地图约束的煤矿井下单目视觉定位方法及装置 Download PDF

Info

Publication number
CN115170847A
CN115170847A CN202211077044.5A CN202211077044A CN115170847A CN 115170847 A CN115170847 A CN 115170847A CN 202211077044 A CN202211077044 A CN 202211077044A CN 115170847 A CN115170847 A CN 115170847A
Authority
CN
China
Prior art keywords
image
rotation matrix
matching point
initial value
coal mine
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.)
Granted
Application number
CN202211077044.5A
Other languages
English (en)
Other versions
CN115170847B (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.)
General Coal Research Institute Co Ltd
Original Assignee
General Coal Research Institute Co Ltd
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 General Coal Research Institute Co Ltd filed Critical General Coal Research Institute Co Ltd
Priority to CN202211077044.5A priority Critical patent/CN115170847B/zh
Publication of CN115170847A publication Critical patent/CN115170847A/zh
Application granted granted Critical
Publication of CN115170847B publication Critical patent/CN115170847B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/757—Matching configurations of points or features
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V2201/00—Indexing scheme relating to image or video recognition or understanding
    • G06V2201/07—Target detection

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Computing Systems (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Image Analysis (AREA)

Abstract

本公开提出一种基于先验地图约束的煤矿井下单目视觉定位方法及装置，该方法包括：获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，第一图像对应有绝对位姿，并确定第一图像和第二图像的多个匹配点对，并根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值，并根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值，以及根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿，能够将旋转矩阵初值和平移向量初值的估计进行解耦，分步估计旋转矩阵和平移向量初值，有效避免煤矿井下巷道特殊几何结构带来的姿态退化估计问题。

Description

基于先验地图约束的煤矿井下单目视觉定位方法及装置
技术领域
本公开涉及图像处理技术领域，尤其涉及一种基于先验地图约束的煤矿井下单目视觉定位方法及装置。
背景技术
目前，地面上很多无人***，如：地面交通无人驾驶、露天矿无人作业***、AGV、服务机器人等，已经进行了大量应用测试，正在逐步完善和成熟，但是煤矿井下无人***技术进展还非常缓慢，一个关键的因素是煤矿井下定位问题还没有得到有效解决。相关技术中，基于单目视觉的同步定位与沟通（SLAM）技术在地面相对成熟，但在煤矿井下场景中面临着较大的挑战：（1）巷道狭长的特殊几何结构给单目定位带来了严重的退化估计问题；（2）煤矿井下光线偏暗带来了严重的匹配不稳定问题。
发明内容
本申请提出了一种基于先验地图约束的煤矿井下单目视觉定位方法及装置，旨在至少在一定程度上解决相关技术中的技术问题之一。
本申请第一方面实施例提出了一种基于先验地图约束的煤矿井下单目视觉定位方法，包括：获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，第一图像对应有绝对位姿；确定第一图像和第二图像的多个匹配点对；根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值；根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值；以及根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿。
本申请第二方面实施例提出了一种基于先验地图约束的煤矿井下单目视觉定位装置，包括：获取模块，用于获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，第一图像对应有绝对位姿；第一确定模块，用于确定第一图像和第二图像的多个匹配点对；第一计算模块，用于根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值；第二计算模块，用于根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值；以及第二确定模块，用于根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿。
本申请第三方面实施例提出了一种电子设备，包括：至少一个处理器；以及与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行本申请实施例的基于先验地图约束的煤矿井下单目视觉定位方法。
本申请第四方面实施例提出了一种存储有计算机指令的非瞬时计算机可读存储介质，所述计算机指令用于使所述计算机执行本申请实施例公开的基于先验地图约束的煤矿井下单目视觉定位方法。
本实施例中，通过获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，第一图像对应有绝对位姿，并确定第一图像和第二图像的多个匹配点对，并根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值，并根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值，以及根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿，能够将旋转矩阵初值和平移向量初值的估计进行解耦，分步估计旋转矩阵和平移向量初值，有效避免煤矿井下巷道特殊几何结构带来的姿态退化估计问题，此外在计算过程中利用井下平面的先验地图，增强了位姿估计中的约束，提高了单目视觉定位的精度和鲁棒性。
本公开附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本公开的实践了解到。
附图说明
本公开上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1是本公开一实施例提出的基于先验地图约束的煤矿井下单目视觉定位方法的流程示意图；
图2是本公开一实施例提出的基于先验地图约束的煤矿井下单目视觉定位方法的流程示意图；
图3是本公开实施例提出的基于先验地图约束的煤矿井下单目视觉定位方法的流程示意图；
图4是本公开另一实施例提出的基于先验地图约束的煤矿井下单目视觉定位装置的示意图；
图5示出了适于用来实现本申请实施方式的示例性计算机设备的框图。
具体实施方式
下面详细描述本公开的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本公开，而不能理解为对本公开的限制。相反，本公开的实施例包括落入所附加权利要求书的精神和内涵范围内的所有变化、修改和等同物。
其中，需要说明的是，本实施例的基于先验地图约束的煤矿井下单目视觉定位方法的执行主体可以为基于先验地图约束的煤矿井下单目视觉定位装置，该装置可以由软件和/或硬件的方式实现，该装置可以配置在电子设备中，电子设备可以包括但不限于终端、服务器端等。
图1是根据本公开一实施例提供的基于先验地图约束的煤矿井下单目视觉定位方法的流程示意图。参考图1所示，该方法包括：
S101：获取单目相机在煤矿井下连续采集的第一图像和第二图像。
本公开实施例中，可以利用单目相机沿煤矿井下的巷道移动以采集煤矿井下的图像，而连续采集的相邻两帧图像可以被称为第一图像和第二图像，其中，第一图像可以对应上一帧图像，第二图像对应当前帧图像，而本实施例的基于先验地图约束的煤矿井下单目视觉定位方法用于对当前帧第二图像进行定位。
其中，本实施例的第一图像可以有对应的绝对位姿（或者称为全局姿态），其可以用
Figure 541436DEST_PATH_IMAGE001
表示，其中，
Figure 227632DEST_PATH_IMAGE002
表示第一图像的绝对旋转矩阵，
Figure 434623DEST_PATH_IMAGE003
表示第一图像的绝对平移向量。
S102：确定第一图像和第二图像的多个匹配点对。
上述获取第一图像和第二图像后，进一步地，本实施例可以确定第一图像和第二图像的多个匹配点对。
其中，第一图像和第二图像中对应的匹配点构成的匹配对可以被称为匹配点对，而第一图像和第二图像多个匹配点可以得到多个匹配点对，其可以表示为
Figure 485755DEST_PATH_IMAGE004
，其中，i=1、2、3...n，表示匹配点对的数量。
一些实施例，例如可以利用光流跟踪方法，在当前帧的第二图像中找到第一图像中特征点（例如Harris角点）的匹配点，将匹配的角点作为一个匹配点对，从而可以得到多个匹配点对。
S103：根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值。
上述确定多个匹配点对
Figure 410986DEST_PATH_IMAGE005
后，进一步地，本实施例可以根据多个匹配点对
Figure 635294DEST_PATH_IMAGE006
的对极约束关系和绝对位姿
Figure 900053DEST_PATH_IMAGE001
，初步估计第二图像的旋转矩阵初值，该旋转矩阵初值对应于第二图像的绝对旋转矩阵，其可以用
Figure 184404DEST_PATH_IMAGE007
表示。
具体地，首先基于单目相机的内参数矩阵K，将多个匹配点对
Figure 331352DEST_PATH_IMAGE006
转化为多个对极支撑向量对，对极支撑向量对表示一个对极平面上的对极支撑向量，其可以表示为
Figure 297034DEST_PATH_IMAGE008
，其中，
Figure 478616DEST_PATH_IMAGE009
和
Figure 933868DEST_PATH_IMAGE010
分别表示经过对应相机光心和图像点
Figure 833691DEST_PATH_IMAGE011
或
Figure 337485DEST_PATH_IMAGE012
反投影射线方向向量，其中，对极支撑向量
Figure 373574DEST_PATH_IMAGE008
计算过程如下：
Figure 999727DEST_PATH_IMAGE013
（1）
Figure 58950DEST_PATH_IMAGE014
（2）
进一步地，根据多个对极支撑向量对
Figure 428752DEST_PATH_IMAGE008
，确定对极平面的法向量矩阵。
其中，可以首先确定每个对极支撑向量对
Figure 584927DEST_PATH_IMAGE008
对应的法向量，其可以用
Figure 381981DEST_PATH_IMAGE015
表示，在纯平移情况下极平面的法向量
Figure 928500DEST_PATH_IMAGE016
，如果存在旋转，则法向量变为
Figure 836413DEST_PATH_IMAGE017
，而本实施例中，第一图像和第二图像之间可以存在旋转；进一步地，将每个对极支撑向量对的法向量以矩阵的形式进行表示，可以得到对极平面的法向量矩阵，其可以用N表示，
Figure 847095DEST_PATH_IMAGE018
。
进一步地，根据法向量矩阵N，计算第一图像和第二图像之间的相对旋转矩阵，该相对旋转矩阵用于描述第一图像和第二图像之间的相对旋转关系，其可以用
Figure 815051DEST_PATH_IMAGE019
表示。其中，可以首先计算法向量矩阵N和法向量矩阵的转置矩阵的乘积，计算过程如下：
Figure 860585DEST_PATH_IMAGE020
Figure 572189DEST_PATH_IMAGE021
（3）
设
Figure 702956DEST_PATH_IMAGE022
表示M的最小特征值，则计算相对旋转矩阵
Figure 779496DEST_PATH_IMAGE019
的过程可以变为优化问题，即：计算使M最小情况下的R值作为相对旋转矩阵
Figure 97345DEST_PATH_IMAGE019
，计算过程如下：
Figure 612640DEST_PATH_IMAGE023
（4）
通过求解公式（4）所示的优化问题，可以得到相邻的第一图像和第二图像之间的相对旋转矩阵
Figure 597913DEST_PATH_IMAGE024
。
进一步地，根据相对旋转矩阵
Figure 845355DEST_PATH_IMAGE019
和绝对位姿
Figure 650500DEST_PATH_IMAGE001
的绝对旋转矩阵
Figure 703907DEST_PATH_IMAGE025
，确定第二图像的旋转矩阵初值
Figure 481370DEST_PATH_IMAGE026
，旋转矩阵初值计算如下：
Figure 962030DEST_PATH_IMAGE027
（5）
S104：根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值。
上述确定第二图像的旋转矩阵初值
Figure 520050DEST_PATH_IMAGE026
后，进一步地，本实施例根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值，平移向量初值对应第二图像的绝对平移向量，其可以用
Figure 111568DEST_PATH_IMAGE028
表示。
其中，本实施例可以根据前一帧的绝对位姿获取先验地图，先验地图例如可以是煤矿井下的点云数据集，其可以表征煤矿井下巷道狭长的结构，而本实施例在计算平移向量初值
Figure 9117DEST_PATH_IMAGE028
的过程中，可以引入先验地图约束，提高单目视觉定位的精度和鲁棒性。
具体地，在计算平移向量初值
Figure 660679DEST_PATH_IMAGE028
的过程中，本实施例首先根据先验地图估计煤矿井下几何平面的平面方程：
Figure 440416DEST_PATH_IMAGE029
（6）
其中，
Figure 773308DEST_PATH_IMAGE030
表示平面方程法向量，P表示世界坐标系下对应平面的三维点，d表示几何平面到世界坐标系原点的距离。
进一步地，基于平面方程、内参数矩阵K、绝对位姿，计算每个匹配点对对应的空间三维点的深度。
具体地，设第一图像和第二图像匹配点对
Figure 587680DEST_PATH_IMAGE005
的坐标表示形式为
Figure 410143DEST_PATH_IMAGE031
，该匹配点对对应的世界坐标系下的三维点为P，该三维点P在第一图像中表示为
Figure 614859DEST_PATH_IMAGE032
，在第二图像中表示为
Figure 813759DEST_PATH_IMAGE033
，相机内参数矩阵为K，第一图像绝对位姿为
Figure 482638DEST_PATH_IMAGE034
，
Figure 476002DEST_PATH_IMAGE035
的深度对应三维点P的深度，其可以用
Figure 168014DEST_PATH_IMAGE036
表示，则有：
Figure 170605DEST_PATH_IMAGE037
（7）
若点P在上述几何平面上，将上述平面方程的公式代入式（7），可得：
Figure 693991DEST_PATH_IMAGE038
（8）
设三维点
Figure 795939DEST_PATH_IMAGE039
，该三维点P的齐次坐标
Figure 37564DEST_PATH_IMAGE040
，其可以表示为
Figure 578267DEST_PATH_IMAGE041
，则三维点P的三维坐标可以表示为：
Figure 893842DEST_PATH_IMAGE042
（9）
进一步地，根据内参数矩阵K和每个匹配点对中第一匹配点和第二匹配点的二维坐标，分别计算空间三维点的第一齐次坐标和第二齐次坐标。
其中，第一匹配点为匹配点对中第一图像的图像点，第二匹配点为第二图像的图像点，也即是说，第一匹配点的二维坐标可以表示为
Figure 494588DEST_PATH_IMAGE035
，第二匹配点的二维坐标可以表示为
Figure 957930DEST_PATH_IMAGE043
。
而第一图像的空间三维点
Figure 240007DEST_PATH_IMAGE044
的齐次坐标可以被称为第一齐次坐标，其可以用
Figure 472405DEST_PATH_IMAGE045
表示，同理，第二图像的空间三维点
Figure 244052DEST_PATH_IMAGE046
的的齐次坐标可以被称为第二齐次坐标，其可以用
Figure 132373DEST_PATH_IMAGE047
，其中，可以根据公式
Figure 14879DEST_PATH_IMAGE048
计算该第一齐次坐标和第二齐次坐标，例如第二齐次坐标
Figure 305046DEST_PATH_IMAGE047
的形式可以表示为
Figure 247594DEST_PATH_IMAGE049
。
当图像点
Figure 685529DEST_PATH_IMAGE035
属于上述巷道特殊几何结构平面时，可以根据公式（8）估计出对应的深度，并计算出第一齐次坐标，进一步根据第一齐次坐标和深度计算空间三维点的第一三维坐标
Figure 309408DEST_PATH_IMAGE050
，同理，根据第二齐次坐标和深度计算空间三维点的第二三维坐标
Figure 454081DEST_PATH_IMAGE051
。
进一步地，根据公式（9），可以推导第一三维坐标
Figure 567531DEST_PATH_IMAGE050
、第二三维坐标
Figure 492762DEST_PATH_IMAGE052
、相对旋转矩阵
Figure 654753DEST_PATH_IMAGE053
、相对位移（可以用
Figure 981829DEST_PATH_IMAGE054
表示）的关系如下：
Figure 600DEST_PATH_IMAGE055
（10）
一些实施例，多个匹配点对中的第一匹配点存在属于井下几何平面和不属于几何平面的情况。在第一匹配点
Figure 350810DEST_PATH_IMAGE056
属于煤矿井下几何平面的情况下，可以根据第一三维坐标、第二三维坐标以及相对旋转矩阵，确定第一约束关系，即：针对每一匹配点对，根据公式（10）推导第一约束关系，第一约束关系可以表示为：
Figure 378809DEST_PATH_IMAGE057
（11）
Figure 560392DEST_PATH_IMAGE058
（12）
其中，相邻的第一图像和第二图像的相对位姿可以表示为
Figure 953327DEST_PATH_IMAGE059
，
Figure 525254DEST_PATH_IMAGE060
表示相对旋转矩阵
Figure 91364DEST_PATH_IMAGE061
第n行，
Figure 393033DEST_PATH_IMAGE062
表示相对位移
Figure 956869DEST_PATH_IMAGE063
第n行。
另一些实施例，在第一匹配点
Figure 78409DEST_PATH_IMAGE056
不属于煤矿井下几何平面的情况下，根据
Figure 448210DEST_PATH_IMAGE064
，本实施例可以确定第一三维坐标、第二三维坐标以及相对旋转矩阵的第二约束关系，第二约束关系可以表示为：
Figure 542068DEST_PATH_IMAGE065
Figure 339123DEST_PATH_IMAGE066
Figure 947959DEST_PATH_IMAGE067
（13）
进一步地，根据第一约束关系和第二约束关系，求解相对位移。具体地，由方程（11）-（13）构成的约束优化问题例如可以通过LM（Levenberg- Marquardt）方法求解，以得到相对位移
Figure 793555DEST_PATH_IMAGE068
。
进一步地，根据相对位移
Figure 804237DEST_PATH_IMAGE069
和前一帧第一图像绝对位姿的绝对平移向量
Figure 772193DEST_PATH_IMAGE070
，可以确定第二图像的平移向量初值
Figure 868325DEST_PATH_IMAGE071
。
S105：根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿。
也即是说，将旋转矩阵初值
Figure 517612DEST_PATH_IMAGE072
和平移向量初值
Figure 382800DEST_PATH_IMAGE071
作为第二图像的目标位姿，也可以称为第二图像的绝对位姿，从而可以根据目标位姿进行单目图像定位。
本实施例中，通过获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，第一图像对应有绝对位姿，并确定第一图像和第二图像的多个匹配点对，并根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值，并根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值，以及根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿，能够将旋转矩阵初值和平移向量初值的估计进行解耦，分步估计旋转矩阵和平移向量初值，有效避免煤矿井下巷道特殊几何结构带来的姿态退化估计问题，此外在计算过程中利用井下平面的先验地图，增强了位姿估计中的约束，提高了单目视觉定位的精度和鲁棒性。
图2是根据本公开另一实施例提供的基于先验地图约束的煤矿井下单目视觉定位方法的流程示意图。参考图2所示，该方法包括：
S201：获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，第一图像对应有绝对位姿。
S202：确定第一图像和第二图像的多个匹配点对。
S203：根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值。
S204：根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值。
S205：根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿。
S201-205的说明具体参见上述实施例，此处不再赘述。
S206：对第二图像进行目标检测和分割，以确定第二图像中的第一矢量目标。
正如背景技术中所述的，煤矿井下光线偏暗带来了严重的匹配不稳定问题，鉴于此，本实施例在估计第二图像的旋转矩阵初值
Figure 521657DEST_PATH_IMAGE073
和平移向量初值
Figure 42768DEST_PATH_IMAGE074
后，还可以对旋转矩阵初值
Figure 558063DEST_PATH_IMAGE075
和平移向量初值
Figure 277757DEST_PATH_IMAGE076
进行优化，即：优化目标位姿。
具体地，本实施例可以对第二图像进行目标检测和分割，以确定第二图像中的矢量目标，其可以被称为第一矢量目标。一些实施例，例如可以采用YOLOv3网络对第二图像进行语义目标检测和分割，以得到第一矢量目标。
可以理解的是，本实施例对于步骤S206的执行顺序可以不作限定，例如可以与S202同步执行。
S207：确定第一矢量目标和先验地图中第二矢量目标的匹配关系。
其中，先验地图中的矢量目标可以被称为第二矢量目标，本实施例例如可以采用匈牙利算法对第一矢量目标和第二矢量目标进行匹配，得到矢量目标初始的匹配关系，即：目标匹配对。
S208：根据匹配关系，对旋转矩阵初值和平移向量初值进行优化。
也即是说，对初步计算的目标位姿进行优化，例如：根据矢量目标的匹配误差，对旋转矩阵初值和平移向量初值进行调整优化，从而优化后的目标位姿更加准确。
一些实施例，在优化过程中，可以获取矢量目标区域内各个匹配点对的三维点P，进一步地，通过最小化优化以下代价函数，对第二图像的目标位姿进行优化，代价函数如下：
Figure 587516DEST_PATH_IMAGE077
（14）
其中，Tt表示目标位姿，
Figure 595923DEST_PATH_IMAGE078
表示当前帧（第二图像）的第一矢量目标元素（
Figure 649330DEST_PATH_IMAGE079
）投影到第二矢量目标元素（
Figure 426793DEST_PATH_IMAGE080
）的重投影误差函数，
Figure 907453DEST_PATH_IMAGE081
表示第二图像的图像
Figure 465473DEST_PATH_IMAGE082
投影到三维点的重投影误差函数。在最小化代价函数时，求取优化后的位姿。
一些实施例，在获取矢量目标区域内各个匹配点对的三维点P之前，还可以定义滑动窗口
Figure 994675DEST_PATH_IMAGE083
，其中，滑动窗口内包括连续的
Figure 626644DEST_PATH_IMAGE084
等多帧图像，多帧图像包括第二图像
Figure 543785DEST_PATH_IMAGE085
，
Figure 261205DEST_PATH_IMAGE086
之间的相对姿态和绝对姿态已经获取，
Figure 656414DEST_PATH_IMAGE087
与
Figure 470787DEST_PATH_IMAGE088
之间的相对姿态（相对位姿）及
Figure 230932DEST_PATH_IMAGE088
对应的绝对姿态初值（目标位姿）已经通过上述实施例计算得到，则本实施例在初步优化位姿后，还可以通过最小化代价函数的方式，对滑动窗口内多帧图像的位姿进行优化：
Figure 497965DEST_PATH_IMAGE089
（15）
从而，本实施例可以利用连续特征跟踪与矢量语义关联匹配，解决了煤矿井下光线偏暗带来的匹配不稳定问题。
本实施例中，通过获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，第一图像对应有绝对位姿，并确定第一图像和第二图像的多个匹配点对，并根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值，并根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值，以及根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿，能够将旋转矩阵初值和平移向量初值的估计进行解耦，分步估计旋转矩阵和平移向量初值，有效避免煤矿井下巷道特殊几何结构带来的姿态退化估计问题，此外在计算过程中利用井下平面的先验地图，增强了位姿估计中的约束，提高了单目视觉定位的精度和鲁棒性。此外，本实施例可以利用连续特征跟踪与矢量语义关联匹配，解决了煤矿井下光线偏暗带来的匹配不稳定问题。
在一个具体实例中，针对煤矿井下光线偏暗、大视角或大位移变化情况下特征匹配难度大的问题，提出了一种煤矿井下矢量地图约束的单目视觉定位方法。首先，进行单目视觉相邻帧间的特征点跟踪、提取与匹配，避免偏暗场景中大视角、大位移变化带来的点匹配问题，同时，针对当前图像帧中进行语义目标检测与分割，并与先验矢量语义地图进行关联匹配；然后，利用点匹配对，采用旋转、平移解耦与巷道特殊几何平面约束的方法估计相邻帧的相对姿态，并预测当前帧的全局位姿；最后，在先验矢量地图的约束下，进行局部区域的位姿优化，如图3所示，包括以下步骤：
步骤1：对当前图像进行特征点提取、跟踪与匹配
利用光流跟踪方法，在当前帧中找到上一帧图像特征点的匹配点，并从当前帧图像中提取Harris角点，与上述匹配点一起，构成当前帧图像的特征点集。
步骤2：语义目标检测与分割
基于YOLOv3网络进行语义目标检测和分割。
步骤3：利用点匹配对估计相邻帧相对姿态，预测当前帧的位姿初值
（1）单目视觉相邻帧的相对旋转矩阵估计
利用单目视觉的内参数矩阵K,将上述匹配点对
Figure 696866DEST_PATH_IMAGE004
（其中，i=1、2、3，表示匹配点对数量）转化为对极支撑向量对
Figure 303427DEST_PATH_IMAGE008
（其中，
Figure 296791DEST_PATH_IMAGE009
和
Figure 51121DEST_PATH_IMAGE010
分别表示经过对应相机光心和图像点
Figure 788132DEST_PATH_IMAGE090
或
Figure 249201DEST_PATH_IMAGE091
反投影射线方向向量）：
Figure 679045DEST_PATH_IMAGE013
（1）
Figure 846635DEST_PATH_IMAGE014
（2）
Figure 121758DEST_PATH_IMAGE008
是一个对极平面上的对极支撑向量，在纯平移情况下极平面的法向量为
Figure 765229DEST_PATH_IMAGE092
，如果存在旋转，则法向量变为：
Figure 100396DEST_PATH_IMAGE093
。
设
Figure 767000DEST_PATH_IMAGE094
，则：
Figure 845815DEST_PATH_IMAGE095
Figure 343792DEST_PATH_IMAGE096
（3）
设
Figure 53122DEST_PATH_IMAGE022
表示M的最小特征值，优化问题变为：
Figure 3761DEST_PATH_IMAGE023
（4）
通过求解公式（4）所示的优化问题得到相邻两帧之间的相对旋转矩阵
Figure 886266DEST_PATH_IMAGE097
。
根据前一帧图像所对应的绝对旋转矩阵
Figure 238750DEST_PATH_IMAGE025
和相对旋转矩阵
Figure 118981DEST_PATH_IMAGE019
，预测当前帧的旋转矩阵初值：
Figure 556916DEST_PATH_IMAGE027
（5）
（2）巷道平面结构约束下的相对旋转矩阵估计
根据前一帧的姿态，获取先验地图，估计巷道地平面（通过步骤2获得相应图像区域）等特殊几何结构的平面方程如下：
Figure 977533DEST_PATH_IMAGE029
（6）
P是世界坐标系下对应平面的三维点。
设上一帧和当前帧的特征匹配对为
Figure 387786DEST_PATH_IMAGE098
，对应的世界坐标系下的三维点为P，相机内参数为K，上一帧图像对应的全局姿态为
Figure 501235DEST_PATH_IMAGE034
，
Figure 426466DEST_PATH_IMAGE036
表示点
Figure 650774DEST_PATH_IMAGE035
的深度，则有：
Figure 649954DEST_PATH_IMAGE037
（7）
若点P在上述平面上，将上述平面公式代入上式，有：
Figure 934305DEST_PATH_IMAGE038
（8）
设三维点
Figure 346832DEST_PATH_IMAGE039
，该点向量除以对应的深度
Figure 312514DEST_PATH_IMAGE036
，可得
Figure 494096DEST_PATH_IMAGE041
，则有：
Figure 683769DEST_PATH_IMAGE042
（9）
根据公式（9），可以推导：
Figure 583592DEST_PATH_IMAGE099
（10）
当图像点
Figure 87386DEST_PATH_IMAGE056
属于上述巷道特殊几何结构平面时，则在前一帧获取姿态后，可以根据公式（8）,估计出对应的深度，进而计算前一帧图像点
Figure 389054DEST_PATH_IMAGE056
对应的三维点坐标
Figure 749628DEST_PATH_IMAGE050
。点
Figure 74430DEST_PATH_IMAGE056
在当前帧图像上的对应点为
Figure 444232DEST_PATH_IMAGE043
，则
Figure 334827DEST_PATH_IMAGE100
，这里，
Figure 131882DEST_PATH_IMAGE101
可表示为
Figure 678401DEST_PATH_IMAGE102
。所以，针对每一对匹配对应点，根据公式（10），可以推导得到两个约束方程如下：
Figure 851893DEST_PATH_IMAGE057
（11）
Figure 862575DEST_PATH_IMAGE058
（12）
其中，
Figure 768214DEST_PATH_IMAGE103
表示相邻两帧的相对姿态，
Figure 864346DEST_PATH_IMAGE060
表示相对旋转矩阵
Figure 575950DEST_PATH_IMAGE104
第n行，
Figure 441138DEST_PATH_IMAGE062
表示相对位移
Figure 517678DEST_PATH_IMAGE063
第n行。
当图像点
Figure 101106DEST_PATH_IMAGE056
不属于上述巷道特殊几何结构平面时，则根据公式
Figure 288505DEST_PATH_IMAGE064
，可以得到以下约束方程：
Figure 273779DEST_PATH_IMAGE065
Figure 521220DEST_PATH_IMAGE066
Figure 591945DEST_PATH_IMAGE067
（13）
由方程（11）-（13）构成的约束优化问题通过LM（Levenberg- Marquardt）方法求解以得到相对位移，进一步，结合前一帧图像所对应的姿态，预测当前帧对应的平移向量初值。
步骤4：根据步骤2所得的语义目标，与先验矢量地图进行匹配关联
采用匈牙利算法获得矢量目标（当前帧矢量语义目标和先验地图矢量目标）的初始匹配关系。
步骤5：利用步骤1所得的点匹配对、步骤3所得的位姿初值、步骤4所得的语义目标匹配对，进行位姿优化
假定滑动窗口
Figure 645351DEST_PATH_IMAGE105
，其中，
Figure 422814DEST_PATH_IMAGE084
之间的相对姿态和绝对姿态已经获取，
Figure 903474DEST_PATH_IMAGE087
与
Figure 133598DEST_PATH_IMAGE088
之间相对姿态及
Figure 990696DEST_PATH_IMAGE088
对应的绝对姿态初值已经通过步骤3获取。通过局部窗口优化窗口内各帧的绝对姿态步骤如下：
步骤5.1：获取矢量语义目标区域内各个track（注：track指连续匹配的特征点序列）的三维坐标。对于track中对应于每帧矢量语义目标区域内的点，根据公式（8）计算对应深度，进一步地，获取其三维坐标，然后求平均，作为该track对应的三维点。
步骤5.2：获取
Figure 622666DEST_PATH_IMAGE088
对应的track及其三维点。
步骤5.3：通过最小化优化以下代价函数，得到
Figure 274227DEST_PATH_IMAGE088
对应的姿态
Figure 319543DEST_PATH_IMAGE106
：
Figure 652436DEST_PATH_IMAGE107
（14）
其中，
Figure 201229DEST_PATH_IMAGE108
表示当前帧矢量元素重投影误差函数，
Figure 289270DEST_PATH_IMAGE109
表示当前帧三维点重投影误差函数。
步骤5.4：根据投影距离最小，重新匹配
Figure 493987DEST_PATH_IMAGE088
和矢量地图对应的矢量元素。
步骤5.5：通过最小化优化以下代价函数，优化滑动窗口内各帧的姿态：
Figure 692887DEST_PATH_IMAGE089
（15）
煤矿井下巷道呈狭长规则几何形状，由于视角问题，直接利用多视几何三角测距方法恢复三维点云会带来很大的误差。煤矿井下的这种几何特征也给视角变化下的特征匹配带来了极大的困难，而煤矿井下光线偏暗问题无疑进一步加剧了特征匹配的难度。上述问题都给煤矿井下的单目视觉定位带来了挑战。对此，本实施例提出的煤矿井下矢量地图约束的单目视觉定位方法，该方法具有如下有益效果：
1、将旋转矩阵和平移向量初值的估计进行解耦，分步估计旋转矩阵和平移向量初值，有效避免煤矿井下巷道特殊几何结构带来的姿态退化估计问题；
2、利用连续特征跟踪与矢量语义关联匹配，解决了煤矿井下光线偏暗带来的匹配不稳定问题；
3、利用煤矿井下巷道中特殊的几何平面，增强了姿态估计中的约束，提高了单目视觉定位的精度和鲁棒性。
图4是根据本公开另一实施例提供的基于先验地图约束的煤矿井下单目视觉定位装置的示意图，如图4所示，该基于先验地图约束的煤矿井下单目视觉定位装置40包括：
获取模块401，用于获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，第一图像对应有绝对位姿；
第一确定模块402，用于确定第一图像和第二图像的多个匹配点对；
第一计算模块403，用于根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值；
第二计算模块404，用于根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值；以及
第二确定模块405，用于根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿。
一些实施例，第一计算模块402，具体用于：基于单目相机的内参数矩阵，将多个匹配点对转化为多个对极支撑向量对；根据多个对极支撑向量对，确定对极平面的法向量矩阵；根据法向量矩阵，计算第一图像和第二图像之间的相对旋转矩阵；以及根据相对旋转矩阵和绝对位姿的绝对旋转矩阵，确定旋转矩阵初值。
一些实施例，第二计算模块403，具体用于：根据先验地图估计煤矿井下几何平面的平面方程；基于平面方程、内参数矩阵、绝对位姿，计算每个匹配点对对应的空间三维点的深度；根据内参数矩阵和每个匹配点对中第一匹配点和第二匹配点的二维坐标，分别计算空间三维点的第一齐次坐标和第二齐次坐标，其中，第一匹配点为匹配点对中第一图像的图像点，第二匹配点为第二图像的图像点；根据第一齐次坐标和深度计算空间三维点的第一三维坐标，并根据第二齐次坐标和深度计算空间三维点的第二三维坐标；根据第一三维坐标、第二三维坐标以及相对旋转矩阵，求解相对位移；以及根据相对位移和绝对位姿的绝对平移向量，确定平移向量初值。
一些实施例，第二计算模块403，具体用于：在第一匹配点属于煤矿井下几何平面的情况下，根据第一三维坐标、第二三维坐标以及相对旋转矩阵的第一约束关系，求解相对位移；在第一匹配点不属于煤矿井下几何平面的情况下，根据第一三维坐标、第二三维坐标以及相对旋转矩阵的第二约束关系，求解相对位移。
一些实施例，装置40还包括：目标检测模块，用于对第二图像进行目标检测和分割，以确定第二图像中的第一矢量目标；匹配模块，用于确定第一矢量目标和先验地图中第二矢量目标的匹配关系；以及第一优化模块，用于根据匹配关系，对旋转矩阵初值和平移向量初值进行优化。
一些实施例，装置40还包括：窗口定义模块，用于定义滑动窗口，其中，滑动窗口内包括连续的多帧图像，多帧图像包括第二图像；以及第二优化模块，用于通过最小化代价函数的方式，对滑动窗口内多帧图像的位姿进行优化。
本实施例中，通过获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，第一图像对应有绝对位姿，并确定第一图像和第二图像的多个匹配点对，并根据多个匹配点对的对极约束关系和绝对位姿，计算第二图像的旋转矩阵初值，并根据多个匹配点对、绝对位姿以及煤矿井下的先验地图，计算第二图像的平移向量初值，以及根据旋转矩阵初值和平移向量初值，确定第二图像的目标位姿，能够将旋转矩阵初值和平移向量初值的估计进行解耦，分步估计旋转矩阵和平移向量初值，有效避免煤矿井下巷道特殊几何结构带来的姿态退化估计问题，此外在计算过程中利用井下平面的先验地图，增强了位姿估计中的约束，提高了单目视觉定位的精度和鲁棒性。
根据本公开的实施例，本公开还提供了一种电子设备、一种可读存储介质和一种计算机程序产品。
为了实现上述实施例，本申请还提出一种计算机程序产品，当计算机程序产品中的指令处理器执行时，执行如本申请前述实施例提出的基于先验地图约束的煤矿井下单目视觉定位方法。
图5示出了适于用来实现本申请实施方式的示例性计算机设备的框图。图5显示的计算机设备12仅仅是一个示例，不应对本申请实施例的功能和使用范围带来任何限制。
如图5所示，计算机设备12以通用计算设备的形式表现。计算机设备12的组件可以包括但不限于：一个或者多个处理器或者处理单元16，***存储器28，连接不同***组件（包括***存储器28和处理单元16）的总线18。
总线18表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器，***总线，图形加速端口，处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说，这些体系结构包括但不限于工业标准体系结构（Industry StandardArchitecture；以下简称：ISA）总线，微通道体系结构（Micro Channel Architecture；以下简称：MAC）总线，增强型ISA总线、视频电子标准协会（Video Electronics StandardsAssociation；以下简称：VESA）局域总线以及***组件互连（Peripheral ComponentInterconnection；以下简称：PCI）总线。
计算机设备12典型地包括多种计算机***可读介质。这些介质可以是任何能够被计算机设备12访问的可用介质，包括易失性和非易失性介质，可移动的和不可移动的介质。
存储器28可以包括易失性存储器形式的计算机***可读介质，例如随机存取存储器（Random Access Memory；以下简称：RAM）30和/或高速缓存存储器32。计算机设备12可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机***存储介质。仅作为举例，存储***34可以用于读写不可移动的、非易失性磁介质（图5未显示，通常称为“硬盘驱动器”）。
尽管图5中未示出，可以提供用于对可移动非易失性磁盘（例如“软盘”）读写的磁盘驱动器，以及对可移动非易失性光盘（例如：光盘只读存储器（Compact Disc Read OnlyMemory；以下简称：CD-ROM）、数字多功能只读光盘（Digital Video Disc Read OnlyMemory；以下简称：DVD-ROM）或者其它光介质）读写的光盘驱动器。在这些情况下，每个驱动器可以通过一个或者多个数据介质接口与总线18相连。存储器28可以包括至少一个程序产品，该程序产品具有一组（例如至少一个）程序模块，这些程序模块被配置以执行本申请各实施例的功能。
具有一组（至少一个）程序模块42的程序/实用工具40，可以存储在例如存储器28中，这样的程序模块42包括但不限于操作***、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块42通常执行本申请所描述的实施例中的功能和/或方法。
计算机设备12也可以与一个或多个外部设备14（例如键盘、指向设备、显示器24等）通信，还可与一个或者多个使得用户能与该计算机设备12交互的设备通信，和/或与使得该计算机设备12能与一个或多个其它计算设备进行通信的任何设备（例如网卡，调制解调器等等）通信。这种通信可以通过输入/输出（I/O）接口22进行。并且，计算机设备12还可以通过网络适配器20与一个或者多个网络（例如局域网（Local Area Network；以下简称：LAN），广域网（Wide Area Network；以下简称：WAN）和/或公共网络，例如因特网）通信。如图所示，网络适配器20通过总线18与计算机设备12的其它模块通信。应当明白，尽管图中未示出，可以结合计算机设备12使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID***、磁带驱动器以及数据备份存储***等。
处理单元16通过运行存储在***存储器28中的程序，从而执行各种功能应用以及数据处理，例如实现前述实施例中提及的基于先验地图约束的煤矿井下单目视觉定位方法。
本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本申请的其它实施方案。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本申请的真正范围和精神由下面的权利要求指出。
应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求来限制。
需要说明的是，在本申请的描述中，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性。此外，在本申请的描述中，除非另有说明，“多个”的含义是两个或两个以上。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本申请的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本申请的实施例所属技术领域的技术人员所理解。
应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列（PGA），现场可编程门阵列（FPGA）等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
此外，在本申请各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。
上述提到的存储介质可以是只读存储器，磁盘或光盘等。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种基于先验地图约束的煤矿井下单目视觉定位方法，其特征在于，所述方法包括：
获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，所述第一图像对应有绝对位姿；
确定所述第一图像和所述第二图像的多个匹配点对；
根据所述多个匹配点对的对极约束关系和所述绝对位姿，计算所述第二图像的旋转矩阵初值；
根据所述多个匹配点对、所述绝对位姿以及所述煤矿井下的先验地图，计算所述第二图像的平移向量初值；以及
根据所述旋转矩阵初值和所述平移向量初值，确定所述第二图像的目标位姿。
2.如权利要求1所述的方法，其特征在于，所述根据所述多个匹配点对的对极约束关系和所述绝对位姿，计算所述第二图像的旋转矩阵初值，包括：
基于所述单目相机的内参数矩阵，将所述多个匹配点对转化为多个对极支撑向量对；
根据所述多个对极支撑向量对，确定对极平面的法向量矩阵；
根据所述法向量矩阵，计算所述第一图像和所述第二图像之间的相对旋转矩阵；以及
根据所述相对旋转矩阵和所述绝对位姿的绝对旋转矩阵，确定所述旋转矩阵初值。
3.如权利要求2所述的方法，其特征在于，所述根据所述多个匹配点对、所述绝对位姿以及所述煤矿井下的先验地图，计算所述第二图像的平移向量初值，包括：
根据所述先验地图估计煤矿井下几何平面的平面方程；
基于所述平面方程、所述内参数矩阵、所述绝对位姿，计算每个匹配点对对应的空间三维点的深度；
根据所述内参数矩阵和每个匹配点对中第一匹配点和第二匹配点的二维坐标，分别计算所述空间三维点的第一齐次坐标和第二齐次坐标，其中，所述第一匹配点为所述匹配点对中第一图像的图像点，第二匹配点为所述第二图像的图像点；
根据所述第一齐次坐标和所述深度计算所述空间三维点的第一三维坐标，并根据所述第二齐次坐标和所述深度计算所述空间三维点的第二三维坐标；
根据所述第一三维坐标、第二三维坐标以及所述相对旋转矩阵，求解相对位移；以及
根据所述相对位移和所述绝对位姿的绝对平移向量，确定所述平移向量初值。
4.如权利要求3所述的方法，其特征在于，所述根据所述第一三维坐标、第二三维坐标以及所述相对旋转矩阵，求解相对位移，包括：
在所述第一匹配点属于所述煤矿井下几何平面的情况下，根据所述第一三维坐标、第二三维坐标以及所述相对旋转矩阵，确定第一约束关系；
在所述第一匹配点不属于所述煤矿井下几何平面的情况下，根据所述第一三维坐标、第二三维坐标以及所述相对旋转矩阵，确定第二约束关系；以及
根据所述第一约束关系和所述第二约束关系，求解所述相对位姿。
5.如权利要求1所述的方法，其特征在于，所述根据所述旋转矩阵初值和所述平移向量初值，确定所述第二图像的目标位姿之后，还包括：
对所述第二图像进行目标检测和分割，以确定所述第二图像中的第一矢量目标；
确定所述第一矢量目标和所述先验地图中第二矢量目标的匹配关系；以及
根据所述匹配关系，对所述旋转矩阵初值和平移向量初值进行优化。
6.如权利要求5所述的方法，其特征在于，所述方法还包括：
定义滑动窗口，其中，所述滑动窗口内包括连续的多帧图像，所述多帧图像包括所述第二图像；以及
通过最小化代价函数的方式，对所述滑动窗口内多帧图像的位姿进行优化。
7.一种基于先验地图约束的煤矿井下单目视觉定位装置，其特征在于，包括：
获取模块，用于获取单目相机在煤矿井下连续采集的第一图像和第二图像，其中，所述第一图像对应有绝对位姿；
第一确定模块，用于确定所述第一图像和所述第二图像的多个匹配点对；
第一计算模块，用于根据所述多个匹配点对的对极约束关系和所述绝对位姿，计算所述第二图像的旋转矩阵初值；
第二计算模块，用于根据所述多个匹配点对、所述绝对位姿以及所述煤矿井下的先验地图，计算所述第二图像的平移向量初值；以及
第二确定模块，用于根据所述旋转矩阵初值和所述平移向量初值，确定所述第二图像的目标位姿。
8.如权利要求7所述的装置，其特征在于，所述第一计算模块，具体用于：
基于所述单目相机的内参数矩阵，将所述多个匹配点对转化为多个对极支撑向量对；
根据所述多个对极支撑向量对，确定对极平面的法向量矩阵；
根据所述法向量矩阵，计算所述第一图像和所述第二图像之间的相对旋转矩阵；以及
根据所述相对旋转矩阵和所述绝对位姿的绝对旋转矩阵，确定所述旋转矩阵初值。
9.如权利要求8所述的装置，其特征在于，所述第二计算模块，具体用于：
根据所述先验地图估计煤矿井下几何平面的平面方程；
基于所述平面方程、所述内参数矩阵、所述绝对位姿，计算每个匹配点对对应的空间三维点的深度；
根据所述内参数矩阵和每个匹配点对中第一匹配点和第二匹配点的二维坐标，分别计算所述空间三维点的第一齐次坐标和第二齐次坐标，其中，所述第一匹配点为所述匹配点对中第一图像的图像点，第二匹配点为所述第二图像的图像点；
根据所述第一齐次坐标和所述深度计算所述空间三维点的第一三维坐标，并根据所述第二齐次坐标和所述深度计算所述空间三维点的第二三维坐标；
根据所述第一三维坐标、第二三维坐标以及所述相对旋转矩阵，求解相对位移；以及
根据所述相对位移和所述绝对位姿的绝对平移向量，确定所述平移向量初值。
10.一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使计算机执行根据权利要求1-6中任一项所述的方法。
CN202211077044.5A 2022-09-05 2022-09-05 基于先验地图约束的煤矿井下单目视觉定位方法及装置 Active CN115170847B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211077044.5A CN115170847B (zh) 2022-09-05 2022-09-05 基于先验地图约束的煤矿井下单目视觉定位方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211077044.5A CN115170847B (zh) 2022-09-05 2022-09-05 基于先验地图约束的煤矿井下单目视觉定位方法及装置

Publications (2)

Publication Number Publication Date
CN115170847A true CN115170847A (zh) 2022-10-11
CN115170847B CN115170847B (zh) 2022-12-13

Family

ID=83481860

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211077044.5A Active CN115170847B (zh) 2022-09-05 2022-09-05 基于先验地图约束的煤矿井下单目视觉定位方法及装置

Country Status (1)

Country Link
CN (1) CN115170847B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110298884A (zh) * 2019-05-27 2019-10-01 重庆高开清芯科技产业发展有限公司 一种适于动态环境中单目视觉相机的位姿估计方法
US20190325597A1 (en) * 2018-04-24 2019-10-24 Ford Global Technologies, Llc Simultaneous Localization And Mapping Constraints In Generative Adversarial Networks For Monocular Depth Estimation
CN111797906A (zh) * 2020-06-15 2020-10-20 北京三快在线科技有限公司 基于视觉和惯性里程定位的方法及装置
CN111862200A (zh) * 2020-06-30 2020-10-30 同济大学 一种煤棚内无人机定位方法
CN111913499A (zh) * 2020-07-17 2020-11-10 天津大学 基于单目视觉slam及深度不确定性分析的云台控制方法
CN114429487A (zh) * 2021-12-29 2022-05-03 浙江大学 基于平面运动的机器人视觉特征匹配和定位方法及装置

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190325597A1 (en) * 2018-04-24 2019-10-24 Ford Global Technologies, Llc Simultaneous Localization And Mapping Constraints In Generative Adversarial Networks For Monocular Depth Estimation
CN110298884A (zh) * 2019-05-27 2019-10-01 重庆高开清芯科技产业发展有限公司 一种适于动态环境中单目视觉相机的位姿估计方法
CN111797906A (zh) * 2020-06-15 2020-10-20 北京三快在线科技有限公司 基于视觉和惯性里程定位的方法及装置
CN111862200A (zh) * 2020-06-30 2020-10-30 同济大学 一种煤棚内无人机定位方法
CN111913499A (zh) * 2020-07-17 2020-11-10 天津大学 基于单目视觉slam及深度不确定性分析的云台控制方法
CN114429487A (zh) * 2021-12-29 2022-05-03 浙江大学 基于平面运动的机器人视觉特征匹配和定位方法及装置

Also Published As

Publication number Publication date
CN115170847B (zh) 2022-12-13

Similar Documents

Publication Publication Date Title
CN110322500B (zh) 即时定位与地图构建的优化方法及装置、介质和电子设备
CN108986037B (zh) 基于半直接法的单目视觉里程计定位方法及定位***
US9420265B2 (en) Tracking poses of 3D camera using points and planes
CN110807809B (zh) 基于点线特征和深度滤波器的轻量级单目视觉定位方法
US8761439B1 (en) Method and apparatus for generating three-dimensional pose using monocular visual sensor and inertial measurement unit
WO2018049581A1 (zh) 一种同时定位与地图构建方法
JP5832341B2 (ja) 動画処理装置、動画処理方法および動画処理用のプログラム
EP2715667B1 (en) Planar mapping and tracking for mobile devices
CN109461208B (zh) 三维地图处理方法、装置、介质和计算设备
CN108776989B (zh) 基于稀疏slam框架的低纹理平面场景重建方法
Liu et al. Direct visual odometry for a fisheye-stereo camera
US8463024B1 (en) Combining narrow-baseline and wide-baseline stereo for three-dimensional modeling
CN110349212B (zh) 即时定位与地图构建的优化方法及装置、介质和电子设备
CN113012197B (zh) 一种适用于动态交通场景的双目视觉里程计定位方法
AliAkbarpour et al. Parallax-tolerant aerial image georegistration and efficient camera pose refinement—without piecewise homographies
CN116468786B (zh) 一种面向动态环境的基于点线联合的语义slam方法
Ok et al. Simultaneous tracking and rendering: Real-time monocular localization for MAVs
CN111829522B (zh) 即时定位与地图构建方法、计算机设备以及装置
CN113763466B (zh) 一种回环检测方法、装置、电子设备和存储介质
CN115170847B (zh) 基于先验地图约束的煤矿井下单目视觉定位方法及装置
CN110211148B (zh) 一种基于目标状态预估的水下图像预分割方法
CN105339981A (zh) 用于使用一组基元配准数据的方法
CN115830116A (zh) 一种鲁棒视觉里程计方法
US11514588B1 (en) Object localization for mapping applications using geometric computer vision techniques
Eudes et al. Weighted local bundle adjustment and application to odometry and visual slam 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
GR01 Patent grant
GR01 Patent grant