CN112114966A - 一种视觉slam的光束平差计算方法 - Google Patents

一种视觉slam的光束平差计算方法 Download PDF

Info

Publication number
CN112114966A
CN112114966A CN202010966181.9A CN202010966181A CN112114966A CN 112114966 A CN112114966 A CN 112114966A CN 202010966181 A CN202010966181 A CN 202010966181A CN 112114966 A CN112114966 A CN 112114966A
Authority
CN
China
Prior art keywords
optimization
sub
graph
pose
error
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
CN202010966181.9A
Other languages
English (en)
Other versions
CN112114966B (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.)
Advanced Institute of Information Technology AIIT of Peking University
Hangzhou Weiming Information Technology Co Ltd
Original Assignee
Advanced Institute of Information Technology AIIT of Peking University
Hangzhou Weiming Information 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 Advanced Institute of Information Technology AIIT of Peking University, Hangzhou Weiming Information Technology Co Ltd filed Critical Advanced Institute of Information Technology AIIT of Peking University
Priority to CN202010966181.9A priority Critical patent/CN112114966B/zh
Publication of CN112114966A publication Critical patent/CN112114966A/zh
Application granted granted Critical
Publication of CN112114966B publication Critical patent/CN112114966B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F9/00—Arrangements for program control, e.g. control units
    • G06F9/06—Arrangements for program control, e.g. control units using stored programs, i.e. using an internal store of processing equipment to receive or retain programs
    • G06F9/46—Multiprogramming arrangements
    • G06F9/50—Allocation of resources, e.g. of the central processing unit [CPU]
    • G06F9/5005—Allocation of resources, e.g. of the central processing unit [CPU] to service a request
    • G06F9/5027—Allocation of resources, e.g. of the central processing unit [CPU] to service a request the resource being a machine, e.g. CPUs, Servers, Terminals
    • G06F9/5038—Allocation of resources, e.g. of the central processing unit [CPU] to service a request the resource being a machine, e.g. CPUs, Servers, Terminals considering the execution order of a plurality of tasks, e.g. taking priority or time dependency constraints into consideration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明公开了一种视觉SLAM的光束平差计算方法，将全局共视图切分成局部子图，进行全局‑局部交替迭代优化，局部子图通过优化过程中的误差反馈，并发执行，异步响应，高效调度计算资源，降低了通信以及额外等待的开销，在不降低系统优化性能的同时，大大提高收敛速度。

Description

