CN113269876B - 地图点坐标优化方法及装置、电子设备及存储介质 - Google Patents
地图点坐标优化方法及装置、电子设备及存储介质 Download PDFInfo
- Publication number
- CN113269876B CN113269876B CN202110506448.0A CN202110506448A CN113269876B CN 113269876 B CN113269876 B CN 113269876B CN 202110506448 A CN202110506448 A CN 202110506448A CN 113269876 B CN113269876 B CN 113269876B
- Authority
- CN
- China
- Prior art keywords
- map point
- optimization
- point coordinate
- map
- coordinate
- 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
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 176
- 238000000034 method Methods 0.000 title claims abstract description 90
- 239000011159 matrix material Substances 0.000 claims abstract description 83
- 238000010276 construction Methods 0.000 claims abstract description 8
- 238000003384 imaging method Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 9
- 230000007613 environmental effect Effects 0.000 claims description 6
- 230000000007 visual effect Effects 0.000 description 10
- 230000006870 function Effects 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 5
- 238000000605 extraction Methods 0.000 description 5
- 238000012216 screening Methods 0.000 description 5
- 238000013507 mapping Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000000059 patterning Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Abstract
本申请公开了一种用于地图构建的地图点坐标优化方法,包括:利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标,并根据第一优化确定相机的位姿误差与坐标误差的关系矩阵,以及根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。本申请实施方式的地图点坐标优化方法,可在BA优化的基础上对矩阵迹值进行最大化求解从而再次对地图点坐标进行优化,有效地校正了BA优化带来的误差,从而在一定程度上提升了地图点坐标的精度。本申请还公开了一种地图点坐标优化装置、电子设备及存储介质。
Description
技术领域
本申请涉及地图技术领域,特别涉及一种地图构建的地图点坐标优化方法、地图点坐标优化装置、电子设备及存储介质。
背景技术
随着AR导航、自动驾驶等领域的应用在不断成熟,高精地图是实现这些应用的关键基础。而同时定位与地图构建(Simultaneous Location and Mapping,SLAM)作为底层的建图技术得到了越来越多的关注,可用于描述机器人在未知环境的未知地点中同步的进行地图构建和自身定位。
在SLAM算法问题中,可利用光束平差法(Bundle Adjustment,BA)对相机或激光的位姿和坐标进行联合优化,得到精度较高的地图点用于设备定位。传统的视觉SLAM的缺点在于BA优化的结果较大程度依赖于相机或激光的位姿估计精度和地图点的匹配精度。而由于相机或激光位姿估计结果不可避免的存在估计误差,导致BA优化后的地图点坐标与实际值会存在一定程度偏差。且对于快速运动或运动剧烈的应用场景,其初始值的误差较高,导致了最终地图点坐标的误差较高。
发明内容
有鉴于此,本申请的实施例提供了一种地图构建的地图点坐标优化方法、地图点坐标优化装置、电子设备及存储介质。
本申请提供了一种地图构建的地图点坐标优化方法,包括:
利用光束平差法对所述地图点坐标进行第一优化得到第一地图点坐标;
根据所述第一优化确定位姿误差与坐标误差的关系矩阵;
根据所述关系矩阵的矩阵迹建立优化模型以对所述第一地图点坐标进行第二优化得到第二地图点坐标。
本申请还提供了一种地图构建的地图点坐标优化方法,包括:
第一优化模块,用于利用光束平差法对所述地图点坐标进行第一优化得到第一地图点坐标;
矩阵模块,用于根据所述第一优化确定位姿误差与坐标误差的关系矩阵;
第二优化模块,用于根据所述关系矩阵的矩阵迹建立优化模型以对所述第一地图点坐标进行第二优化得到第二地图点坐标。
本申请还提供了一种电子设备,其特征在于,所述电子设备包括存储器和处理器,所述存储器中存储有计算机程序,所述计算机程序被所述处理器执行时,实现上述实施方式的地图点坐标优化方法。
本申请还提供了一种计算机程序的非易失性计算机可读存储介质,当所述计算机程序被一个或多个处理器执行时,实现上述实施方式的地图点坐标优化方法。
如此,本申请实施方式的地图点坐标优化方法、地图点坐标优化装置、电子设备及存储介质中,通过利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标,并根据第一优化确定位姿误差与坐标误差的关系矩阵,以及根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。相较于现有的BA优化仅通过对矩阵进行局部筛选以优化地图点坐标,本申请在BA优化的基础上通过对坐姿误差与坐标误差的关系矩阵迹建立优化模型,从全局对地图点坐标值进行优化,有效地校正了BA优化带来的误差,从而在一定程度上提升了地图点坐标的精度。同时,利用优化后的地图估计得到的相机或激光位姿的精度也更高。同时,对于相机或激光位姿及地图点坐标的初始值不精确的应用场景,如快速运动或运动剧烈等应用场景,容易导致相机或激光位姿及地图点坐标的间隔较大,其获取的初始值准确度较低,而现有的BA优化只是基于局部矩阵的筛选,对于初始值较差的场景,其最终的优化结果较差,导致地图精度较低。而本申请通过对全局的关系矩阵的迹进行最大化约束求解,可以在较大程度上校正初始值准确度较低的场景中BA优化带来的误差。另外,本申请的对所有利用BA优化得到的地图的系统均可适用,如单目视觉SLAM、双目视觉SLAM、RGB-D或者激光SLAM系统中都具有一定的适用性,其适应性较广。
附图说明
本申请的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解。
图1是本申请某些实施方式的地图点坐标优化方法流程示意图;
图2是本申请某些实施方式的地图点坐标优化装置模块图;
图3是本申请某些实施方式的地图点坐标优化方法流程示意图;
图4是本申请某些实施方式的地图点坐标优化方法流程示意图;
图5是本申请某些实施方式的地图点坐标优化方法流程示意图;
图6是本申请某些实施方式的地图点坐标优化方法流程示意图;
图7是本申请某些实施方式的地图点坐标优化方法流程示意图;
图8是本申请某些实施方式的地图点坐标优化装置模块图;
图9是本申请某些实施方式的地图点坐标优化方法流程示意图;
图10是本申请某些实施方式的地图点坐标优化装置模块图;
图11是本申请某些实施方式的地图点坐标优化方法实验结果图;
图12是本申请某些实施方式的地图点坐标优化方法实验结果图;
图13是本申请某些实施方式的地图点坐标优化方法实验结果图。
具体实施方法
下面详细描述本申请的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本申请,而不能理解为对本申请的限制。
请参阅图1,本申请提供了一种用于地图构建的地图点坐标优化方法,其特征在于,包括:
S10:利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标;
S20:根据第一优化确定位姿误差与坐标误差的关系矩阵;
S30:根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。
相应地,请参阅图2,本申请实施方式还提供了一种地图点坐标优化装置100,本申请实施方式的地图点坐标优化方法可以由地图点坐标优化装置100实现。地图点坐标优化装置100包括第一优化模块110、矩阵模块120及第二优化模块130。S10可以由第一优化模块110实现,S20可以由矩阵模块120实现,S30可以由第二优化模块130实现。或者说,第一优化模块110用于利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标。矩阵模块120用于根据第一优化确定相机的位姿误差与坐标误差的关系矩阵。第二优化模块130用于根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。
本申请实施方式还提供了一种电子设备。电子设备包括存储器和处理器。存储器中存储有计算机程序,处理器用于利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标,并根据第一优化确定相机的位姿误差与坐标误差的关系矩阵,以及根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。
具体地,在步骤S10中,利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标。
可以理解,在光束平差法(Bundle Adjustment,BA)BA优化前还包括前端得到粗略的地图点坐标,得到粗略的地图点坐标的方法为SLAM的通常前端方法,此处不展开说明。
当得到粗略的地图点坐标之后,对地图点坐标进行BA优化。可以理解,BA优化为视觉SLAM中的后端优化方法,通过优化模型进行最小化重投影误差。BA优化可以优化相机或激光的位姿,还可以优化特征点的空间位置即地图点坐标。
进一步地,通过BA优化算法求解得到第一优化后的地图点坐标,即就是第一地图点坐标。对于BA优化算法,可利用高斯牛顿求解法、牛顿法、梯度下降法等算法对BA优化的优化模型进行求解。
请参阅图3,优选地,在某些实施方法中,步骤S10包括:
S11:根据位姿和地图点坐标确定光束平差法的最小二乘问题,其中位姿包括相机位姿或激光位姿;
S12:利用高斯-牛顿法对最小二乘问题进行求解以得到第一地图点坐标。
在某些实施方法中,S11和S12可以由地图点坐标优化装置100的第一优化模块110实现。或者说,第一优化模块110用于根据位姿和地图点坐标确定光束平差法的最小二乘问题,其中位姿包括相机位姿或激光位姿,并利用高斯-牛顿法对最小二乘问题进行求解以得到第一地图点坐标。
在某些实施方法中,处理器用于根据位姿和地图点坐标确定光束平差法的最小二乘问题,其中位姿包括相机位姿或激光位姿,并利用高斯-牛顿法对最小二乘问题进行求解以得到第一地图点坐标。
需要说明的是,本申请可用于单目视觉SLAM、双目视觉SLAM、RGB-D或者激光SLAM系统,以下为单目视觉SLAM为例展开实施例。
具体地,在视觉SLAM中,由于两张图片有很多对特征匹配点,将所有特征点误差求和,取平方,再乘上1/2,可构建一个非线性最小二乘问题。再将其中的位姿、地图点坐标作为优化的对象,最小化重投影误差,即可得到一个优化后的结果。如可采用如下BA优化公式:
其中,h(x,pi)表示相机成像模型函数,x表示相机位姿,pi表示地图点坐标,zi表示观测像素坐标。相机成像模型函数h(x,pi)根据实际相机成像模型确定函数,为通常含义,包括针孔相机成像模型、鱼眼相机成像模型、双目相机成像模型等。
进一步地,可利用高斯-牛顿法对最小二乘问题进行求解以得到第一地图点坐标。高斯-牛顿法是非线性最小二乘的一种优化算法,采用逐步迭代的方法,把非线性问题变成一步一步迭代的线性问题,从而达到极值收敛。
在步骤S20中,当利用BA优化进行第一优化时,可确定位姿误差与坐标误差的关系矩阵。其中,位姿误差包括但不限于相机的位姿误差。关系矩阵可为H稀疏矩阵。例如利用高斯-牛顿法对BA优化进行计算求解得到相机位姿误差E(εx)与地图点坐标误差bp间的增量方程为:HcE(εx)=bp,其中,Hc为所述H矩阵。
进一步地,在步骤S30中,根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。
具体地,矩阵迹为矩阵的主对角线(从左上方至右下方的对角线)上各个元素的总和。根据矩阵迹建立优化模型包括但不限于对矩阵迹的值的最大化求解、或矩阵迹达到阈值的优化模型等。可通过调整各参数,如相机或激光位姿或地图点坐标等参数,同时在调整过程中计算H矩阵的矩阵迹值/>使得矩阵迹值达到优化模型的所需的值。
请参阅图4,优选地,在某些实施方法中,步骤S30包括:
S31:建立对矩阵迹的值最大化的优化模型;
S32:对优化模型求解以得到第二地图点坐标。
在某些实施方法中,S31和S32可以由地图点坐标优化装置100的第二优化模块130实现。或者说,第二优化模块130用于建立对矩阵迹的值最大化的优化模型,对优化模型求解以得到第二地图点坐标。
在某些实施方法中,处理器用于建立对矩阵迹的值最大化的优化模型,对优化模型求解以得到第二地图点坐标。
具体地,对步骤S20中所得到的关系矩阵即H稀疏矩阵进行矩阵迹求解,并对矩阵迹的值进行最大化的优化,如带有约束条件的最大化。约束条件可通过对地图点坐标进行调整或对位姿进行调整以使得矩阵迹的值最大。其中对地图点坐标进行调整包括但不限于对地图点坐标与相机坐标值的距离进行阈值设定,或直接调整各地图点坐标值等方法。
如此,根据机器学习理论中信息选择理论可知,矩阵的迹在一定程度上表征了地图点的质量,迹的值越大则地图点的质量越高。本实施方法通过矩阵迹值进行最大化求解从而再次对地图点坐标进行优化,在一定程度上提高了地图点的质量。相较于现有的BA优化仅通过对矩阵进行局部筛选以优化地图点坐标,本申请在BA优化的基础上从全局对地图点坐标值进行优化,有效地校正了BA优化带来的误差,从而在一定程度上提升了地图点坐标的精度。
请参阅图5,优选地,在某些实施方法中,优化模型包括:
s.t.
|h(x,p1′)-x1|2<δ
|h(x,p2′)-z2|2<δ
…
|h(x,pn′)-zn|2<δ
其中,表示所述矩阵迹,x表示相机的位姿,pi′表示第一地图点坐标,0≤i≤n,h(x,pi′)表示相机成像模型,zi表示观测像素坐标,δ为预设的阈值;
相应地,步骤S32包括:
S321:通过对第一地图点坐标进行调整以使得矩阵迹的值最大化。
具体地,以矩阵迹为目标函数求取值范围的最大值,同时pi′满足约束条件:|h(x,pi′)-z1|2<δ,其中,δ为预设的阈值。例如δ=2,则约束条件为:
|h(x,p1′)-z1|2<2
|h(x,p2′)-z2|2<2
…
|h(x,pn′)-zn|2<2
进一步地,通过对第一地图坐标点pi进行调整使得目标函数中的矩阵迹的值最大。即就是对第一地图点坐标进行了第二优化,最终得到第二地图点坐标。
需要说明的是,上述以视觉SLAM为例对以地图点坐标为约束条件进行说明,对于其他SLAM系统,可相应地调整优化模型。
如此,通过调整地图点的坐标与相机坐标点的距离在阈值范围内以使得H矩阵的矩阵迹的值最大。相较于以其它参数如位姿为约束条件,其计算量更小,同时最终优化后的地图点坐标更准确。
请参阅图6,在某些实施方法中,地图点坐标包括里程计地图点坐标和回环地图点坐标,步骤S10还包括:
S13:当地图点坐标为里程计地图点坐标时,利用光束平差法对里程计地图点坐标进行第一优化得到第一地图点坐标;
S14:当地图点坐标为回环地图点坐标时,利用光束平差法对回环地图点坐标进行第一优化得到第一地图点坐标。
在某些实施方法中,S13和S14可以由地图点坐标优化装置100的第一优化模块110实现。或者说,第一优化模块110用于当地图点坐标为里程计地图点坐标时,利用光束平差法对里程计地图点坐标进行第一优化得到第一地图点坐标,当地图点坐标为回环地图点坐标时,利用光束平差法对回环地图点坐标进行第一优化得到第一地图点坐标。
在某些实施方法中,处理器用于当地图点坐标为里程计地图点坐标时,利用光束平差法对里程计地图点坐标进行第一优化得到第一地图点坐标,当地图点坐标为回环地图点坐标时,利用光束平差法对回环地图点坐标进行第一优化得到第一地图点坐标。
可以理解,视觉SLAM分为前端和后端,而前端包括视觉里程计和回环检测。其中,视觉里程计研究相邻图像帧之间的变换关系以完成实时的位姿跟踪从而得到运动关系。而回环检测,又称为闭环检测,主要是利用图像之间的相似性来判断是否到达先前的位置,以此来消除累计误差,从而得到全局一致性轨迹和地图。故当优化的地图点坐标为里程计地图点坐标时,利用光束平差法对里程计地图点坐标进行第一优化得到第一地图点坐标,当优化的地图点坐标为回环地图点坐标时,利用光束平差法对回环地图点坐标进行第一优化得到第一地图点坐标。即就是,本申请可同时用于里程计线程与回环检测的后端优化。
请参阅图7,在某些实施方法中,步骤S10之前还包括:
S01:利用摄像头采集环境视频;
S02:对环境视频进行特征提取以得到帧间图像点的匹配关系;
S03:利用匹配关系确定地图点坐标。
相应地,请参阅图8,本申请实施方式的地图点坐标优化装置100还包括采集模块140、提取模块150及确定模块160。S01可以由采集模块140实现,S02可以由提取模块150实现,S03可以由确定模块160实现。或者说,采集模块140用于利用摄像头采集环境视频。提取模块150用于对环境视频进行特征提取以得到帧间图像点的匹配关系。确定模块160用于利用匹配关系确定地图点坐标。
在某些实施方法中,处理器用于利用摄像头采集环境视频,对环境视频进行特征提取以得到帧间图像点的匹配关系,及利用匹配关系确定地图点坐标。
具体地,首先需要利用摄像头在需要建图的环境中不断采集视频,可以理解的是,为保证能够建出高质量的地图,同一地点可重复多次拍摄。进一步地,对拍摄的视频进行特征提取或光流计算,从而得到帧间图像点的匹配关系。特征点匹配可采用光流跟踪得到关键点的匹配。同时为了排除误跟踪,可以采用一致性检测。而如果两帧之间的运动量和外观变化较大,则需要计算两帧之间的特征点和描述子,比较描述子之间的距离。由于计算量关系,也可采用恒速模型在预期区域中搜索潜在的对应关系。而如果是双目匹配或者深度滤波器中计算每个像素的深度,则可采用极线搜索和归一化互相关或绝对误差和找到匹配点。当确定特征匹配后,利用该匹配关系可以计算出基本矩阵F或单应矩阵H,然后对F或H进行奇异值分解得到两帧间相机的位姿,根据该位姿和图像点的对应关系利用三角化方法即可得到粗略的地图点坐标。
进一步地,利用本申请的上述任一种优化方法可对粗略的地图点坐标进行优化从而得到第二地图点坐标。
请参阅图9,在某些实施方法中,地图点坐标优化方法还包括:
S40:根据第二地图点坐标构建地图。
相应地,请参阅图10,本申请实施方式的地图点坐标优化装置100还包括构图模块170。S40可以由构图模块170实现。或者说,构图模块170用于根据第二地图点坐标构建地图。
在某些实施方法中,处理器用于根据第二地图点坐标构建地图。
具体地,当前端以及后端的第二优化完成后得到第二地图点坐标,则视觉SLAM可根据优化后的第二地图点坐标构建地图。可以理解的是,视觉SLAM中的构建地图包括路标地图、拓扑地图、度量地图和混合地图等。可通过三角测量或深度估计,将平面图像中的信息转换为空间3D路标点等。
如此,在视觉SLAM中在BA优化的基础上通过第二次优化,可以有效地校正BA优化带来的误差,在一定程度上提升了地图点坐标的精度。
请参阅图11-13,在一些实例中,将本申请的方案在ORBSLAM系统中进行了实际应用,并同时与没有实施本申请的ORBSLAM优化进行了对比验证。图11-13为三个实施例的EuroC公开数据集上的测试结果,每一个为不同的应用场景。其中纵坐标为绝对位姿误差或者说绝对轨迹误差APE,横坐标左边方框our表示应用本申请的ORBSLAM优化得到的相机或激光位姿误差,横坐标右边ORB-SLAM表示传统ORBSLAM系统相机或激光位姿估计误差。
从图中可以看出应用本申请的ORBSLAM优化得到的相机或激光位姿误差估计较大程度低于ORBSLAM。
综上所述,在本申请实施方式的地图点坐标优化方法、地图点坐标优化装置、电子设备及存储介质中,通过利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标,并根据第一优化确定位姿误差与坐标误差的关系矩阵,以及根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。至少具有以下有益效果:
一、在BA优化的基础上对矩阵迹值进行最大化求解从而再次对地图点坐标进行优化,在一定程度上提高了地图点的质量。
二、相较于现有的BA优化仅通过对矩阵进行局部筛选以优化地图点坐标,本申请在BA优化的基础上通过对坐姿误差与坐标误差的关系矩阵迹建立优化模型,从全局对地图点坐标值进行优化,有效地校正了BA优化带来的误差,从而在一定程度上提升了地图点坐标的精度。
三、利用优化后的地图估计得到的相机或激光位姿的精度也更高。
四、本申请的对所有利用BA优化得到的地图的系统均可适用,如单目视觉SLAM、双目视觉SLAM、RGB-D或者激光SLAM系统中都具有一定的适用性,其适应性较广。
五、对于相机或激光位姿及地图点坐标的初始值不精确的应用场景,如快速运动或运动剧烈等应用场景,容易导致相机或激光位姿及地图点坐标的间隔较大,其获取的初始值准确度较低,而现有的BA优化只是基于局部矩阵的筛选,对于初始值较差的场景,其最终的优化结果较差,导致地图精度较低。而本申请通过对全局的关系矩阵的迹进行最大化约束求解,可以在较大程度上校正初始值准确度较低的场景中BA优化带来的误差。
本申请实施方式还提供了一种计算机可读存储介质。一个或多个存储有计算机程序的非易失性计算机可读存储介质,当计算机程序被一个或多个处理器执行时,实现上述任一实施方式的地图点坐标优化方法。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的软件来完成。程序可存储于一非易失性计算机可读存储介质中,该程序在执行时,可包括如上述各方法的实施例的流程。其中存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory,ROM)等。
以上实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对本申请专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (10)
1.一种地图构建的地图点坐标优化方法,其特征在于,包括:
利用光束平差法对所述地图点坐标进行第一优化得到第一地图点坐标;
根据所述第一优化确定位姿误差与坐标误差的关系矩阵,所述位姿包括相机位姿或激光位姿;
根据所述关系矩阵的矩阵迹建立优化模型,以通过所述优化模型对所述第一地图点坐标进行第二优化得到第二地图点坐标,所述优化模型与所述矩阵迹、所述相机位姿、所述第一地图点坐标、相机成像模型、观测像素坐标和预设的阈值有关。
2.根据权利要求1所述的地图点坐标优化方法,其特征在于,所述利用光束平差法对所述地图点坐标进行第一优化得到第一地图点坐标包括:
根据位姿和所述地图点坐标确定所述光束平差法的最小二乘问题;
利用高斯-牛顿法对所述最小二乘问题进行求解以得到所述第一地图点坐标。
3.根据权利要求2所述的地图点坐标优化方法,其特征在于,所述根据所述关系矩阵的矩阵迹建立优化模型以对所述第一地图点坐标进行第二优化得到第二地图点坐标包括:
建立对所述矩阵迹的值最大化的所述优化模型;
对所述优化模型求解以得到所述第二地图点坐标。
4.根据权利要求3所述的地图点坐标优化方法,其特征在于,所述优化模型包括:
s.t.
|h(x,p1′)-z1|2<δ
|h(x,p2′)-z2|2<δ
…
|h(x,pn′)-zn|2<δ
其中,表示所述矩阵迹,x表示所述相机位姿,pi′表示所述第一地图点坐标,h(x,pi′)表示所述相机成像模型,zi表示所述观测像素坐标,δ为所述预设的阈值;
所述对所述优化模型求解以得到所述第二地图点坐标包括:
通过对所述第一地图点坐标进行调整以使得所述矩阵迹的值最大化。
5.根据权利要求1所述的地图点坐标优化方法,其特征在于,所述地图点坐标包括里程计地图点坐标和回环地图点坐标,所述利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标还包括:
当所述地图点坐标为所述里程计地图点坐标时,利用所述光束平差法对所述里程计地图点坐标进行所述第一优化得到所述第一地图点坐标;
当所述地图点坐标为所述回环地图点坐标时,利用所述光束平差法对所述回环地图点坐标进行所述第一优化得到所述第一地图点坐标。
6.根据权利要求1所述的地图点坐标优化方法,其特征在于,所述利用光束平差法对所述地图点坐标进行第一优化得到第一地图点坐标之前包括:
利用摄像头采集环境视频;
对所述环境视频进行特征提取得到帧间图像点的匹配关系;
利用所述匹配关系确定所述地图点坐标。
7.根据权利要求6所述的地图点坐标优化方法,其特征在于,所述地图点坐标优化方法还包括:
根据所述第二地图点坐标构建地图。
8.一种地图构建的地图点坐标优化装置,其特征在于,包括:
第一优化模块,用于利用光束平差法对所述地图点坐标进行第一优化得到第一地图点坐标;
矩阵模块,用于根据所述第一优化确定位姿误差与坐标误差的关系矩阵,所述位姿包括相机位姿或激光位姿;
第二优化模块,用于根据所述关系矩阵的矩阵迹建立优化模型,以通过所述优化模型对所述第一地图点坐标进行第二优化得到第二地图点坐标,所述优化模型与所述矩阵迹、所述相机位姿、所述第一地图点坐标、相机成像模型、观测像素坐标和预设的阈值有关。
9.一种电子设备,其特征在于,所述电子设备包括存储器和处理器,所述存储器中存储有计算机程序,所述计算机程序被所述处理器执行时,实现权利要求1-7任一项所述的地图点坐标优化方法。
10.一种计算机程序的非易失性计算机可读存储介质,其特征在于,当所述计算机程序被一个或多个处理器执行时,实现权利要求1-7任一项所述的地图点坐标优化方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110506448.0A CN113269876B (zh) | 2021-05-10 | 地图点坐标优化方法及装置、电子设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110506448.0A CN113269876B (zh) | 2021-05-10 | 地图点坐标优化方法及装置、电子设备及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113269876A CN113269876A (zh) | 2021-08-17 |
CN113269876B true CN113269876B (zh) | 2024-06-21 |
Family
ID=
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109697734A (zh) * | 2018-12-25 | 2019-04-30 | 浙江商汤科技开发有限公司 | 位姿估计方法及装置、电子设备和存储介质 |
CN111462207A (zh) * | 2020-03-30 | 2020-07-28 | 重庆邮电大学 | 一种融合直接法与特征法的rgb-d同时定位与地图创建方法 |
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109697734A (zh) * | 2018-12-25 | 2019-04-30 | 浙江商汤科技开发有限公司 | 位姿估计方法及装置、电子设备和存储介质 |
CN111462207A (zh) * | 2020-03-30 | 2020-07-28 | 重庆邮电大学 | 一种融合直接法与特征法的rgb-d同时定位与地图创建方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109166149B (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 | |
CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
WO2021196294A1 (zh) | 一种跨视频人员定位追踪方法、系统及设备 | |
CN107990899B (zh) | 一种基于slam的定位方法和系统 | |
CN110807809B (zh) | 基于点线特征和深度滤波器的轻量级单目视觉定位方法 | |
CN108615246B (zh) | 提高视觉里程计系统鲁棒性和降低算法计算消耗的方法 | |
CN109579825B (zh) | 基于双目视觉和卷积神经网络的机器人定位系统及方法 | |
CN108519102B (zh) | 一种基于二次投影的双目视觉里程计算方法 | |
CN111354043A (zh) | 一种基于多传感器融合的三维姿态估计方法及装置 | |
KR101544021B1 (ko) | 3차원 지도 생성 장치 및 3차원 지도 생성 방법 | |
WO2024045632A1 (zh) | 基于双目视觉和imu的水下场景三维重建方法及设备 | |
CN112419497A (zh) | 基于单目视觉的特征法与直接法相融合的slam方法 | |
KR20080029080A (ko) | 단안 줌 카메라를 이용한 이동로봇의 자기위치 추정 시스템및 방법 | |
CN110243390B (zh) | 位姿的确定方法、装置及里程计 | |
CN110827321B (zh) | 一种基于三维信息的多相机协同的主动目标跟踪方法 | |
EP3786891A1 (en) | Method and system for visual localization based on dual dome cameras | |
CN112767546B (zh) | 移动机器人基于双目图像的视觉地图生成方法 | |
CN112598706B (zh) | 无需精确时空同步的多相机运动目标三维轨迹重建方法 | |
CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
CN116242374A (zh) | 一种基于直接法的多传感器融合的slam定位方法 | |
CN117237431A (zh) | 深度估计模型的训练方法、装置、电子设备及存储介质 | |
Wang et al. | An attitude estimation method based on monocular vision and inertial sensor fusion for indoor navigation | |
CN111429571B (zh) | 一种基于时空图像信息联合相关的快速立体匹配方法 | |
Dang et al. | Stereo calibration in vehicles | |
CN113269876B (zh) | 地图点坐标优化方法及装置、电子设备及存储介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant |