[发明专利]一种基于SLAM的室内定位方法

摘要

本发明提供了一种基于SLAM的室内定位方法，包括如下步骤：加载SLAM地图得到关键帧集合KFS与3D点云，下采样关键帧集合KFS得到sKFS；提取输入图像的特征点，并对图像进行下采样得到缩略图；求出sKFS中每一个关键帧与输入图像之间的变化矩阵H；对齐每个sKFS与输入图像，算出最小误差ei，若最小误差ei小于阈值T，则认为输入图像与KFS中第i个关键帧Ki匹配；根据Ki的位姿Ei将3D点云映射到Ki中；根据变化矩阵H将Ki中的点映射到输入图像中，并在输入图像中查找与之匹配的特征点；根据匹配上的特征点，利用对极几何原理计算出输入图像的位姿E。本发明的室内定位方法在SLAM对室内已经建图的基础上，对设备进行定位，通过匹配特征点，优化相机位姿，得到更准确的结果。

主权项

一种基于SLAM的室内定位方法，其特征在于，包括如下步骤：步骤1，加载SLAM地图，得到关键帧集合KFS与3D点云，KFS中包含每一个关键帧的位姿Ei，下采样关键帧集合KFS得到sKFS；步骤2，利用FAST算法提取输入图像的角点作为特征点，并对输入图像进行下采样得到缩略图；步骤3，用ESM‑tracking算法求出sKFS中每一个关键帧与输入图像之间的变化矩阵H，H中包含了二维图像之间的旋转和平移变换信息；步骤4，对齐每个sKFS与输入图像，算出最小误差ei，若最小误差ei小于阈值T，则认为输入图像与KFS中第i个关键帧Ki匹配，最小误差ei的计算公式为：ei＝I(PsKFS(i))‑I(H·Pquery)式中，PsKFS(i)表示sKFS中的第i帧，Pquery为输入图像的缩略图，I表示像素值；步骤5，根据Ki的位姿Ei，将3D点云映射到Ki中，映射公式为：ukvk=CamProj(Ei·Pi)]]>式中，Pi表示3D点云中的点，Ei为相机姿态，Ei·Pi表示将3D点投影到相机坐标系，CamProj(E·Pi)表示将相机坐标系中的点投影到Ki中；步骤6，根据变化矩阵H将Ki中的点映射到输入图像中，并利用SSD算法在输入图像中查找与之匹配的特征点，若匹配上的特征点数量小于N，则重定位失败，否则重定位成功；步骤7，根据匹配上的特征点，利用对极几何原理计算出输入图像的位姿E，E中包括R和T，R表示当前相机的旋转信息，T表示平移矩阵。