CN114445591A - 地图构建方法、系统、设备以及计算机存储介质 - Google Patents

地图构建方法、系统、设备以及计算机存储介质 Download PDF

Info

Publication number
CN114445591A
CN114445591A CN202210043361.9A CN202210043361A CN114445591A CN 114445591 A CN114445591 A CN 114445591A CN 202210043361 A CN202210043361 A CN 202210043361A CN 114445591 A CN114445591 A CN 114445591A
Authority
CN
China
Prior art keywords
plane
data
map
dimensional coordinate
point
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
CN202210043361.9A
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.)
Shenzhen TetrasAI Technology Co Ltd
Original Assignee
Shenzhen TetrasAI 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 Shenzhen TetrasAI Technology Co Ltd filed Critical Shenzhen TetrasAI Technology Co Ltd
Priority to CN202210043361.9A priority Critical patent/CN114445591A/zh
Publication of CN114445591A publication Critical patent/CN114445591A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Processing Or Creating Images (AREA)

Abstract

本申请公开了一种地图构建方法、系统、设备以及计算机存储介质，该地图构建方法包括：接收图像数据以及惯性数据；基于所述图像数据和惯性数据，获取所述地图的三维坐标点；利用所述三维坐标点生成平面数据；结合所述三维坐标点和所述平面数据，构建所述地图。本申请的地图构建方法实现图像数据、惯性数据以及平面数据紧耦合，通过平面数据有效提高对环境的定位，通过惯性数据提供精确的短时跟踪，实现弱纹理环境下实时高效位姿和地图估计。

Description

