CN109191526A - 基于rgbd摄像头和光编码器的三维环境重建方法及*** - Google Patents

Abstract

本发明公开了一种基于RGBD摄像头和光编码器的三维环境重建方法及***，在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化信息，得到当前帧的位姿估计；在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差。其通过利用视觉和光编码器估计相机位姿，可以减少环境光照变化对位姿估计的影响；当关键帧初始化失败时，利用光编码器给出位姿变化的估计，可以增强对缺少纹理的环境重建的鲁棒性，可以更广泛地使用。

Description

基于RGBD摄像头和光编码器的三维环境重建方法及***

技术领域

本发明涉及图像处理技术领域，尤其涉及一种基于RGBD摄像头和光编码器的三维环境重建方法及***。

背景技术

在计算机视觉中,三维重建是指根据单视图或者多视图的图像重建三维信息的过程。由于单视频的信息不完全,因此三维重建比较困难。而多视图的三维重建(类似人的双目定位)相对比较容易,其大致方法是先对摄像机进行标定,即计算出摄像机的图像坐标系与世界坐标系的关系，然后再利用多个二维图像中的信息重建出三维信息。

目前，单纯基于RGBD(RGB+Depth，彩色图+深度图)摄像头的三维环境重建方法，其输入图像为彩色图像和深度图像。其中，彩色图像容易受到过亮、过暗、过曝等环境光照变化的影响，从而导致图像降质，甚至不可用。此外，在缺少纹理的环境中，从彩色图像中无法提取物体的边缘和特征点。而物体的边缘和特征点对相机的位姿估计非常重要，如果无法提取物体的边缘和特征点，则会直接导致相机的位姿估计误差过大，进而影响重建效果。

因此，现有的基于RGBD摄像头的三维环境重建方法，容易受环境及其光照变化的影响，在重建过程中对环境及其光照变化的鲁棒性较低，应用受限。

发明内容

基于此，本发明的目的在于提供一种基于RGBD摄像头和光编码器的三维环境重建方法及***，在环境光照或环境纹理变化导致彩色图像降质或不可作为关键帧使用时，利用光编码器的输出来补偿相机的位姿估计，从而增加了重建过程对环境及其光照变化的鲁棒性，可以更广泛地使用。

本发明提供的一种基于RGBD摄像头和光编码器的三维环境重建方法，包括以下步骤：

在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化信息，得到当前帧的位姿估计；

在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差。

作为一种可实施方式，根据图像的边缘和特征点，选取关键帧。

作为一种可实施方式，在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化信息，得到当前帧的位姿估计，包括以下步骤：

在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿信息和当前帧的光编码器的位姿信息，计算当前帧和前一帧之间的帧间位姿变化信息；

根据所述前一帧的位姿和所述帧间位姿变化信息，得到当前帧的位姿估计。

作为一种可实施方式，本发明的基于RGBD摄像头和光编码器的三维环境重建方法，还包括以下步骤：

初始化关键帧；

若初始化成功，则根据当前帧和最近的关键帧，得到当前帧的位姿估计；

若初始化失败，则判断前一帧是否可以作为关键帧；

若前一帧可以作为关键帧，则将前一帧保存为新的关键帧，并将前一帧位姿初始化为单位阵，标记初始化成功。

作为一种可实施方式，在根据当前帧和最近的关键帧，得到当前帧的位姿估计之后，还包括以下步骤：

计算当前帧相对于最近关键帧的位姿及误差；

若误差小于第二设定阈值时，将当前帧保存为前一帧；

若误差大于第二设定阈值时，则标记初始化失败。

作为一种可实施方式，在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差，包括以下步骤：

基于通用的视觉方法估计得到当前帧相对于最近关键帧的位姿及其误差；

若该误差大于第一设定阈值，则根据前一帧光编码器的输出位姿和当前帧光编码器的输出位姿，得到帧间位姿；

根据帧间位姿和前一帧的位姿，得到当前帧的位姿的初始估计；

根据最近关键帧和当前帧的位姿的初始估计，得到当前帧的位姿。

相应地，本发明提供的一种基于RGBD摄像头和光编码器的三维环境重建***，包括选取模块和重建模块；

所述选取模块，用于在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿信息和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化，得到当前帧的位姿估计；

所述重建模块，用于在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差。

