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

权利要求书

1.一种双目视觉室内定位与建图方法，其特征在于，包括以下步骤：

步骤1、通过双目视觉传感器实时采集左右图像，并根据左右图像计算出相机的初始位姿；

步骤2、通过惯性测量单元实时采集角速度信息和加速度信息，并根据角速度信息和加速度信息预积分得到惯性测量单元状态量；

步骤3、构建一个包含若干图像帧的滑动窗口，以滑动窗口内三维点在当前图像帧下的投影位置与当前图像帧实际像素观测值之间产生的视觉误差项，和由图像帧与上一图像帧之间预积分项的值作为的测量值与估计值之间差值产生的惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，通过最小化误差得到优化后的相机位姿及惯性测量单元测量值；

步骤4、构建词袋模型进行回环检测，通过全局优化校正优化后的相机位姿，且根据校正后的相机位姿更新关键帧对应的地图点；

步骤5、提取步骤1采集的左右图像特征后转换为单词，并与离线地图的词袋进行匹配，如果词袋匹配成功，优化求解当前帧与闭环帧之间的相似变换矩阵，使得二维对应点的投影误差最小以得到优化后的相机位姿，获得重定位后的若干图像帧用于初始化惯性测量单元的零偏值；如果词袋匹配不成功，则通过双目视觉传感器重新采集左右图像并进行词袋匹配。

2.根据权利要求1所述双目视觉室内定位与建图方法，其特征在于，所述步骤1根据左右图像计算出相机的初始位姿，具体为：

对左右图像采用自适应直方图均衡化处理；

对构建的滑动窗口内左右图像分别提取特征点，为所述特征点找到对应双目匹配的特征点，并剔除误匹配点对；通过双目匹配获得特征点对应三维空间点的深度值，利用3D-2D点对之间的几何关系求解相机的初始位姿；

利用所述三维空间点和滑动窗口内其他特征点计算滑动窗口内其他图像帧位姿及特征点的三维坐标，以得到局部地图。

3.根据权利要求2所述双目视觉室内定位与建图方法，其特征在于：所述步骤1中为特征点找到对应双目匹配的特征点，包括：

通过光流法跟踪得到当前帧特征点在下一图像帧中的特征点位置，若跟踪到的特征点数量不满足阈值，则提取新的特征点。

4.根据权利要求1所述双目视觉室内定位与建图方法，其特征在于，所述步骤1中剔除误匹配点对，具体为：

在双目视觉左、右图像之间做双目匹配；

利用光流法对左图像上一帧特征点进行跟踪，得到左图像当前帧的特征点，并在两帧之间做RANSAC算法剔除误匹配；对右图像上一帧及当前帧直接做RANSAC算法剔除误匹配。

5.根据权利要求1所述双目视觉室内定位与建图方法，其特征在于，所述步骤2预积分得到的惯性测量单元状态量包括位移、速度、旋转和零偏值。

6.根据权利要求1所述双目视觉室内定位与建图方法，其特征在于，所述步骤3还包括判断最新获取的图像帧是否为关键帧，若是关键帧，则丢弃滑动窗口内时间戳最小的图像帧；若不是关键帧，则丢弃此图像帧且保留惯性测量单元测量值。

7.根据权利要求6所述双目视觉室内定位与建图方法，其特征在于，所述步骤3中判断最新获取的图像帧是否为关键帧的原则为：计算当前帧和上一关键帧之间跟踪点的平均视差，若超过设定阈值，则视当前帧为关键帧；或如果跟踪到的特征点数目超过设定阈值，则视该帧为关键帧。

8.一种双目视觉室内定位与建图装置，其特征在于，包括：

双目视觉传感器，用于实时采集左右图像；

惯性测量单元，用于实时采集角速度信息和加速度信息；

初始化模块，用于根据左右图像计算出相机的初始位姿，及根据角速度信息和加速度信息预积分得到惯性测量单元状态量；

优化模块，用于构建一个包含若干图像帧的滑动窗口，以滑动窗口内三维点在当前图像帧下的投影位置与当前图像帧实际像素观测值之间产生的视觉误差项，和由图像帧与上一图像帧之间预积分项的值作为的测量值与估计值之间差值产生的惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，通过最小化误差得到优化后的相机位姿及惯性测量单元测量值；

回环检测模块，用于构建词袋模型进行回环检测，通过全局优化校正优化后的相机位姿，且根据校正后的相机位姿更新关键帧对应的地图点；

重定位模块，用于提取采集的左右图像特征后转换为单词，并与离线地图的词袋进行匹配，如果词袋匹配成功，优化求解当前帧与闭环帧之间的相似变换矩阵，使得二维对应点的投影误差最小以得到优化后的相机位姿，获得重定位后的若干图像帧用于初始化惯性测量单元的零偏值；如果词袋匹配不成功，则通过双目视觉传感器重新采集左右图像并进行词袋匹配。