一种视觉SLAM的光束平差计算方法
技术领域
本发明属于视觉同步定位与地图构建领域，特别是一种视觉SLAM的光束平差计算方法。
背景技术
视觉同步定位与地图构建(simultaneous localization and mapping,简称视觉SLAM)技术是指机器人仅仅依赖摄像机，从未知环境的未知地点出发，在运动过程中通过重复观测到的地图特征(比如墙角、柱子等)定位自身位置和姿态，再根据自身位置增量式的构建地图的过程。光束平差方法(Bundle Adjustment，简称BA)作为一种优化方法逐渐替代滤波在视觉SLAM系统中的角色，成为不可或缺的核心组成部分。
现有的分布式BA的框架通过交替方向乘子法执行并行优化的同时来保证子图与全局状态的统一，但系统的总体效率不够高。交替方向乘子法是同步算法，整个算法的收敛速度与效果取决与子图的划分是否合理，若子图不均衡，系统的额外开销较大，算法很难收敛或直接失效。另外，在交替方向乘子法的现有算法中，子图的划分算法是固定的且一次性的，系统不能动态分配计算资源，也会影响收敛速度。
发明内容
本发明的要解决的是现有光束平差方法效率不高的技术问题。
为了解决上述问题，本发明提出一种视觉SLAM的光束平差计算方法。
本发明所采用的技术方案是：
一种视觉SLAM的光束平差计算方法，包括以下步骤：
S1，将全局共视图切分成T个子图；
S2，从子图中选择d个位姿节点，集合后形成代理位姿集合，选择能被代理位姿集合中两个或两个以上位姿同时观测到的路标点以及该路标点对应的观测，对选择出来的位姿、路标和观测执行BA优化，得到优化后的位姿和路标点；
S3，将S2优化后的位姿传递到其对应的子图中，随后将子图中的位姿固定，然后执行每个子图的BA优化，并根据当前子图优化返回的误差，判断该子图是否收敛，如果收敛则停止优化，转而优化下一子图，否则继续优化该子图直至其收敛；
S4，当S3中所有子图全部收敛后，根据S3返回的误差判断全局共视图是否收敛，如果收敛，则保存结果，否则，增加d的值后返回S1。
作为优选，所述S1中，切分的方法为平均切分。
作为优选，所述S1中，全局共视图的位姿为ξi∈SE(3)，i＝1，2…N；路标为Pj∈R3，j＝1，2…M；观测数据为
Figure BDA0002682414730000021
其中
Figure BDA0002682414730000022
Figure BDA0002682414730000023
作为优选，所述S2中，BA优化时，计算本次优化之后的误差，如果本次优化返回的误差值大于上次或者两次误差值小于设定值m，则停止优化；否则继续优化。
作为优选，所述S3中，使用任务队列分配计算资源，已收敛的子图不再放入队列，未收敛的子图继续加入队列；队列内的任务并行执行，直到队列为空。
作为优选，所述S3中，如果本次优化返回的误差值比上次返回的误差值小于设定值n，则判断为收敛；否则不收敛。
作为优选，所述S3中，采用异步并行的方式执行每个子图的BA优化。
作为优选，所述S4中，如果不收敛，则将d的值增加一倍。
作为优选，所述m＝0.1，n＝0.1。
本发明同现有技术相比具有以下优点及效果：
本发明将全局共视图切分成局部子图，进行全局-局部交替迭代优化，局部子图通过优化过程中的误差反馈，并发执行，异步响应，高效调度计算资源，降低了通信以及额外等待的开销，在不降低系统优化性能的同时，大大提高收敛速度。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
图1为本发明的框架组织图；
图2为全图骨架结构的采样和子图的生成示意图；
图3为本发明的结果与其他算法结果的对比。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
实施例1：
一种视觉SLAM的光束平差计算方法，如图1所示，本系统中一共有三种任务序列：服务节点、工作节点和调度节点。其中服务节点负责维护全局地图中相机位姿和路标点的状态，它异步响应来自工作节点的请求。工作节点是实际的计算节点，其接受来自调度节点分发的任务，迭代执行“拉数据—计算—推数据”的过程，并会定时向调度节点报告子图的优化进度。调度节点负责整个系统的控制与调度，它首先将BA问题中顶点状态传递给服务节点存储，然后会不断从全局共视图中采样骨架结构、生成子图并向工作节点分发任务达到并行优化的目的，最后从服务节点中获取相机位姿和路标点的稳定状态作为优化结果。
在步骤开始前的准备工作为：启动一个服务节点，一个调度节点，根据集群内核心的数量，启动W个工作节点(W为正整数，例如W＝12)。调度节点从本地磁盘或者网络读入要优化的全局共视图，其包括N个相机位姿状态，M个路标点状态，S个观测，N、M和S均为正整数。调度节点将相机位姿和路标点坐标发送到服务节点，服务节点提供异步请求服务。地图大小为：D＝√N。
步骤1：计算需要生成的子图个数
Figure BDA0002682414730000041
步骤2.1：对N个位姿中每个位姿的平移部分(即为每个相机在空间中的坐标)执行聚类算法，例如K-Means算法、K-MEDOIDS算法、CLARANS算法等，设置簇的个数为T，聚类算法收敛后得到T个位姿集合。在每个集合中按样本空间分布均匀采样d个位姿，例如采用八叉树、聚类等算法均匀采样，作为代理位姿，将得到的代理位姿进行集合。
步骤2.2：选择能被代理位姿集合中两个或两个以上位姿同时观测到的路标点以及该路标点对应的观测，与代理位姿集合一起构成一个二部图，作为全局共视图的骨架结构。对于T个代理位姿集合中的每一个集合，采取上述相同的方法，产生T个骨架结构。如图2所示，其中深色的顶点构成了骨架结构(其中圆形为骨架位姿，方形为骨架路标)，全局共视图被切分为4(T＝4)个局部子图，每个局部子图有2(d＝2)个位姿，全局共视图和局部子图之间靠位姿建立起联系，维持状态的一致性。
步骤2.3，调度节点将骨架结构的拓扑关系以及观测信息(边)发送给任意一个工作节点，工作节点接受并解析来自调度节点的参数。首先从服务节点拉取本次迭代所需要的数据，服务节点异步的接收来自工作节点的请求，如果收到指令是“推数据”,即工作节点向服务节点更新数据，那么服务节点立即更新当前状态，更新策略为滑动平均(历史权重为0-1之间的可变参数)；如果指令是“拉数据”，即工作节点从服务节点获取数据，那么服务节点从数据库中选择被请求的位姿或者路标的状态，立即返回给工作节点。然后执行k轮优化(k为正整数)，将优化后的位姿与路标状态推送给服务节点。计算本次k轮优化之后的误差χ2，返回给调度节点，调度节点根据工作节点返回的误差大小确定是否继续优化，若本次优化返回的误差值大于上次，或者两次误差值小于一个预先设定的值m，本实施例中m＝0.1，则判断全局骨架结构收敛，停止优化。此步骤中工作节点与参数服务器之间的异步请求与应答大大减小了系统的额外开销。
步骤3，调度节点的队列初始状态下有T个子图优化任务，任意一个时刻，只要队列不为空且工作节点有空闲，则从队列中弹出一个任务分配给空闲的工作节点并发执行。工作节点先从服务节点拉取本次迭代所需要的数据，服务节点异步的接收来自工作节点的请求，如果收到指令是“推数据”，那么服务节点立即更新当前状态，更新策略为滑动平均(历史权重为0-1之间的可变参数)；如果指令是“拉数据”，那么服务节点从数据库中选择被请求的位姿或者路标的状态，立即返回给工作节点。固定工作节点中的代理位姿，然后执行k轮优化(k为正整数)，将优化后的位姿与路标状态推送给服务节点。计算本次k轮优化之后的误差χ2，返回给调度节点。计算本次k轮优化之后的误差χ2，返回给调度节点。调度节点通过判断当前子图的误差变化，确定是否收敛，若本次优化返回的误差值比上次返回的误差值小于预先设定的值n，本实施例中n＝0.1，那么当前子图的优化已经收敛，舍弃任务，否则，再次将任务进入队列。直至任务队列为空，此时意味着所有子图已经收敛。此步骤用最小的成本维持全局共视图和局部子图之间的状态一致性，把预优化的节点状态传递到子图中，避免了交替方向乘子算法较高的通信和同步开销；另外异步监听每个子图的优化进度，通过自适应的策略，我们的系统能够动态地把有限的计算资源分配到那些局部状态更加不稳定的区域，同样可以大大加快系统的收敛速度。
步骤4.1，所有子图收敛后，调度节点统计并对任务返回的误差求和，若全局误差小于某个阈值
Figure BDA0002682414730000051
则认为整个系统收敛，调度节点从参数服务器拉取全部节点的状态，保存结果，系统退出。若系统未收敛，则将子图大小以及代理位姿数目增加后转向步骤1，优选为将数目翻倍，即t←t*2；D←D*2。此步骤导致子图的规模逐渐变大，一定程度上实现了系统优化过程从关注局部信息到关注全局信息的平滑过渡；另一方面，因为在子图规模变大的过程中，系统中的状态逐渐稳定，大的子图的迭代时间会响应减小，因此不会增加系统的额外的计算开销.反而会让系统的收敛更加稳定，降低陷入局部极小值的概率。
经过验证，例如在KITTI Odometry数据集上进行评测，系统的处理器核心数量为6，Intel(R)Core(TM)i7-8700K CPU@3.70GHz。如图3所示，对比g2o的BA优化以及本发明的BA优化性能，算法输入为ORB-SLAM2程序得到的未执行全局优化的中间结果，并且其中的每个状态和观测被加入了不同程度的高斯噪声，来模仿真实世界中的数据，在几乎相近收敛效果的情况下，本发明的方法能够大大提高算法收敛的速度。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种视觉SLAM的光束平差计算方法，其特征在于，包括以下步骤：
S1，将全局共视图切分成T个子图；
S2，从子图中选择d个位姿节点，集合后形成代理位姿集合，选择能被代理位姿集合中两个或两个以上位姿同时观测到的路标点以及该路标点对应的观测，对选择出来的位姿、路标和观测执行BA优化，得到优化后的位姿和路标点；
S3，将S2优化后的位姿传递到其对应的子图中，随后将子图中的位姿固定，然后执行每个子图的BA优化，并根据当前子图优化返回的误差，判断该子图是否收敛，如果收敛则停止优化，转而优化下一子图，否则继续优化该子图直至其收敛；
S4，当S3中所有子图全部收敛后，根据S3返回的误差判断全局共视图是否收敛，如果收敛，则保存结果，否则，增加d的值后返回S1。
2.根据权利要求1所述的视觉SLAM的光束平差计算方法，其特征在于，所述S1中，切分的方法为平均切分。
3.根据权利要求1所述的视觉SLAM的光束平差计算方法，其特征在于，所述S1中，全局共视图的位姿为ξi∈SE(3)，i＝4，2…N；路标为Pj∈R3，j＝4，2…M；观测数据为
Figure FDA0002682414720000011
其中
Figure FDA0002682414720000012
4.根据权利要求1所述的视觉SLAM的光束平差计算方法，其特征在于，所述S2中，BA优化时，计算本次优化之后的误差，如果本次优化返回的误差值大于上次或者两次误差值小于设定值m，则停止优化；否则继续优化。
5.根据权利要求1所述的视觉SLAM的光束平差计算方法，其特征在于，所述S3中，使用任务队列分配计算资源，已收敛的子图不再放入队列，未收敛的子图继续加入队列；队列内的任务并行执行，直到队列为空。
6.根据权利要求1所述的视觉SLAM的光束平差计算方法，其特征在于，所述S3中，如果本次优化返回的误差值比上次返回的误差值小于设定值n，则判断为收敛；否则不收敛。
7.根据权利要求1所述的视觉SLAM的光束平差计算方法，其特征在于，所述S3中，采用异步并行的方式执行每个子图的BA优化。
8.根据权利要求1所述的视觉SLAM的光束平差计算方法，其特征在于，所述S4中，如果不收敛，则将d的值增加一倍。
9.根据权利要求4所述的视觉SLAM的光束平差计算方法，其特征在于，所述m＝0.1。
10.根据权利要求6所述的视觉SLAM的光束平差计算方法，其特征在于，所述n＝0.1。
CN202010966181.9A 2020-09-15 2020-09-15 一种视觉slam的光束平差计算方法 Active CN112114966B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010966181.9A CN112114966B (zh) 2020-09-15 2020-09-15 一种视觉slam的光束平差计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010966181.9A CN112114966B (zh) 2020-09-15 2020-09-15 一种视觉slam的光束平差计算方法

