[发明专利]一种双目视觉室内定位与建图方法及装置

摘要

本发明公开了一种双目视觉室内定位与建图方法及装置，包括：实时采集左右图像，并计算出相机的初始位姿；实时采集角速度信息和加速度信息，并预积分得到惯性测量单元状态量；构建一个包含若干图像帧的滑动窗口，以图像帧与帧之间的视觉误差项和惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，得到优化后的相机位姿及惯性测量单元测量值；构建词袋模型进行回环检测，校正优化后的相机位姿；提取左右图像特征后转换为单词并与离线地图的词袋进行匹配，成功则优化求解得到优化后的相机位姿，否则重新采集左右图像并进行词袋匹配。本发明可在未知环境中实现定位与建图，以及在已建图场景中的定位功能，具有较好的精度和鲁棒性。

主权项

1.一种双目视觉室内定位与建图方法，其特征在于，包括以下步骤：步骤1、通过双目视觉传感器实时采集左右图像，并根据左右图像计算出相机的初始位姿；步骤2、通过惯性测量单元实时采集角速度信息和加速度信息，并根据角速度信息和加速度信息预积分得到惯性测量单元状态量；步骤3、构建一个包含若干图像帧的滑动窗口，以滑动窗口内三维点在当前图像帧下的投影位置与当前图像帧实际像素观测值之间产生的视觉误差项，和由图像帧与上一图像帧之间预积分项的值作为的测量值与估计值之间差值产生的惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，通过最小化误差得到优化后的相机位姿及惯性测量单元测量值；步骤4、构建词袋模型进行回环检测，通过全局优化校正优化后的相机位姿，且根据校正后的相机位姿更新关键帧对应的地图点；步骤5、提取步骤1采集的左右图像特征后转换为单词，并与离线地图的词袋进行匹配，如果词袋匹配成功，优化求解当前帧与闭环帧之间的相似变换矩阵，使得二维对应点的投影误差最小以得到优化后的相机位姿，获得重定位后的若干图像帧用于初始化惯性测量单元的零偏值；如果词袋匹配不成功，则通过双目视觉传感器重新采集左右图像并进行词袋匹配。