地图构建方法、系统、设备以及计算机存储介质
技术领域
本申请涉及计算机视觉处理技术领域，特别是涉及一种地图构建方法、系统、设备以及计算机存储介质。
背景技术
视觉SLAM(Visual Simultaneous Localization and Mapping)在过去20年被深入研究且已经广泛应用于机器人，自动驾驶，AR(Augmented Reality，增强现实)/VR(Virtual Reality，虚拟现实)等新兴领域。现有的视觉SLAM算法可以根据前端跟踪模式的不同分为基于特征点的间接法与基于灰度信息的直接法两类。而根据后端优化方法不同，又可以分成基于滤波和基于光束法平差两种方案。
在基于单目的SLAM模式下，受制于相机视场角不足的问题，当发生剧烈运动，光照变化，或者纹理稀少的情况时，无论是基于特征点法还是直接法都不够鲁棒。而全景视觉传感器可以获取周围360°的场景信息，恰好可以有效解决视场角不足的问题。但是由于视觉传感器本身的缺陷，即便使用全景相机仍然还是会发生跟踪丢失的现象。
发明内容
本申请提供一种地图构建方法、系统、设备以及计算机存储介质。
本申请采用的一个技术方案是提供一种地图构建方法，所述地图构建方法包括：
接收图像数据以及惯性数据；
基于所述图像数据和惯性数据，获取所述地图的三维坐标点；
利用所述三维坐标点生成平面数据；
结合所述三维坐标点和所述平面数据，构建所述地图。
其中，所述利用所述三维坐标点生成平面数据之后，所述地图构建方法还包括：
利用所述惯性数据获取惯性约束；
利用所述平面数据和所述三维坐标点获取重投影误差和深度约束；
按照所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数；
利用所述第一平面优化函数对所述平面数据进行优化。
其中，所述按照所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数，包括：
基于所述平面数据和所述三维坐标点，获取平面关联的观测点在第一关键帧的第一坐标，以及所述观测点在第二关键帧的第二坐标；
利用所述第一坐标、所述第二坐标以及预设的投影方程，获取所述观测点与所述平面的点面约束；
按照所述点面约束、所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数。
其中，所述利用所述三维坐标点生成平面数据之后，所述地图构建方法还包括：
基于所述图像数据，获取采集第一关键帧的第一相机参数，以及采集第二关键帧的第二相机参数；
获取所述第一相机参数和所述第二相机参数的相对相机参数；
利用所述平面数据、所述相对相机参数、所述第一相机参数和所述第二相机参数生成第二平面优化函数；
利用所述第二平面优化函数对所述平面数据进行优化。
其中，所述基于所述图像数据和惯性数据，获取所述地图的三维坐标点，包括：
利用前端对所述图像数据和所述惯性数据进行数据处理，得到实时的第一三维坐标点；
将所述前端的图像数据和惯性数据传输到后端；
利用所述后端对所述前端的图像数据和惯性数据进行数据处理，得到非实时的第二三维坐标点。
其中，所述利用所述三维坐标点生成平面数据，包括：
利用所述前端基于所述第一三维坐标点，生成实时的第一平面数据；
利用所述后端基于所述第二三维坐标点，生成非实时的第二平面数据；
所述结合所述三维坐标点和所述平面数据，构建所述地图，包括
结合所述第一三维坐标点、所述第一平面数据、所述第二三维坐标点以及所述第二平面数据构建所述地图。
其中，所述结合所述第一三维坐标点、所述第一平面数据、所述第二三维坐标点以及所述第二平面数据构建所述地图，包括：
利用所述第一三维坐标点和所述第二三维坐标点创建三维网格；
利用所述第一平面数据、所述第二平面数据以及所述三维网格，生成若干地图平面；
利用所述若干地图平面获取最终的地图。
其中，所述生成若干地图平面之后，所述地图构建方法还包括：
基于所述若干地图平面在所述三维网格中的位置，建立所述第一三维坐标点、所述第二三维坐标点与所述地图平面的关联，以及建立相邻地图平面之间的关联；
所述利用所述若干地图平面获取最终的地图，包括：
利用所述第一三维坐标点、所述第二三维坐标点与所述地图平面的关联，和相邻地图平面之间的关联构建最终的地图。
本申请采用的另一个技术方案是提供一种地图构建系统，所述地图构建系统包括：
前端，用于接收图像数据以及惯性数据，基于所述图像数据和所述惯性数据获取实时的第一三维坐标点，以及基于所述第一三维坐标点生成第一平面数据；
后端，用于接收所述前端的图像数据以及惯性数据，基于所述图像数据和所述惯性数据获取非实时的第二三维坐标点，以及基于所述第二三维坐标点生成第二平面数据；
平面模块，用于结合所述第一三维坐标点、所述第一平面数据、所述第二三维坐标点以及所述第二平面数据构建所述地图。
其中，所述地图构建系统，还包括：
地图模块，用于存储所述地图。
本申请采用的另一个技术方案是提供一种终端设备，所述终端设备包括存储器以及与所述存储器耦接的处理器；
其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如上述的地图构建方法。
本申请采用的另一个技术方案是提供一种计算机存储介质，所述计算机存储介质用于存储程序数据，所述程序数据在被计算机执行时，用以实现如上述的地图构建方法。
本申请的有益效果是：本申请提供的终端设备接收图像数据以及惯性数据；基于所述图像数据和惯性数据，获取所述地图的三维坐标点；利用所述三维坐标点生成平面数据；结合所述三维坐标点和所述平面数据，构建所述地图。本申请的地图构建方法实现图像数据、惯性数据以及平面数据紧耦合，通过平面数据有效提高对环境的定位，通过惯性数据提供精确的短时跟踪，实现弱纹理环境下实时高效位姿和地图估计。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请提供的地图构建系统一实施例的框架示意图；
图2是本申请提供的地图构建系统另一实施例的框架示意图；
图3是本申请提供的单应性因子的结构示意图；
图4是本申请提供的压缩单应性因子的结构示意图；
图5是本申请提供的地图构建方法一实施例的流程示意图；
图6是本申请提供的终端设备一实施例的结构示意图；
图7是本申请提供的终端设备另一实施例的结构示意图；
图8是本申请提供的计算机存储介质一实施例的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
具体请参见图1和图2，图1是本申请提供的地图构建系统一实施例的框架示意图，图2是本申请提供的地图构建系统另一实施例的框架示意图。
如图1所示，本申请提供的地图构建系统100包括前端11、后端12以及平面模块13。进一步地，在图2所示的系统中，地图构建系统100还可以包括地图模块14。
其中，前端11，用于接收图像数据以及惯性数据，基于所述图像数据和所述惯性数据获取实时的第一三维坐标点，以及基于所述第一三维坐标点生成第一平面数据。
后端12，用于接收所述前端的图像数据以及惯性数据，基于所述图像数据和所述惯性数据获取非实时的第二三维坐标点，以及基于所述第二三维坐标点生成第二平面数据。
后端12，还用于基于所述图像数据和所述惯性数据获取相机位姿，以及利用所述相机位姿对所述第二平面数据进行优化。
平面模块13，用于基于所述第一三维坐标点、所述第二三维坐标点，以及优化后的第一平面数据、优化后的第二平面数据构建所述地图。
地图模块14，用于存储所述地图，以及所述图像数据中的关键帧、地图和/或三维坐标点等。
下面分别对以上系统组成部分进行具体介绍：
前端11主要包含特征点的提取与跟踪，IMU(Inertial Measurement Unit，惯性测量单元)预计分以及基于滑窗的实时位姿估计。前端11输入的数据是RGB数据(图像色彩数据)，Depth数据(图像深度数据)和IMU数据(惯性数据)，最终输出的是滑窗内的pose(相机位姿)和3D点坐标(三维坐标点)，最后的结果作为后端12和平面模块13的输入。
由于IMU与视觉天然的互补特性，VIO(Visual Inertial Odometry，视觉惯性里程计)近几年成为了SLAM领域的研究热点。即使发生剧烈的光照变化，或纹理缺失等极端问题而导致视觉里程计发生跟踪丢失，IMU也可以在短时间内提供较好的定位结果，有效提高视觉里程计的鲁棒性。MSCKF和VINS-Mono分别为基于图优化和基于滤波两种典型VIO系统的代表。其中基于图优化的方案需要进行迭代步骤，会不断对IMU进行积分操作而导致计算量急剧上升，因此IMU预积分理论被提出。IMU预积分将帧与帧之间的绝对测量转换为相对测量，有效地解决了这一问题。
本申请的VIO紧密集成了RGB数据、Depth数据和IMU数据，使用平方反滤波器融合了以上所有测量值，且将Depth数据的深度信息添加到视觉测量中。
具体地，前端11需要检测所有采集图像中的特征点，如ORB特征点，并计算特征点的描述符。其中，ORB特征点采用FAST(features from accelerated segment test)算法来检测特征点。FAST核心思想就是找出那些鹤立鸡群的点，即拿一个点跟它周围的点比较，如果它和其中大部分的点都不一样就可以认为它是一个特征点。
当前端11接收到新的采集图像时，前端11使用KLT从最后一张采集图像的特征点按照描述符跟踪到当前的采集图像。然后，前端11将具有三维信息的特征点投影到当前的采集图像上，并使用汉明距离在当前的采集图像上选取距离最近的投影特征点。前端11将距离最近的投影特征点作为初始值匹配剩余特征点，直至找到最优观测点。最后，前端11利用基于随机抽样一致算法(RANSAC，RANdom SAmple Consensus)去除其中的异常值。
平面模块13主要包含有平面的提取，扩展与合并，点面的关联，平面模块13采用德洛内三角剖分的方法实现以上功能。平面模块13的输入是前后端模块的pose，前后端模块的3D点，以及Depth数据，输出的是地图中的平面数据。
平面模块13一旦检测到一组平面数据，即可使用前端11的数据和后端12的数据扩展平面，并将平面与地图地标关联起来。具体地，平面模块13使用德洛内三角剖分来创建三维网格，利用直方图来检测平面。在本申请实施例中，平面模块13检测垂直平面和水平面，可以通过检测平面在三维网格内的网格法线是垂直还是平行来区分垂直平面和水平面。
进一步地，平面模块13还可以采用以下方法提高平面的精度：但从直方图检测平面时，平面模块13使用图像数据和直方图中的三维平面点来细化平面的参数，而不是直接使用直方图的尺度值。
例如，对于水平面而言，假设n＝[0,0,1]T，平面距离d为平面点的Z轴数值的平均值；对于垂直平面而言，假设n＝[nx,ny,0]T，其中，垂直平面的平面参数可以通过以下公式进行细化：
Figure BDA0003471225160000071
其中，n为平面点的个数，
Figure BDA0003471225160000072
是世界坐标系下的第k个坐标的位置，
Figure BDA0003471225160000073
在本申请，采用QR分解来求解上述式子(1)。当检测到平面参数时，平面模块13可以通过角度和距离关联到三维网格。
进一步的，平面模块13，还用于在所述三维网格内按照所述优化后的平面数据扩展出若干平面，建立平面与平面之间的关联，以及平面与三维坐标点之间的关联。
具体地，平面模块13检验一个平面和其他平面是否满足一定的夹角角度和距离，在两个平面的夹角角度小于预设角度阈值，且距离小于第一预设距离阈值时，即可对两个平面进行平面合并。其中，本申请实施例的预设角度阈值可以设置为10°，第一预设距离阈值可以设置为10cm。需要说明的是，以上平面合并过程可以发生在检测新的平面的过程中，也可以发生在调整现有平面的过程中。
具体地，平面模块13还可以使用三维网格将更多的地图点与平面联系起来。当三维网格与一个平面相关联时，平面模块13获取平面的二维网格，如果地图点的二维坐标都在二维网格内，并且地图点到平面的距离小于第二预设距离阈值，如10cm时，平面模块13将该地图点添加到与该平面相关的候选集中。如果候选集中的一个地图点在超过3个关键帧中都被观察到时，平面模块13进一步检查该地图点的几何一致性。平面模块13可以通过计算地图点从一个关键帧到该平面的重投影误差，从而强迫地图点与平面相关联；然后，计算从另一个关键帧到该平面的重投影误差。如果两个重投影误差接近，且最大重投影误差小于一定阈值时，则认为该地图点为该平面内的平面点。如果一个地图点多次未能通过几何一致性的检查，则将其从候选点集中删除。
后端12包含有关键帧的选择，局部的bundle adjustment(光束平差)优化，回环检测与位姿优化以及全局的bundle adjustment优化。后端12的输入为前端11的pose数据，Depth数据，IMU数据以及平面数据，输出的是整个系统一致性的地图模块14。
由于深度图像是有效的，后端12将深度信息整合到视觉点特征测量中。其中，投影和深度残差在第i帧关键帧定义为：
Figure BDA0003471225160000081
Figure BDA0003471225160000082
其中，
Figure BDA0003471225160000083
是第i个关键帧的第l个特征点的三维坐标，
Figure BDA0003471225160000084
分别是从惯性帧到图像帧的旋转和平移。Proj()是将观测点从相机坐标系投影到图像坐标系的函数，
Figure BDA0003471225160000085
是在第i个关键帧观测到的第l个特征，z()是该矢量的第三个分类，λ是关键帧的深度。
当三维地图点与待优化平面相关联时，终端设备可以强制该三维地图点落在待优化平面上，从而使得三维地图点转化为归一化点。因此，终端设备可以不使用公共点到平面的距离约束，而是使用单应性矩阵来约束两个关键帧和一个待优化平面。
具体地，假设待优化平面上的观测点为πW是由第一相机的第一相机坐标系ith以及第二相机的第二相机坐标系jth观测到的，由此，可以写出以下点到平面的重投影方程：
Figure BDA0003471225160000091
Figure BDA0003471225160000092
其中，本申请实施例定义pi＝(xi,yi,1)T和pj＝(xj,yj,1)T。其中，(xi,yi,1)T＝K-1(ui,vi,1)T为第一相机采集的图像坐标系中坐标点到相机坐标系ith中坐标点的映射，K为本征矩阵，(ui,vi)为第一相机采集的关键帧中的二维图像特征，λ为图像的深度信息，s为一个未知的尺度参数。
结合上述式子，一个观测点的投影方程可以表征为以下公式：
Figure BDA0003471225160000093
其中，W表示世界坐标系，C表示相机坐标系，i和j是两个不同的相机，pi＝(xi,yi1)为第一相机的相机坐标系下的归一化点的点，pj＝(xj,yj1)为第二相机的相机坐标系下的归一化点的坐标，
Figure BDA0003471225160000094
是对应第一相机的旋转，
Figure BDA0003471225160000095
是对应第二相机的旋转，
Figure BDA0003471225160000096
是对应第一相机的位置，
Figure BDA0003471225160000097
是对应第二相机的位置；待优化平面π＝(nπ,dπ)分别是平面的方向和距离。另外，H为单应性矩阵，I为单位矩阵，s为一个尺度参数。
其中，单应性矩阵H包括所述第一关键帧的相机参数
Figure BDA0003471225160000098
和
Figure BDA0003471225160000099
所述第二关键帧的相机参数
Figure BDA00034712251600000910
和
Figure BDA00034712251600000911
以及所述待优化平面的平面参数
Figure BDA00034712251600000912
和
Figure BDA00034712251600000913
具体地，当观测点在待优化平面上时，单应性约束和重投影约束是相等的，其中，单应性约束不需要点特征的三维位置。在BA(bundle adjustment，光束法平差)问题中，本申请将重投影约束转化为单应性约束，这等价于去除待优化平面上观测点的许多状态变量。最后，利用更小、更稀疏的黑塞矩阵(Hessian Matrix)，大大提高了光束法平差的效率。
在本申请实施例中，由上述式子(6)中的投影方程，可以获取观测点的单应性约束方程，如下：
Figure BDA0003471225160000101
进一步地，本申请可以从上述式子(7)中消去未知的尺度参数s，从而得到单应性代价函数，如下：
Figure BDA0003471225160000102
Figure BDA0003471225160000103
其中，Cl为观测点的坐标矩阵。
在本申请实施例中，单应性将两个关键帧和一个待优化平面的状态联系起来，这三种状态可能有许多共同的观察结果，因此，可以将这些观测结果合并为一个观测结果，以进一步提高优化速度。
假设待优化平面上有N个点特征，则待优化平面上N个点特征的总单应性代价函数可以表示为：
Figure BDA0003471225160000104
其中，
Figure BDA0003471225160000105
为矩阵分解。为了保证解的稳定性，本申请使用特征值分解，Gh在优化过程中为常数9×9矩阵，仅依赖于常数数值，因此，可以提前计算这个矩阵。
在本申请实施例中，本申请可以将待优化平面上N个点特征的观测合并到一个观测矩阵Gh中，如图3和图4，图3为合并前的单应性因子，图4为合并后的压缩单应性因子。本申请通过将多个代价函数组合为一个代价函数，可以有效提高束调整的效率。
进一步地，压缩单应性代价函数，即总单应性代价函数的雅可比矩阵可以定义为：
Figure BDA0003471225160000111
其中，Lh为下三角矩阵，
Figure BDA0003471225160000112
为上三角矩阵，Lh和
Figure BDA0003471225160000113
由上述常数9×9矩阵Gh通过矩阵分解得到。
其中，残差函数，即单应性代价函数为：
Figure BDA0003471225160000114
通过对总单应性代价函数的雅可比矩阵进行求解，即可得到待优化平面的优化参数。
另外，对于已知存在点面约束的一些平面点，例如内部观测点，本申请还可以采用删除这部分平面点的重投影观测，从而降低整个优化状态量和优化矩阵的维度，可以进一步降低计算量。
进一步地，单应性测量依赖于地图点的特征匹配，容易受到环境光照和纹理的影响，因此，本申请针对该问题增加了几何约束。
具体地，后端12采用单帧点云与平面关联的方法，提高了无纹理场景下平面估计的准确性和运动估计的稳定性。后端12使用压缩的点到平面代价函数，
Figure BDA0003471225160000115
是在第i个关键帧中观察到的第l个平面
Figure BDA0003471225160000116
上N个观测点的集合，
Figure BDA0003471225160000117
是
Figure BDA0003471225160000118
中的第k个三维坐标点。可以将第k个平面点到平面的残差定义为：
Figure BDA0003471225160000119
其中，
Figure BDA00034712251600001110
是第i个关键帧的相机位姿，rilk(X)仅为一维，因此，第l个平面上N个观测点到平面的代价函数可以表示为：
Figure BDA00034712251600001111
其中，GP为一个4×4的常数矩阵，只取决于观测值，而不取决于状态。通过
Figure BDA00034712251600001112
的矩阵分解，新的代价函数的雅可比矩阵可以表示为：
Figure BDA0003471225160000121
以及，对应的残差函数为：
Figure BDA0003471225160000122
进一步地，因为本申请提供的地图构建系统100融合了IMU数据，所以在地图构建系统100中重力是可观测的。由此，后端12可以通过平面法向和重力方向，判断该平面是水平面还是垂直平面。其中，若第i个平面为水平面，第j个平面为垂直平面，则残差可以计算为：
Figure BDA0003471225160000123
Figure BDA0003471225160000124
当一个新的关键帧插入到映射中时，后端12执行LBA(局部平面和点束调整)优化。通过LBA优化了最新的K个关键帧的相机位姿，IMU数据，以及这部分关键帧观察到的点和平面。观察这些点和平面的其他关键帧在LBA优化中保持固定。在本申请实施例中，K可以设置为20。后端12采用LM(Levenberg-Marquardt，最小二乘优化)算法解决最小化问题，将最大迭代次数设置为10，最大求解时间设置为0.2s。
后端12在局部/全局的bundle adjustment优化中融合了高效的平面约束，同时去除了大量的平面点的重投影约束，因此可快速高效的优化地图状态量。整体的优化公式，即第一平面优化函数表示如下：
Figure BDA0003471225160000125
其中，
Figure BDA0003471225160000126
ΣIMU是IMU的约束和协方差；
Figure BDA0003471225160000127
ΣC是图像的重投影误差和协方差；
Figure BDA0003471225160000128
和
Figure BDA0003471225160000129
是深度的约束和协方差；
Figure BDA00034712251600001210
ΣCH是基于homography(单应性)的点面约束和协方差；
Figure BDA00034712251600001211
ΣCPP是基于3D ToF点的点面约束和协方差。
位姿平面图优化后，需要更新所有状态，包括关键帧位姿、IMU位姿、点和平面等。对于GBA(全局平面和点束调整)问题，后端12以紧密耦合的形式融合地图构建系统100的所有测量值，包括IMU预积分、重投影、压缩单应性、压缩点到平面和先验平面等。因此，后端12在回环检测与位姿优化中也引入了平面的调整，该优化的公式，即第二平面优化函数表示如下：
Figure BDA0003471225160000131
其中，W是世界坐标系，C是相机坐标系；N是关键帧个数，M是平面个数，m和n是回环帧。
Figure BDA0003471225160000132
分别是i和i+1时刻的关键帧位姿，
Figure BDA0003471225160000133
是两者的相对位姿，
Figure BDA0003471225160000134
分别是世界坐标系和相机坐标系下第l个平面。
对于以上优化函数的最小化问题，后端12同样可以使用LM算法来解决，可以将最大迭代次数设置为100，最大求解时间设置为2s。
请继续参阅图5，图5是本申请提供的地图构建方法一实施例的流程示意图。其中，本申请实施例的地图构建方法应用于如图1和/图2所示的地图构建系统，对于地图构建系统的具体结构在此不再赘述。
如图5所示，本申请实施例的地图构建方法具体包括以下步骤：
步骤S51：接收图像数据以及惯性数据。
在本申请实施例中，前端接收图像数据以及惯性数据，并将图像数据以及惯性数据传输至后端。其中，图像数据可以通过相机设备进行采集，惯性数据可以通过惯性传感器进行采集。
步骤S52：基于图像数据和惯性数据，获取地图的三维坐标点。
在本申请实施例中，终端设备基于图像数据和惯性数据，可以拟合成地图中的三维坐标点。
例如，在地图构建系统中，前端直接从相机设备获取图像数据，从惯性传感器获取惯性数据，后端从前端获取图像数据和惯性数据。前端和后端都需要基于图像数据和惯性数据生成三维坐标点，以及利用三维坐标点拟合出若干平面。前端和后端的区别在于前端实时处理图像数据和惯性数据，后端非实时处理图像数据和惯性数据。具体地，前端能及时地根据输入的图像数据以及惯性数据拟合出实时的三维坐标点和平面，拟合速度快，能够满足地图构建系统实时的地图构建需求。后端需要接收前端传输的图像数据以及惯性数据，然后再拟合成非实时的三维坐标点和平面，由于后端负责的是非实时任务，因此对于三维坐标点和平面的拟合结果较前端而已更加精确。
在本申请实施例中，前端基于图像数据和惯性数据获取实时的第一三维坐标点；后端基于前端的图像数据、惯性数据以及第一三维坐标点获取非实时的第二三维坐标点。前端基于所述第一三维坐标点生成第一平面数据；后端基于所述第二三维坐标点生成第二平面数据。
步骤S53：利用三维坐标点生成平面数据。
在本申请实施例中，终端设备利用三维坐标点拟合出若干平面，即生成平面数据。生成平面数据之后，终端设备还可以利用图像数据和关系数据确定下的约束对平面数据进行优化，使得优化后的平面数据更准确。
具体地，在地图构建系统中，后端利用所述惯性数据获取惯性约束；利用所述平面数据和所述三维坐标点获取重投影误差和深度约束；按照所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数；利用所述第一平面优化函数对所述平面数据进行优化。
其中，上述惯性约束、重投影误差、深度约束等约束条件均可以作为平面优化中的限制条件，以使得平面优化过程中，平面数据能够在限定范围内不断迭代优化，以最低的迭代代价优化出最准确的平面数据。需要说明的是，本申请实施例的第一平面优化函数可以包括以上一种或多种的组合形成的限制条件，即终端设备可以从上述约束条件中选择一个或多个，甚至全部的约束条件组成平面优化函数对平面数据进行优化。
具体地，在一种实施例中，第一平面优化函数的表达式可以表现为：
Figure BDA0003471225160000151
其中，
Figure BDA0003471225160000152
ΣIMU是IMU的约束和协方差；
Figure BDA0003471225160000153
ΣC是图像的重投影误差和协方差；
Figure BDA0003471225160000154
和
Figure BDA0003471225160000155
是深度的约束和协方差。
进一步地，终端设备还可以在第一平面优化函数中加入其它的约束条件，如点面约束等。
例如，在另一实施例中，后端还可以基于所述平面数据和所述三维坐标点，获取平面关联的观测点在第一关键帧的第一坐标，以及所述观测点在第二关键帧的第二坐标；利用所述第一坐标、所述第二坐标以及预设的投影方程，获取所述观测点与所述平面的点面约束；按照所述点面约束、所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数。
具体地，终端设备可以通过计算观测点与待优化平面之间的点面关系确定平面的点面约束，将点面约束也作为第一平面优化函数的约束条件，可以增加第一平面优化函数的约束类型，进一步提高平面优化效果。
其中，第一平面优化函数引入点面约束后，表达式可以表现为：
Figure BDA0003471225160000156
其中，
Figure BDA0003471225160000157
ΣIMU是IMU的约束和协方差；
Figure BDA0003471225160000158
ΣC是图像的重投影误差和协方差；
Figure BDA0003471225160000159
和
Figure BDA00034712251600001510
是深度的约束和协方差；
Figure BDA00034712251600001511
ΣCH是基于homography(单应性)的点面约束和协方差；
Figure BDA00034712251600001512
ΣCPP是基于3D ToF点的点面约束和协方差。
除了通过约束条件来优化平面外，本申请实施例的终端设备还可以通过相机位姿等信息来优化平面。
具体地，在地图构建系统中，后端还可以基于所述图像数据，获取采集第一关键帧的第一相机参数，以及采集第二关键帧的第二相机参数；获取所述第一相机参数和所述第二相机参数的相对相机参数；利用所述平面数据、所述相对相机参数、所述第一相机参数和所述第二相机参数生成第二平面优化函数；利用所述第二平面优化函数对所述平面数据进行优化。
在一实施例中，第二平面优化函数的表达式可以表现为：
Figure BDA0003471225160000161
其中，W是世界坐标系，C是相机坐标系；N是关键帧个数，M是平面个数，m和n是回环帧。
Figure BDA0003471225160000162
分别是i和i+1时刻的关键帧位姿，即第一相机参数和第二相机参数，
Figure BDA0003471225160000163
是两者的相对位姿，即相对相机参数，
Figure BDA0003471225160000164
分别是世界坐标系和相机坐标系下第l个平面。
需要说明的是，后端对于平面数据的优化过程在上述实施例的表述以及式子(18)、式子(19)均有具体介绍，在此不再赘述。
步骤S54：结合三维坐标点和平面数据，构建地图。
最后，终端设备综合前端的第一三维坐标点、第一平面数据，以及后端的第二三维坐标点，优化后的第二平面数据构建最终的地图。
具体地，地图构建系统中的平面模块利用所述第一三维坐标点和所述第二三维坐标点创建三维网格；利用所述第一平面数据、所述第二平面数据以及所述三维网格，生成若干地图平面。平面模块通过将前端的第一三维坐标点和后端的第二三维坐标点进行连接和关联，形成三维网格，然后，按照前端的第一平面数据和后端的第二平面数据在三维网格中拟合成若干地图平面。需要说明的是，上述三维坐标点除了本身在地图平面内的坐标点外，其他坐标点也可以与最近的地图平面进行强制关联，使得在最终的地图构建过程中，可以仅通过地图平面构建出地图，可以有效减少三维坐标点在地图构建过程中耗费的计算资源，提高地图构建效率。
具体地，平面模块基于所述若干地图平面在所述三维网格中的位置，建立所述第一三维坐标点、所述第二三维坐标点与所述地图平面的关联，以及建立相邻地图平面之间的关联；利用所述第一三维坐标点、所述第二三维坐标点与所述地图平面的关联，和相邻地图平面之间的关联构建最终的地图。
例如，平面模块检验一个平面和其他平面是否满足一定的夹角角度和距离，在两个平面的夹角角度小于预设角度阈值，且距离小于第一预设距离阈值时，即可对两个平面进行平面合并。其中，本申请实施例的预设角度阈值可以设置为10°，第一预设距离阈值可以设置为10cm。需要说明的是，以上平面合并过程可以发生在检测新的平面的过程中，也可以发生在调整现有平面的过程中。
又例如，平面模块还可以使用三维网格将更多的地图点与平面联系起来。当三维网格与一个平面相关联时，平面模块获取平面的二维网格，如果地图点的二维坐标都在二维网格内，并且地图点到平面的距离小于第二预设距离阈值，如10cm时，平面模块将该地图点添加到与该平面相关的候选集中。如果候选集中的一个地图点在超过3个关键帧中都被观察到时，平面模块进一步检查该地图点的几何一致性。平面模块可以通过计算地图点从一个关键帧到该平面的重投影误差，从而强迫地图点与平面相关联；然后，计算从另一个关键帧到该平面的重投影误差。如果两个重投影误差接近，且最大重投影误差小于一定阈值时，则认为该地图点为该平面内的平面点。如果一个地图点多次未能通过几何一致性的检查，则将其从候选点集中删除。
以上实施例，仅是对本申请的其中一种常见案例而已，并非对本申请的技术范围做任何限制，故凡是依据本申请方案的实质对以上内容所做的任何细微修改、等同变化或者修饰，均仍属于本申请技术方案的范围内。
请继续参见图6，图6是本申请提供的终端设备一实施例的结构示意图。其中，终端设备30包括接收模块31、平面模块32以及构建模块33。
接收模块31，用于接收图像数据以及惯性数据。
平面模块32，用于基于所述图像数据和惯性数据，获取所述地图的三维坐标点；利用所述三维坐标点生成平面数据。
构建模块33，用于结合所述三维坐标点和所述平面数据，构建所述地图。
平面模块32，还用于利用所述惯性数据获取惯性约束；利用所述平面数据和所述三维坐标点获取重投影误差和深度约束；按照所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数；利用所述第一平面优化函数对所述平面数据进行优化。
平面模块32，还用于基于所述平面数据和所述三维坐标点，获取平面关联的观测点在第一关键帧的第一坐标，以及所述观测点在第二关键帧的第二坐标；利用所述第一坐标、所述第二坐标以及预设的投影方程，获取所述观测点与所述平面的点面约束；按照所述点面约束、所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数。
平面模块32，还用于基于所述图像数据，获取采集第一关键帧的第一相机参数，以及采集第二关键帧的第二相机参数；获取所述第一相机参数和所述第二相机参数的相对相机参数；利用所述平面数据、所述相对相机参数、所述第一相机参数和所述第二相机参数生成第二平面优化函数；利用所述第二平面优化函数对所述平面数据进行优化。
平面模块32，还用于利用前端对所述图像数据和所述惯性数据进行数据处理，得到实时的第一三维坐标点；将所述前端的图像数据和惯性数据传输到后端；利用所述后端对所述前端的图像数据和惯性数据进行数据处理，得到非实时的第二三维坐标点。
平面模块32，还用于利用所述前端基于所述第一三维坐标点，生成实时的第一平面数据；利用所述后端基于所述第二三维坐标点，生成非实时的第二平面数据；所述结合所述三维坐标点和所述平面数据，构建所述地图，包括结合所述第一三维坐标点、所述第一平面数据、所述第二三维坐标点以及所述第二平面数据构建所述地图。
构建模块33，还用于利用所述第一三维坐标点和所述第二三维坐标点创建三维网格；利用所述第一平面数据、所述第二平面数据以及所述三维网格，生成若干地图平面；利用所述若干地图平面获取最终的地图。
构建模块33，还用于基于所述若干地图平面在所述三维网格中的位置，建立所述第一三维坐标点、所述第二三维坐标点与所述地图平面的关联，以及建立相邻地图平面之间的关联；利用所述第一三维坐标点、所述第二三维坐标点与所述地图平面的关联，和相邻地图平面之间的关联构建最终的地图。
请继续参见图7，图7是本申请提供的终端设备另一实施例的结构示意图。本申请实施例的终端设备500包括处理器51、存储器52、输入输出设备53以及总线54。
该处理器51、存储器52、输入输出设备53分别与总线54相连，该存储器52中存储有程序数据，处理器51用于执行程序数据以实现上述任意实施例所述的地图构建方法。
在本申请实施例中，处理器51还可以称为CPU(Central Processing Unit，中央处理单元)。处理器51可能是一种集成电路芯片，具有信号的处理能力。处理器51还可以是通用处理器、数字信号处理器(DSP，Digital Signal Process)、专用集成电路(ASIC，Application Specific Integrated Circuit)、现场可编程门阵列(FPGA，FieldProgrammable Gate Array)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器51也可以是任何常规的处理器等。
本申请还提供一种计算机存储介质，请继续参阅图8，图8是本申请提供的计算机存储介质一实施例的结构示意图，该计算机存储介质600中存储有程序数据61，该程序数据61在被处理器执行时，用以实现上述任意实施例的地图构建方法。
本申请的实施例以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，方式利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (12)