Publications (2)

Publication Number Publication Date
CN112114966A true CN112114966A (zh) 2020-12-22
CN112114966B CN112114966B (zh) 2024-08-13

Family

ID=73802735

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010966181.9A Active CN112114966B (zh) 2020-09-15 2020-09-15 一种视觉slam的光束平差计算方法

Country Status (1)

Country Link
CN (1) CN112114966B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110060277A (zh) * 2019-04-30 2019-07-26 哈尔滨理工大学 一种多特征融合的视觉slam方法
CN110501017A (zh) * 2019-08-12 2019-11-26 华南理工大学 一种基于orb_slam2的移动机器人导航地图生成方法
CN110675307A (zh) * 2019-08-19 2020-01-10 杭州电子科技大学 基于vslam的3d稀疏点云到2d栅格图的实现方法
WO2020048618A1 (en) * 2018-09-07 2020-03-12 Huawei Technologies Co., Ltd. Device and method for performing simultaneous localization and mapping
WO2020155615A1 (zh) * 2019-01-28 2020-08-06 速感科技（北京）有限公司 Vslam方法、控制器和可移动设备

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020048618A1 (en) * 2018-09-07 2020-03-12 Huawei Technologies Co., Ltd. Device and method for performing simultaneous localization and mapping
WO2020155615A1 (zh) * 2019-01-28 2020-08-06 速感科技（北京）有限公司 Vslam方法、控制器和可移动设备
CN110060277A (zh) * 2019-04-30 2019-07-26 哈尔滨理工大学 一种多特征融合的视觉slam方法
CN110501017A (zh) * 2019-08-12 2019-11-26 华南理工大学 一种基于orb_slam2的移动机器人导航地图生成方法
CN110675307A (zh) * 2019-08-19 2020-01-10 杭州电子科技大学 基于vslam的3d稀疏点云到2d栅格图的实现方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
HAUKE STRASDAT: "Local Accuracy and Global Consistency for Efficient Visual SLAM", IMPERIAL COLLEGE LONDON, 30 November 2012 (2012-11-30) *
RAÚL MUR-ARTAL等: "ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras", IEEE TRANSACTIONS ON ROBOTICS, vol. 33, no. 5, 12 June 2017 (2017-06-12), pages 1255, XP055597959, DOI: 10.1109/TRO.2017.2705103 *
姚二亮;张合新;张国良;徐慧;赵欣;: "基于Vision-IMU的机器人同时定位与地图创建算法", 仪器仪表学报, no. 04, 15 April 2018 (2018-04-15), pages 233 - 241 *
孔德慧;李文超;虞义兰;李敬华;尹宝才: "一种改进的面向SLAM系统的相机位姿估计方法", 华南理工大学学报(自然科学版), no. 012, 31 December 2018 (2018-12-31), pages 10 - 19 *