作为一种可实施方式，根据图像的边缘和特征点，选取关键帧。

作为一种可实施方式，所述选取模块包括第一计算单元和第二计算单元；

所述第一计算单元，用于在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿信息和当前帧的光编码器的位姿信息，计算当前帧和前一帧之间的帧间位姿变化信息；

所述第二计算单元，用于根据所述前一帧的位姿和所述帧间位姿变化信息，得到当前帧的位姿估计。

本发明提供的基于RGBD摄像头和光编码器的三维环境重建***，还包括初始化模块；

所述初始化模块，用于初始化关键帧；若初始化成功，则根据当前帧和最近的关键帧，得到当前帧的位姿估计；若初始化失败，则判断前一帧是否可以作为关键帧，若前一帧可以作为关键帧，则将前一帧保存为新的关键帧，并将前一帧位姿初始化为单位阵，标记初始化成功。

作为一种可实施方式，所述选取模块还包括第三计算单元；

所述第三计算单元用于计算当前帧相对于最近关键帧的位姿及误差；若误差小于第二设定阈值时，将当前帧保存为前一帧；若误差大于第二设定阈值时，则标记初始化失败。

作为一种可实施方式，所述重建模块包括位姿估计单元、比较单元、第四计算单元和第五计算单元；

所述位姿估计单元，用于基于通用的视觉方法估计得到当前帧相对于最近关键帧的位姿及其误差；

所述比较单元，用于若该误差大于第一设定阈值，则根据前一帧光编码器的输出位姿和当前帧光编码器的输出位姿，得到帧间位姿；

所述第四计算单元，用于根据帧间位姿和前一帧的位姿，得到当前帧的位姿的初始估计；

所述第五计算单元，用于根据最近关键帧和当前帧的位姿的初始估计，得到当前帧的位姿。

与现有技术相比，本技术方案具有以下优点：

本发明提供的基于RGBD摄像头和光编码器的三维环境重建方法及***，在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化信息，得到当前帧的位姿估计；在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差。其通过利用视觉和光编码器估计相机位姿，可以减少环境光照变化对位姿估计的影响；当关键帧初始化失败时，利用光编码器给出位姿变化的估计，可以增强对缺少纹理的环境重建的鲁棒性，可以更广泛地使用。

附图说明

图1为本发明实施例一提供的基于RGBD摄像头和光编码器的三维环境重建方法中的前一关键帧、最近关键帧、前一帧和当前帧在时间轴上的位置示意图；

图2为本发明实施例一提供的基于RGBD摄像头和光编码器的三维环境重建方法的流程示意图；

图3为本发明实施例一提供的基于RGBD摄像头和光编码器的三维环境重建方法中的关键帧的初始化流程示意图；

图4为本发明实施例一提供的基于RGBD摄像头和光编码器的三维环境重建方法中的重建流程示意图；

图5为本发明实施例一提供的基于RGBD摄像头和光编码器的三维环境重建方法中的估计当前位姿的流程示意图；

图6为本发明实施例二提供的基于RGBD摄像头和光编码器的三维环境重建***的结构示意图。

具体实施方式

以下结合附图，对本发明上述的和另外的技术特征和优点进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的部分实施例，而不是全部实施例。

本发明中的每一个普通帧包含如下信息：摄像头输出的彩色图和深度图、光编码器输出的位姿和相机的位姿。前一关键帧、最近关键帧、前一帧和当前帧在时间轴上的顺序位置，如图1所示。相邻两个关键帧之间一般隔着若干普通帧，前一帧中保存了当前帧的信息。

参见图2，本发明实施例一提供的一种基于RGBD摄像头和光编码器的三维环境重建方法，包括以下步骤：

S100、在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化信息，得到当前帧的位姿估计。

S200、在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差。

步骤S100中，一般在三维环境重建过程中，第一步做的都是初始化关键帧。关键帧是从普通帧中挑选出来、依时间的先后顺序组成的。关键帧的初始化，就是从普通帧中选出一帧作为第一个关键帧。为了估计后面普通帧的位姿，需要关键帧有足够丰富的边缘或特征点。

本实施例中关键帧的初始化，如图3所示，具体实现过程如下：

S010，接收新到来的普通帧，保存为当前帧；

S011，判断当前帧是否可以作为关键帧；即判断当前帧是否有足够丰富的边缘或特征点。