1.一种地图构建方法，其特征在于，包括：
接收图像数据以及惯性数据；
基于所述图像数据和惯性数据，获取所述地图的三维坐标点；
利用所述三维坐标点生成平面数据；
结合所述三维坐标点和所述平面数据，构建所述地图。
2.根据权利要求1所述的地图构建方法，其特征在于，
所述利用所述三维坐标点生成平面数据之后，所述地图构建方法还包括：
利用所述惯性数据获取惯性约束；
利用所述平面数据和所述三维坐标点获取重投影误差和深度约束；
按照所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数；
利用所述第一平面优化函数对所述平面数据进行优化。
3.根据权利要求2所述的地图构建方法，其特征在于，
所述按照所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数，包括：
基于所述平面数据和所述三维坐标点，获取平面关联的观测点在第一关键帧的第一坐标，以及所述观测点在第二关键帧的第二坐标；
利用所述第一坐标、所述第二坐标以及预设的投影方程，获取所述观测点与所述平面的点面约束；
按照所述点面约束、所述惯性约束、所述重投影误差和所述深度约束生成第一平面优化函数。
4.根据权利要求1～3任一项所述的地图构建方法，其特征在于，
所述利用所述三维坐标点生成平面数据之后，所述地图构建方法还包括：
基于所述图像数据，获取采集第一关键帧的第一相机参数，以及采集第二关键帧的第二相机参数；
获取所述第一相机参数和所述第二相机参数的相对相机参数；
利用所述平面数据、所述相对相机参数、所述第一相机参数和所述第二相机参数生成第二平面优化函数；
利用所述第二平面优化函数对所述平面数据进行优化。
5.根据权利要求1所述的地图构建方法，其特征在于，
所述基于所述图像数据和惯性数据，获取所述地图的三维坐标点，包括：
利用前端对所述图像数据和所述惯性数据进行数据处理，得到实时的第一三维坐标点；
将所述前端的图像数据和惯性数据传输到后端；
利用所述后端对所述前端的图像数据和惯性数据进行数据处理，得到非实时的第二三维坐标点。
6.根据权利要求5所述的地图构建方法，其特征在于，
所述利用所述三维坐标点生成平面数据，包括：
利用所述前端基于所述第一三维坐标点，生成实时的第一平面数据；
利用所述后端基于所述第二三维坐标点，生成非实时的第二平面数据；
所述结合所述三维坐标点和所述平面数据，构建所述地图，包括：
结合所述第一三维坐标点、所述第一平面数据、所述第二三维坐标点以及所述第二平面数据构建所述地图。
7.根据权利要求6所述的地图构建方法，其特征在于，
所述结合所述第一三维坐标点、所述第一平面数据、所述第二三维坐标点以及所述第二平面数据构建所述地图，包括：
利用所述第一三维坐标点和所述第二三维坐标点创建三维网格；
利用所述第一平面数据、所述第二平面数据以及所述三维网格，生成若干地图平面；
利用所述若干地图平面获取最终的地图。
8.根据权利要求7所述的地图构建方法，其特征在于，
所述生成若干地图平面之后，所述地图构建方法还包括：
基于所述若干地图平面在所述三维网格中的位置，建立所述第一三维坐标点、所述第二三维坐标点与所述地图平面的关联，以及建立相邻地图平面之间的关联；
所述利用所述若干地图平面获取最终的地图，包括：
利用所述第一三维坐标点、所述第二三维坐标点与所述地图平面的关联，和相邻地图平面之间的关联构建最终的地图。
9.一种地图构建系统，其特征在于，所述地图构建系统包括：
前端，用于接收图像数据以及惯性数据，基于所述图像数据和所述惯性数据获取实时的第一三维坐标点，以及基于所述第一三维坐标点生成第一平面数据；
后端，用于接收所述前端的图像数据以及惯性数据，基于所述图像数据和所述惯性数据获取非实时的第二三维坐标点，以及基于所述第二三维坐标点生成第二平面数据；
平面模块，用于结合所述第一三维坐标点、所述第一平面数据、所述第二三维坐标点以及所述第二平面数据构建所述地图。
10.根据权利要求9所述的地图构建系统，其特征在于，所述地图构建系统，还包括：
地图模块，用于存储所述地图。
11.一种终端设备，其特征在于，所述终端设备包括存储器以及与所述存储器耦接的处理器；
其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如权利要求1～8任一项所述的地图构建方法。
12.一种计算机存储介质，其特征在于，所述计算机存储介质用于存储程序数据，所述程序数据在被计算机执行时，用以实现如权利要求1～8任一项所述的地图构建方法。
CN202210043361.9A 2022-01-14 2022-01-14 地图构建方法、系统、设备以及计算机存储介质 Pending CN114445591A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210043361.9A CN114445591A (zh) 2022-01-14 2022-01-14 地图构建方法、系统、设备以及计算机存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210043361.9A CN114445591A (zh) 2022-01-14 2022-01-14 地图构建方法、系统、设备以及计算机存储介质

Publications (1)

Publication Number Publication Date
CN114445591A true CN114445591A (zh) 2022-05-06

Family

ID=81368091

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210043361.9A Pending CN114445591A (zh) 2022-01-14 2022-01-14 地图构建方法、系统、设备以及计算机存储介质

Country Status (1)

Country Link
CN (1) CN114445591A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116124152A (zh) * 2023-04-14 2023-05-16 北京博能科技股份有限公司 一种跨建筑物室内路径的路网拓扑结构生成方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116124152A (zh) * 2023-04-14 2023-05-16 北京博能科技股份有限公司 一种跨建筑物室内路径的路网拓扑结构生成方法

Similar Documents

Publication Publication Date Title
CN111258313B (zh) 多传感器融合slam系统及机器人
US11668571B2 (en) Simultaneous localization and mapping (SLAM) using dual event cameras
CN112785702B (zh) 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN109345588B (zh) 一种基于Tag的六自由度姿态估计方法
CN107833236B (zh) 一种动态环境下结合语义的视觉定位系统和方法
CN110555901B (zh) 动静态场景的定位和建图方法、装置、设备和存储介质
EP2153409B1 (en) Camera pose estimation apparatus and method for augmented reality imaging
US10719727B2 (en) Method and system for determining at least one property related to at least part of a real environment
CN108229416B (zh) 基于语义分割技术的机器人slam方法
CN111275763A (zh) 闭环检测系统、多传感器融合slam系统及机器人
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN116449384A (zh) 基于固态激光雷达的雷达惯性紧耦合定位建图方法
El Bouazzaoui et al. Enhancing RGB-D SLAM performances considering sensor specifications for indoor localization
CN114445591A (zh) 地图构建方法、系统、设备以及计算机存储介质
Li et al. An effective point cloud registration method based on robust removal of outliers
Shao A Monocular SLAM System Based on the ORB Features
CN116894876A (zh) 基于实时图像的6-dof的定位方法
Svedman et al. Structure from stereo vision using unsynchronized cameras for simultaneous localization and mapping
CN114119885A (zh) 图像特征点匹配方法、装置及系统、地图构建方法及系统
Wu et al. DPC-SLAM: Discrete Plane Constrained VSLAM for Intelligent Vehicle in Road Environment
WO2013173383A1 (en) Methods and apparatus for processing image streams
Butt et al. Multi-task Learning for Camera Calibration
CN118314162B (zh) 一种面向时序性稀疏重建的动态视觉slam方法及装置
CN117523428B (zh) 基于飞行器平台的地面目标检测方法和装置
US20230106749A1 (en) Three-dimensional measurement device

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