CN108572939B - Vi-slam的优化方法、装置、设备及计算机可读介质 - Google Patents
Vi-slam的优化方法、装置、设备及计算机可读介质 Download PDFInfo
- Publication number
- CN108572939B CN108572939B CN201810390462.7A CN201810390462A CN108572939B CN 108572939 B CN108572939 B CN 108572939B CN 201810390462 A CN201810390462 A CN 201810390462A CN 108572939 B CN108572939 B CN 108572939B
- Authority
- CN
- China
- Prior art keywords
- equation
- updating
- updated
- observation
- normal
- 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 73
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000002939 conjugate gradient method Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 33
- 230000000295 complement effect Effects 0.000 claims description 21
- 238000010276 construction Methods 0.000 claims description 6
- 238000004590 computer program Methods 0.000 claims description 3
- 230000033001 locomotion Effects 0.000 abstract description 14
- 238000004891 communication Methods 0.000 description 9
- 230000006870 function Effects 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Computational Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Operations Research (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Studio Devices (AREA)
- Image Processing (AREA)
Abstract
本发明提出一种VI‑SLAM的优化方法,包括:如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化;根据重新线性化后的观测方程,对法向方程进行增量式更新;根据更新后的法向方程,对舒尔补方程进行增量式更新;采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解,获得观测量的最优解。本发明实施例通过利用VI‑SLAM问题特殊的稀疏结构提升优化效率,以及通过利用SLAM问题的增量特性提升优化效率。同时,本发明优化效率的提升不依赖于运动方式,既适用于持续扩展地图的运动方式,又适用于在已扩展的地图中来回运动。
Description
技术领域
本发明涉及定位与制图技术领域,尤其涉及一种VI-SLAM的增量式优化方法及装置、设备和计算机可读介质。
背景技术
移动机器人,如仓储、物流、服务机器人、无人车、无人机等,能够协助或取代人类工作,有着广泛的应用前景,是目前科学技术发展最为活跃的领域之一。移动机器人的应用中一个至关重要的环节,是需要实时定位机器人在场景中的方位。只有获取了机器人的方位信息,才能控制其如何移动。目前主流的方案需要依靠激光雷达、差分GPS、高精度惯导系统等昂贵的硬件设备,或事先在场景中部署信号源或电磁轨道,才能实现精确定位和导航,极大限制了移动机器人的应用场景。SLAM(Simultaneous Localization and Mapping,同时定位与制图)技术仅通过低成本的相机、IMU(Inertial measurement unit,惯性测量单元)等传感器,即可在未知环境中实现移动机器人的自身定位,从而突破上述方案在硬件成本、应用场景方面的局限。其中,纯视觉SLAM(简称V-SLAM)融合视觉和IMU数据的SLAM称为Visual-Inertial SLAM,简称VI-SLAM。
在实际应用中,SLAM的定位精度和计算效率是最为重要的两项指标。现有的SLAM方法在精度和效率方面存在权衡。高精度的SLAM方案计算量大、功耗高,需要配备高性能计算设备,加大了硬件成本;高效的SLAM方案又往往需要牺牲定位精度,无法满足高精度的定位需求。VI-SLAM的计算分为前端和后端两个部分。前端负责从传感器中提取可用于判断机器人运动的观测量,比如提取场景在图像中的运动;后端由前端提取的观测量,同时推测机器人的运动以及场景的三维结构,比如随着机器人运动,场景在图像中不断放大,则可推测出机器人正在靠近场景。前端本质上可以看成是一个图像处理的过程,可以通过特定的硬件(比如GPU、FPGA等)提升计算效率。后端本质上是一个优化问题,只有通过降低优化算法复杂度才能提升计算效率。因此,如何在不牺牲优化精度的前提下,提升后端优化效率,是一个急需解决的技术问题。
发明内容
本发明实施例提供一种VI-SLAM的优化方法、装置、设备及计算机可读介质,以解决或缓解现有技术中的一个或多个技术问题。
第一方面,本发明实施例提供了一种VI-SLAM的优化方法,包括:
如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化;
根据重新线性化后的观测方程,对法向方程进行增量式更新;
根据更新后的法向方程,对舒尔补方程进行增量式更新;
采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解,获得观测量的最优解。
结合第一方面,本发明在第一方面的第一种实施方式中,采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解时,所采用的预条件子为对角线带状子矩阵B,其中,所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同,对角线带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等,其余部分为0,b为正整数。
结合第一方面的第一种实施方式,本发明在第一方面的第二种实施方式中,采用预条件共轭梯度法对更新后舒尔补方程进行增量式求解时,若更新后的舒尔补方程的上一次迭代结果Ci的绝对值大于设定阈值,则本次迭代初值Ci=0,否则用上一次迭代结果Ci作为本次迭代初值。
结合第一方面,本发明在第一方面的第三种实施方式中,一组观测量包括一组相机参数和一个三维点坐标,所述方法还包括:
根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标;
其中,如果m组相机参数为C={C1,C2,···,Cm},n个三维点坐标为X={X1,X2,···,Xn},则所述优化方程为:
其中,fij为观测方程,每个观测方程fij关联一组相机参数Ci和一个三维点Xj;
根据线性化后的观测方程,构造法向方程:
根据法向方程构造舒尔补方程:
结合第一方面的第三种实施方式,本发明在第一方面的第四种实施方式中,如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化的步骤,包括:
当相机参数差异|Ci|大于设定阈值、三维点坐标差异|Xj|大于设定阈值或接收到最新一帧的观测量作为增量时,重新线性化观测方程;
所述根据重新线性化后的观测方程,对法向方程进行增量式更新的步骤,包括:
结合第一方面的第五种实施方式,本发明在第一方面的第六种实施方式中,所述根据更新后的法向方程,对舒尔补方程进行增量式更新,包括:
第二方面,本发明实施例提供了一种VI-SLAM的优化装置,包括:
重新线性化模块,用于如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化;
法向方程更新模块,用于根据重新线性化后的观测方程,对法向方程进行增量式更新;
舒尔补方程更新模块,用于根据更新后的法向方程,对舒尔补方程进行增量式更新;
求解模块,用于采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解,获得观测量的最优解。
结合第二方面,本发明在第二方面的第一种实施方式中,所述求解模块在求解过程中,所采用的预条件子为对角线带状子矩阵B,其中,所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同,对角线带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等,其余部分为0,b为正整数。
结合第二方面的第一种实施方式,本发明在第二方面的第二种实施方式中,所述求解模块在求解过程中,若更新后的舒尔补方程的上一次迭代结果|Ci|大于设定阈值,则本次迭代初值Ci=0,否则用上一次迭代结果Ci作为本次迭代初值。
结合第二方面,本发明在第二方面的第三种实施方式中,一组观测量包括一组相机参数和一个三维点坐标,所述装置还包括:
优化方程构造模块,用于根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标;
其中,如果m组相机参数为C={C1,C2,···,Cm},n个三维点坐标为X={X1,X2,···,Xn},则所述优化方程为:
其中,fij为观测方程,每个观测方程fij关联一组相机参数Ci和一个三维点Xj;
法向方程构造模块,用于根据线性化后的观测方程,构造法向方程:
舒尔补方程构造模块,用于根据法向方程构造舒尔补方程:
结合第二方面的第二种实施方式,本发明在第二方面的第四种实施方式中,所述重新线性化模块具体用于当相机参数差异|Ci|大于设定阈值、三维点坐标差异|Xj|大于设定阈值或接收到最新一帧的观测量作为增量时,重新线性化观测方程;
所述装置的功能可以通过硬件实现,也可以通过硬件执行相应的软件实现。所述硬件或软件包括一个或多个与上述功能相对应的模块。
在一个可能的设计中,VI-SLAM的优化装置的结构中包括处理器和存储器,所述存储器用于存储支持VI-SLAM的优化装置执行上述第一方面中VI-SLAM的优化方法的程序,所述处理器被配置为用于执行所述存储器中存储的程序。所述VI-SLAM的优化装置还可以包括通信接口,用于VI-SLAM的优化装置与其他设备或通信网络通信。
第三方面,本发明实施例提供了一种计算机可读介质,用于存储VI-SLAM的优化装置所用的计算机软件指令,其包括用于执行上述第一方面的VI-SLAM的优化方法所涉及的程序。
本发明实施例通过利用VI-SLAM问题特殊的稀疏结构提升优化效率,以及通过利用SLAM问题的增量特性提升优化效率。同时,本发明优化效率的提升不依赖于运动方式,既适用于持续扩展地图的运动方式,又适用于在已扩展的地图中来回运动。
上述概述仅仅是为了说明书的目的,并不意图以任何方式进行限制。除上述描述的示意性的方面、实施方式和特征之外,通过参考附图和以下的详细描述,本发明进一步的方面、实施方式和特征将会是容易明白的。
附图说明
在附图中,除非另外规定,否则贯穿多个附图相同的附图标记表示相同或相似的部件或元素。这些附图不一定是按照比例绘制的。应该理解,这些附图仅描绘了根据本发明公开的一些实施方式,而不应将其视为是对本发明范围的限制。
图1为本发明一实施例的构造优化方程的流程图;
图2为本发明一实施例的VI-SLAM的优化方法的流程图;
图3为本发明一实施例的预条件子的示意图;
图4为本发明另一实施例的构造优化方程的模块框图;
图5为本发明另一实施例的VI-SLAM的优化装置的连接框图;
图6为本发明另一实施例的VI-SLAM的优化设备框图。
具体实施方式
在下文中,仅简单地描述了某些示例性实施例。正如本领域技术人员可认识到的那样,在不脱离本发明的精神或范围的情况下,可通过各种不同方式修改所描述的实施例。因此,附图和描述被认为本质上是示例性的而非限制性的。本发明实施例主要提供了一种VI-SLAM的优化的方法及装置,下面分别通过以下实施例进行技术方案的展开描述。
本发明提供了一种VI-SLAM优化方法,在BA(bundle adjustment,集束调整)算法的基础上结合iSAM(Incremental Smoothing and Mapping,增量式平滑和制图)进行优化。
如图1所示,在该VI-SLAM优化方法的一种示例中,可以先利用初始的采集的观测量建立优化方程,其中每一组观测量包括一组相机参数和一个三维点坐标,具体包括以下步骤:
S110:根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标;
其中,如果m组相机参数为C={C1,C2,···,Cm},n个三维点坐标为X={X1,X2,···,Xn},则所述优化方程为:
其中,fij为观测方程,每个观测方程fij关联一组相机参数Ci和一个三维点Xj。
S130:根据线性化后的观测方程,构造法向方程:
在一种可能的实现方式中,Wij当且仅当Xj在Ci中可见时不为0。
S140:根据法向方程构造舒尔补(Schur Complement)方程:
求解舒尔补方程,得到C1…Cm。将C1…Cm代入法向方程,可以求解X1…Xn。
具体请参阅图2,其为本发明实施例的VI-SLAM的优化方法的流程图。本发明提供了一种VI-SLAM的优化方法,包括:
S210:如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化。
在本实施例的一种实施方式中,可以记录下每次迭代每个观测方程对法向方程的贡献,例如法向方程公式中的和当且仅当变量发生明显变化,或关联于新的观测方程,才更新这些量,并相应更新法向方程。例如,在步骤S210中如果当相机参数差异|Ci|大于设定阈值、三维点坐标差异|Xj|大于设定阈值或接收到最新一帧的观测量作为增量时,重新线性化观测方程。
S220:根据重新线性化后的观测方程,对法向方程进行增量式更新。
S230:根据更新后的法向方程,对舒尔补方程进行增量式更新。
S240:采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解,获得观测量的最优解。
由于舒尔补方程有着特殊的稀疏结构,因此对于VI-SLAM(Visual-InertialSLAM)来说,当且仅当满足如下条件时不为0:存在三维点同时可见于相机i1、i2,或相邻的i1、i2,其中i2=i1+1。因为相邻相机存在IMU(Inertial measurement unit,惯性测量单元)运动约束。因此,在本实施例中采用共轭梯度法(Conjugate Gradient,简称CG)可以显式利用这一特殊的稀疏结构加速舒尔补方程的求解。CG是一种迭代的求解方法,其收敛效率取决于:a)S的条件数;b)C1…Cm的初值。
如图3所示,对于条件a),在本实施例的一种实施方式中,步骤S140中所采用的预条件子为对角线带状子矩阵B,如图3中的X部分。其中,所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同,对角线带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等,其余部分为0,b为正整数。在附图5中,b=3。选用这样的预条件子,如果对角线带之外的相机之间并无关联,则一次收敛。即使对角线带之外的相机有关联,通常也非常稀疏,因此这样的预条件子也能极大加快收敛。
另外,对于条件b),在本实施例的一种实施方式中,在步骤S140中若更新后的舒尔补方程的上一次迭代结果|Ci|大于设定阈值,则本次迭代初值Ci=0,否则用上一次迭代结果Ci作为本次迭代初值。这样设置的原因是相邻迭代之间,对于大多数相机来说,其所承载的信息非常相似。
如图4所示,本发明提供了一种VI-SLAM的优化装置,包括:
重新线性化模块210,用于如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化。
所述重新线性化模块210具体用于当相机参数差异|Ci|大于设定阈值、三维点坐标差异|Xj|大于设定阈值或接收到最新一帧的观测量作为增量时,重新线性化观测方程。
法向方程更新模块220,用于根据重新线性化后的观测方程,对法向方程进行增量式更新。
所述法向方程更新模块具体用于根据重新线性化的观测方程,更新法向方程中的和并采用更新后的和更新法向方程。所述法向方程更新模块在更新法向方程的过程中,如果重新线性化的观测方程为则采用eij +、和更新法向方程中的和
舒尔补方程更新模块230,用于根据更新后的法向方程,对舒尔补方程进行增量式更新。
求解模块240,用于采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解,获得观测量的最优解。
其中,所述求解模块240在求解过程中,所采用的预条件子为对角线带状子矩阵B,其中,所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同,对角线带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等,其余部分为0,b为正整数。
所述求解模块240在求解过程中,若更新后的舒尔补方程的上一次迭代结果|Ci|大于设定阈值,则本次迭代初值Ci=0,否则用上一次迭代结果Ci作为本次迭代初值。
在一种可能的实现方式中,如图5所示,该VI-SLAM的优化装置还可以包括:
优化方程构造模块110,用于根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标。
其中,如果m组相机参数为C={C1,C2,···,Cm},n个三维点坐标为X={X1,X2,···,Xn},则所述优化方程为:
其中,fij为观测方程,每个观测方程fij关联一组相机参数Ci和一个三维点Xj;
法向方程构造模块130,用于根据线性化后的观测方程,构造法向方程:
舒尔补方程构造模块140,用于根据法向方程构造舒尔补方程:
本实施例装置的各个模块的功能与上述实施例的VI-SLAM的优化方法的原理类似,故不再赘述。
本发明实施例提供一种VI-SLAM的优化设备,如图6所示,该设备包括:存储器310和处理器320,存储器310内存储有可在处理器320上运行的计算机程序。所述处理器320执行所述计算机程序时实现上述实施例中的VI-SLAM的优化方法。所述存储器310和处理器320的数量可以为一个或多个。
该设备还包括:
通信接口330,用于与外界设备进行通信,进行数据交互传输。
存储器310可能包含高速RAM存储器,也可能还包括非易失性存储器(non-volatile memory),例如至少一个磁盘存储器。
如果存储器310、处理器320和通信接口330独立实现,则存储器310、处理器320和通信接口330可以通过总线相互连接并完成相互间的通信。所述总线可以是工业标准体系结构(ISA,Industry Standard Architecture)总线、外部设备互连(PCI,PeripheralComponent)总线或扩展工业标准体系结构(EISA,Extended Industry StandardComponent)总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示,图6中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
可选的,在具体实现上,如果存储器310、处理器320及通信接口330集成在一块芯片上,则存储器310、处理器320及通信接口330可以通过内部接口完成相互间的通信。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。而且,描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或隐含地包括至少一个该特征。在本发明的描述中,“多个”的含义是两个或两个以上,除非另有明确具体的限定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本发明的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本发明的实施例所属技术领域的技术人员所理解。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤,例如,可以被认为是用于实现逻辑功能的可执行指令的定序列表,可以具体实现在任何计算机可读介质中,以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用,或结合这些指令执行系统、装置或设备而使用。就本说明书而言,“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
本发明实施例所述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质的更具体的示例至少(非穷尽性列表)包括以下:具有一个或多个布线的电连接部(电子装置),便携式计算机盘盒(磁装置),随机存取存储器(RAM),只读存储器(ROM),可擦除可编辑只读存储器(EPROM或闪速存储器),光纤装置,以及便携式只读存储器(CDROM)。另外,计算机可读存储介质甚至可以是可在其上打印所述程序的纸或其他合适的介质,因为可以例如通过对纸或其他介质进行光学扫描,接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序,然后将其存储在计算机存储器中。
在本发明实施例中,计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质,该计算机可读介质可以发送、传播或者传输用于指令执行系统、输入法或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输,包括但不限于:无线、电线、光缆、射频(Radio Frequency,RF)等等,或者上述的任意合适的组合。
应当理解,本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如,如果用硬件来实现,和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,该程序在执行时,包括方法实施例的步骤之一或其组合。
此外,在本发明各个实施例中的各功能单元可以集成在一个处理模块中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现,也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时,也可以存储在一个计算机可读存储介质中。所述存储介质可以是只读存储器,磁盘或光盘等。
综上所述,本发明实施例通过利用VI-SLAM问题特殊的稀疏结构提升优化效率,以及通过利用SLAM问题的增量特性提升优化效率。同时,本发明优化效率的提升不依赖于运动方式,既适用于持续扩展地图的运动方式,又适用于在已扩展的地图中来回运动。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到其各种变化或替换,这些都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应以所述权利要求的保护范围为准。
Claims (16)
1.一种VI-SLAM的优化方法,其特征在于,包括:
如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化;
根据重新线性化后的观测方程,对法向方程进行增量式更新;
根据更新后的法向方程,对舒尔补方程进行增量式更新;
采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解,获得观测量的最优解;
一组观测量包括一组相机参数和一个三维点坐标,所述方法还包括:
根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标;
所述如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化的步骤,包括:
当相机参数差异|C i|大于设定阈值、三维点坐标差异|X j|大于设定阈值或接收到最新一帧的观测量作为增量时,重新线性化观测方程。
2.根据权利要求1所述的方法,其特征在于,采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解时,所采用的预条件子为对角线带状子矩阵B,其中,所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同,对角线带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等,其余部分为0,b为正整数。
3.根据权利要求2所述的方法,其特征在于,采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解时,若更新后的舒尔补方程的上一次迭代结果Ci的绝对值大于设定阈值,则本次迭代初值Ci=0,否则用上一次迭代结果Ci作为本次迭代初值。
4.根据权利要求1所述的方法,其特征在于,所述根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标中,如果m组相机参数为C={C1,C2,···,Cm},n个三维点坐标为X={X1,X2,···,Xn},则所述优化方程为:
其中,fij为观测方程,每个观测方程fij关联一组相机参数Ci和一个三维点Xj;
所述方法还包括:
根据线性化后的观测方程,构造法向方程:
根据法向方程构造舒尔补方程:
8.一种VI-SLAM的优化装置,其特征在于,包括:
重新线性化模块,用于如果发生变化的观测量满足更新条件,则根据发生变化的观测量对集束调整的优化方程中的观测方程进行重新线性化;
法向方程更新模块,用于根据重新线性化后的观测方程,对法向方程进行增量式更新;
舒尔补方程更新模块,用于根据更新后的法向方程,对舒尔补方程进行增量式更新;
求解模块,用于采用预条件共轭梯度法对更新后的舒尔补方程进行增量式求解,获得观测量的最优解;
一组观测量包括一组相机参数和一个三维点坐标,所述装置还包括:
优化方程构造模块,用于根据多组观测量构造集束调整的优化方程,所述优化方程中的每个观测方程关联一组相机参数和一个三维点坐标;
所述重新线性化模块具体用于当相机参数差异|Ci|大于设定阈值、三维点坐标差异|Xj|大于设定阈值或接收到最新一帧的观测量作为增量时,重新线性化观测方程。
9.根据权利要求8所述的装置,其特征在于,所述求解模块在求解过程中,所采用的预条件子为对角线带状子矩阵B,其中,所述对角线带状子矩阵B与更新后的舒尔补方程的原矩阵S维度相同,对角线带状子矩阵B的对角线处宽度为b的部分与原矩阵S相等,其余部分为0,b为正整数。
10.根据权利要求9所述的装置,其特征在于,所述求解模块在求解过程中,若更新后的舒尔补方程的上一次迭代结果Ci的绝对值大于设定阈值,则本次迭代初值Ci=0,否则用上一次迭代结果Ci作为本次迭代初值。
15.一种用于VI-SLAM的优化设备,其特征在于,所述设备包括:
一个或多个处理器;
存储装置,用于存储一个或多个程序;
当所述一个或多个程序被所述一个或多个处理器执行时,使得所述一个或多个处理器实现如权利要求1-7中任一所述的VI-SLAM的优化方法。
16.一种计算机可读介质,其存储有计算机程序,其特征在于,该程序被处理器执行时实现如权利要求1-7中任一所述的VI-SLAM的优化方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810390462.7A CN108572939B (zh) | 2018-04-27 | 2018-04-27 | Vi-slam的优化方法、装置、设备及计算机可读介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810390462.7A CN108572939B (zh) | 2018-04-27 | 2018-04-27 | Vi-slam的优化方法、装置、设备及计算机可读介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108572939A CN108572939A (zh) | 2018-09-25 |
CN108572939B true CN108572939B (zh) | 2020-05-08 |
Family
ID=63574354
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810390462.7A Active CN108572939B (zh) | 2018-04-27 | 2018-04-27 | Vi-slam的优化方法、装置、设备及计算机可读介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108572939B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111931523A (zh) * | 2020-04-26 | 2020-11-13 | 永康龙飘传感科技有限公司 | 在新闻播报实时翻译文字和手语的方法和系统 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105989586A (zh) * | 2015-03-04 | 2016-10-05 | 北京雷动云合智能技术有限公司 | 一种基于语义光束平差法的slam方法 |
WO2017099097A1 (en) * | 2015-12-08 | 2017-06-15 | Mitsubishi Electric Corporation | Method and system for detecting and localizing object and slam method |
-
2018
- 2018-04-27 CN CN201810390462.7A patent/CN108572939B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105989586A (zh) * | 2015-03-04 | 2016-10-05 | 北京雷动云合智能技术有限公司 | 一种基于语义光束平差法的slam方法 |
WO2017099097A1 (en) * | 2015-12-08 | 2017-06-15 | Mitsubishi Electric Corporation | Method and system for detecting and localizing object and slam method |
Non-Patent Citations (4)
Title |
---|
Bundle Adjustment in the Large;Sameer Agarwal et al.;《11th European Conference on Computer Vision》;20100905;第29-42页 * |
Pushing the Envelope of Modern Methods for Bundle Adjustment;Yekeun Jeong et al.;《IEEE Transactions on Pattern Analysis and Machine Intelligence》;20120831;第34卷(第8期);第1-8页 * |
Robust Keyframe-based Dense SLAM with an RGB-D Camera;Haomin Liu et al.;《arXiv:1711.05166v1》;20171114;第1-7页 * |
面向大尺度场景的单目同时定位与地图构建;刘浩敏 等;《中国科学:信息科学》;20161231;第46卷(第12期);第1748-1761页 * |
Also Published As
Publication number | Publication date |
---|---|
CN108572939A (zh) | 2018-09-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107610146B (zh) | 图像场景分割方法、装置、电子设备及计算机存储介质 | |
JP7300550B2 (ja) | 視覚標識に基づき標識マップを構築する方法、装置 | |
US11300964B2 (en) | Method and system for updating occupancy map for a robotic system | |
CN108332758B (zh) | 一种移动机器人的走廊识别方法及装置 | |
CN108896047B (zh) | 分布式传感器网络协同融合与传感器位置修正方法 | |
CN107679489B (zh) | 基于场景分割的自动驾驶处理方法、装置及计算设备 | |
CN110887493B (zh) | 基于局部地图匹配的轨迹推算方法、介质、终端和装置 | |
CN105004337B (zh) | 基于直线匹配的农用无人机自主导航方法 | |
CN108592919B (zh) | 制图与定位方法、装置、存储介质和终端设备 | |
WO2021115061A1 (zh) | 图像分割方法、装置及服务器 | |
CN114926549B (zh) | 三维点云处理方法、装置、设备以及存储介质 | |
CN114863385B (zh) | 道路曲面信息生成方法、装置、设备和计算机可读介质 | |
CN108572939B (zh) | Vi-slam的优化方法、装置、设备及计算机可读介质 | |
CN112629565B (zh) | 像机与惯性测量单元旋转关系校准方法、装置和设备 | |
CN111368860A (zh) | 重定位方法及终端设备 | |
CN110824496B (zh) | 运动估计方法、装置、计算机设备和存储介质 | |
CN112097772A (zh) | 机器人及其地图构建方法和装置 | |
CN116929343A (zh) | 位姿估计方法、相关设备及存储介质 | |
CN110853098A (zh) | 机器人定位方法、装置、设备及存储介质 | |
CN112669196B (zh) | 一种硬件加速引擎中因子图优化数据的方法和设备 | |
CN114549945A (zh) | 遥感影像变化检测方法及相关装置 | |
CN112729349A (zh) | 里程计在线标定的方法、装置、电子设备及存储介质 | |
CN117475399B (zh) | 车道线拟合方法、电子设备及可读介质 | |
CN114851185B (zh) | 机械臂摆位优化方法、装置、计算机设备和存储介质 | |
CN116299300B (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 |