S012，如果当前帧可以作为关键帧，那么将当前帧保存为第一个关键帧，标记关键帧初始化成功，将当前帧保存为前一帧；然后，可以开始重建过程，进入步骤S021。

S013，如果当前帧不可以作为关键帧，那么接收新到来的普通帧为当前帧，继续判断当前帧是否可以作为关键帧；同时，若当前帧不可以作为关键帧，则标记关键帧初始化失败，进入步骤S022。

S021，若初始化成功，则根据当前帧和最近的关键帧，得到当前帧的位姿估计；

S022，若初始化失败，则判断前一帧是否可以作为关键帧；

S023，若前一帧可以作为关键帧，则将前一帧保存为新的关键帧，并将前一帧位姿初始化为单位阵，标记初始化成功。

具体地，步骤S100可以通过以下步骤实现：

首先，在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿信息和当前帧的光编码器的位姿信息，计算当前帧和前一帧之间的帧间位姿变化信息；然后，根据前一帧的位姿和帧间位姿变化信息，得到当前帧的位姿估计。

接下来，计算当前帧相对于最近关键帧的位姿及误差；若误差小于第二设定阈值时，将当前帧保存为前一帧；若误差大于第二设定阈值时，则标记初始化失败。

上述摄像头和光编码器相结合的重建过程，通过求当前帧相对于最近关键帧的位姿估计及其误差，当误差小于第二设定阈值时，表示该位姿估计是正确的，所以将当前帧保存为前一帧，并接收新来的普通帧保存为当前帧；当误差大于第二设定阈值时，表示该位姿估计是不正确的，所以需要生成新的关键帧。

进一步地，如果前一帧满足关键帧的条件，那么，保存前一帧为新的关键帧，将前一帧的位姿初始化为4X4的单位阵，标记关键帧初始化成功；然后，估计当前帧相对于最近的关键帧的位姿。

如果前一帧不满足关键帧的条件，那么利用前一帧光编码器的位姿和当前帧光编码器的位姿，得到当前帧和前一帧之间的帧间位姿变化信息，进一步可以得到当前帧的位姿估计；由于前一帧未能成为关键帧，所以标记关键帧初始化不成功。

以上，在关键帧初始化失败时，利用光编码器给出位姿变化的估计，可以增强对缺少纹理的环境重建的鲁棒性。

作为一种可实施方式，上述摄像头和光编码器相结合的重建过程，如图4所示：

S41，接收新到来的普通帧，保存为当前帧；

S42，在关键帧初始化成功后，估计当前帧相对于最近关键帧的位姿及其误差；若误差大于设定阈值，则标记初始化失败，执行步骤S43；若误差小于设定阈值，表示该位姿估计是准确的，执行步骤S47；

S43，在关键帧初始化失败后，判断前一帧是否可以作为关键帧；

S44，如果前一帧可以作为关键帧，那么将前一帧保存为新的关键帧，将前一帧位姿估计初始化为4X4的单位阵，标记关键帧初始化成功；

S45，估计当前帧与最近的关键帧之间的位姿，将当前帧保存为前一帧；

S46，如果前一帧不可以作为关键帧，利用前一帧的光编码器位姿信息和当前帧的光编码器信息得到当前帧和前一帧之间的位姿，进而得到当前帧的位姿估计，标记关键帧初始化不成功；然后，保存当前帧到前一帧(S47)，接收新的普通帧到当前帧(S41)；

S47，保存当前帧到前一帧，接收新的普通帧并保存为当前帧(S41)；

接下来，参见图5，步骤S200可以通过以下步骤实现：

S210、基于通用的视觉方法估计得到当前帧相对于最近关键帧的位姿及其误差；

S220、若该误差大于第一设定阈值，则根据前一帧光编码器的输出位姿和当前帧光编码器的输出位姿，得到帧间位姿；

S230、根据帧间位姿和前一帧的位姿，得到当前帧的位姿的初始估计；

S240、根据最近关键帧和当前帧的位姿的初始估计，得到当前帧的位姿。

其中，基于通用的视觉方法估计当前帧相对于最近关键帧的位姿及其误差，当误差大于第一设定阈值时，表示视觉方法估计的位姿误差较大；所以，以光编码器输出的位姿做为初始值，再次用视觉方法估计当前帧相对于最近关键帧的位姿估计及其误差。

