CN107808395B - 一种基于slam的室内定位方法 - Google Patents
一种基于slam的室内定位方法 Download PDFInfo
- Publication number
- CN107808395B CN107808395B CN201711050186.1A CN201711050186A CN107808395B CN 107808395 B CN107808395 B CN 107808395B CN 201711050186 A CN201711050186 A CN 201711050186A CN 107808395 B CN107808395 B CN 107808395B
- Authority
- CN
- China
- Prior art keywords
- input image
- skfs
- key frame
- points
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Image Analysis (AREA)
Abstract
本发明提供了一种基于SLAM的室内定位方法,包括如下步骤:加载SLAM地图得到关键帧集合KFS与3D点云,下采样关键帧集合KFS得到sKFS;提取输入图像的特征点,并对图像进行下采样得到缩略图;求出sKFS中每一个关键帧与输入图像之间的变化矩阵H;对齐每个sKFS与输入图像,算出最小误差ei,若最小误差ei小于阈值T,则认为输入图像与KFS中第i个关键帧Ki匹配;根据Ki的位姿Ei将3D点云映射到Ki中;根据变化矩阵H将Ki中的点映射到输入图像中,并在输入图像中查找与之匹配的特征点;根据匹配上的特征点,利用对极几何原理计算出输入图像的位姿E。本发明的室内定位方法在SLAM对室内已经建图的基础上,对设备进行定位,通过匹配特征点,优化相机位姿,得到更准确的结果。
Description
技术领域
本发明涉及一种室内定位方法,尤其是一种基于SLAM的室内定位方法。
背景技术
SLAM(simultaneous localization and mapping)分为两大功能:定位与建图。其中建图主要作用是对周边环境的理解,建立周边环境与空间的对应关系;定位主要是根据建好的图,判断设备在地图中的位姿,从而得到在环境中的信息。
常见的室内定位技术主要采用无线通讯、基站定位、惯导定位、蓝牙、wifi等多种技术集成一套定位体系。然而该类方法对环境的依赖比较大,比如:气温,距离等蓝牙影响较大,故在不同环境下都需要特定的方法重新矫正,而且往往需要部署多个设备;此外,该类方法只能得到远近信息,无法知道设备在空间中的姿态。
发明内容
本发明的目的在于:提供一种基于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中,映射公式为:
式中,Pi表示3D点云中的点,Ei为相机姿态,Ei·Pi表示将3D点投影到相机坐标系,CamProj(E·Pi)表示将相机坐标系中的点投影到Ki中;
步骤6,根据变化矩阵H将Ki中的点映射到输入图像中,并利用SSD算法在输入图像中查找与之匹配的特征点,若匹配上的特征点数量小于N,则重定位失败,否则重定位成功;
步骤7,根据匹配上的特征点,利用对极几何原理计算出输入图像的位姿E,E中包括R和T,R表示当前相机的旋转信息,T表示平移矩阵。
进一步地,对极几何原理计算出输入图像的R和T的具体公式为:
Pl=R(Pr-T)
(RTPr)T·T×Pl=0
式中,Pl和Pr为两幅图像中匹配上的特征点,RT为R的转置,×为叉乘。
本发明的有益效果在于:本发明在SLAM对室内已经建图的基础上,对设备进行定位,SLAM得到的地图主要包括3D点云和关键帧信息,一般根据相机获取的图像与关键帧对齐,从而得到相机的姿态,为了增加算法的实时性及鲁棒性,本发明先对图像与关键帧进行下采样,在低分辨率上实现对齐,估算出粗略位姿;然后再将3D点云映射到当前关键帧,匹配特征点,优化相机位姿,得到更准确的结果。
附图说明
图1为本发明的方法流程图。
具体实施方式
如图1所示,本发明公开的一种基于SLAM的室内定位方法,包括如下步骤:
步骤1,加载SLAM地图,得到关键帧集合KFS与3D点云,KFS中包含每一个关键帧的位姿Ei,下采样关键帧集合KFS得到sKFS,本实施例中下采样到40*30;
步骤2,利用FAST算法提取输入图像的角点作为特征点,并对输入图像进行下采样得到缩略图,本实施例中大小为40*30;
步骤3,用ESM-tracking算法求出sKFS中每一个关键帧与输入图像之间的变化矩阵H,H中包含了二维图像之间的旋转和平移变换信息;
步骤4,对齐每个sKFS与输入图像,算出最小误差ei,若最小误差ei小于阈值T,则认为输入图像与KFS中第i个关键帧Ki匹配,本实施例中T=100000,最小误差ei的计算公式为:
ei=I(PsKFS(i))-I(H·Pquery)
式中,PsKFS(i)表示sKFS中的第i帧,Pquery为输入图像的缩略图,I表示像素值;
步骤5,根据Ki的位姿Ei,将3D点云映射到Ki中,映射公式为:
式中,Pi表示3D点云中的点,Ei为相机姿态,Ei·Pi表示将3D点投影到相机坐标系,CamProj(E·Pi)表示将相机坐标系中的点投影到Ki中;
步骤6,根据变化矩阵H将Ki中的点映射到输入图像中,并利用SSD算法在输入图像中查找与之匹配的特征点,若匹配上的特征点数量小于N,则重定位失败,否则重定位成功,本实施例中N=50;
步骤7,根据匹配上的特征点,利用对极几何原理计算出输入图像的位姿E,E中包括R和T,R表示当前相机的旋转信息,T表示平移矩阵,其中,对极几何原理计算出输入图像的R和T的具体公式为:
Pl=R(Pr-T)
(RTPr)T·T×Pl=0
式中,Pl和Pr为两幅图像中匹配上的特征点,RT为R的转置,×为叉乘。
基于SLAM的定位***,可以得到设备在空间中6自由度的信息,比传统的室内定位技术用途更加广泛。因此本发明在SLAM对室内已经建图的基础上,对设备进行定位,SLAM得到的地图主要包括3D点云和关键帧信息,一般根据相机获取的图像与关键帧对齐,从而得到相机的姿态,为了增加算法的实时性及鲁棒性,本发明先对图像与关键帧进行下采样,在低分辨率上实现对齐,估算出粗略位姿;然后再将3D点云映射到当前关键帧,匹配特征点,优化相机位姿,得到更准确的结果。
Claims (2)
1.一种基于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中,映射公式为:
式中,Pi表示3D点云中的点,Ei为相机姿态,Ei·Pi表示将3D点投影到相机坐标系,CamProj(E·Pi)表示将相机坐标系中的点投影到Ki中;
步骤6,根据变化矩阵H将Ki中的点映射到输入图像中,并利用SSD算法在输入图像中查找与之匹配的特征点,若匹配上的特征点数量小于N,则重定位失败,否则重定位成功;
步骤7,根据匹配上的特征点,利用对极几何原理计算出输入图像的位姿E,E中包括R和T,R表示当前相机的旋转信息,T表示平移矩阵。
2.根据权利要求1所述的基于SLAM的室内定位方法,其特征在于,对极几何原理计算出输入图像的R和T的具体公式为:
Pl=R(Pr-T)
(RTPr)T·T×Pl=0
式中,Pl和Pr为两幅图像中匹配上的特征点,RT为R的转置,×为叉乘。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711050186.1A CN107808395B (zh) | 2017-10-31 | 2017-10-31 | 一种基于slam的室内定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711050186.1A CN107808395B (zh) | 2017-10-31 | 2017-10-31 | 一种基于slam的室内定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107808395A CN107808395A (zh) | 2018-03-16 |
CN107808395B true CN107808395B (zh) | 2020-12-04 |
Family
ID=61583170
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711050186.1A Active CN107808395B (zh) | 2017-10-31 | 2017-10-31 | 一种基于slam的室内定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107808395B (zh) |
Families Citing this family (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108648235B (zh) | 2018-04-27 | 2022-05-17 | 腾讯科技(深圳)有限公司 | 相机姿态追踪过程的重定位方法、装置及存储介质 |
CN109061703B (zh) * | 2018-06-11 | 2021-12-28 | 阿波罗智能技术(北京)有限公司 | 用于定位的方法、装置、设备和计算机可读存储介质 |
CN109146966B (zh) * | 2018-06-26 | 2021-02-23 | 广州视源电子科技股份有限公司 | 视觉slam前端处理方法、***、存储介质及计算机设备 |
CN109671119A (zh) * | 2018-11-07 | 2019-04-23 | 中国科学院光电研究院 | 一种基于slam的室内定位方法及装置 |
CN111209353B (zh) | 2018-11-21 | 2024-06-14 | 驭势科技(北京)有限公司 | 一种视觉定位地图加载方法、装置、***和存储介质 |
CN110296686B (zh) * | 2019-05-21 | 2021-11-09 | 北京百度网讯科技有限公司 | 基于视觉的定位方法、装置及设备 |
CN111311684B (zh) * | 2020-04-01 | 2021-02-05 | 亮风台(上海)信息科技有限公司 | 一种进行slam初始化的方法与设备 |
CN112004196B (zh) * | 2020-08-24 | 2021-10-29 | 唯羲科技有限公司 | 定位方法、装置、终端及计算机存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104732518A (zh) * | 2015-01-19 | 2015-06-24 | 北京工业大学 | 一种基于智能机器人地面特征的ptam改进方法 |
CN106092104A (zh) * | 2016-08-26 | 2016-11-09 | 深圳微服机器人科技有限公司 | 一种室内机器人的重定位方法及装置 |
US20160379375A1 (en) * | 2014-03-14 | 2016-12-29 | Huawei Technologies Co., Ltd. | Camera Tracking Method and Apparatus |
CN106384383A (zh) * | 2016-09-08 | 2017-02-08 | 哈尔滨工程大学 | 一种基于fast和freak特征匹配算法的rgb‑d和slam场景重建方法 |
CN106934827A (zh) * | 2015-12-31 | 2017-07-07 | 杭州华为数字技术有限公司 | 三维场景的重建方法和装置 |
-
2017
- 2017-10-31 CN CN201711050186.1A patent/CN107808395B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160379375A1 (en) * | 2014-03-14 | 2016-12-29 | Huawei Technologies Co., Ltd. | Camera Tracking Method and Apparatus |
CN104732518A (zh) * | 2015-01-19 | 2015-06-24 | 北京工业大学 | 一种基于智能机器人地面特征的ptam改进方法 |
CN106934827A (zh) * | 2015-12-31 | 2017-07-07 | 杭州华为数字技术有限公司 | 三维场景的重建方法和装置 |
CN106092104A (zh) * | 2016-08-26 | 2016-11-09 | 深圳微服机器人科技有限公司 | 一种室内机器人的重定位方法及装置 |
CN106384383A (zh) * | 2016-09-08 | 2017-02-08 | 哈尔滨工程大学 | 一种基于fast和freak特征匹配算法的rgb‑d和slam场景重建方法 |
Also Published As
Publication number | Publication date |
---|---|
CN107808395A (zh) | 2018-03-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107808395B (zh) | 一种基于slam的室内定位方法 | |
US9098229B2 (en) | Single image pose estimation of image capture devices | |
WO2021057742A1 (zh) | 定位方法及装置、设备、存储介质 | |
US9154919B2 (en) | Localization systems and methods | |
CN109544636A (zh) | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 | |
CN103900583A (zh) | 用于即时定位与地图构建的设备和方法 | |
CN110986969B (zh) | 地图融合方法及装置、设备、存储介质 | |
CN112556685B (zh) | 导航路线的显示方法、装置和存储介质及电子设备 | |
CN113048980B (zh) | 位姿优化方法、装置、电子设备及存储介质 | |
CN103810286A (zh) | 一种用于二维地图与三维地图匹配的坐标点定位方法 | |
KR101885961B1 (ko) | 이미지를 기반으로 한 객체 위치 추정 방법 및 장치 | |
WO2022183638A1 (zh) | 图像特征匹配方法和相关装置、设备及存储介质 | |
WO2022247548A1 (zh) | 定位方法和装置、电子设备及存储介质 | |
WO2022247286A1 (zh) | 定位方法和装置、设备及存储介质 | |
TW201839356A (zh) | 追蹤系統及方法 | |
WO2023005457A1 (zh) | 位姿计算方法和装置、电子设备、可读存储介质 | |
CN101865656B (zh) | 一种使用少数共面点精确定位多摄像头***位姿的方法 | |
JP2021103168A (ja) | 拡張現実装置及び位置決め方法 | |
CN107958491B (zh) | 移动增强现实虚拟坐标与施工现场坐标匹配方法 | |
CN113326769A (zh) | 高精地图生成方法、装置、设备及存储介质 | |
CN113628284B (zh) | 位姿标定数据集生成方法、装置、***、电子设备及介质 | |
CN115468568A (zh) | 室内导航方法、装置及***、服务器设备及存储介质 | |
JP2018194366A (ja) | 位置推定装置、方法、及びプログラム | |
Feng et al. | Visual location recognition using smartphone sensors for indoor environment | |
US20240029350A1 (en) | Computing apparatus and model generation method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |