[发明专利]基于RGBD摄像头和光编码器的三维环境重建方法及系统

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

初始化关键帧；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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