CN113091767B - 滤波式后端回环校正方法及其系统和电子设备 - Google Patents
滤波式后端回环校正方法及其系统和电子设备 Download PDFInfo
- Publication number
- CN113091767B CN113091767B CN202010022908.8A CN202010022908A CN113091767B CN 113091767 B CN113091767 B CN 113091767B CN 202010022908 A CN202010022908 A CN 202010022908A CN 113091767 B CN113091767 B CN 113091767B
- Authority
- CN
- China
- Prior art keywords
- key frame
- pose
- loop
- frame
- data
- 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
- 238000012937 correction Methods 0.000 title claims abstract description 121
- 238000000034 method Methods 0.000 title claims abstract description 106
- 238000001914 filtration Methods 0.000 title claims abstract description 96
- 238000005457 optimization Methods 0.000 claims abstract description 33
- 238000012545 processing Methods 0.000 claims abstract description 25
- 239000011159 matrix material Substances 0.000 claims description 20
- 238000001514 detection method Methods 0.000 claims description 18
- 230000005540 biological transmission Effects 0.000 claims description 6
- 238000000605 extraction Methods 0.000 claims description 4
- 230000008901 benefit Effects 0.000 description 19
- 238000004422 calculation algorithm Methods 0.000 description 13
- 238000004891 communication Methods 0.000 description 10
- 238000013519 translation Methods 0.000 description 7
- 238000004590 computer program Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 6
- 230000033001 locomotion Effects 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000008676 import Effects 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 4
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012546 transfer Methods 0.000 description 3
- 230000003190 augmentative effect Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000004807 localization Effects 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
- 239000004065 semiconductor Substances 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 101000822695 Clostridium perfringens (strain 13 / Type A) Small, acid-soluble spore protein C1 Proteins 0.000 description 1
- 101000655262 Clostridium perfringens (strain 13 / Type A) Small, acid-soluble spore protein C2 Proteins 0.000 description 1
- 101000655256 Paraclostridium bifermentans Small, acid-soluble spore protein alpha Proteins 0.000 description 1
- 101000655264 Paraclostridium bifermentans Small, acid-soluble spore protein beta Proteins 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000007177 brain activity Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000005684 electric field Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000000644 propagated effect Effects 0.000 description 1
- 230000006798 recombination Effects 0.000 description 1
- 238000005215 recombination Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000026683 transduction Effects 0.000 description 1
- 238000010361 transduction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- 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
- G06T7/85—Stereo camera calibration
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10004—Still image; Photographic image
- G06T2207/10012—Stereo images
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Image Analysis (AREA)
Abstract
一种滤波式后端回环校正方法及其系统和电子设备。该滤波式后端回环校正方法包括步骤：接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作；基于该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧，如果不存在，则退出该回环线程；如果存在，则将该当前帧作为新关键帧存储至该词袋数据库，以进行下一步操作；求解出该新关键帧和该旧关键帧之间的相对位姿；基于该相对位姿，通过非线性优化方法对该词袋数据库中存储的所有关键帧进行位姿图优化处理，以得到该关键帧的校正位姿数据；以及将该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的回环校正。
Description
技术领域
本发明涉及SLAM技术领域，尤其是涉及滤波式后端回环校正方法及其系统和电子设备。
背景技术
SLAM(Simultaneous Localization and Mapping；同时定位与地图构建)指的是在没有环境先验数据的情况下，在运动过程中建立环境的模型，同时估计自己的运动。在增强现实等应用场景中，SLAM产品往往采用多传感器融合的方式来提高定位的精度。但由于商业化的SLAM产品对成本的要求，因此SLAM产品往往无法使用高精度的传感器，这就导致随着算法运行时间的增加，传感器引入的测量误差将累积而开始明显影响定位的准确度。
目前，回环校正方法是一种常用的消除累积误差的方法，其主要思想是保存算法运行过程中场景关键帧的视觉数据，并将这些数据根据预先训练好的词典导入词袋数据库中。当新的关键帧生成时，在词袋数据库中寻找场景相似的旧关键并通过预先导入的路标点坐标和图像点坐标得到新的位姿约束，之后将该位姿约束添加至后端以减少进一步的累积误差。现有的回环校正技术方案基本都是用于后端优化的SLAM算法中，而对于后端滤波的SLAM算法则存在一定的局限性，其通常在后端的滑动窗口中记录一系列的关键帧来构建词袋数据库，当出现新的图像帧时，在数据库中寻找与其相似的旧帧，以提取两帧的图像特征点匹配关系并将其用于后续雅克比矩阵来计算新位姿估计，进而通过新旧帧之间的特征点约束来校正当前已经偏离的位姿。
然而，虽然上述回环校正技术方案能够实现回环校正的目的，但仍然存在着一些缺点。首先，后端滤波的SLAM算法中为了保证较块的运行速度，需要逐步删除后端活动窗口内的观测帧，但上述回环校正技术方案则需要在滑动窗口中添加不可删除的关键帧，而随着关键帧数量的增多，该后端滤波的SLAM算法中协方差矩阵的维度将会变大，使得运行速度大幅地降低，进而对实时性产生影响。其次，随着运行时间的增加，在该后端滤波的SLAM算法中计算得到的路标点坐标值也会产生偏差，从而影响该后端滤波的SLAM算法的回环校正效果，导致该后端滤波的SLAM算法的定位精度下降。
发明内容
本发明的一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其能够解决基于后端滤波的SLAM系统在长时间运动后会产生无法忽略的累积的误差的问题。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中，在本发明的一实施例中，所述滤波式后端回环校正方法能够使所述基于后端滤波的SLAM系统在长时间运行后仍然尽可能保持原有精度。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中，在本发明的一实施例中，所述滤波式后端回环校正方法能够在保证较高运行速度的同时，减少累积误差，也就是说，所述滤波式后端回环校正方法能够在确保实时性的同时，尽可能保持原有的定位精度。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中，在本发明的一实施例中，所述滤波式后端回环校正方法能够在独立创建的回环线程中执行，并将关键帧数据传入新线程的数据库，以避免了在后端滑动窗口中添加关键帧带来的时间开销。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中，在本发明的一实施例中，所述滤波式后端回环校正方法在所述回环线程中使用了基于位姿图优化方法对当前各关键帧的位姿进行精确计算，避免了滤波方案中雅克比矩阵随着关键帧增加而增大所带来的额外计算。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中，在本发明的一实施例中，所述滤波式后端回环校正方法能够通过前端跟踪特征点而不再重新提取新的特征点进行词袋匹配得到相应旧帧数据，有助于减少回环检测的时间。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中，在本发明的一实施例中，所述滤波式后端回环校正方法能够使用两帧之间精确的相对位姿约束代替描述子匹配得到的点约束，减少了约束量。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中，在本发明的一实施例中，所述滤波式后端回环校正方法能够采用左右相机匹配点的三角化重新计算路标点的相对坐标值，避免了使用路标点的绝对值带来的累计误差影响。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中，在本发明的一实施例中，所述滤波式后端回环校正方法能够利用优化前后关键帧位姿的相对偏移量校正当前窗内各帧位姿，较为简便的完成了回环约束传递。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中，在本发明的一实施例中，所述滤波式后端回环校正方法的实施简单，试用性强，在提高长时间定位精度上具有重要意义，对基于后端滤波的SLAM算法均具有很大的应用价值，有望使用在增强现实、定位跟踪等领域。
本发明的另一优势在于提供一滤波式后端回环校正方法及其系统和电子设备，其中为了达到上述优势，在本发明中不需要采用复杂的结构和庞大的计算量，对软硬件要求低。因此，本发明成功和有效地提供一解决方案，不只提供一滤波式后端回环校正方法及其系统和电子设备，同时还增加了所述滤波式后端回环校正方法及其系统和电子设备的实用性和可靠性。
为了实现上述至少一优势或其他优势和目的，本发明提供了滤波式后端回环校正方法，包括步骤：
接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作；
基于该当前帧的该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧，如果不存在，则退出该回环线程；如果存在，则将该当前帧作为新关键帧存储至该词袋数据库，以进行下一步操作；
基于该当前帧的该后端数据和该旧关键帧的数据，求解出该新关键帧和该旧关键帧之间的相对位姿；
基于该新关键帧和该旧关键帧之间的该相对位姿，通过非线性优化方法对该词袋数据库中存储的所有关键帧进行位姿图优化处理，以得到该关键帧的校正位姿数据；以及
将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的回环校正。
在本发明的一实施例中，所述基于该当前帧的该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧的步骤，包括步骤：
根据该当前帧的该后端数据，判断该当前帧相对于该词袋数据库中上一关键帧的跟踪点数是否小于预定数量阈值或平均视差是否大于预定视差阈值；
如果是，则将该当前帧存储至该词袋数据库以作为该词袋数据库的该新关键帧，并从该词袋数据库中寻找与该新关键帧间隔预定距离且大于预定相似度的关键帧；以及
如果否，则退出此次回环检测。
在本发明的一实施例中，所述基于该当前帧的该后端数据和该旧关键帧的数据，求解出该新关键帧和该旧关键帧之间的相对位姿的步骤，包括步骤：
基于该当前帧的该后端数据中的左右相机归一化坐标和左右相机之间的相对位姿，求解路标点的空间坐标；和
通过PnP方法，基于该路标点的空间坐标，获得该新关键帧和该旧关键帧之间的该相对位姿。
在本发明的一实施例中，所述将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的校正的步骤，包括步骤：
从该词袋数据库中提取与该滤波式后端的滑动窗口中的观测帧相对应的关键帧，以作为对应关键帧，其中该对应关键帧与相应的该观测帧之间的时间间隔最小；
基于该对应关键帧在校正前和校正后的位姿，求解得到该对应关键帧的位姿偏移量；以及
通过位姿叠加模型，将该对应关键帧的该位姿偏移量叠加至相应的该观测帧，以得到该观测帧的校正位姿。
在本发明的一实施例中，所述将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的校正的步骤，进一步包括步骤：
基于该对应关键帧的该位姿偏移量，通过坐标叠加模型对该路标点的空间坐标进行叠加处理，以得到该路标点的校正路标，使得该滤波式后端使用该观测帧的该校正位姿和该路标点的该校正坐标来更新雅克比矩阵和协方差矩阵。
在本发明的一实施例中，所述的滤波式后端回环校正方法，在所述接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作的步骤之前，进一步包括步骤：
对该回环线程的各项相关内容进行初始化，以初始化回环数量和回环检测间隔。
根据本发明的另一方面，本发明还提供了滤波式后端回环校正系统，用于对滤波化后端进行回环校正，其中所述滤波式后端回环校正系统包括相互可通信地连接的：
一接收模块，用于接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作；
一匹配模块，用于基于该当前帧的该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧，如果不存在，则退出该回环线程；如果存在，则将该当前帧作为新关键帧存储至该词袋数据库，以进行下一步操作；
一求解模块，用于基于该当前帧的该后端数据和该旧关键帧的数据，求解出该新关键帧和该旧关键帧之间的相对位姿；
一优化模块，用于基于该新关键帧和该旧关键帧之间的该相对位姿，通过非线性优化方法对该词袋数据库中存储的所有关键帧进行位姿图优化处理，以得到该关键帧的校正位姿数据；以及
一传递模块，用于将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的回环校正。
在本发明的一实施例中，所述匹配模块还用于根据该当前帧的该后端数据，判断该当前帧相对于该词袋数据库中上一关键帧的跟踪点数是否小于预定数量阈值或平均视差是否大于预定视差阈值；如果是，则将该当前帧存储至该词袋数据库以作为该词袋数据库的该新关键帧，并从该词袋数据库中寻找与该新关键帧间隔预定距离且大于预定相似度的关键帧；如果否，则退出此次回环检测。
在本发明的一实施例中，所述求解模块还用于基于该当前帧的该后端数据中的左右相机归一化坐标和左右相机之间的相对位姿，求解路标点的空间坐标；和通过PnP方法，基于该路标点的空间坐标，获得该新关键帧和该旧关键帧之间的该相对位姿。
在本发明的一实施例中，所述传递模块包括相互可通信地连接的一提取模块、一偏移量求解模块以及一位姿叠加模块，其中所述提取模块用于从该词袋数据库中提取与该滤波式后端的滑动窗口中的观测帧相对应的关键帧，以作为对应关键帧，其中该对应关键帧与相应的该观测帧之间的时间间隔最小；其中所述偏移量求解模块用于基于该对应关键帧在校正前和校正后的位姿，求解得到该对应关键帧的位姿偏移量；其中所述位姿叠加模块用于通过位姿叠加模型，将该对应关键帧的该位姿偏移量叠加至相应的该观测帧，以得到该观测帧的校正位姿。
在本发明的一实施例中，所述传递模块进一步包括一坐标叠加模型，其中所述坐标叠加模型用于基于该对应关键帧的该位姿偏移量，通过坐标叠加模型对该路标点的空间坐标进行叠加处理，以得到该路标点的校正路标，使得该滤波式后端使用该观测帧的该校正位姿和该路标点的该校正坐标来更新雅克比矩阵和协方差矩阵。
在本发明的一实施例中，所述的滤波式后端回环校正系统，进一步包括一初始化模块，其中所述初始化模块用于对该回环线程的各项相关内容进行初始化，以初始化回环数量和回环检测间隔。
根据本发明的另一方面，本发明还提供了电子设备，包括：
至少一处理器，用于执行指令；和
与所述至少一处理器可通信地连接的存储器，其中，所述存储器具有至少一指令，其中，所述指令被所述至少一处理器执行，以使得所述至少一处理器执行滤波式后端回环校正方法中的部分或全部步骤，其中所述滤波式后端回环校正方法包括步骤：
接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作；
基于该当前帧的该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧，如果不存在，则退出该回环线程；如果存在，则将该当前帧作为新关键帧存储至该词袋数据库，以进行下一步操作；
基于该当前帧的该后端数据和该旧关键帧的数据，求解出该新关键帧和该旧关键帧之间的相对位姿；
基于该新关键帧和该旧关键帧之间的该相对位姿，通过非线性优化方法对该词袋数据库中存储的所有关键帧进行位姿图优化处理，以得到该关键帧的校正位姿数据；以及
将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的回环校正。
通过对随后的描述和附图的理解，本发明进一步的目的和优势将得以充分体现。
本发明的这些和其它目的、特点和优势，通过下述的详细说明，附图和权利要求得以充分体现。
附图说明
图1是根据本发明的一实施例的滤波式后端回环校正方法的流程示意图。
图2示出了根据本发明的上述实施例的所述滤波式后端回环校正方法的步骤之一的流程示意图。
图3示出了根据本发明的上述实施例的所述滤波式后端回环校正方法的步骤之二的流程示意图。
图4示出了根据本发明的上述实施例的所述滤波式后端回环校正方法的步骤之三的流程示意图。
图5示出了根据本发明的一实施例的基于后端滤波的SLAM方法的流程示意图。
图6示出了根据本发明的一实施例的滤波式后端回环校正系统的框图示意图。
图7示出了根据本发明的一实施例的电子设备的框图示意图。
具体实施方式
以下描述用于揭露本发明以使本领域技术人员能够实现本发明。以下描述中的优选实施例只作为举例，本领域技术人员可以想到其他显而易见的变型。在以下描述中界定的本发明的基本原理可以应用于其他实施方案、变形方案、改进方案、等同方案以及没有背离本发明的精神和范围的其他技术方案。
在本发明中，权利要求和说明书中术语“一”应理解为“一个或多个”，即在一个实施例，一个元件的数量可以为一个，而在另外的实施例中，该元件的数量可以为多个。除非在本发明的揭露中明确示意该元件的数量只有一个，否则术语“一”并不能理解为唯一或单一，术语“一”不能理解为对数量的限制。
在本发明的描述中，需要理解的是，属于“第一”、“第二”等仅用于描述目的，而不能理解为指示或者暗示相对重要性。本发明的描述中，需要说明的是，除非另有明确的规定和限定，属于“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接或者一体地连接；可以是机械连接，也可以是电连接；可以是直接连接，也可以是通过媒介间接连结。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
由于商业化的SLAM产品对于成本的要求，往往无法使用高精度的传感器设备，这就导致了随着算法运行时间的增加，传感器引入的测量误差开始明显影响定位的准确度。现有的滤波式后端回环校正方法通常是在滤波式后端的滑动窗口中记录一系列的关键帧来构建词袋数据库，以在出现新的图像帧时，先从词袋数据库中寻找与其相似的旧帧，再提取两帧的图像特征点匹配关系，并将其用于后续雅克比矩阵计算进行新位姿估计，从而通过新旧帧之间的特征点约束就可以校正当前已经偏离的位姿。
然而，该现有的滤波式后端回环校正方法需要在滑动窗口中添加不可删除的关键帧，而随着关键帧数量的增多，滤波算法中协方差矩阵的维度将会变大而使运行速度大幅地降低，导致实时性变差，从而丧失了滤波式后端运行速度快的优点。此外，该现有的滤波式后端回环校正方法又不将路标点坐标作为优化变量，或者使用路标点绝对坐标来进行滤波处理。这样，随着运行时间的增加，计算得到的路标点坐标值也将产生偏差，从而影响校正效果。
因此，为了解决多传感器融合的SLAM算法在长时间运动后会产生无法忽略的累积误差问题，本发明提供了一种滤波式后端回环校正方法及其系统和电子设备。
示意性方法
参考说明书附图之图1至图4所示，根据本发明的一实施例的一种滤波式后端回环校正方法被阐明。具体地，如图1所示，所述滤波式后端回环校正方法，包括步骤：
S100：接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作；
S200：基于所述当前帧的所述后端数据，从所述回环线程中的词袋数据库中寻找与所述当前帧相匹配的旧关键帧，如果不存在，则退出所述回环线程；如果存在，则将所述当前帧作为新关键帧存储至所述词袋数据库，以进行下一步操作；
S300：基于所述当前帧的所述后端数据和所述旧关键帧的数据，求解出所述新关键帧和所述旧关键帧之间的相对位姿；
S400：基于所述新关键帧和所述旧关键帧之间的所述相对位姿，通过非线性优化方法对所述词袋数据库中存储的所有关键帧进行位姿图优化处理，以得到所述关键帧的校正位姿数据；以及
S500：将所述关键帧的校正位姿数据传递至所述滤波式后端，以进行所述滤波式后端的校正。
值得注意的是，在本发明的上述实施例中，所述滤波式后端的滤波处理过程是在所述后端线程中进行的，而所述滤波式后端的回环校正过程则是在所述回环线程中进行的。换言之，所述滤波式后端回环校正方法是通过独立创建的回环线程进行校正的，并且关键帧的各种数据被存储至所述回环线程的数据库，以避免在所述滤波式后端的滑动窗口中添加关键帧而带来的时间开销。此外，在所述回环线程中，使用基于位姿图优化方法对当前词袋数据库中各个关键帧的位姿进行精确计算，以避免滤波式后端中雅克比矩阵随着关键帧的数量增加而增大，有助于确保所述滤波式后端运行速度快的优点。换言之，本发明的所述滤波式后端回环校正方法无需在所述滤波式后端的滑动窗口中添加不可删除的关键帧(或添加额外的关键帧)，从而消除了窗口过大而对实时性造成的不利影响。
此外，在本发明的所述步骤S100之前(即接收所述当前帧的所述后端数据以导入所述回环线程之前)，通常还需要对所述回环线程的各项相关内容进行初始化。例如，初始化回环数量、回环检测间隔；导入预先训练好的词典；读取左右相机的内参、畸变系数和外参(即左右相机之间的相对位姿)；设定相关阈值等等。
更具体地，在本发明的所述滤波式后端回环校正方法的所述步骤S100中，所述当前帧的所述后端数据可以但不限于包括当前帧的左相机图像，当前帧的左相机特征点图像坐标，当前帧的左右相机归一化坐标以及当前帧的初始位姿。可以理解的是，所述当前帧的所述后端数据可以是通过所述滤波式后端处理传感器数据而获得的图像点数据，以导入所述回环线程进行后续计算。
特别地，在本发明的所述滤波式后端回环校正方法的所述步骤S200中，通过所述当前帧的所述后端数据中的所述左相机特征点图像坐标计算对应的描述子，而不再重新提取新的特征点和描述子，有助于加强鲁棒性，并加快检测速度。换言之，本发明的所述滤波式后端回环校正方法通过前端跟踪的特征点而不再重新提取新的特征点进行词袋匹配得到相应旧帧数据，再通过PnP求解两帧之间精确的相对位姿，减少了回环检测的时间。
值得一提的是，由于随着所述词袋数据库中的所述关键帧的数量增加，所述回环线程的运行速度将会减小，因此为了进一步减少回环检测时间，提高所述回环线程的运行速度，优选地事先判断当前帧的有效性，以便确定是否进行词袋匹配。示例性地，如图2所示，本发明的所述滤波式后端回环校正方法的所述步骤S200，包括步骤：
S210：根据所述当前帧的所述后端数据，判断所述当前帧相对于所述词袋数据库中上一关键帧的跟踪点数是否小于预定数量阈值或平均视差是否大于预定视差阈值；
S220：如果是，则将该当前帧存储至该词袋数据库以作为该词袋数据库的该新关键帧，并从该词袋数据库中寻找与该新关键帧间隔预定距离且大于预定相似度的关键帧；以及
S230：如果否，则退出此次回环检测。
可以理解的是，本发明的所述预定数量阈值、所述预定视差阈值、所述的预定距离以及所述预定相似度均可以通过自适应调整来获得，以便满足各种应用场景的具体需求，本发明对此不再赘述。
根据本发明的上述实施例，如图3所示，本发明的所述滤波式后端回环校正方法的所述步骤S300，包括步骤：
S310：基于所述当前帧的所述后端数据中的左右相机归一化坐标和左右相机之间的相对位姿，求解路标点的空间坐标；和
S320：通过PnP方法，基于所述路标点的空间坐标，获得所述新关键帧和所述旧关键帧之间的所述相对位姿。
值得注意的是，在本发明的所述步骤S300中，将所述当前帧的所述左右相机归一化坐标代入左右相机重投影模型，以求解来获取所述路标点的空间坐标；进而此使用PnP方法就可以得到所述新关键帧和所述旧关键帧之间的所述相对位姿。换言之，本发明采用左右相机匹配点的三角化重新计算路标点的相对坐标值，以在利用相对精确的左右目相机的外参标定结果(如左目相机与右目相机之间的相对位姿)的同时，又利用RANSAC方法进行校正，避免了使用路标点的绝对值而带来的累积误差影响。此外，本发明利用两帧之间精确的相对位姿约束来代替描述子匹配得到的点约束，减少了约束量。
例如，所述左右相机重投影模型可以但不限于别实施为：
drXr＝dlRr-lXl+Tr-l；其中，dl和dr分别是与左相机和右相机对应的深度；Rr-l和Tr-l分别是左右相机之间的旋转矩阵和平移矩阵；Xl和Xr分别是与左相机和右相机对应的路标点坐标值。
此外，根据本发明的上述实施例，在所述滤波式后端回环校正方法的所述步骤S400中，所述关键帧的所述校正位姿数据优选地被实施为所述关键帧在校正前后的位姿偏移量，以便后续在所述步骤S500中将所述位姿偏移量叠加至所述滤波式后端的所述滑动窗口中对应的观测帧中，以完成校正位姿的传递。
优选地，在本发明的所述滤波式后端回环校正方法的所述步骤S400中：藉由多次迭代优化模型，对所述关键帧的旋转量偏航角和平移量进行多次迭代优化，以获得每所述关键帧在校正后的位姿。这样，在优化处理过程中，去除了路标点数据，而只选择对关键帧的旋转量偏航角(即旋转量yaw角)和平移量进行优化，有助于减小优化过程中的计算量。
示例性地，所述多次迭代优化模型可以但不限于被实施为：
值得注意的是，在本发明的所述步骤S500中，所述滤波式后端回环校正方法优选地假定间隔时间较短的两帧相对位姿变化保护不变，以采用时间间隔最小的搜索策略来确定与所述滑动窗口中每观测帧最邻近的关键帧，便于选择相对化的方法来传递数据，也就是说，本发明利用优化前后关键帧位姿的相对偏移量来校正所述滤波式后端中当前窗口内各个观测帧的位姿，从而较为简便地完成回环约束传递。
示例性地，如图4所示，本发明的所述滤波式后端回环校正方法的所述步骤S500，包括步骤：
S510：从所述词袋数据库中提取与所述滤波式后端的滑动窗口中观测帧对应的关键帧，以作为对应关键帧，其中所述对应关键帧与相应的所述观测帧之间的时间间隔最小；
S520：基于所述对应关键帧在校正前和校正后的位姿，求解得到所述对应关键帧的位姿偏移量；以及
S530：通过位姿叠加模型，将所述对应关键帧的所述位姿偏移量叠加至相应的所述观测帧，以得到所述观测帧的校正位姿。
进一步地，如图4所示，所述滤波式后端回环校正方法的所述步骤S500，还可以包括步骤：
S540：基于所述对应的关键帧的所述位姿偏移量，通过坐标叠加模型对所述路标点的空间坐标进行叠加处理，以得到所述路标点的校正坐标，使得所述滤波式后端使用所述观测帧的所述校正位姿和所述路标点的所述校正坐标来更新雅克比矩阵和协方差矩阵。
值得注意的是，所述位姿叠加模型可以但不限于被实施为：
其中，Rdrift和Tdrift分别为所述对应的关键帧的所述位姿偏移量中的旋转偏移量和平移偏移量；
和
分别是所述观测帧在叠加前后的旋转量；Ti before和Ti after分别是所述观测帧在叠加前后的平移量。
此外，所述坐标叠加模型可以但不限于被实施为：
其中，Rdrift和Tdrift分别为所述对应的关键帧的所述位姿偏移量中的旋转偏移量和平移偏移量；
和
分别是所述路标点在校正前后的空间坐标。
值得注意的是，本发明的所述步骤S500先通过从所述词袋数据库中提取与该滑动窗口内各个观测帧对应的最邻近关键帧(即对应关键帧)，再利用优化前后的所述最邻近关键帧的相对偏移量来校正当前滑动窗口内各个观测帧的位姿，以便较为简便地完成回环约束的传递，避免因采用绝对位姿进行校正而引入的误差，有助于提高校正精度，进而提高基于后端滤波的SLAM方法的定位精度。
根据本发明的另一方面，本发明进一步提供了一基于后端滤波的SLAM方法。具体地，如图5所示，所述基于后端滤波的SLAM方法，包括步骤：
获取经由传感器采集的传感器数据；
通过前端视觉里程计对所述传感器数据进行前端处理，以得到前端数据；
通过滤波式后端对所述前端数据进行后端处理，获得后端数据以进行建图；以及
通过上述滤波式后端回环校正方法对所述滤波式后端进行回环校正(回环检测和回环传递)。
示意性系统
参考说明书附图之图6所示，根据本发明的一实施例的一滤波式后端回环校正系统被阐明，其中所述滤波式后端回环校正系统1包括相互可通信地连接的一接收模块10、一匹配模块20、一求解模块30、一优化模块40以及一传递模块50。所述接收模块10用于接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作。所述匹配模块20用于基于该当前帧的该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧，如果不存在，则退出该回环线程；如果存在，则将该当前帧作为新关键帧存储至该词袋数据库，以进行下一步操作。所述求解模块30用于基于该当前帧的该后端数据和该旧关键帧的数据，求解出该新关键帧和该旧关键帧之间的相对位姿。所述优化模块40用于基于该新关键帧和该旧关键帧之间的该相对位姿，通过非线性优化方法对该词袋数据库中存储的所有关键帧进行位姿图优化处理，以得到该关键帧的校正位姿数据。所述传递模块50用于将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的回环校正。
值得注意的是，在本发明的一示例中，所述匹配模块20还用于根据该当前帧的该后端数据，判断该当前帧相对于该词袋数据库中上一关键帧的跟踪点数是否小于预定数量阈值或平均视差是否大于预定视差阈值；如果是，则将该当前帧存储至该词袋数据库以作为该词袋数据库的该新关键帧，并从该词袋数据库中寻找与该新关键帧间隔预定距离且大于预定相似度的关键帧；以及如果否，则删除该当前帧，以退出此次回环检测。
此外，所述求解模块30还用于基于该当前帧的该后端数据中的左右相机归一化坐标和左右相机之间的相对位姿，求解路标点的空间坐标；和通过PnP方法，基于该路标点的空间坐标，获得该新关键帧和该旧关键帧之间的该相对位姿。
更进一步地，在本发明的一示例中，如图6所示，所述传递模块50包括相互可通信地连接的一提取模块51、一偏移量求解模块52以及一位姿叠加模块53，其中所述提取模块51用于从该词袋数据库中提取与该滤波式后端的滑动窗口中的观测帧相对应的关键帧，以作为对应关键帧，其中该对应关键帧与相应的该观测帧之间的时间间隔最小；其中所述偏移量求解模块52用于基于该对应关键帧在校正前和校正后的位姿，求解得到该对应关键帧的位姿偏移量；其中所述位姿叠加模块53用于通过位姿叠加模型，将该对应关键帧的该位姿偏移量叠加至相应的该观测帧，以得到该观测帧的校正位姿。
在本发明的一示例中，如图6所示，所述传递模块50进一步包括一坐标叠加模块54，其中所述坐标叠加模块54用于基于该对应关键帧的该位姿偏移量，通过坐标叠加模型对该路标点的空间坐标进行叠加处理，以得到该路标点的校正路标，使得该滤波式后端使用该观测帧的该校正位姿和该路标点的该校正坐标来更新雅克比矩阵和协方差矩阵。
值得一提的是，如图6所示，所述的滤波式后端回环校正系统1进一步包括一初始化模块60，其中所述初始化模块60用于对该回环线程的各项相关内容进行初始化，以初始化回环数量和回环检测间隔。
示意性电子设备
下面，参考图7来描述根据本发明的一实施例的电子设备。如图7所示，电子设备90包括一个或多个处理器91和存储器92。
所述处理器91可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备90中的其他组件以执行期望的功能。换言之，所述处理器91包括被配置成执行指令的一个或多个物理设备。例如，所述处理器91可被配置成执行作为以下各项的一部分的指令：一个或多个应用、服务、程序、例程、库、对象、组件、数据结构、或其他逻辑构造。这种指令可被实现以执行任务、实现数据类型、转换一个或多个部件的状态、实现技术效果、或以其他方式得到期望结果。
所述处理器91可包括被配置成执行软件指令的一个或多个处理器。作为补充或替换，所述处理器91可包括被配置成执行硬件或固件指令的一个或多个硬件或固件逻辑机。所述处理器91的处理器可以是单核或多核，且在其上执行的指令可被配置为串行、并行和/或分布式处理。所述处理器91的各个组件可任选地分布在两个或更多单独设备上，这些设备可以位于远程和/或被配置成进行协同处理。所述处理器91的各方面可由以云计算配置进行配置的可远程访问的联网计算设备来虚拟化和执行。
所述存储器92可以包括一个或多个计算程序产品，所述计算程序产品可以包括各种形式的计算可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算可读存储介质上可以存储一个或多个计算程序指令，所述处理器11可以运行所述程序指令，以实现上文所述的本发明的上述示意性方法中的部分或全部步骤，以及/或者其他期望的功能。
换言之，所述存储器92包括被配置成保存可由所述处理器91执行以实现此处所述的方法和过程的机器可读指令的一个或多个物理设备。在实现这些方法和过程时，可以变换所述存储器92的状态(例如，保存不同的数据)。所述存储器92可以包括可移动和/或内置设备。所述存储器92可包括光学存储器(例如，CD、DVD、HD-DVD、蓝光盘等)、半导体存储器(例如，RAM、EPROM、EEPROM等)和/或磁存储器(例如，硬盘驱动器、软盘驱动器、磁带驱动器、MRAM等)等等。所述存储器92可包括易失性、非易失性、动态、静态、读/写、只读、随机存取、顺序存取、位置可寻址、文件可寻址和/或内容可寻址设备。
可以理解，所述存储器92包括一个或多个物理设备。然而，本文描述的指令的各方面可另选地通过不由物理设备在有限时长内持有的通信介质(例如，电磁信号、光信号等)来传播。所述处理器91和所述存储器92的各方面可被一起集成到一个或多个硬件逻辑组件中。这些硬件逻辑组件可包括例如现场可编程门阵列(FPGA)、程序和应用专用的集成电路(PASIC/ASIC)、程序和应用专用的标准产品(PSSP/ASSP)、片上系统(SOC)以及复杂可编程逻辑器件(CPLD)。
在一个示例中，如图7所示，所述电子设备90还可以包括输入装置93和输出装置94，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。例如，该输入装置93可以是例如用于采集图像数据或视频数据的摄像模组等等。又如，所述输入装置93可以包括诸如键盘、鼠标、触摸屏或游戏控制器之类的一个或多个用户输入设备或者与其对接。在一些实施例中，所述输入装置93可以包括所选择的自然用户输入(NUI)部件或与其对接。这种元件部分可以是集成的或外围的，并且输入动作的转导和/或处理可以在板上或板外被处理。示例NUI部件可包括用于语言和/或语音识别的话筒；用于机器视觉和/或姿势识别的红外、色彩、立体显示和/或深度相机；用于运动检测和/或意图识别的头部跟踪器、眼睛跟踪器、加速计和/或陀螺仪；以及用于评估脑部活动和/或身体运动的电场感测部件；和/或任何其他合适的传感器。
该输出装置94可以向外部输出各种数据，包括分类结果等。该输出装置94可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。
当然，所述电子设备90还可以进一步包括所述通信装置，其中所述通信装置可被配置成将所述电子设备90与一个或多个其他计算机设备通信地耦合。所述通信装置可以包括与一个或多个不同通信协议兼容的有线和/或无线通信设备。作为非限制性示例，通信子系统可被配置成用于经由无线电话网络或者有线或无线局域网或广域网来进行通信。在一些实施例中，所述通信装置可允许所述电子设备90经由诸如因特网这样的网络将消息发送至其他设备以及/或者从其它设备接收消息。
将会理解，此处描述的配置和/或方法本质是示例性的，这些具体实施例或示例不应被视为限制性的，因为许多变体是可能的。此处描述的具体例程或方法可以表示任何数量的处理策略中的一个或多个。如此，所示和/或所述的各种动作可以以所示和/或所述顺序、以其他顺序、并行地执行，或者被省略。同样，上述过程的次序可以改变。
当然，为了简化，图7中仅示出了该电子设备90中与本发明有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备90还可以包括任何其他适当的组件。
示意性计算程序产品
除了上述方法和设备以外，本发明的实施例还可以是计算程序产品，其包括计算程序指令，所述计算程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本发明各种实施例的方法中的步骤。
所述计算程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本发明实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如C语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
此外，本发明的实施例还可以是计算可读存储介质，其上存储有计算程序指令，所述计算程序指令在被处理器运行时使得所述处理器执行本说明书上述方法中的步骤。
所述计算可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
以上结合具体实施例描述了本发明的基本原理，但是，需要指出的是，在本发明中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本发明的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本发明为必须采用上述具体的细节来实现。
本发明中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。
还需要指出的是，在本发明的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本发明的等效方案。
提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本发明。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本发明的范围。因此，本发明不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
本领域的技术人员应理解，上述描述及附图中所示的本发明的实施例只作为举例而并不限制本发明。本发明的目的已经完整并有效地实现。本发明的功能及结构原理已在实施例中展示和说明，在没有背离所述原理下，本发明的实施方式可以有任何变形或修改。
Claims (13)
1.滤波式后端回环校正方法，其特征在于，包括步骤：
接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作；
基于该当前帧的该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧，如果不存在，则退出该回环线程；如果存在，则将该当前帧作为新关键帧存储至该词袋数据库，以进行下一步操作；
基于该当前帧的该后端数据和该旧关键帧的数据，求解出该新关键帧和该旧关键帧之间的相对位姿；
基于该新关键帧和该旧关键帧之间的该相对位姿，通过非线性优化方法对该词袋数据库中存储的所有关键帧进行位姿图优化处理，以得到该关键帧的校正位姿数据；以及
将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的回环校正。
2.如权利要求1所述的滤波式后端回环校正方法，其中，所述基于该当前帧的该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧的步骤，包括步骤：
根据该当前帧的该后端数据，判断该当前帧相对于该词袋数据库中上一关键帧的跟踪点数是否小于预定数量阈值或平均视差是否大于预定视差阈值；
如果是，则将该当前帧存储至该词袋数据库以作为该词袋数据库的该新关键帧，并从该词袋数据库中寻找与该新关键帧间隔预定距离且大于预定相似度的关键帧；以及
如果否，则退出此次回环检测。
3.如权利要求2所述的滤波式后端回环校正方法，其中，所述基于该当前帧的该后端数据和该旧关键帧的数据，求解出该新关键帧和该旧关键帧之间的相对位姿的步骤，包括步骤：
基于该当前帧的该后端数据中的左右相机归一化坐标和左右相机之间的相对位姿，求解路标点的空间坐标；和
通过PnP方法，基于该路标点的空间坐标，获得该新关键帧和该旧关键帧之间的该相对位姿。
4.如权利要求3所述的滤波式后端回环校正方法，其中，所述将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的校正的步骤，包括步骤：
从该词袋数据库中提取与该滤波式后端的滑动窗口中的观测帧相对应的关键帧，以作为对应关键帧，其中该对应关键帧与相应的该观测帧之间的时间间隔最小；
基于该对应关键帧在校正前和校正后的位姿，求解得到该对应关键帧的位姿偏移量；以及
通过位姿叠加模型，将该对应关键帧的该位姿偏移量叠加至相应的该观测帧，以得到该观测帧的校正位姿。
5.如权利要求4所述的滤波式后端回环校正方法，其中，所述将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的校正的步骤，进一步包括步骤：
基于该对应关键帧的该位姿偏移量，通过坐标叠加模型对该路标点的空间坐标进行叠加处理，以得到该路标点的校正路标，使得该滤波式后端使用该观测帧的该校正位姿和该路标点的该校正坐标来更新雅克比矩阵和协方差矩阵。
6.如权利要求1至5中任一所述的滤波式后端回环校正方法，在所述接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作的步骤之前，进一步包括步骤：
对该回环线程的各项相关内容进行初始化，以初始化回环数量和回环检测间隔。
7.滤波式后端回环校正系统，用于对滤波化后端进行回环校正，其特征在于，其中所述滤波式后端回环校正系统包括相互可通信地连接的：
一接收模块，用于接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作；
一匹配模块，用于基于该当前帧的该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧，如果不存在，则退出该回环线程；如果存在，则将该当前帧作为新关键帧存储至该词袋数据库，以进行下一步操作；
一求解模块，用于基于该当前帧的该后端数据和该旧关键帧的数据，求解出该新关键帧和该旧关键帧之间的相对位姿；
一优化模块，用于基于该新关键帧和该旧关键帧之间的该相对位姿，通过非线性优化方法对该词袋数据库中存储的所有关键帧进行位姿图优化处理，以得到该关键帧的校正位姿数据；以及
一传递模块，用于将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的回环校正。
8.如权利要求7所述的滤波式后端回环校正系统，其中，所述匹配模块还用于根据该当前帧的该后端数据，判断该当前帧相对于该词袋数据库中上一关键帧的跟踪点数是否小于预定数量阈值或平均视差是否大于预定视差阈值；如果是，则将该当前帧存储至该词袋数据库以作为该词袋数据库的该新关键帧，并从该词袋数据库中寻找与该新关键帧间隔预定距离且大于预定相似度的关键帧；以及如果否，则退出此次回环检测。
9.如权利要求8所述的滤波式后端回环校正系统，其中，所述求解模块还用于基于该当前帧的该后端数据中的左右相机归一化坐标和左右相机之间的相对位姿，求解路标点的空间坐标；和通过PnP方法，基于该路标点的空间坐标，获得该新关键帧和该旧关键帧之间的该相对位姿。
10.如权利要求9所述的滤波式后端回环校正系统，其中，所述传递模块包括相互可通信地连接的一提取模块、一偏移量求解模块以及一位姿叠加模块，其中所述提取模块用于从该词袋数据库中提取与该滤波式后端的滑动窗口中的观测帧相对应的关键帧，以作为对应关键帧，其中该对应关键帧与相应的该观测帧之间的时间间隔最小；其中所述偏移量求解模块用于基于该对应关键帧在校正前和校正后的位姿，求解得到该对应关键帧的位姿偏移量；其中所述位姿叠加模块用于通过位姿叠加模型，将该对应关键帧的该位姿偏移量叠加至相应的该观测帧，以得到该观测帧的校正位姿。
11.如权利要求10所述的滤波式后端回环校正系统，其中，所述传递模块进一步包括一坐标叠加模型，其中所述坐标叠加模型用于基于该对应关键帧的该位姿偏移量，通过坐标叠加模型对该路标点的空间坐标进行叠加处理，以得到该路标点的校正路标，使得该滤波式后端使用该观测帧的该校正位姿和该路标点的该校正坐标来更新雅克比矩阵和协方差矩阵。
12.如权利要求7至11中任一所述的滤波式后端回环校正系统，进一步包括一初始化模块，其中所述初始化模块用于对该回环线程的各项相关内容进行初始化，以初始化回环数量和回环检测间隔。
13.电子设备，其特征在于，包括：
至少一处理器，用于执行指令；和
与所述至少一处理器可通信地连接的存储器，其中，所述存储器具有至少一指令，其中，所述指令被所述至少一处理器执行，以使得所述至少一处理器执行滤波式后端回环校正方法中的部分或全部步骤，其中所述滤波式后端回环校正方法包括步骤：
接收在后端线程中经滤波式后端处理后的当前帧的后端数据，以导入回环线程进行后续操作；
基于该当前帧的该后端数据，从该回环线程中的词袋数据库中寻找与该当前帧相匹配的旧关键帧，如果不存在，则退出该回环线程；如果存在，则将该当前帧作为新关键帧存储至该词袋数据库，以进行下一步操作；
基于该当前帧的该后端数据和该旧关键帧的数据，求解出该新关键帧和该旧关键帧之间的相对位姿；
基于该新关键帧和该旧关键帧之间的该相对位姿，通过非线性优化方法对该词袋数据库中存储的所有关键帧进行位姿图优化处理，以得到该关键帧的校正位姿数据；以及
将该关键帧的该校正位姿数据传递至该滤波式后端，以进行该滤波式后端的回环校正。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010022908.8A CN113091767B (zh) | 2020-01-09 | 2020-01-09 | 滤波式后端回环校正方法及其系统和电子设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010022908.8A CN113091767B (zh) | 2020-01-09 | 2020-01-09 | 滤波式后端回环校正方法及其系统和电子设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113091767A CN113091767A (zh) | 2021-07-09 |
| CN113091767B true CN113091767B (zh) | 2022-08-30 |
Family
ID=76664086
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010022908.8A Active CN113091767B (zh) | 2020-01-09 | 2020-01-09 | 滤波式后端回环校正方法及其系统和电子设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113091767B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107869989A (zh) * | 2017-11-06 | 2018-04-03 | 东北大学 | 一种基于视觉惯导信息融合的定位方法及系统 |
| CN109615698A (zh) * | 2018-12-03 | 2019-04-12 | 哈尔滨工业大学（深圳） | 基于互回环检测的多无人机slam地图融合算法 |
| CN110118556A (zh) * | 2019-04-12 | 2019-08-13 | 浙江工业大学 | 一种基于协方差交叉融合slam的机器人定位方法及装置 |
| CN110472585A (zh) * | 2019-08-16 | 2019-11-19 | 中南大学 | 一种基于惯导姿态轨迹信息辅助的vi-slam闭环检测方法 |
| CN110599545A (zh) * | 2019-09-06 | 2019-12-20 | 电子科技大学中山学院 | 一种基于特征的构建稠密地图的系统 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9286810B2 (en) * | 2010-09-24 | 2016-03-15 | Irobot Corporation | Systems and methods for VSLAM optimization |
| US10586344B2 (en) * | 2018-02-21 | 2020-03-10 | Beijing Jingdong Shangke Information Technology Co., Ltd. | System and method for feature screening in SLAM |
-
2020
- 2020-01-09 CN CN202010022908.8A patent/CN113091767B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107869989A (zh) * | 2017-11-06 | 2018-04-03 | 东北大学 | 一种基于视觉惯导信息融合的定位方法及系统 |
| CN109615698A (zh) * | 2018-12-03 | 2019-04-12 | 哈尔滨工业大学（深圳） | 基于互回环检测的多无人机slam地图融合算法 |
| CN110118556A (zh) * | 2019-04-12 | 2019-08-13 | 浙江工业大学 | 一种基于协方差交叉融合slam的机器人定位方法及装置 |
| CN110472585A (zh) * | 2019-08-16 | 2019-11-19 | 中南大学 | 一种基于惯导姿态轨迹信息辅助的vi-slam闭环检测方法 |
| CN110599545A (zh) * | 2019-09-06 | 2019-12-20 | 电子科技大学中山学院 | 一种基于特征的构建稠密地图的系统 |
Non-Patent Citations (2)
| Title |
|---|
| Stereo vision SLAM system based on ORB;Jiao Yu et al.;《2019 IEEE 4th Advanced Information Technology, Electronic and Automation Control Conference (IAEAC)》;20191231;第1卷;418-421 * |
| 基于ORB-SLAM2算法的水下机器人实时定位研究;张阳等;《测绘通报》;20191231(第12期);1-7 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113091767A (zh) | 2021-07-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107990899B (zh) | 一种基于slam的定位方法和系统 | |
| EP3627180B1 (en) | Sensor calibration method and device, computer device, medium, and vehicle | |
| CN109544629B (zh) | 摄像头位姿确定方法和装置以及电子设备 | |
| Kneip et al. | Robust real-time visual odometry with a single camera and an IMU | |
| CN110310326B (zh) | 一种视觉定位数据处理方法、装置、终端及计算机可读存储介质 | |
| EP2572319B1 (fr) | Procede et systeme pour fusionner des donnees issues de capteurs d'images et de capteurs de mouvement ou de position | |
| US20200116498A1 (en) | Visual assisted distance-based slam method and mobile robot using the same | |
| EP3711025A1 (en) | Graphical coordinate system transform for video frames | |
| CN113916243A (zh) | 目标场景区域的车辆定位方法、装置、设备和存储介质 | |
| CN111986261B (zh) | 一种车辆定位方法、装置、电子设备及存储介质 | |
| CN110068824B (zh) | 一种传感器位姿确定方法和装置 | |
| WO2021031790A1 (zh) | 一种信息处理方法、装置、电子设备、存储介质和程序 | |
| WO2023050634A1 (zh) | 定位方法及装置、设备、存储介质及计算机程序产品 | |
| KR20200037502A (ko) | 포즈 정보를 출력하는 방법 및 장치 | |
| CN115035456A (zh) | 视频去噪方法、装置、电子设备和可读存储介质 | |
| CN113091767B (zh) | 滤波式后端回环校正方法及其系统和电子设备 | |
| CN113639782A (zh) | 车载传感器的外参标定方法和装置、设备和介质 | |
| CN111089579B (zh) | 异构双目slam方法、装置及电子设备 | |
| US20240087155A1 (en) | Prior informed pose and scale estimation | |
| WO2023226156A1 (zh) | 时间戳校正方法、装置、设备、介质及计算机程序产品 | |
| CN114415698B (zh) | 机器人、机器人的定位方法、装置和计算机设备 | |
| KR102371594B1 (ko) | 스테레오 카메라 영상 실시간 자동 캘리브레이션 장치, 그를 포함한 시스템 및 그 방법 | |
| CN115388882A (zh) | 用于深度辅助视觉惯性测距的方法和装置 | |
| CN110864685A (zh) | 一种基于松耦合的车辆单目视觉轮式里程计定位方法 | |
| CN113012216B (zh) | 特征分类优化方法和slam定位方法及其系统和电子设备 |
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 | ||
| EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20210709 Assignee: Zhejiang Shunwei Technology Co.,Ltd. Assignor: SUNNY OPTICAL (ZHEJIANG) RESEARCH INSTITUTE Co.,Ltd. Contract record no.: X2024330000055 Denomination of invention: Filter based backend loop correction method and its system and electronic devices Granted publication date: 20220830 License type: Common License Record date: 20240515 |
|
| EE01 | Entry into force of recordation of patent licensing contract |