Also Published As

Publication number Publication date
CN112114966B (zh) 2024-08-13

Similar Documents

Publication Publication Date Title
Han et al. Tailored learning-based scheduling for kubernetes-oriented edge-cloud system
WO2023240845A1 (zh) 一种分布式计算方法、系统、设备及存储介质
Goudarzi et al. A distributed deep reinforcement learning technique for application placement in edge and fog computing environments
Liu et al. A hierarchical framework of cloud resource allocation and power management using deep reinforcement learning
JP5584914B2 (ja) 分散計算システム
Jayanetti et al. Deep reinforcement learning for energy and time optimized scheduling of precedence-constrained tasks in edge–cloud computing environments
CN110688219B (zh) 基于反向混沌布谷鸟搜索的自适应权重负载均衡算法
Yang et al. An utility-based job scheduling algorithm for cloud computing considering reliability factor
CN105117292B (zh) 随机扩散动态负载均衡方法
Jing et al. Multi-agent reinforcement learning based on graph convolutional network for flexible job shop scheduling
CN112214301A (zh) 面向智慧城市基于用户偏好的动态计算迁移方法及装置
CN115836303A (zh) 异步量子信息处理
Salehnia et al. SDN-based optimal task scheduling method in Fog-IoT network using combination of AO and WOA
CN108304253A (zh) 基于缓存感知和数据本地性的map任务调度方法
CN112114966A (zh) 一种视觉slam的光束平差计算方法
Goudarzi et al. $\mu $-DDRL: A QoS-Aware Distributed Deep Reinforcement Learning Technique for Service Offloading in Fog computing Environments
Shi et al. Drl-based and bsld-aware job scheduling for apache spark cluster in hybrid cloud computing environments
CN112698911B (zh) 一种基于深度强化学习的云作业调度方法
CN115456184A (zh) 量子电路处理方法、量子态制备方法、装置、设备及介质
CN111158898B (zh) 针对输变电工程场地布置标准化的bim数据处理方法及装置
Kontos et al. Cloud-Native Applications' Workload Placement over the Edge-Cloud Continuum.
Shen et al. Collaborative Learning-Based Scheduling for Kubernetes-Oriented Edge-Cloud Network
Keshk et al. Adaptive fault tolerance for online tasks scheduling in cloud computing
Han et al. Dynamic task-scheduling algorithm in CNC system based on cloud computing
Nejatzadeh et al. A new heuristic approach for scheduling independent tasks on grid computing systems

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