若误差小于第一设定阈值时，表示此时的位姿估计值更准确，用该位姿估计替换原来的当前帧位姿估计，将该估计误差替换原来的误差，并输出。

本发明实施例提供的一种基于RGBD摄像头和光编码器的三维环境重建方法，在关键帧初始化成功后，开始重建过程；在RGBD摄像头的输出上，用通用的视觉方法得到位姿估计及其误差；当误差过大时，用光编码器的输出作为位姿初始值，重新估计位姿及其误差；直到误差落入设定误差范围后，取新的位姿估计为输出。在生成关键帧时，若前一帧图像不可以作为关键帧，那么用前一帧和当前帧的光编码器输出求帧间位姿变化，得到当前帧的位姿估计。

本实施例中，通过利用视觉和光编码器估计相机位姿，可以减少环境光照变化对位姿估计的影响；当关键帧初始化失败时，利用光编码器给出位姿变化的估计，可以增强对缺少纹理的环境重建的鲁棒性。

基于同一发明构思，本发明实施例二还提供了一种基于RGBD摄像头和光编码器的三维环境重建***，该***与前述方法的原理相同，故该***的实施，可参照前述方法的流程实现，重复之处，不再冗述。

参见图6，本发明实施例二提供的一种基于RGBD摄像头和光编码器的三维环境重建***，包括选取模块100和重建模块200。

其中，选取模块100用于在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿信息和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化，得到当前帧的位姿估计；

重建模块200用于在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差。

本实施例提供的基于RGBD摄像头和光编码器的三维环境重建***，根据图像的边缘和特征点，选取关键帧。

其中，选取模块100包括第一计算单元和第二计算单元。第一计算单元用于在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿信息和当前帧的光编码器的位姿信息，计算当前帧和前一帧之间的帧间位姿变化信息；第二计算单元用于根据前一帧的位姿和帧间位姿变化信息，得到当前帧的位姿估计。

选取模块还包括第三计算单元；第三计算单元用于计算当前帧相对于最近关键帧的位姿及误差；若误差小于第二设定阈值时，将当前帧保存为前一帧；若误差大于第二设定阈值时，则标记初始化失败。

进一步地，本实施例提供的基于RGBD摄像头和光编码器的三维环境重建***，还包括初始化模块010；

初始化模块010用于初始化关键帧；若初始化成功，则根据当前帧和最近的关键帧，得到当前帧的位姿估计；若初始化失败，则判断前一帧是否可以作为关键帧，若前一帧可以作为关键帧，则将前一帧保存为新的关键帧，并将前一帧位姿初始化为单位阵，标记初始化成功。

重建模块200包括位姿估计单元、比较单元、第四计算单元和第五计算单元。位姿估计单元用于基于通用的视觉方法估计得到当前帧相对于最近关键帧的位姿及其误差；比较单元用于若该误差大于第一设定阈值，则根据前一帧光编码器的输出位姿和当前帧光编码器的输出位姿，得到帧间位姿；第四计算单元用于根据帧间位姿和前一帧的位姿，得到当前帧的位姿的初始估计；第五计算单元用于根据最近关键帧和当前帧的位姿的初始估计，得到当前帧的位姿。

本实施例提供的基于RGBD摄像头和光编码器的三维环境重建方法及***，在当环境光照或环境纹理变化，导致彩色图像降质或不可作为关键帧使用时，利用光编码器的输出来补偿相机的位姿估计，增加了重建过程对环境及其光照变化的鲁棒性，使该三维重建方法及***可以更广泛地使用。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (12)

1.一种基于RGBD摄像头和光编码器的三维环境重建方法，其特征在于，包括以下步骤：

在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化信息，得到当前帧的位姿估计；

在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差。

2.根据权利要求1所述的基于RGBD摄像头和光编码器的三维环境重建方法，其特征在于，根据图像的边缘和特征点，选取关键帧。

3.根据权利要求1所述的基于RGBD摄像头和光编码器的三维环境重建方法，其特征在于，在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化信息，得到当前帧的位姿估计，包括以下步骤：

在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿信息和当前帧的光编码器的位姿信息，计算当前帧和前一帧之间的帧间位姿变化信息；

