[go: up one dir, main page]

CN111795704B - 一种视觉点云地图的构建方法、装置 - Google Patents

一种视觉点云地图的构建方法、装置 Download PDF

Info

Publication number
CN111795704B
CN111795704B CN202010615170.6A CN202010615170A CN111795704B CN 111795704 B CN111795704 B CN 111795704B CN 202010615170 A CN202010615170 A CN 202010615170A CN 111795704 B CN111795704 B CN 111795704B
Authority
CN
China
Prior art keywords
key frame
frame
feature points
map
image
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
CN202010615170.6A
Other languages
English (en)
Other versions
CN111795704A (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.)
Hangzhou Hikrobot Co Ltd
Original Assignee
Hangzhou Hikrobot Technology 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 Hangzhou Hikrobot Technology Co Ltd filed Critical Hangzhou Hikrobot Technology Co Ltd
Priority to CN202010615170.6A priority Critical patent/CN111795704B/zh
Publication of CN111795704A publication Critical patent/CN111795704A/zh
Priority to PCT/CN2021/103653 priority patent/WO2022002150A1/zh
Application granted granted Critical
Publication of CN111795704B publication Critical patent/CN111795704B/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G06V20/46—Extracting features or characteristics from the video content, e.g. video fingerprints, representative shots or key frames
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/60—Type of objects
    • G06V20/62—Text, e.g. of license plates, overlay texts or captions on TV images

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Multimedia (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

Abstract

本申请公开了一种视觉点云地图的构建方法，该方法包括，对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点，对源图像帧进行帧间跟踪，确定关键帧，将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点，计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息，其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图。本发明构建地图的过程将建图和定位予以分离，有效地除去了建图与定位的相互影响，对于在复杂多变的环境中，具有更好的适应性和稳定性。

Description

一种视觉点云地图的构建方法、装置
技术领域
本发明涉及导航定位领域，特别地，涉及一种视觉点云地图的构建方法。
背景技术
构建地图与定位是即时定位与地图(SLAM)研究中的重点技术，而构建地图是实现定位的前提条件，地图的好坏直接影响到定位的精度。视觉点云地图是所构建的地图中的一种，其通过空间中的三维点集，描述了环境中点的视觉、位姿等信息，故而，构建视觉点云地图需要两类数据信息：关键帧和地图点，其中，关键帧描述了环境中点的视觉，地图点描述了点的位姿，由大量地图点形成的集合构成了点云。
SLAM是将机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的地图特征定位自身位置和姿态，再根据自身位置增量式的构建地图，从而达到同时定位和地图构建的目的。从输入而言，在机器人运行之前，没有特别的输入，开始运行的时候，有传感器原始数据，从输出而言，估计位姿和估计地图；也就是，在建立新地图模型或者改进已知地图的同时，在该地图模型上定位机器人，这类似于把一个人放到陌生的城市，让这个人熟悉环境的过程，可见，现有的SLAM地图构建通常是将建图问题和定位问题耦合在一起，二者相互影响。
发明内容
本发明提供了一种视觉点云地图的构建方法，以避免定位对建图的影响。
本发明提供的一种视觉点云地图的构建方法是这样实现的：
对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点，
对源图像帧进行帧间跟踪，确定关键帧，
将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点，
计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息，
其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图。
较佳地，所述对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点，进一步包括，
对源图像帧进行图像预处理，得到目标图像帧，
基于目标图像帧进行特征提取，得到目标图像帧的特征点；
该方法进一步包括，
基于第一视觉点云地图，
根据存在闭环约束的闭环关键帧，采用最小二乘法，对关键帧位姿进行图优化，
和/或，根据重投影误差，对地图点的空间位置信息进行优化，
得到第二视觉点云地图。
本发明提供的一种视觉点云地图的构建装置，包括，第一视觉点云地图构建单元，该单元包括，
特征提取模块，对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点，
地图点生成模块，对源图像帧进行帧间跟踪，确定关键帧，将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点，计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息，
其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图。
本发明提供的视觉点云地图的构建方法，通过对待建地图的空间所采集的图像帧进行特征提取，通过帧间匹配，获取匹配特征点的空间位置信息，将匹配特征点作为地图点，得到由所有关键帧的地图点集合所构成的视觉点云地图，实现了物理环境中三维点的生成和描述。本发明构建地图的过程将建图和定位予以分离，有效地除去了建图与定位的相互影响，对于在复杂多变的环境中，具有更好的适应性和稳定性。由于点云地图具有连续性，相比于有地图节点所构成的地图，能实现连续的定位，避免了定位过程中的跳变问题，降低了重定位发生概率。进一步地，通过对地图关键帧位姿优化和/或地图点位姿优化，提高了地图的精度，在发生错误闭环情况，能够及时对地图进行修正，且不会丢失初始地图数据，扩增性强，有利于与改进的地图构建方法进行融合。
附图说明
图1为基于单目相机所采集的图像数据构建地图的一种流程示意图。
图2为特征点筛选示意图。
图3为基于单目相机所采集的前视图像数据构建地图的一种流程示意图。
图4为基于双目相机所采集的图像数据构建地图的一种流程示意图
图5为累计误差的一种示意图。
图6为对第一视觉点云地图进行优化的一种流程示意图。
图7为视觉字典的一种示意图。
图8为本申请的视觉点云地图构建装置的一种示意图。
图9为图像预处理模块的一种示意图。
具体实施方式
为了使本申请的目的、技术手段和优点更加清楚明白，以下结合附图对本申请做进一步详细说明。
本申请基于采集的图像数据，通过特征提取和帧间跟踪的特征点匹配，从而获得视觉点云地图；进一步地，通过闭环约束进行位姿图优化和/或通过重投影误差进行地图点优化，以提高地图的精度。所构建的视觉点云地图至少包括关键帧位姿信息和地图点位置信息，其中，每个地图点还具有特征点描述符信息。
实施例一
为便于理解，在本实施例中，以图像数据是由单目相机采集、图像为地面纹理为例来说明。所应理解的是，本实施例可不限于地面纹理图像，其它图像也可适用。
参见图1所示，图1为基于单目相机所采集的图像数据构建地图的一种流程示意图。主要包括图像预处理、特征提取、以及帧间跟踪，具体地，对于每一帧源图像，执行如下步骤：
步骤101，将所采集的图像作为源图像，对图像进行预处理，得到目标图像，以便于提取图像中的特征点，例如，对于地面纹理图像，所进行的预处理的目的是得到以纹理信息为主的图像。
步骤1011，根据相机的畸变系数对源图像进行去畸变处理，得到去畸变图像I(u，v)，其中，u、v表示像素坐标，I(u，v)表示该像素坐标的像素值。
步骤1012，对于去畸变图像I(u，v)，进行图像滤波，得到背景图像Ib(u，v)。例如，进行高斯滤波，其中高斯滤波核大小可以设置为45×45。用数学式表达为：
Ib(u，v)＝G×I(u，v)，
其中，G为滤波核，Ib(u，v)为背景图像，I(u，v)为去畸变图像；
较佳地，图像滤波核一般设置的比较大，使得滤波后的图像尽可能的接近背景图像。
若纹理区域为图像中的较暗部分，可先将去畸变图像I(u，v)进行取反操作，用数学式表达为：像素最大值-I(u，v)，
例如，像素最大值为255，则取反操作为：255-I(u，v)。
步骤1013，用去畸变图像减去背景图像，得到以纹理信息为主的前景图像If(u，v)，数学式表达为：
If(u，v)＝I(u，v)-Ib(u，v)
步骤1014，对前景图像进行拉伸，得到目标图像；
在该步骤中，由于通常采集的图像中纹理信息较弱，纹理区域的像素值(灰度)大多分布在狭窄的灰度区间，因此将该区间的像素值拉伸到像素取值范围上。具体为，
当前景图像像素值小于等于最小灰度值时，将该前景图像像素值取值为像素取值范围内的最小值，即像素值为0；
当前景图像像素值大于最小灰度值、且小于最大灰度值时，将增加该前景图像像素值的对比度；较佳地，按照与像素最大值成一定比例的像素值作为该前景图像像素值；较佳地，所述比例为：前景图像像素值与最小灰度值之差与最大灰度值与最小灰度值之差的比值。
当前景图像像素值大于等于最大灰度值时，将该前景图像像素值取值为像素取值范围内的最大值，例如，像素最大值为255。
用数学式表达为：
拉伸后的前景图像If'(u，v)表示为：
Figure BDA0002561641320000041
其中，Imin为最小灰度值，Imax为最大灰度值，在上式中，像素取值范围为0～255。
图像预处理有利于提升图像的对比度，在一些纹理信息较弱的环境，有利于提高纹理区域灰度的对比度，从而提取到更多的特征点。
步骤102，基于当前目标图像帧提取特征点，以将图像信息转换为特征信息，得到当前目标图像帧的特征点集合；可采用ORB(Oriented FAST and Rotated BRIEF)、尺度不变特征变换(SIFT)、SURF(Speeded Up Robust Features)等特征。
以ORB算法为例，基于一帧目标图像：
步骤1021，采用FAST(Features from Accelerated Segment Test)算法进行特征检测，得到FAST特征点，
步骤1022，对FAST特征点进行筛选，以有效控制特征点的规模。为了保证特征点分布均匀的同时尽可能筛选出显著的特征点，将目标图像划分成一定数量的网格，如图2所示，图2为特征点筛选示意图。所有的特征点按网格进行筛选：针对任一网格，将该网格内的特征点按FAST特征点响应值降序排列，保留前Q个特征点，其中，Q根据一帧目标图像中特征点的数量和所设特征点总数上限、以及该网格中的特征点总数确定，不同网格所保留的特征点数可以不同。例如，一帧目标图像中，特征点总数的上限设定为100个，当前目标图像特征点的数量为2000个，则该帧目标图像中每20个特征点选出一个，如果某网格中有20个特征点，则该网格保留的特征点为1，即，Q＝1。用数学式表达为：
Figure BDA0002561641320000051
其中，符号
Figure BDA0002561641320000052
表示向下取整。
步骤1023，对筛选出来的每个FAST特征点，分别确定该FAST特征点的方向，也就是说计算特征点以r为半径范围内的质心，特征点坐标到质心形成一个向量作为该特征点的方向。
步骤1024，对筛选出来的每个FAST特征点，计算一个二进制串的特征描述符，从而得到当前目标图像中的特征点信息。具体地，可以采用rBRIEF、oBRIEF等特征描述。
步骤103，帧间跟踪以对前后帧中的特征点进行匹配，计算匹配特征点在世界坐标系下的坐标，作为具有三维空间位置信息的地图点保存。
在该步骤中，对当前已提取特征点的目标图像帧：
步骤1031，判断当前目标图像帧是否为首帧，如果是，则将该目标图像帧作为关键帧，否则，则执行步骤1032，进行帧间匹配以确定当前目标图像帧是否为关键帧，
步骤1032，将当前目标图像帧与上一关键帧进行匹配，即：
对于当前目标图像帧的任一特征点i，计算当前目标图像帧中的特征点i与上一关键帧中特征点i描述符之间的匹配度是否小于设定的匹配阈值，如果是，则判定两特征点匹配，否则，判定该两特征点不匹配；其中，匹配度可以用汉明距离来描述，匹配阈值为汉明距离阈值；
步骤1033，根据关键帧条件判断当前目标图像帧是否为关键帧，如果是，则将当前目标图像帧作为关键帧，执行步骤1034，以基于该关键帧进行地图更新，否则，则不进行地图更新。
在该步骤1033中，当匹配特征点数量大于设定的第一阈值时，判定当前目标图像帧为关键帧；
关键帧条件还可以是以下条件之一：
与上一关键帧之间的空间距离大于设定的第二阈值；
与上一关键帧之间的空间角度大于设定的第三阈值；
当关键帧条件为处理匹配特征点数量大于设定的第一阈值之外的条件时，步骤1033与步骤1032进行对调，即：先确定关键帧，然后再确定匹配特征点。
步骤1034，基于当前关键帧，计算各个匹配的特征点(简称为匹配特征点)的坐标，并作为地图点信息保存；
由于每一当前关键帧与上一关键帧的匹配特征点不尽相同，这样，上一关键帧中匹配特征点会被该步骤1034的计算结果更新，而未匹配特征点的坐标则未被更新，从而使得得到的当前地图信息包括，未更新的地图点信息和已更新的地图点信息，其中每个地图点对应有三维空间位置信息。
在本实施例中，鉴于单目相机所采集的地面纹理处于同一平面，相机通常安装于移动机器人底部，故而当前关键帧任一匹配特征点i在世界坐标系下的坐标可以通过相机的外参将该匹配特征点投影到图像平面上，具体为：
x坐标为当前关键帧匹配特征点i的像素横坐标u与相机安装高度的乘积结果与相机焦距的比值，
y坐标为当前关键帧匹配特征点i的像素纵坐标v与相机安装高度的乘积结果与相机焦距的比值，
z坐标为相机安装高度。
用数学式表达：
Figure BDA0002561641320000061
Figure BDA0002561641320000062
Z＝H
其中，H为相机的安装高度，f为相机焦距，匹配特征点i在图像坐标系中的像素坐标为(u，v)。
反复执行步骤101～103，直至所有的源图像处理完毕，得到由大量地图点构成的第一视觉点云地图。
本实施例提供了一种基于地面纹理的视觉点云构建方法，通过对所采集的源图像的后处理，以相邻关键帧进行特征点匹配，基于匹配的特征点的像素坐标生成地图点的空间三维坐标，从而得到视觉点云地图，本实施例地图构建过程中无需移动机器人的定位信息，避免了定位信息对地图构建的影响，相比于基于移动机器人定位信息构建地图节点的构建方式，避免了由于地图节点之间有间距而导致的地图信息离散(不连续)，实现了连续地图的构建，使得定位应用中能实现连续的定位，无跳变问题存在。
实施例二
在本实施例中，以图像数据是由单目相机采集、所采集的图像为非同一平面为例来说明。例如，相机采用前视安装，即，移动机器人通过前视相机采集图像。
参见图3所示，图3为基于单目相机所采集的前视图像数据构建地图的一种流程示意图。对于每一帧图像，执行如下步骤：
步骤301，根据相机的畸变系数对源图像进行去畸变处理，得到去畸变图像I(u，v)，其中，u、v表示像素坐标。
步骤302，判断去畸变图像中各个像素点的像素值是否大于设定的第一像素阈值，如果是，则将像素值大于第一像素阈值的像素点进行取反操作，然后对进行了取反操作后的去畸变图像进行滤波，否则，直接对去畸变图像I(u，v)，进行图像滤波，得到背景图像Ib(u，v)。
步骤303，用去畸变图像减去背景图像，得到前景图像If(u，v)，其中，前景图像的数学式表达为：
If(u，v)＝I(u，v)-Ib(u，v)
步骤304，判断前景图像If(u，v)的像素值是否分布均匀，如果是，则将前景图像作为目标图像，否则，则对前景图像进行拉伸，得到目标图像，具体拉伸处理与步骤1014相同；
在本步骤中，使得高图像质量不进行图像拉伸处理，而低图像质量进行图像拉伸处理，从而使得图像拉伸处理根据图像质量进行选择性处理。
步骤305，基于当前目标图像帧提取特征点，以将图像信息转换为特征信息，得到当前目标图像帧的特征点集合；可采用ORB(Oriented FAST and Rotated BRIEF)、尺度不变特征变换(SIFT)、SIFT的高效改良版SURF(Speeded Up Robust Features)等特征。该步骤305与步骤102相同。
步骤306，判断当前目标图像帧是否为首帧，如果是，则将该目标图像帧作为关键帧，然后返回步骤310，否则，则执行步骤307，进行帧间匹配以确定当前目标图像帧是否为关键帧，
步骤307，将当前目标图像帧与上一关键帧进行匹配，即：
对于当前目标图像帧的任一特征点i，计算当前目标图像帧中的特征点i与上一关键帧中特征点i描述符之间的匹配度是否小于设定的匹配阈值，如果是，则判定两特征点匹配，否则，判定该两特征点不匹配；其中，匹配度可以用汉明距离来描述，匹配阈值为汉明距离阈值；
步骤308，根据关键帧条件判断当前目标图像帧是否为关键帧，如果是，则将当前目标图像帧作为关键帧，执行步骤309，以基于该关键帧进行地图更新，否则，则不进行地图更新，直接执行步骤310。当满足以下关键帧条件之一时，判定当前目标图像帧为关键帧：
匹配特征点数量大于设定的第一阈值；
与上一关键帧之间的空间距离大于设定的第二阈值；
与上一关键帧之间的空间角度大于设定的第三阈值；
步骤309，基于当前关键帧，计算各个匹配特征点的坐标，并作为地图点信息保存，这样，得到的当前地图信息包括，未更新的地图点信息和已更新的地图点信息，其中每个地图点对应有三维空间位置信息。
在本实施例中，鉴于单目相机所采集图像处于非同一平面，故而，根据匹配特征点像素坐标，采用八点法计算两帧之间的本质矩阵，对本质矩阵进行SVD分解得到相机相对位姿，然后基于三角化计算原理，根据两帧的位姿，采用最小二乘法计算任一匹配特征点i在世界坐标系下的坐标。具体按照以下步骤计算：
步骤3091，根据本质矩阵E和当前关键帧匹配特征点i的归一化平面坐标p1、上一关键帧匹配特征点i归一化平面坐标p2满足：对于任一匹配特征点，上一关键帧中该匹配特征点归一化平面坐标的转置矩阵、本质矩阵、当前关键帧该匹配特征点的归一化平面坐标的矩阵之乘积等于0的关系。用数学式表达为：
Figure BDA0002561641320000081
其中，本质矩阵E是反映空间一点P的像点在不同视角相机下相机坐标系中的表示之间的关系，为3×3矩阵，其作用是，第一图像帧上的一个点被本质矩阵相乘，其结果为此点在第二图像帧上的对极线；当前关键帧匹配特征点i的归一化平面坐标
Figure BDA0002561641320000091
上一关键帧匹配特征点i归一化平面坐标
Figure BDA0002561641320000092
代入八对匹配特征点的归一化平面坐标，可求解出本质矩阵。其中，当前关键帧匹配特征点i与上一关键帧匹配特征点i组成一对匹配特征点。
步骤3092，对本质矩阵E进行奇异值(SVD)分解，得到当前关键帧与上一关键帧之间帧间相对位姿，即相机的相对位姿，包括平移矩阵t和旋转矩阵R。
步骤3093，基于三角化计算原理，当前关键帧匹配特征点i的深度值s1、上一关键帧匹配特征点i的深度值s2满足：
s1p1＝s2Rp2+t
采用最小二乘法，可以求解得出s1和s2。
或者，
将上式两边同时乘以p1的反对称矩阵p1^，可得：
s1p1^p1＝0＝s2p1^Rp2+p1^t
由此可求得s2，将求得的s2代入式1，得到s1。
步骤3094，根据当前关键帧匹配特征点i的深度值s1，计算当前关键帧匹配特征点i在世界坐标系下的坐标：
x坐标为：当前关键帧中该匹配特征点归一化平面的像素横坐标与该匹配特征点深度值的乘积，
y坐标为：当前关键帧中该匹配特征点归一化平面的像素纵坐标与该匹配特征点深度值的乘积，
z坐标为相机焦距。
数学式表示为：
X＝s1u1
Y＝s1v1
Z＝f
其中，f为相机焦距，以将归一化平面坐标转化为成像平面。
步骤310，判断源图像是否处理完毕，如果是，则结束，否则，处理下一帧，返回执行步骤301，直至所有的源图像处理完毕，得到由大量地图点构成的第一视觉点云地图。
本实施例提供了一种基于前视采集的非同一平面图像的视觉点云构建方法，通过对所采集的源图像的后处理，以相邻关键帧进行特征点匹配，基于匹配特征点的像素坐标生成地图点的空间三维坐标，从而得到视觉点云地图，本实施例地图构建过程中无需移动机器人的定位信息，使得地图构建与定位分离，提高了稳定性和复杂环境的适应能力。
实施例三
在本实施例中，以图像数据是由双目相机采集为例来说明，所采集的图像为非同一平面。
参见图4所示，图4为基于双目相机所采集的图像数据构建地图的一种流程示意图。对于每一双目图像帧，即，同一时间来自第一目的第一源图像帧和来自第二目的第二源图像帧，执行如下步骤：
步骤401，对第一源图像帧和第二源图像帧进行图像预处理，得到当前双目目标图像帧，包括第一目标图像帧和第二目标图像帧；
在该步骤中，可以并行地对第一图像帧和第二图像帧进行图像预处理，也可以分别串行地对第一图像帧和第二图像帧进行图像预处理，
具体的处理与步骤301～步骤304相同，
步骤402，基于当前双目目标图像帧，分别提取第一目标图像帧的特征点和第二目标图像帧的特征点，以将图像信息转换为特征信息，得到当前目标图像帧的特征点集合；可采用ORB(Oriented FAST and Rotated BRIEF)、尺度不变特征变换(SIFT)、SURF(SpeededUp Robust Features)等特征。该步骤与步骤102相同。
步骤403，判断当前双目目标图像帧是否为首个双目图像帧，如果是，则将该双目目标图像帧中的任一帧作为关键帧，执行步骤406，否则，则执行步骤404，进行帧间跟踪以确定当前双目目标图像帧中的任一帧是否为关键帧，
步骤404，为了提高匹配的效率，可将当前双目目标图像帧中的任一目标图像帧与上一关键帧进行匹配，可以得到目标图像帧的匹配特征点，
具体的匹配方式与步骤307相同。
步骤405，根据关键帧条件判断当前目标图像帧是否为关键帧，如果是，则将当前目标图像帧作为该双目目标图像帧的关键帧，执行步骤406，以基于该关键帧进行地图更新，否则，则不进行地图更新；
当满足以下关键帧条件之一时，判定当前目标图像帧为关键帧：
匹配特征点数量大于设定的第一阈值；
与上一关键帧之间的空间距离大于设定的第二阈值；
与上一关键帧之间的空间角度大于设定的第三阈值；
步骤406，基于当前关键帧中的第一匹配特征点，搜索双目图像帧中匹配成功的第二匹配特征点，计算第二匹配特征点的坐标，并作为地图点信息保存，这样，得到的当前地图信息包括，未更新的地图点信息和已更新的地图点信息，其中每个地图点对应有三维空间位置信息，即地图点坐标。
在本实施例中，当前关键帧中任一匹配特征点(第一匹配特征点)i的坐标的计算过程如下：
将当前关键帧作为当前双目目标图像帧中的第一帧，该帧中的匹配特征点已通过步骤404得到；将该双目目标图像帧中的另一目标图像帧作为第二帧；将第一帧中的匹配特征点i与第二帧中的特征点j进行匹配，即，计算匹配特征点i与特征点j描述符之间的匹配度，如果匹配，得到第二帧中的匹配特征点，则匹配特征点i的地图点坐标为：
x坐标为：当前关键帧中该匹配特征点的归一化平面的像素横坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点归一化平面的像素横坐标与第二帧中的匹配特征点归一化平面的像素横坐标之差的绝对值；
y坐标为：当前关键帧中该匹配特征点归一化平面的像素纵坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点归一化平面的像素横坐标与第二帧中的匹配特征点归一化平面的像素横坐标之差的绝对值；
z坐标为：相机焦距与双目基线长度的乘积，除以当前关键帧中该匹配特征点归一化平面的像素横坐标与第二帧中的匹配特征点归一化平面的像素横坐标之差的绝对值。
数学式表示为：
Figure BDA0002561641320000111
Figure BDA0002561641320000112
Figure BDA0002561641320000113
其中，(u1，v1)为第一帧匹配特征点的归一化平面像素坐标，(u3，v2)为第二帧特征点的归一化平面像素坐标，f表示相机焦距，b表示双目基线长度；
如果不匹配，则放弃匹配特征点i的坐标计算。
反复执行步骤401～406，直至所有的源双目图像处理完毕，得到由大量地图点构成的第一视觉点云地图。
本实施例提供了一种基于双目图像的视觉点云构建方法，利用双目图像获得匹配特征点的空间坐标，计算简单，构建过程中无需移动机器人的定位信息，使得地图构建与定位分离，提高了稳定性和复杂环境的适应能力。
鉴于第一视觉点云地图通过帧间图像的连续匹配、不断记录生成的地图点而得到，帧间连续匹配的地图点生成方式会产生累计误差，随着运动距离的增加，误差会越来越大。参见图5所示，图5为累计误差的一种示意图。图中，Ti表示第i帧数据的位置，左侧客观的真实轨迹，右侧为计算得到的轨迹，其中，T1和T19在同一位置附近，但计算的轨迹会因为累计误差而导致不在同一位置附近。为了消除累计误差，提高第一视觉点云地图中地图点空间位置信息的精度，通过构建闭环约束，采用最小二乘法进行优化。
参见图6所示，图6为对第一视觉点云地图进行优化的一种流程示意图。该优化方法主要包括，闭环点识别、闭环约束计算、地图优化，其中，地图优化包括位姿图优化和/或地图点优化。具体如下。
步骤601，通过人工标记或关键帧相似度计算，识别出第一视觉点云地图中存在闭环约束的关键帧。
实施方式之一，采用人工标记的方法：图像数据采集时在环境中布置唯一的标识图案，以使得不同次地采集到相同标识的关键帧之间产生闭环。该方式具有可靠性高的优点。
实施方式之二，自然标识的方法：通过计算两关键帧图像间的相似度是否大于设定的相似度阈值来判断是否发生闭环，其中相似度包括特征点分布上的相似度和图像像素的相似度。具体可以是，
构建自然特征的视觉字典，若存在N个特征点，构建一个深度为d，每次分叉为k的树，流程如下：
在根节点，用k均值聚类算法(k-means)把所有特征点聚成k类，这样得到了第一层节点。
对第一层的每个节点，把属于该节点的特征点再聚成k类，得到下一层。
依此类推，直至叶子层，其中，从根节点到叶子层共计d层。叶子层即为字典中的单词特征点。
参见图7所示，图7为视觉字典的一种示意图。
对于图像中所有特征数量为n，单词特征点ω出现的次数为ci，该单词特征点的权重表示为：
Figure BDA0002561641320000131
根据字典模型，任一关键帧A描述为以各个单词特征点ωi及其权重为元素的集合，数学式表达为：
A＝{(ω1，η1)，(ω2，η2)，……(ωN，ηN)}
其中，N为视觉字典中特征点的总数。
根据上述模型，可采用L1范数形式对任一两关键帧A和关键帧B之间的相似度S描述：
Figure BDA0002561641320000132
其中，vAi为根据字典模型所描述的关键帧A的集合中的元素，vBi为根据字典模型所描述的关键帧B的集合中的元素。
若相似度S大于设定的相似度阈值，判定两帧之间存在闭环约束。
步骤602，基于判定为存在闭环约束的关键帧(后文简称为闭环关键帧)，计算闭环约束。具体包括，
步骤6021，基于闭环关键帧，计算该闭环关键帧中的匹配特征点：
若第一闭环关键帧A和第二闭环关键帧B存在闭环，计算第一闭环关键帧A中的任一地图点i和第二闭环关键帧B中的任一地图点j的匹配度，若匹配度小于设定的匹配度阈值，则判定该两地图点匹配，如此反复地对每个地图点进行匹配，得到m个匹配特征点，数学式表达为：
P＝{p1，p2……pm}，P′＝{p1′，p2′……pm′}
其中，P为第一闭环关键帧中的m个匹配特征点集合，P为第二闭环关键帧中m个匹配特征点集合。
上述匹配度计算可以是计算两地图点描述符之间的汉明距离，若汉明距离小于设定的汉明阈值，则判定为匹配。
步骤6022，根据闭环关键帧中的匹配特征点，计算闭环关键帧之间的帧间运动信息，即，计算闭环关键帧之间的相对位姿，该相对位姿表征了累计误差。
鉴于闭环关键帧中的任一匹配特征点满足关系：
pi＝Rp′i+t
其中，(R，t)反映了两闭环关键帧之间的闭环约束，通过该关系可计算得到相对位姿，作为初始值；i为自然数，且，1≤i≤m，pi、p′i为像素坐标矩阵。
对于上述闭环约束的求解，可采用最小二乘法求解，例如，采用非线性优化的方法LM算法。通过构造第一目标函数，代入闭环关键帧中所有匹配特征点的像素位置信息，迭代求解使得第一目标函数取得最小值时的ζ，从而求得R和t。用数学式表达为：
Figure BDA0002561641320000141
其中ζ为(R，t)的李代数表示。
步骤603，根据闭环约束，对第一视觉地图中的地图点进行优化。具体包括，位姿图优化和地图点优化，其中，位姿图优化是按照步骤6031处理，地图点优化按照步骤6032处理。步骤6031与步骤6032无先后顺序。
所应理解的是，步骤6031与步骤6032还可以选择性的执行其中任一步骤的优化过程，例如，仅进行位姿图优化，或者，仅进行地图点优化。
步骤6031，鉴于任一关键帧i的位姿Ti的李代数表示ζi和任一关键帧j的位姿Tj的李代数表示ζj之间相对位姿的误差表示为：
Figure BDA0002561641320000142
其中，符号^表示反对称矩阵，符号∨表示反对称矩阵的逆运算。
故而，构造用于关键帧的位姿图优化的第二目标函数：
Figure BDA0002561641320000143
其中，Ω为误差项的权重，ε为关键帧集合。
在第二目标函数中代入测量得到的关键帧i和关键帧j的帧间相对位姿误差，作为初始值，以步骤6022求得的闭环关键帧之间的相对位姿为约束，采用高斯-牛顿方法或LM方法迭代求解使得第二目标函数取得最小值时的关键帧i的位姿Ti的李代数表示ζi和关键帧j位姿Tj的李代数表示ζj，这样，根据闭环关键帧所确定的累积误差被分配至各个关键帧中，从而修正了关键帧的位姿。
步骤6032，根据任一关键帧i的位姿Ti采集到三维地图点j的坐标yj在关键帧i中的像素位置zij，构造重投影误差：
Figure BDA0002561641320000144
其中，
Figure BDA0002561641320000145
表示地图点重投影在图像中的位置为：
Figure BDA0002561641320000146
Figure BDA0002561641320000151
其中，I为单位矩阵，[I3×303×1y构成3×4的矩阵，Ti为4×4的矩阵，
Figure BDA0002561641320000152
为4×1的矩阵，K为相机内参。
构造重投影误差的第三目标函数：
Figure BDA0002561641320000153
其中，Ω为误差项的权重，j为地图点。
在第三目标函数中代入上述根据关键帧i的位姿Ti、地图点j的坐标、相机内参、以及地图点j在关键帧i中的像素坐标所得到的重投影误差，并作为初始值，采用高斯-牛顿方法或LM方法迭代求解使得第三目标函数取得最小值时的三维地图点j坐标yj，从而对地图点的三维空间位置信息进行修正。
在该步骤中，较佳地，所述关键帧i的位姿Ti为经过步骤6031优化后的位姿。
将优化后的关键帧位姿和/或优化后的地图点位置作为视觉点云的地图信息保存，这样，通过关键帧的位姿优化和/或地图点坐标的优化，得到第二视觉点云地图。
本发明将建图过程分离为独立的第一视觉点云地图构建、通过闭环约束计算和地图优化，获得第二视觉点云地图，每一处理阶段都有对应的输出地图保存，即使发生建图不理想的情况，也保存了上一过程中的原始数据；扩展性强，方便与各种改进方法进行融合。
参见图8所示，图8为本申请的视觉点云地图构建装置的一种示意图。该装置包括，第一视觉点云地图构建单元801，闭环单元802，地图优化单元803，以及用于实现地图文件的读取和保存的输入输出IO单元804，其中，来自外部的源图像输入至第一视觉点云地图构建单元801，闭环单元802用于向第一视觉点云地图构建单元801生成的第一视觉点云地图添加闭环约束，地图优化单元803基于闭环约束对第一视觉点云地图进行关键帧位姿图优化和地图点位置信息优化。
所述第一视觉点云地图构建单元801包括，
图像预处理模块8011，用于对图像进行预处理；
特征提取模块8012，用于将图像信息转换为特征信息；
地图点生成模块8013，用于对图像帧进行帧间跟踪，确定关键帧，将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点，计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息，
其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图。
所述闭环单元802包括，
闭关关键帧识别模块8021，根据人工标记或关键帧相似度计算，识别出第一视觉点云地图中的闭环关键帧；
闭环约束计算模块8022，基于闭环关键帧，计算闭环关键帧之间的相对位姿，作为闭环约束；构造用于关键帧位姿图优化的第二目标函数，采用最小二乘法，求解使得第二目标函数取得最小值时的关键帧位姿。
所述地图优化单元803包括关键帧位姿图优化模块8031和/或地图点优化模块8032，其中，
关键帧位姿图优化模块8031，基于第一视觉点云地图，根据存在闭环约束的闭环关键帧，采用最小二乘法，对关键帧位姿进行图优化，得到第二视觉点云地图；
地图点优化模块8032，基于第一视觉点云地图，根据重投影误差，对地图点的空间位置信息进行优化，得到第二视觉点云地图。
参见图9所示，图9为图像预处理模块的一种示意图。该图像预处理模块包括，
图像去畸变子模块，根据相机的畸变系数对源图像帧进行去畸变处理，得到去畸变图像，
图像滤波子模块，将去畸变图像进行图像滤波，得到背景图像，
图像差分子模块，用去畸变图像减去背景图像，得到前景图像，
图像拉伸子模块，对前景图像进行拉伸处理，得到目标图像帧。
本申请将建图过程分离为独立的第一视觉点云地图构建单元、闭环单元和地图优化单元，各单元间无耦合关系，每一处理阶段都有对应的输出地图保存，即使发生建图不理想的情况，也保存了上一过程中的原始数据；扩展性强，方便与各种改进方法进行融合。
本申请还可提供一种视觉点云地图构建设备，包括存储器和处理器，所述存储器存储有可执行的计算机指令，所述处理器被配置执行存储器中的指令以实现所述视觉点云地图构建方法的步骤。
存储器可以包括随机存取存储器(Random Access Memory，RAM)，也可以包括非易失性存储器(Non-Volatile Memory，NVM)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital SignalProcessing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
本发明实施例还提供了一种计算机可读存储介质，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现所述视觉点云地图构建方法的步骤。
对于装置/网络侧设备/存储介质实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (15)

1.一种视觉点云地图的构建方法，其特征在于，该方法包括，
对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点，
对源图像帧进行帧间跟踪，确定关键帧，
将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点，
计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息，所述空间位置信息为世界坐标系下的空间位置信息，其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图；
识别出所述第一视觉点云地图中的闭环关键帧，根据存在闭环约束的闭环关键帧，对关键帧位姿进行图优化，和/或，基于所述第一视觉点云地图，根据重投影误差，对地图点的空间位置信息进行优化，得到第二视觉点云地图。
2.如权利要求1所述的方法，其特征在于，所述对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点，进一步包括，
对源图像帧进行图像预处理，得到目标图像帧，
基于目标图像帧进行特征提取，得到目标图像帧的特征点。
3.如权利要求2所述的方法，其特征在于，所述图像预处理包括，
根据相机的畸变系数对源图像帧进行去畸变处理，得到去畸变图像，
判断去畸变图像中各个像素点的像素值是否大于第一像素阈值，如果是，则将像素值大于第一像素阈值的像素点进行取反操作，然后进行图像滤波，得到背景图像，否则，将去畸变图像进行图像滤波，得到背景图像，
用去畸变图像减去背景图像，得到前景图像，
判断前景图像中的像素值是否分布均匀，如果均匀，则将该前景图像作为目标图像帧，否则，对前景图像进行拉伸处理，得到目标图像帧。
4.如权利要求3所述的方法，其特征在于，所述前景图像进行拉伸处理包括，
若前景图像像素值小于等于设定的最小灰度值时，将该前景图像像素值取值为像素取值范围内的最小值；
若前景图像像素值大于最小灰度值、且小于设定的最大灰度值时，按照与像素最大值成一定比例的像素值作为该前景图像像素值；所述比例为前景图像像素值与最小灰度值之差与最大灰度值与最小灰度值之差的比值；
若前景图像像素值大于等于最大灰度值时，将该前景图像像素值取值为像素取值范围内的最大值；
所述基于目标图像帧进行特征提取，得到目标图像帧的特征点，包括，
对目标图像帧进行特征检测，得到特征点，
将目标图像帧划分成一定数量的网格，
对于任一网格中的特征点，将网格内的特征点按特征点响应值降序排列，保留前Q个特征点，得到筛选后的特征点；其中，Q根据该目标图像帧中特征点的数量和设定的特征点总数上限、该网格中的特征点总数确定；
对筛选后的各特征点，分别计算特征描述符。
5.如权利要求4所述的方法，其特征在于，所述Q根据该目标图像帧中特征点的数量和设定的特征点总数上限确定，包括，Q为目标图像帧中特征点的数量除以特征点总数上限之商，乘以网格中的特征点总数后的结果向下取整；
所述对源图像帧进行帧间跟踪，确定关键帧，包括，
对于每一目标图像帧：判断该目标图像帧是否为首帧，如果是，则将该目标图像帧作为关键帧，否则，根据关键帧条件确定该目标图像帧是否为关键帧；
其中，所述关键帧条件至少满足以下条件之一：
匹配特征点数量大于设定的第一阈值；
与上一关键帧之间的空间距离大于设定的第二阈值；
与上一关键帧之间的空间角度大于设定的第三阈值。
6.如权利要求5所述的方法，其特征在于，所述源图像帧为来源于单目相机、且为同一平面的图像帧；
所述计算当前关键帧中匹配特征点的空间位置信息包括，
对于每一匹配特征点：
x坐标为：当前关键帧中该匹配特征点的像素横坐标与相机安装高度的乘积结果与相机焦距的比值，
y坐标为：当前关键帧中该匹配特征点的像素纵坐标与相机安装高度的乘积结果与相机焦距的比值，
z坐标为相机安装高度。
7.如权利要求5所述的方法，其特征在于，所述源图像帧为来源于单目相机、且为非同一平面的图像帧；
所述计算当前关键帧中匹配特征点的空间位置信息包括，
根据由当前关键帧匹配特征点与上一关键帧匹配特征点的匹配特征点对组成的至少8对匹配特征点的像素坐标，得到当前关键帧与上一关键帧的本质矩阵，
对本质矩阵进行奇异值分解，得到当前关键帧与上一关键帧之间帧间相对位姿；
对于每一匹配特征点：根据当前关键帧与上一关键帧之间帧间相对位姿，按照三角化计算关系，至少得到当前关键帧该匹配特征点的深度值；根据当前关键帧该匹配特征点的深度值，得到该匹配特征点的空间位置信息。
8.如权利要求7所述的方法，其特征在于，所述根据由当前关键帧匹配特征点与上一关键帧匹配特征点的匹配特征点组成的至少8对匹配特征点的像素坐标，得到当前关键帧与上一关键帧的本质矩阵，包括，
对于任一匹配特征点，
根据上一关键帧中该匹配特征点归一化平面坐标的转置矩阵、本质矩阵、当前关键帧中该匹配特征点的归一化平面坐标的矩阵之乘积等于0的关系，
代入8对匹配特征点的像素坐标，得到本质矩阵；
所述根据当前关键帧与上一关键帧之间帧间相对位姿，按照三角化计算关系，至少得到当前关键帧该匹配特征点的深度值，包括，
对于任一匹配特征点，
基于当前关键帧中该匹配特征点的深度值与该匹配特征点归一化平面坐标的矩阵之乘积等于上一关键帧中该匹配特征点的深度值、相对位姿中的旋转矩阵、以及上一关键帧中该匹配特征点归一化平面坐标的矩阵之乘积与相对位姿中的平移矩阵之和，
根据相对位姿中的旋转矩阵和平移矩阵、当前关键帧和上一关键帧中匹配特征点归一化平面坐标的矩阵，得到当前关键帧中该匹配特征点的深度值；
所述根据当前关键帧该匹配特征点的深度值，得到该匹配特征点的空间位置信息，包括，
x坐标为：当前关键帧中该匹配特征点归一化平面的像素横坐标与该匹配特征点深度值的乘积，
y坐标为：当前关键帧中该匹配特征点归一化平面的像素纵坐标与该匹配特征点深度值的乘积，
z坐标为相机焦距。
9.如权利要求5所述的方法，其特征在于，所述源图像帧为来源于双目相机、且为非同一平面的双目图像帧；
所述对源图像帧进行图像预处理，得到目标图像帧，包括，对来自第一目的第一源图像帧、来自第二目的第二源图像帧分别进行图像预处理，分别得到第一目标图像帧和第二目标图像帧，作为双目目标图像帧；
基于目标图像帧进行特征提取，得到目标图像帧的特征点，包括，分别提取第一目标图像帧的特征点和第二目标图像帧的特征点；
所述判断该目标图像帧是否为首帧包括，判断双目目标图像帧是否为首帧，如果是，则将该双目目标图像帧中的任一帧作为关键帧，否则，根据关键帧条件确定该目标图像帧中的任一帧是否为关键帧；
所述计算当前关键帧中匹配特征点的空间位置信息包括，
对于当前关键帧中每一匹配特征点：
将当前关键帧作为当前双目目标图像帧中的第一帧，将该双目目标图像帧中的另一目标图像帧作为第二帧，将第一帧中该匹配特征点与第二帧中的特征点进行匹配，如果匹配成功，得到第二帧中的匹配特征点，则
当前关键帧中该匹配特征点的x坐标为：当前关键帧中该匹配特征点的归一化平面的像素横坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点归一化平面的像素横坐标与第二帧中的匹配特征点归一化平面的像素横坐标之差的绝对值；
当前关键帧中该匹配特征点的y坐标为：当前关键帧中该匹配特征点归一化平面的像素纵坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点归一化平面的像素横坐标与第二帧中的匹配特征点归一化平面的像素横坐标之差的绝对值；
当前关键帧中该匹配特征点的z坐标为：相机焦距与双目基线长度的乘积，除以当前关键帧中该匹配特征点归一化平面的像素横坐标与第二帧中的匹配特征点归一化平面的像素横坐标之差的绝对值。
10.如权利要求2至9任一所述的方法，其特征在于，所述识别出所述第一视觉点云地图中的闭环关键帧，根据存在闭环约束的闭环关键帧，对关键帧位姿进行图优化，包括，
根据人工标记或关键帧相似度计算，识别出第一视觉点云地图中的闭环关键帧；
基于闭环关键帧，计算闭环关键帧之间的相对位姿，作为闭环约束；
构造用于关键帧位姿图优化的第二目标函数，采用最小二乘法，求解使得第二目标函数取得最小值时的关键帧位姿。
11.如权利要求10所述的方法，其特征在于，所述根据人工标记或关键帧相似度计算，识别出第一视觉点云地图中的闭环关键帧，包括，
将不同次地采集到相同标识的关键帧作为闭环关键帧；
或者，
计算两关键帧间的相似度是否大于设定的相似度阈值，如果是，则判定该两关键帧为闭环关键帧，其中，相似度包括，特征点分布上的相似度和图像像素的相似度；
所述基于闭环关键帧，计算闭环关键帧之间的相对位姿，作为闭环约束，包括，
基于闭环关键帧，计算该闭环关键帧中的匹配特征点，
对于该闭环关键帧中的任一匹配特征点，根据第一闭环关键帧中该匹配特征点的像素坐标矩阵等于相对位姿中的旋转矩阵与第二闭环关键帧的像素坐标矩阵的乘积加上相对位姿中的平移矩阵的关系，计算得到相对位姿，作为初始值；
构建累计闭环关键帧中的所有匹配特征点的像素位置信息误差的第一目标函数，代入所有匹配特征点的像素坐标矩阵，迭代求解使得第一目标函数取得最小值时的相对位姿；
所述构造用于关键帧位姿图优化的第二目标函数，采用最小二乘法，求解使得第二目标函数取得最小值时的关键帧位姿，包括，
根据任一第一关键帧的位姿和任一第二关键帧的位姿之间相对位姿的误差，构建累计该第一关键帧和该第二关键帧的位姿误差的第二目标函数，
以该第一关键帧和第二关键帧的帧间相对位姿误差作为初始值，以所述闭环约束为约束，迭代求解使得第二目标函数取得最小值时的第一关键帧的位姿和第二关键帧的位姿。
12.如权利要求11所述的方法，其特征在于，所述计算两关键帧间的相似度是否大于设定的相似度阈值，包括，
在根节点，用k均值聚类算法将所有特征点聚成k类，得到第一层节点，
对于第一层的每个节点，将属于该节点的特征点聚成k类，得到下一层节点，
依此类推，直至最后的叶子层，得到视觉字典，该视觉字典包括N个特征点、且每次分叉为k的树，
其中，从根节点到叶子层共计d层，叶子层中包括字典中的单词特征点；k、d、N均为自然数，N为字典中特征点的总数；
对于任一关键帧，根据该关键帧中所有特征数量以及任一单词特征点出现的次数，计算该单词特征点的权重，将关键帧描述为以各个单词特征点及其权重为元素的集合，该集合包括有N个元素，
根据第一关键帧所描述的集合中的所有元素和第二关键帧所描述集合中的所有元素，计算第一关键帧与第二关键帧的相似度，
若相似度大于设定的相似度阈值，判定两关键帧之间存在闭环约束。
13.如权利要求2至9任一所述的方法，其特征在于，所述根据重投影误差，对地图点的空间位置信息进行优化，包括，
对任一关键帧，根据在该关键帧位姿所采集到任一地图点在该关键帧中的像素位置所存在的重投影误差，构建关于重投影误差的第三目标函数，
以重投影误差初始值，迭代求解使得第三目标函数取得最小值时的空间地图点的空间位置信息，
其中，重投影误差初始值为：该地图点在该关键帧中的像素位置与该地图点重投影在图像中的位置之差，
所述该地图点重投影在图像中的位置根据相机内参、该关键帧的位姿、该地图点的空间位置得到。
14.一种视觉点云地图的构建装置，其特征在于，该装置包括，第一视觉点云地图构建单元，该单元包括，
特征提取模块，对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点，
地图点生成模块，对源图像帧进行帧间跟踪，确定关键帧，将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点，计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息，所述空间位置信息为世界坐标系下的空间位置信息，其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图；
识别出所述第一视觉点云地图中的闭环关键帧，根据存在闭环约束的闭环关键帧，对关键帧位姿进行图优化，和/或，基于所述第一视觉点云地图，根据重投影误差，对地图点的空间位置信息进行优化，得到第二视觉点云地图。
15.一种计算机可读存储介质，其特征在于，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至13任一所述视觉点云地图的构建方法的步骤。
CN202010615170.6A 2020-06-30 2020-06-30 一种视觉点云地图的构建方法、装置 Active CN111795704B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202010615170.6A CN111795704B (zh) 2020-06-30 2020-06-30 一种视觉点云地图的构建方法、装置
PCT/CN2021/103653 WO2022002150A1 (zh) 2020-06-30 2021-06-30 一种视觉点云地图的构建方法、装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010615170.6A CN111795704B (zh) 2020-06-30 2020-06-30 一种视觉点云地图的构建方法、装置

Publications (2)

Publication Number Publication Date
CN111795704A CN111795704A (zh) 2020-10-20
CN111795704B true CN111795704B (zh) 2022-06-03

Family

ID=72809796

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010615170.6A Active CN111795704B (zh) 2020-06-30 2020-06-30 一种视觉点云地图的构建方法、装置

Country Status (2)

Country Link
CN (1) CN111795704B (zh)
WO (1) WO2022002150A1 (zh)

Families Citing this family (73)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111311756B (zh) * 2020-02-11 2023-04-11 Oppo广东移动通信有限公司 增强现实ar显示方法及相关装置
CN113701766B (zh) * 2020-05-20 2024-09-20 浙江欣奕华智能科技有限公司 一种机器人地图的构建方法、机器人的定位方法及装置
CN111795704B (zh) * 2020-06-30 2022-06-03 杭州海康机器人技术有限公司 一种视觉点云地图的构建方法、装置
CN114494612B (zh) * 2020-10-26 2025-07-04 广州视源电子科技股份有限公司 构建点云地图的方法、装置和设备
CN113761091B (zh) * 2020-11-27 2024-04-05 北京京东乾石科技有限公司 闭环检测方法、装置、电子设备、系统和存储介质
CN112614185B (zh) * 2020-12-29 2022-06-21 浙江商汤科技开发有限公司 地图构建方法及装置、存储介质
CN112767546B (zh) * 2021-01-22 2022-08-02 湖南大学 移动机器人基于双目图像的视觉地图生成方法
CN112837241B (zh) * 2021-02-09 2025-01-10 贵州京邦达供应链科技有限公司 建图重影去除方法、设备及存储介质
CN113160270B (zh) * 2021-02-24 2025-03-04 广州视源电子科技股份有限公司 视觉地图生成方法、装置、终端及存储介质
CN113063424B (zh) * 2021-03-29 2023-03-24 湖南国科微电子股份有限公司 一种商场内导航方法、装置、设备及存储介质
CN113284224B (zh) * 2021-04-20 2024-06-18 北京行动智能科技有限公司 基于单纯码的自动建图方法和装置、定位方法及设备
CN113256719B (zh) * 2021-06-03 2025-01-21 舵敏智能科技(苏州)有限公司 停车导航定位方法、装置、电子设备及存储介质
CN113515536B (zh) * 2021-07-13 2022-12-13 北京百度网讯科技有限公司 地图的更新方法、装置、设备、服务器以及存储介质
CN113674411B (zh) * 2021-07-29 2024-06-07 浙江大华技术股份有限公司 基于位姿图调整的建图方法及相关设备
CN113670293B (zh) * 2021-08-11 2024-07-30 北京小米移动软件有限公司 地图构建方法及装置
CN113536024B (zh) * 2021-08-11 2022-09-09 重庆大学 一种基于fpga的orb_slam重定位特征点检索加速方法
CN113837201B (zh) * 2021-09-06 2025-01-21 广州极飞科技股份有限公司 特征点的提取方法、图像的重建方法及装置
CN113837202B (zh) * 2021-09-06 2025-03-07 广州极飞科技股份有限公司 特征点的提取方法、图像的重建方法及装置
CN113808269B (zh) * 2021-09-23 2024-12-31 视辰信息科技(上海)有限公司 地图生成方法、定位方法、系统及计算机可读存储介质
CN114088099B (zh) * 2021-11-18 2024-06-25 北京易航远智科技有限公司 基于已知地图的语义重定位方法、装置、电子设备及介质
CN114187418B (zh) * 2021-12-14 2025-02-11 北京易航远智科技有限公司 回环检测方法、点云地图构建方法、电子设备及存储介质
CN114511719A (zh) * 2022-01-07 2022-05-17 广州极飞科技股份有限公司 一种图像匹配方法、装置、设备及存储介质
CN114935338B (zh) * 2022-01-24 2025-04-11 杭州申昊科技股份有限公司 一种交互式地图构建方法
CN114494886A (zh) * 2022-02-11 2022-05-13 美智纵横科技有限责任公司 位姿恢复方法、装置、可读存储介质和机器人
CN115205419A (zh) * 2022-02-22 2022-10-18 维沃移动通信有限公司 即时定位与地图构建方法、装置、电子设备及可读存储介质
CN114565847B (zh) * 2022-02-25 2025-01-07 哈尔滨工程大学 一种基于实例分割的动态环境下视觉slam方法
CN114565777B (zh) * 2022-02-28 2025-06-24 维沃移动通信有限公司 数据处理方法和装置
CN114745533B (zh) * 2022-02-28 2024-05-07 杭州小伴熊科技有限公司 一种空间关键点数据采集极值定准方法及系统
CN114627184B (zh) * 2022-03-01 2025-03-28 西安理工大学 一种基于直接法的移动机器人双目视觉定位与建图方法
CN114739382B (zh) * 2022-03-03 2025-07-04 深圳市优必选科技股份有限公司 机器人及其建图方法、装置及存储介质
CN114608558A (zh) * 2022-03-08 2022-06-10 中国科学技术大学 基于特征匹配网络的slam方法、系统、设备及存储介质
CN114638941B (zh) * 2022-03-11 2024-08-23 广州高新兴机器人有限公司 一种3d点云地图自动扩展方法、装置及介质
CN114677484B (zh) * 2022-03-15 2024-11-08 南京邮电大学 一种基于单目相机的半稠密点云地图构建方法
CN114663482B (zh) * 2022-03-30 2024-08-06 中国农业银行股份有限公司 一种图像处理方法、装置及设备
CN114820784B (zh) * 2022-04-07 2025-02-11 智道网联科技(北京)有限公司 路牌的生成方法、装置及电子设备
CN114924287B (zh) * 2022-04-21 2025-07-18 深圳市正浩创新科技股份有限公司 地图构建方法、设备与介质
CN114529705B (zh) * 2022-04-22 2022-07-19 山东捷瑞数字科技股份有限公司 一种三维引擎编辑器的界面布局处理方法
CN114821280B (zh) * 2022-04-28 2024-10-29 西安交通大学 一种基于滑动窗口的slam局部实时重定位方法
CN114782499B (zh) * 2022-04-28 2025-06-20 杭州电子科技大学 一种基于光流和视图几何约束的图像静态区域提取方法及装置
CN114998385A (zh) * 2022-05-27 2022-09-02 中国计量大学 一种基于视觉特征的室内空间辅助定位方法
CN115018999B (zh) * 2022-05-31 2024-10-29 广东工业大学 一种多机器人协作的稠密点云地图构建方法及装置
CN115205383A (zh) * 2022-06-17 2022-10-18 深圳市优必选科技股份有限公司 相机位姿的确定方法、装置、电子设备及存储介质
CN114998389B (zh) * 2022-06-20 2024-12-03 珠海格力电器股份有限公司 室内定位方法
CN115271197A (zh) * 2022-07-22 2022-11-01 江苏富衍信息科技有限公司 一种小区人口预测方法及装置
CN117670785B (zh) * 2022-08-31 2025-02-25 北京三快在线科技有限公司 点云地图的重影检测方法
CN115824192A (zh) * 2022-11-14 2023-03-21 杭州申昊科技股份有限公司 地图更新方法、装置、电子设备和存储介质
CN115731385A (zh) * 2022-11-22 2023-03-03 中国电子科技南湖研究院 基于语义分割的图像特征提取方法、装置及slam系统
CN115930989A (zh) * 2022-12-08 2023-04-07 北京集度科技有限公司 视觉里程计、车辆及定位方法
CN116164726A (zh) * 2022-12-26 2023-05-26 深圳市优必选科技股份有限公司 机器人及其地图更新方法、装置及存储介质
CN116147618B (zh) * 2023-01-17 2023-10-13 中国科学院国家空间科学中心 一种适用动态环境的实时状态感知方法及系统
CN116189145B (zh) * 2023-02-15 2024-06-11 清华大学 一种线形地图要素的提取方法、系统和可读介质
CN116030136B (zh) * 2023-03-29 2023-06-09 中国人民解放军国防科技大学 基于几何特征的跨视角视觉定位方法、装置和计算机设备
CN116630423A (zh) * 2023-05-29 2023-08-22 哈尔滨工业大学 一种基于orb特征的微小型机器人多目标双目定位方法及系统
CN116452776B (zh) * 2023-06-19 2023-10-20 国网浙江省电力有限公司湖州供电公司 基于视觉同步定位与建图系统的低碳变电站场景重建方法
CN116567166B (zh) * 2023-07-07 2023-10-17 广东省电信规划设计院有限公司 一种视频融合方法、装置、电子设备及存储介质
CN116958603A (zh) * 2023-07-31 2023-10-27 咪咕新空文化科技(厦门)有限公司 一种视觉定位的方法、装置、电子设备和可读存储介质
CN116681733B (zh) * 2023-08-03 2023-11-07 南京航空航天大学 一种空间非合作目标近距离实时位姿跟踪方法
CN116883251B (zh) * 2023-09-08 2023-11-17 宁波市阿拉图数字科技有限公司 基于无人机视频的图像定向拼接与三维建模方法
CN117542008B (zh) * 2023-10-12 2024-10-01 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种融合语义点云的自动驾驶场景识别方法、存储介质
CN117788730B (zh) * 2023-12-08 2024-10-15 中交机电工程局有限公司 一种语义点云地图构建方法
CN118015115B (zh) * 2024-01-25 2024-11-19 中山大学·深圳 一种联合优化点线面特征和动态分割的建图方法
CN117635875B (zh) * 2024-01-25 2024-05-14 深圳市其域创新科技有限公司 一种三维重建方法、装置及终端
CN118623848B (zh) * 2024-06-05 2025-02-18 山东润宇土地房地产资产评估经纪有限公司 一种复杂地形测绘系统及测绘方法
CN119205855B (zh) * 2024-06-07 2025-02-14 深圳飞马机器人股份有限公司 点云图像的配准方法、装置、电子设备和存储介质
CN118310538B (zh) * 2024-06-11 2024-08-27 山东云海国创云计算装备产业创新中心有限公司 基于多模态数据的重定位方法和装置
CN118334125B (zh) * 2024-06-14 2024-11-12 魔视智能科技(武汉)有限公司 稠密点云估计方法、装置、计算机设备以及存储介质
CN118887644B (zh) * 2024-06-20 2025-05-13 中国科学院自动化研究所 视觉slam方法、装置、系统、电子设备及存储介质
CN118674786B (zh) * 2024-08-22 2024-11-08 浙江吉利控股集团有限公司 图像位姿数据的确定方法、装置、设备、介质及程序产品
CN119043302B (zh) * 2024-10-29 2024-12-31 杭州萤石软件有限公司 一种地图优化方法、装置及移动机器人
CN119206154B (zh) * 2024-11-29 2025-02-14 江西立盾光电科技有限公司 一种用于植物生长的光照智能优化方法及系统
CN119284579B (zh) * 2024-12-12 2025-04-04 西南科技大学 适用于矿区散料转运的无人装载车自主装卸料方法
CN119722961B (zh) * 2025-02-28 2025-05-02 中国电建集团西北勘测设计研究院有限公司 地下建筑物高空间分辨率三维形貌的双目视觉重建方法
CN120014054B (zh) * 2025-04-14 2025-07-08 上海粲高教育设备有限公司 一种基于图像识别的无人值守视力检测方法及系统

Family Cites Families (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107330373A (zh) * 2017-06-02 2017-11-07 重庆大学 一种基于视频的违章停车监控系统
CN107341814B (zh) * 2017-06-14 2020-08-18 宁波大学 基于稀疏直接法的四旋翼无人机单目视觉测程方法
CN107369183A (zh) * 2017-07-17 2017-11-21 广东工业大学 面向mar的基于图优化slam的跟踪注册方法及系统
US10636198B2 (en) * 2017-12-28 2020-04-28 Beijing Jingdong Shangke Information Technology Co., Ltd. System and method for monocular simultaneous localization and mapping
CN110657803B (zh) * 2018-06-28 2021-10-29 深圳市优必选科技有限公司 机器人定位方法、装置以及存储装置
CN109671120A (zh) * 2018-11-08 2019-04-23 南京华捷艾米软件科技有限公司 一种基于轮式编码器的单目slam初始化方法及系统
CN111322993B (zh) * 2018-12-13 2022-03-04 杭州海康机器人技术有限公司 一种视觉定位方法和装置
CN109887029A (zh) * 2019-01-17 2019-06-14 江苏大学 一种基于图像颜色特征的单目视觉里程测量方法
CN110378345B (zh) * 2019-06-04 2022-10-04 广东工业大学 基于yolact实例分割模型的动态场景slam方法
CN110570453B (zh) * 2019-07-10 2022-09-27 哈尔滨工程大学 一种基于双目视觉的闭环式跟踪特征的视觉里程计方法
CN110533722B (zh) * 2019-08-30 2024-01-12 的卢技术有限公司 一种基于视觉词典的机器人快速重定位方法及系统
CN110782494A (zh) * 2019-10-16 2020-02-11 北京工业大学 一种基于点线融合的视觉slam方法
CN111325842B (zh) * 2020-03-04 2023-07-28 Oppo广东移动通信有限公司 地图构建方法、重定位方法及装置、存储介质和电子设备
CN111795704B (zh) * 2020-06-30 2022-06-03 杭州海康机器人技术有限公司 一种视觉点云地图的构建方法、装置

Also Published As

Publication number Publication date
CN111795704A (zh) 2020-10-20
WO2022002150A1 (zh) 2022-01-06

Similar Documents

Publication Publication Date Title
CN111795704B (zh) 一种视觉点云地图的构建方法、装置
CN111780764B (zh) 一种基于视觉地图的视觉定位方法、装置
CN111780763B (zh) 一种基于视觉地图的视觉定位方法、装置
CN113012212B (zh) 一种基于深度信息融合的室内场景三维点云重建方法和系统
US11341738B2 (en) Using a probabtilistic model for detecting an object in visual data
CN103295242B (zh) 一种多特征联合稀疏表示的目标跟踪方法
CN110334762B (zh) 一种基于四叉树结合orb和sift的特征匹配方法
CN106991388B (zh) 关键点定位方法
CN110472585A (zh) 一种基于惯导姿态轨迹信息辅助的vi-slam闭环检测方法
Atrevi et al. A very simple framework for 3D human poses estimation using a single 2D image: Comparison of geometric moments descriptors
CN111998862B (zh) 一种基于bnn的稠密双目slam方法
CN111709317B (zh) 一种基于显著性模型下多尺度特征的行人重识别方法
CN114463503B (zh) 三维模型和地理信息系统的融合方法及装置
CN110796691A (zh) 一种基于形状上下文和hog特征的异源图像配准方法
CN111368733B (zh) 一种基于标签分布学习的三维手部姿态估计方法、存储介质及终端
CN114067128A (zh) 一种基于语义特征的slam回环检测方法
CN116894876A (zh) 基于实时图像的6-dof的定位方法
CN113569600A (zh) 物体重识别的方法、装置、电子设备及存储介质
CN111724438B (zh) 一种数据处理方法、装置
CN106611030A (zh) 基于视频的对象相似度比较方法和检索方法及其系统
Srivastava et al. Drought stress classification using 3D plant models
Yang et al. Non-rigid point set registration via global and local constraints
CN119580985A (zh) 基于多模态图像分析的机器人辅助伤口治疗方法及系统
CN118762055A (zh) 基于YOLOv5s及关键帧选择优化机器人定位与地图构建的方法
CN117315518A (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
CP01 Change in the name or title of a patent holder

Address after: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province

Patentee after: Hangzhou Hikvision Robot Co.,Ltd.

Address before: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province

Patentee before: HANGZHOU HIKROBOT TECHNOLOGY Co.,Ltd.

CP01 Change in the name or title of a patent holder