根据所述前一帧的位姿和所述帧间位姿变化信息，得到当前帧的位姿估计。

4.根据权利要求1所述的基于RGBD摄像头和光编码器的三维环境重建方法，其特征在于，还包括以下步骤：

初始化关键帧；

若初始化成功，则根据当前帧和最近的关键帧，得到当前帧的位姿估计；

若初始化失败，则判断前一帧是否可以作为关键帧；

若前一帧可以作为关键帧，则将前一帧保存为新的关键帧，并将前一帧位姿初始化为单位阵，标记初始化成功。

5.根据权利要求3所述的基于RGBD摄像头和光编码器的三维环境重建方法，其特征在于，在根据当前帧和最近的关键帧，得到当前帧的位姿估计之后，还包括以下步骤：

计算当前帧相对于最近关键帧的位姿及误差；

若误差小于第二设定阈值时，将当前帧保存为前一帧；

若误差大于第二设定阈值时，则标记初始化失败。

6.根据权利要求1至5任一项所述的基于RGBD摄像头和光编码器的三维环境重建方法，其特征在于，在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差，包括以下步骤：

基于通用的视觉方法估计得到当前帧相对于最近关键帧的位姿及其误差；

若该误差大于第一设定阈值，则根据前一帧光编码器的输出位姿和当前帧光编码器的输出位姿，得到帧间位姿；

根据帧间位姿和前一帧的位姿，得到当前帧的位姿的初始估计；

根据最近关键帧和当前帧的位姿的初始估计，得到当前帧的位姿。

7.一种基于RGBD摄像头和光编码器的三维环境重建***，其特征在于，包括选取模块和重建模块；

所述选取模块，用于在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿信息和当前帧的光编码器的位姿计算当前帧和前一帧之间的帧间位姿变化，得到当前帧的位姿估计；

所述重建模块，用于在重建过程中，当基于视觉方法估计得到的当前帧相对于最近关键帧的位姿及其误差超过第一设定阈值时，以光编码器的输出位姿作为相机的初始位姿，重新估计当前帧相对于最近关键帧的位姿及其误差。

8.根据权利要求7所述的基于RGBD摄像头和光编码器的三维环境重建***，其特征在于，根据图像的边缘和特征点，选取关键帧。

9.根据权利要求7所述的基于RGBD摄像头和光编码器的三维环境重建***，其特征在于，所述选取模块包括第一计算单元和第二计算单元；

所述第一计算单元，用于在选取关键帧时，若前一帧不能作为关键帧，则利用前一帧的光编码器的位姿信息和当前帧的光编码器的位姿信息，计算当前帧和前一帧之间的帧间位姿变化信息；

所述第二计算单元，用于根据所述前一帧的位姿和所述帧间位姿变化信息，得到当前帧的位姿估计。

10.根据权利要求7所述的基于RGBD摄像头和光编码器的三维环境重建***，其特征在于，还包括初始化模块；

所述初始化模块，用于初始化关键帧；若初始化成功，则根据当前帧和最近的关键帧，得到当前帧的位姿估计；若初始化失败，则判断前一帧是否可以作为关键帧，若前一帧可以作为关键帧，则将前一帧保存为新的关键帧，并将前一帧位姿初始化为单位阵，标记初始化成功。

11.根据权利要求9所述的基于RGBD摄像头和光编码器的三维环境重建***，其特征在于，所述选取模块还包括第三计算单元；

所述第三计算单元用于计算当前帧相对于最近关键帧的位姿及误差；若误差小于第二设定阈值时，将当前帧保存为前一帧；若误差大于第二设定阈值时，则标记初始化失败。

12.根据权利要求7至11任一项所述的基于RGBD摄像头和光编码器的三维环境重建***，其特征在于，所述重建模块包括位姿估计单元、比较单元、第四计算单元和第五计算单元；

所述位姿估计单元，用于基于通用的视觉方法估计得到当前帧相对于最近关键帧的位姿及其误差；

所述比较单元，用于若该误差大于第一设定阈值，则根据前一帧光编码器的输出位姿和当前帧光编码器的输出位姿，得到帧间位姿；

所述第四计算单元，用于根据帧间位姿和前一帧的位姿，得到当前帧的位姿的初始估计；

所述第五计算单元，用于根据最近关键帧和当前帧的位姿的初始估计，得到当前帧的位姿。