CN105225240B - 一种基于视觉特征匹配与拍摄角度估计的室内定位方法 - Google Patents

一种基于视觉特征匹配与拍摄角度估计的室内定位方法 Download PDF

Info

Publication number
CN105225240B
CN105225240B CN201510622949.XA CN201510622949A CN105225240B CN 105225240 B CN105225240 B CN 105225240B CN 201510622949 A CN201510622949 A CN 201510622949A CN 105225240 B CN105225240 B CN 105225240B
Authority
CN
China
Prior art keywords
msub
mrow
mtd
mtr
matrix
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
Application number
CN201510622949.XA
Other languages
English (en)
Other versions
CN105225240A (zh
Inventor
谭学治
冯冠元
马琳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Xinzhida Automation Complete Equipment Co ltd
Original Assignee
Harbin Institute of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201510622949.XA priority Critical patent/CN105225240B/zh
Publication of CN105225240A publication Critical patent/CN105225240A/zh
Application granted granted Critical
Publication of CN105225240B publication Critical patent/CN105225240B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Magnetic Resonance Imaging Apparatus (AREA)
  • Image Analysis (AREA)

Abstract

一种基于视觉特征匹配与拍摄角度估计的室内定位方法，本发明涉及基于视觉特征匹配与拍摄角度估计的室内定位方法。本发明的目的是为了解决现有室内定位方法均需要在前期进行大量的设备和基础设施投入，而且对定位场景的要求也较为严格的问题。通过以下技术方案实现的：一构建室内场景中的视觉数据库，该视觉数据库中包含室视觉特征在室内坐标系下的位置坐标以及视觉特征在室内采集过程中的摄像机坐标系；二：根据步骤一中得到的室内场景中的视觉数据库，求解与查询图像P1 Q和P2 Q匹配率最大的数据库图像；三、计算查询图像P1 Q对应的旋转矩阵R1和查询图像P2 Q对应的旋转矩阵R2；四：根据R1和R2计算查询图像摄像机的位置。本发明应用于计算机视觉和图像处理领域。

Description

一种基于视觉特征匹配与拍摄角度估计的室内定位方法
技术领域
本发明涉及基于视觉特征匹配与拍摄角度估计的室内定位方法。
背景技术
随着信息技术的不断发展，基于信息技术手段的位置服务逐渐受到越来越多研究人员和科研机构的关注。目前，基于位置服务的各种应用已逐渐渗透到生活中的各个方面。例如，为出行提供方便的电子地图，为驾驶提供帮助的导航系统等等。这些应用主要基于全球卫星定位系统。GPS系统、GLONASS导航系统、伽利略导航系统和北斗导航系统是目前使用较为广泛的卫星定位系统。但是，全球卫星定位系统仅能够为室外场景中的用户提供位置信息服务。由于室内环境因素的影响，卫星定位信号无法直接满足室内位置服务的需求。因此，不依赖于卫星信号的室内定位技术受到了国内外学者的广泛关注。
基于WIFI、UWB和RFID技术的室内定位方法是目前室内定位领域的研究热点。但是，基于以上技术的室内定位方法均需要在前期进行大量的设备和基础设施投入，而且对定位场景的要求也较为严格。
发明内容
本发明的目的是为了解决现有室内定位方法均需要在前期进行大量的设备和基础设施投入，而且对定位场景的要求也较为严格的问题，而提出了一种基于视觉特征匹配与拍摄角度估计的室内定位方法。
上述的发明目的是通过以下技术方案实现的：
步骤一：构建室内场景中的视觉数据库，该视觉数据库中包含室内视觉特征、视觉特征在室内坐标系下的位置坐标以及视觉特征在采集过程中的摄像机坐标系位置；
步骤二：根据步骤一中得到的室内场景中的视觉数据库，求解与查询图像P1 Q和P2 Q匹配率最大的数据库图像；
步骤三：计算查询图像P1 Q对应的旋转矩阵R1和查询图像P2 Q对应的旋转矩阵R2；
步骤四：根据R1和R2计算查询图像摄像机的位置。
发明效果
采用本发明的一种基于视觉特征匹配与拍摄角度估计的室内定位方法，该方法通过查询视觉特征的位置坐标以及数据库图像的拍摄角度，对查询图像的拍摄角度进行估计，实现查询图像的定位功能。本专利中所提算法通过视觉特征匹配和拍摄角度估计实现用户位置的确定，不需要在前期进行大量的设备和基础设施投入，而且对定位场景的要求相对宽松，解决了现有室内定位方法均需要在前期进行大量的设备和基础设施投入，而且对定位场景的要求也较为严格的问题。本专利所提算法，在已有的基于图像检索的视觉定位算法的基础上，引入了摄像机拍摄角度估计算法，有效提高了视觉定位精度。与单纯依靠图像特征匹配的视觉定位算法相比，本专利中所提算法具有更高的定位精度。与已有的基于图像检索的室内定位技术相比，本发明中所述的室内定位技术在定位精度方面提高了20％。
附图说明
图1为本发明流程图；
图2为具体实施方式一步骤一中流程图；
图3为具体实施方式一步骤二中流程图；
图4为具体实施方式一步骤三中流程图；
图5为具体实施方式一步骤四中流程图。
具体实施方式
具体实施方式一：结合图1说明本实施方式，一种基于视觉特征匹配与拍摄角度估计的室内定位方法，其特征在于，一种基于视觉特征匹配与拍摄角度估计的室内定位方法具体是按以下步骤进行的：
步骤一：构建室内场景中的视觉数据库，该视觉数据库中包含室内视觉特征、视觉特征在室内坐标系下的位置坐标以及视觉特征在采集过程中的摄像机坐标系位置；如图2；
步骤二：根据步骤一中得到的室内场景中的视觉数据库，求解与查询图像P1 Q和P2 Q匹配率最大的数据库图像；如图3；
步骤三：计算查询图像P1 Q对应的旋转矩阵R1和查询图像P2 Q对应的旋转矩阵R2；如图4；
步骤四：根据R1和R2计算查询图像摄像机的位置；如图5。
具体实施方式二：本实施方式与具体实施方式一不同的是，所述步骤一中构建室内场景中的视觉数据库，该视觉数据库中包含室内视觉特征、视觉特征在室内坐标系下的位置坐标以及视觉特征在采集过程中的摄像机坐标系位置；具体过程为：
步骤一一：定义室内坐标系；
在室内场景中，定义一个三维直角正交坐标系ODXDYDZD，其中，ZD轴的方向为正北方向，XD轴的方向为正东方向，YD轴的方向为垂直于室内地平面向下，OD为三维直角正交坐标系ODXDYDZD的原点；
步骤一二：定义摄像机坐标系和图像坐标系；
定义摄像机坐标系为三维直角正交坐标系OCXCYCZC，其中，ZC轴与摄像机光轴重合，OC为坐标系原点，XC轴与YC分别垂直于ZC轴，且坐标系OCXCYCZC满足右手准则；
定义图像坐标系为二维直角坐标系OPXPYP，在图像坐标系中，定义图像左上角为图像坐标系的原点OP，图像坐标系的XP轴与YP轴分别与摄像机坐标系的XC和YC轴平行；
步骤一三：地平面网格划分；
在室内地平面ODXDZD上，以0.5米为一个长度单位，每隔一个长度单位，分别作平行于XD和ZD轴的直线，在整个地平面范围内，得到N个0.5米×0.5米的正方形网格区域，N为正整数；
其中，室内地平面ODXDZD为三维直角正交坐标系ODXDYDZD中的一个平面；
步骤一四：视觉特征采集；
在每个正方形网格区域的4个顶点处，利用CCD摄像机对室内固定视觉特征进行采集，并将该图像作为数据库图像；在每个图像采集点，以XD轴方向为正方向，摄像机以光心为圆心绕YD轴旋转360°，每隔15°采集一幅图像，记为Pi D，其中，上标D表示该图像为数据库图像，下标i为数据库图像序号，i取值范围为i＝1,2,…,datanum，datanum为数据库图像总数，datanum为正整数，在图像采集过程中，CCD摄像机坐标系中的YCOCZC平面与地平面平行，在图像采集的同时，记录数据库图像摄像机坐标系与室内坐标系之间的旋转关系；在采集每一幅数据库图像后，需要对图像所包含的视觉特征的位置坐标进行记录，视觉特征的位置坐标是视觉特征在室内坐标系下的位置坐标值。
其它步骤及参数与具体实施方式一相同。
具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步骤二中根据步骤一中得到的室内场景中的视觉数据库，求解与查询图像P1 Q和P2 Q匹配率最大的数据库图像；具体过程为：
步骤二一：查询图像与数据库图像的SURF特征提取；
SURF算法为加速鲁棒特征算法，全拼为Speeded up Robust Features；在本步骤中，SURF算法的输入是数据库图像，SURF算法输出是数据库图像的特征向量；待定位用户利用相机在同一位置进行两次图像采集，可以得到两张包含有不同视觉特征的查询图像，记为Pk Q，上标Q表示该图像为查询图像，下标k表示查询图像编号，其中k＝1,2，分别对查询图像P1 Q和P2 Q进行SURF特征提取，可以得到查询图像P1 Q的特征以及查询图像P2 Q的特征，用l1和l2分别表示查询图像P1 Q和P2 Q的SURF特征向量数目，l1为正整数，l2为正整数；分别对数据库图像Pi D进行SURF特征提取，可以得到数据库图像Pi D的SURF特征，上标D表示该图像为数据库图像，下标i为数据库图像序号，i取值范围为i＝1,2,…,datanum，datanum为数据库图像总数，datanum为正整数；
步骤二二：SURF特征匹配；
根据SURF特征匹配算法，利用查询图像P1 Q的特征分别与数据库图像Pi D的特征进行SURF特征匹配，下标i为数据库图像序号，其中，i＝1,2,…,datanum，datanum为数据库图像总数，datanum为正整数，由此可得datanum个匹配结果，每个匹配结果中所包含的匹配特征点数量记为Ni Q1，特征点在图像中的位置坐标记为其中Q1表示该特征对应于查询图像P1 Q；
利用查询图像P2 Q的特征分别与数据库图像Pi D的特征进行SURF特征匹配后，得到datanum个匹配结果，每个匹配结果中所包含的匹配特征点数量记为Ni Q2，特征点在图像中的位置坐标记为其中Q2表示该特征对应于查询图像P2 Q；
步骤二三：选取与查询图像匹配率最大的数据库图像：
由式(1)计算查询图像P1 Q与每一幅数据库图像的匹配率Ri Q1：
由式(1)得到datanum个匹配率，取这datanum个匹配率中的最大值，并记录其对应的数据库图像，将与查询图像P1 Q匹配率最大的数据库图像记为将数据库图像的视觉特征的位置坐标向量记为其中，m1表示数据库图像编号，xm1表示在室内坐标系中视觉特征在XD方向上的坐标值，ym1表示在室内坐标系中视觉特征在YD方向上的坐标值，zm1表示在室内坐标系中视觉特征在ZD方向上的坐标值；
由式(2)计算查询图像P2 Q与每一幅数据库图像的匹配率Ri Q2：
由式(2)可以得到datanum个匹配率，取这datanum个匹配率中的最大值，并记录其对应的数据库图像，将与查询图像P2 Q匹配率最大的数据库图像记为将数据库图像的视觉特征的位置坐标向量记为其中，m2表示数据库图像编号，xm2表示在室内坐标系中视觉特征在XD方向上的坐标值，ym2表示在室内坐标系中视觉特征在YD方向上的坐标值，zm2表示在室内坐标系中视觉特征在ZD方向上的坐标值。
其它步骤及参数与具体实施方式一或二相同。
具体实施方式四：本实施方式与具体实施方式一、二或三不同的是，所述步骤三中计算查询图像P1 Q对应的旋转矩阵R1和查询图像P2 Q对应的旋转矩阵R2；具体过程为：
步骤三一：特征点选取：
在数据库图像的视觉特征中随机选择8个视觉特征，并将数据库图像中的视觉特征与对应的查询图像视觉特征的位置坐标用一个矩阵表示，记为S1：
其中，元素xj1表示查询图像P1 Q坐标系中特征点的横坐标，j＝1,2,…,8；元素yj1表示查询图像P1 Q坐标系中特征点的纵坐标，j＝1,2,…,8；元素xj2表示数据库图像坐标系中特征点的横坐标，j＝1,2,…,8；元素yj2表示数据库图像坐标系中特征点的纵坐标，j＝1,2,…,8，T为转置运算符；
步骤三二：计算基础矩阵F1：
对于查询图像P1 Q的任意特征点，在数据库图像上对应着一条极线，而与查询图像特征点相匹配的数据库图像特征点位于极线之上，因此，存在一个从查询图像特征点到数据库图像对应极线的映射，基础矩阵F1所表示的就是极线与特征点之间映射关系；
基础矩阵F1是一个秩为2的3阶矩阵，通过8个与查询图像特征点相匹配的数据库图像特征点，可以求得基础矩阵F1，设基础矩阵F1如公式(4)所示：
每一组与查询图像相匹配的特征点对可以得到一个关于基础矩阵F1的约束方程，8个与查询图像相匹配的特征点对可以得到8个关于基础矩阵F1的约束方程；
x11x12f11+x12y11f12+x12f13+y12x11f21+y11y12f22+y12f23+x11f31+y11f32+f33＝0 (5)
x21x22f11+x22y21f12+x22f13+y22x21f21+y21y22f22+y22f23+x21f31+y21f32+f33＝0 (6)
x31x32f11+x32y31f12+x32f13+y32x31f21+y31y32f22+y32f23+x31f31+y31f32+f33＝0 (7)
x41x42f11+x42y41f12+x42f13+y42x41f21+y41y42f22+y42f23+x41f31+y41f32+f33＝0 (8)
x51x52f11+x52y51f12+x52f13+y52x51f21+y51y52f22+y52f23+x51f31+y51f32+f33＝0 (9)
x61x62f11+x62y61f12+x62f13+y62x61f21+y61y62f22+y62f23+x61f31+y61f32+f33＝0 (10)
x71x72f11+x72y71f12+x72f13+y72x71f21+y71y72f22+y72f23+x71f31+y71f32+f33＝0 (11)
x81x82f11+x82y81f12+x82f13+y82x81f21+y81y82f22+y82f23+x81f31+y81f32+f33＝0 (12)
通过式(5)至式(12)，根据线性方程组求解方法解出f11、f12、f13、f21、f22、f23、f31、f32、f33；
其中，f11是F1的第一行第一列元素，f12是F1的第一行第二列元素，f13是F1的第一行第三列元素，f21是F1的第二行第一列元素，f22是F1的第二行第二列元素，f23是F1的第二行第三列元素，f31是F1的第三行第一列元素，f32是F1的第三行第二列元素，f33是F1的第三行第三列元素；
解出f11、f12、f13、f21、f22、f23、f31、f32、f33即可求得基础矩阵F1；
步骤三三：计算本质矩阵E1：
本质矩阵E1可由式(13)计算得出：
Ε1＝CQ TF1CD (13)
式中，F1为基础矩阵，T为转置运算符，CD为数据库图像相机的内部参数矩阵，为已知的矩阵；CQ为查询图像相机的内部参数矩阵，为已知的矩阵；
步骤三四：计算查询图像P1 Q对应的旋转矩阵R1；
步骤三三中计算得到的本质矩阵E1包含了数据库图像与查询图像之间的旋转关系和平移关系，如式(14)所示，其中T1表示平移向量，R1表示查询图像P1 Q对应的旋转矩阵；
E1＝[T1]×R1 (14)
式中，[]×表示矩阵的反对称运算，反对称运算如公式(15)所示，式中对任意一个1×3维矩阵X＝[x1,x2,x3]T做反对称运算：
式中，x1为X的第一行第一列元素，x2为X的第二行第一列元素，x3为X的第三行第一列元素；
由公式(14)求得的本质矩阵E1是一个3×3维的矩阵，将本质矩阵表示成一个由变量eij组成的矩阵，如式(16)所示，其中i＝1,2,3，j＝1,2,3：
式中，e11为本质矩阵E1的第一行第一列元素，e12为本质矩阵E1的第一行第二列元素，e13为本质矩阵E1的第一行第三列元素，e21为本质矩阵E1的第二行第一列元素，e22为本质矩阵E1的第二行第二列元素，e23为本质矩阵E1的第二行第三列元素，e31为本质矩阵E1的第三行第一列元素，e32为本质矩阵E1的第三行第二列元素，e33为本质矩阵E1的第三行第三列元素；
令E11＝[e11,e21,e31]T，E12＝[e12,e22,e32]T，E13＝[e13,e23,e33]T；
式中，E11为本质矩阵E1的第一列列向量，E12为本质矩阵E1的第二列列向量，E13为本质矩阵E1的第三列列向量；
分别求解E11×E12，E12×E13，E13×E11，其中符号×表示向量的内积运算，由此可得三个内积运算结果中的最大值，设内积E11×E12是三个内积中的最大值；
根据公式(17)和公式(18)，分别计算出矩阵A＝[a1,a2,a3]和B＝[b1,b2,b3]；
其中，W1＝E11×E12，W2＝E11，V1＝E1a1，V2＝E1a2；
构造矩阵C0如式(19)所示：
其中，矩阵A＝[a1,a2,a3]是通过本质矩阵求得的矩阵一，矩阵B＝[b1,b2,b3]为通过本质矩阵求得的矩阵二，矩阵为构造矩阵；
旋转矩阵的两种形式如公式(20)所示：
式中，R11为旋转矩阵的形式一，R12为旋转矩阵的形式二；
设平移向量T1如公式(21)所示：
T1＝[b13,b23,b33] (21)
其中，b13表示矩阵B中位于第一行第三列的元素，b23表示矩阵B中位于第二行第三列的元素，b33表示矩阵B中位于第三行第三列的元素；
构造四个包含平移和旋转平移关系的矩阵G1，G2，G3和G4，如式(22)至(25)所示：
设向量I＝[1,1,1,1]T，并计算K1＝G1I，K2＝G2I，K3＝G3I，K4＝G4I；
当Kt满足公式(26)时，t＝1,2,3,4，取Kt对应的旋转矩阵作为最终的查询图像P1 Q对应的旋转矩阵R1；
式中，Kt(3,1)为在向量Kt中，位置位于第三行第一列的元素，Kt(4,1)为在向量Kt中，位置位于第四行第一列的元素；
步骤三五：重复步骤三一至步骤三四，计算出查询图像P2 Q对应的旋转矩阵R2。
其它步骤及参数与具体实施方式一、二或三相同。
具体实施方式五：本实施方式与具体实施方式一、二、三或四不同的是，所述步骤四中根据R1和R2计算查询图像摄像机的位置；具体过程为：
步骤四一：计算查询图像P1 Q摄像机坐标系与室内坐标系之间的旋转角度α1：
根据查询图像P1 Q对应的旋转矩阵R1，由式(27)得到查询图像摄像机坐标系与室内坐标系之间的旋转矩阵RD1：
RD1＝R1RC1 (27)
式中，矩阵RC1为数据库图像摄像机坐标系与室内坐标系之间的旋转关系，RD1为查询图像P1 Q摄像机坐标系与室内坐标系之间的旋转矩阵，矩阵RD1中位于第一行第一列的元素表示为式(28)：
式中，α1为摄像机坐标系的ZC轴与室内坐标系ZD轴之间的旋转角度，通过计算出角度α1，如式(29)所示：
步骤四二：计算查询图像P2 Q摄像机坐标系与室内坐标系之间的旋转角度α2：
根据查询图像P2 Q对应的旋转矩阵R2，得到查询图像摄像机坐标系与室内坐标系之间的旋转矩阵RD2：
RD2＝R2RC2 (30)
式中，矩阵RC2为数据库图像摄像机坐标系与室内坐标系之间的旋转关系，RD2为查询图像P2 Q摄像机坐标系与室内坐标系之间的旋转矩阵，矩阵RD2中位于第一行第一列的元素表示为：
式中，α2为摄像机坐标系的ZC轴与室内坐标系ZD轴之间的旋转角度，通过计算出角度α2，如式所示：
步骤四三：计算视觉特征的投影点坐标：
在室内坐标系中，将位于位置的视觉特征投影到XDODZD平面上，记为特征投影位置点Qt1，可得该视觉特征在XDODZD平面上的投影点坐标Pt1＝[xt1,zt1]，
式中，xt1为投影点坐标Pt1的X轴坐标值，zt1为投影点坐标Pt1的Z轴坐标值，xm1为位置的X轴坐标值，zm1为位置的Z轴坐标值；
在室内坐标系中，将位于位置的视觉特征投影到XDODZD平面上，记为特征投影位置点Qt2，可得该视觉特征在XDODZD平面上的投影点坐标Pt2＝[xt2,zt2]；
式中，xt2为投影点坐标Pt2的X轴坐标值，zt2为为投影点坐标Pt2的Z轴坐标值，xm2为位置的X轴坐标值，zm2为位置的Z轴坐标值；
步骤四四：计算查询图像摄像机的位置：
设查询图像摄像机在XDODZD平面上的投影为点Q，则点Q与特征投影位置点Qt1的连线lt1的斜率为投影为点Q与特征投影位置点Qt2的连线lt2的斜率为设投影为点Q的位置坐标为PQ＝[xQ,zQ]，连线lt1的直线方程可以表示为式(35)：
式中，zQ为投影点Q的Z轴坐标值，zt1为投影点坐标Pt1的Z轴坐标值，xQ为投影点Q的X轴坐标值，xt1为投影点坐标Pt1的X轴坐标值；
连线lt2的直线方程可以表示为式(36)：
式中，zt2为投影点坐标Pt2的Z轴坐标值，xt2为投影点坐标Pt2的X轴坐标值；
联立公式(35)与公式(36)求得投影点Q的位置坐标[xQ,zQ]，投影点Q位于地平面上，该点在地平面XDODZD上的坐标即为查询图像摄像机的位置(在本专利中，认为查询图像摄像机的位置即为用户的定位点)。
其它步骤及参数与具体实施方式一、二、三或四相同。

Claims (3)

1.一种基于视觉特征匹配与拍摄角度估计的室内定位方法，其特征在于，一种基于视觉特征匹配与拍摄角度估计的室内定位方法具体是按照以下步骤进行的：
步骤一：构建室内场景中的视觉数据库，该视觉数据库中包含视觉特征、视觉特征在室内坐标系下的位置坐标以及视觉特征在采集过程中的摄像机坐标系位置；具体过程为：
步骤一一：定义室内坐标系；
在室内场景中，定义一个三维直角正交坐标系ODXDYDZD，其中，ZD轴的方向为正北方向，XD轴的方向为正东方向，YD轴的方向为垂直于室内地平面向下，OD为三维直角正交坐标系ODXDYDZD的原点；
步骤一二：定义摄像机坐标系和图像坐标系；
定义摄像机坐标系为三维直角正交坐标系OCXCYCZC，其中，ZC轴与摄像机光轴重合，OC为坐标系原点，XC轴与YC分别垂直于ZC轴，且坐标系OCXCYCZC满足右手准则；
定义图像坐标系为二维直角坐标系OPXPYP，在图像坐标系中，OP为图像坐标系原点，图像坐标系的XP轴与YP轴分别与摄像机坐标系的XC和YC轴平行；
步骤一三：地平面网格划分；
在室内地平面ODXDZD上，以0.5米为一个长度单位，每隔一个长度单位，分别作平行于XD和ZD轴的直线，得到N个0.5米×0.5米的正方形网格区域，N为正整数；
其中，室内地平面ODXDZD为三维直角正交坐标系ODXDYDZD中的一个平面；
步骤一四：视觉特征采集；
在每个正方形网格区域的4个顶点处，利用CCD摄像机对室内固定视觉特征进行采集，并将该图像作为数据库图像；在每个图像采集点，以XD轴方向为正方向，摄像机以光心为圆心绕YD轴旋转360°，每隔15°采集一幅图像，记为Pi D，其中，上标D表示该图像为数据库图像，下标i为数据库图像序号，i取值范围为i＝1,2,…,datanum，datanum为数据库图像总数，datanum为正整数，在图像采集过程中，CCD摄像机坐标系中的YCOCZC平面与地平面平行，在图像采集的同时，记录数据库图像摄像机坐标系与室内坐标系之间的旋转关系；在采集每一幅数据库图像后，需要对图像所包含的视觉特征的位置坐标进行记录，视觉特征的位置坐标是视觉特征在室内坐标系下的位置坐标值；
步骤二：根据步骤一中得到的室内场景中的视觉数据库，求解与查询图像P1 Q和匹配率最大的数据库图像；具体过程为：
步骤二一：查询图像与数据库图像的SURF特征提取；
待定位用户利用摄像机在同一位置进行两次图像采集，可以得到两张包含有不同视觉特征的查询图像，记为上标Q表示该图像为查询图像，下标k表示查询图像编号，其中k＝1,2，分别对查询图像P1 Q和进行SURF特征提取，可以得到查询图像P1 Q的特征以及查询图像的特征，用l1和l2分别表示查询图像P1 Q和的SURF特征向量数目，l1为正整数，l2为正整数，分别对数据库图像Pi D进行SURF特征提取，可以得到数据库图像Pi D的特征，上标D表示该图像为数据库图像，下标i为数据库图像序号，i取值范围为i＝1,2,…,datanum，datanum为数据库图像总数，datanum为正整数；
步骤二二：SURF特征匹配；
根据SURF特征匹配算法，利用查询图像P1 Q的特征分别与数据库图像Pi D的特征进行SURF特征匹配，下标i为数据库图像序号，其中，i＝1,2,…,datanum，datanum为数据库图像总数，datanum为正整数，由此可得datanum个匹配结果，每个匹配结果中所包含的匹配特征点数量记为特征点的位置坐标记为其中Q1表示该特征对应于查询图像P1 Q；
利用查询图像的特征分别与数据库图像Pi D的特征进行SURF特征匹配后，得到datanum个匹配结果，每个匹配结果中所包含的匹配特征点数量记为特征点的位置坐标记为其中Q2表示该特征对应于查询图像
步骤二三：选取与查询图像匹配率最大的数据库图像：
由式(1)计算查询图像P1 Q与每一幅数据库图像的匹配率
<mrow> <msubsup> <mi>R</mi> <mi>i</mi> <mrow> <mi>Q</mi> <mn>1</mn> </mrow> </msubsup> <mo>=</mo> <mfrac> <msubsup> <mi>N</mi> <mi>i</mi> <mrow> <mi>Q</mi> <mn>1</mn> </mrow> </msubsup> <msub> <mi>l</mi> <mn>1</mn> </msub> </mfrac> <mo>,</mo> <mrow> <mo>(</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mi>d</mi> <mi>a</mi> <mi>tan</mi> <mi>u</mi> <mi>m</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
由式(1)得到datanum个匹配率，取这datanum个匹配率中的最大值，并记录其对应的数据库图像，将与查询图像P1 Q匹配率最大的数据库图像记为将数据库图像的视觉特征的位置坐标向量记为其中，m1表示数据库图像编号，xm1表示在室内坐标系中视觉特征在XD方向上的坐标值，ym1表示在室内坐标系中视觉特征在YD方向上的坐标值，zm1表示在室内坐标系中视觉特征在ZD方向上的坐标值；
由式(2)计算查询图像与每一幅数据库图像的匹配率
<mrow> <msubsup> <mi>R</mi> <mi>i</mi> <mrow> <mi>Q</mi> <mn>2</mn> </mrow> </msubsup> <mo>=</mo> <mfrac> <msubsup> <mi>N</mi> <mi>i</mi> <mrow> <mi>Q</mi> <mn>2</mn> </mrow> </msubsup> <msub> <mi>l</mi> <mn>2</mn> </msub> </mfrac> <mo>,</mo> <mrow> <mo>(</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mi>d</mi> <mi>a</mi> <mi>tan</mi> <mi>u</mi> <mi>m</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
由式(2)可以得到datanum个匹配率，取这datanum个匹配率中的最大值，并记录其对应的数据库图像，将与查询图像匹配率最大的数据库图像记为将数据库图像的视觉特征的位置坐标向量记为其中，m2表示数据库图像编号，xm2表示在室内坐标系中视觉特征在XD方向上的坐标值，ym2表示在室内坐标系中视觉特征在YD方向上的坐标值，zm2表示在室内坐标系中视觉特征在ZD方向上的坐标值；
步骤三：计算查询图像P1 Q对应的旋转矩阵R1和查询图像对应的旋转矩阵R2；
步骤四：根据R1和R2计算查询图像摄像机的位置。
2.根据权利要求1所述一种基于视觉特征匹配与拍摄角度估计的室内定位方法，其特征在于，所述步骤三中计算查询图像P1 Q对应的旋转矩阵R1和查询图像对应的旋转矩阵R2；具体过程为：
步骤三一：特征点选取：
在数据库图像的视觉特征中随机选择8个视觉特征，并将数据库图像中的视觉特征与对应的查询图像视觉特征的位置坐标用一个矩阵表示，记为S1：
<mrow> <msub> <mi>S</mi> <mn>1</mn> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>41</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>51</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>61</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>71</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>81</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>41</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>51</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>61</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>71</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>81</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>x</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>42</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>52</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>62</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>72</mn> </msub> </mtd> <mtd> <msub> <mi>x</mi> <mn>82</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>42</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>52</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>62</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>72</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>82</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
其中，元素xj1表示查询图像P1 Q坐标系中特征点的横坐标，j＝1,2,…,8；元素yj1表示查询图像P1 Q坐标系中特征点的纵坐标，j＝1,2,…,8；元素xj2表示数据库图像坐标系中特征点的横坐标，j＝1,2,…,8；元素yj2表示数据库图像坐标系中特征点的纵坐标，j＝1,2,…,8，T为转置运算符；
步骤三二：计算基础矩阵F1：
基础矩阵F1是一个秩为2的3阶矩阵，通过8个与查询图像特征点相匹配的数据库图像特征点，求得基础矩阵F1，设基础矩阵F1如公式(4)所示：
<mrow> <msub> <mi>F</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>f</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
每一组与查询图像相匹配的特征点对可以得到一个关于基础矩阵F1的约束方程，8个与查询图像相匹配的特征点对可以得到8个关于基础矩阵F1的约束方程；
x11x12f11+x12y11f12+x12f13+y12x11f21+y11y12f22+y12f23+x11f31+y11f32+f33＝0 (5)
x21x22f11+x22y21f12+x22f13+y22x21f21+y21y22f22+y22f23+x21f31+y21f32+f33＝0 (6)
x31x32f11+x32y31f12+x32f13+y32x31f21+y31y32f22+y32f23+x31f31+y31f32+f33＝0 (7)
x41x42f11+x42y41f12+x42f13+y42x41f21+y41y42f22+y42f23+x41f31+y41f32+f33＝0 (8)
x51x52f11+x52y51f12+x52f13+y52x51f21+y51y52f22+y52f23+x51f31+y51f32+f33＝0 (9)
x61x62f11+x62y61f12+x62f13+y62x61f21+y61y62f22+y62f23+x61f31+y61f32+f33＝0 (10)
x71x72f11+x72y71f12+x72f13+y72x71f21+y71y72f22+y72f23+x71f31+y71f32+f33＝0 (11)
x81x82f11+x82y81f12+x82f13+y82x81f21+y81y82f22+y82f23+x81f31+y81f32+f33＝0 (12)
通过式(5)至式(12)，根据线性方程组求解方法解出f11、f12、f13、f21、f22、f23、f31、f32、f33；
其中，f11是F1的第一行第一列元素，f12是F1的第一行第二列元素，f13是F1的第一行第三列元素，f21是F1的第二行第一列元素，f22是F1的第二行第二列元素，f23是F1的第二行第三列元素，f31是F1的第三行第一列元素，f32是F1的第三行第二列元素，f33是F1的第三行第三列元素；
解出f11、f12、f13、f21、f22、f23、f31、f32、f33即可求得基础矩阵F1；
步骤三三：计算本质矩阵E1：
本质矩阵E1可由式(13)计算得出：
Ε1＝CQ TF1CD (13)
式中，F1为基础矩阵，T为转置运算符，CD为数据库图像摄像机的内部参数矩阵，CQ为查询图像摄像机的内部参数矩阵；
步骤三四：计算查询图像P1 Q对应的旋转矩阵R1；
步骤三三中计算得到的本质矩阵E1包含了数据库图像与查询图像之间的旋转关系和平移关系，如式(14)所示，其中T1表示平移向量，R1表示查询图像P1 Q对应的旋转矩阵；
E1＝[T1]×R1 (14)
式中，[]×表示矩阵的反对称运算，反对称运算如公式(15)所示，式中对任意一个1×3维矩阵X＝[x1,x2,x3]T做反对称运算：
<mrow> <msub> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>x</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>x</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>&amp;times;</mo> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <msub> <mi>x</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>x</mi> <mn>3</mn> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>x</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <msub> <mi>x</mi> <mn>1</mn> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
式中，x1为X的第一行第一列元素，x2为X的第二行第一列元素，x3为X的第三行第一列元素；
由公式(14)求得的本质矩阵E1是一个3×3维的矩阵，将本质矩阵表示成一个由变量eij组成的矩阵，如式(16)所示，其中i＝1,2,3，j＝1,2,3：
<mrow> <msub> <mi>E</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>e</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>e</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>e</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>e</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>e</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>e</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>e</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>e</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>e</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
式中，e11为本质矩阵E1的第一行第一列元素，e12为本质矩阵E1的第一行第二列元素，e13为本质矩阵E1的第一行第三列元素，e21为本质矩阵E1的第二行第一列元素，e22为本质矩阵E1的第二行第二列元素，e23为本质矩阵E1的第二行第三列元素，e31为本质矩阵E1的第三行第一列元素，e32为本质矩阵E1的第三行第二列元素，e33为本质矩阵E1的第三行第三列元素；
令E11＝[e11,e21,e31]T，E12＝[e12,e22,e32]T，E13＝[e13,e23,e33]T；
式中，E11为本质矩阵E1的第一列列向量，E12为本质矩阵E1的第二列列向量，E13为本质矩阵E1的第三列列向量；
分别求解E11×E12，E12×E13，E13×E11，其中符号×表示向量的内积运算，由此可得三个内积运算结果的最大值，设内积E11×E12是三个内积中的最大值；
根据公式(17)和公式(18)，分别计算出矩阵A＝[a1,a2,a3]和B＝[b1,b2,b3]；
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>W</mi> <mn>1</mn> </msub> <mo>/</mo> <mrow> <mo>|</mo> <msub> <mi>W</mi> <mn>1</mn> </msub> <mo>|</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mn>1</mn> </msub> <mo>=</mo> <msub> <mi>W</mi> <mn>2</mn> </msub> <mo>/</mo> <mrow> <mo>|</mo> <msub> <mi>W</mi> <mn>2</mn> </msub> <mo>|</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mn>2</mn> </msub> <mo>=</mo> <msub> <mi>a</mi> <mn>3</mn> </msub> <mo>&amp;times;</mo> <msub> <mi>a</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>b</mi> <mn>1</mn> </msub> <mo>=</mo> <msub> <mi>V</mi> <mn>1</mn> </msub> <mo>/</mo> <mrow> <mo>|</mo> <msub> <mi>V</mi> <mn>1</mn> </msub> <mo>|</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>b</mi> <mn>2</mn> </msub> <mo>=</mo> <msub> <mi>V</mi> <mn>2</mn> </msub> <mo>/</mo> <mrow> <mo>|</mo> <msub> <mi>V</mi> <mn>2</mn> </msub> <mo>|</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>b</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>b</mi> <mn>1</mn> </msub> <mo>&amp;times;</mo> <msub> <mi>b</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
其中，W1＝E11×E12，W2＝E11，V1＝E1a1，V2＝E1a2；
构造矩阵C0如式(19)所示：
<mrow> <msub> <mi>C</mi> <mn>0</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
其中，矩阵A＝[a1,a2,a3]是通过本质矩阵求得的矩阵一，矩阵B＝[b1,b2,b3]为通过本质矩阵求得的矩阵二，矩阵为构造矩阵；
旋转矩阵的两种形式如公式(20)所示：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>R</mi> <mn>11</mn> </msub> <mo>=</mo> <msub> <mi>BC</mi> <mn>0</mn> </msub> <msup> <mi>A</mi> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>R</mi> <mn>12</mn> </msub> <mo>=</mo> <msubsup> <mi>BC</mi> <mn>0</mn> <mi>T</mi> </msubsup> <msup> <mi>A</mi> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>
式中，R11为旋转矩阵的形式一，R12为旋转矩阵的形式二；
设平移向量T1如公式(21)所示：
T1＝[b13,b23,b33] (21)
其中，b13表示矩阵B中位于第一行第三列的元素，b23表示矩阵B中位于第二行第三列的元素，b33表示矩阵B中位于第三行第三列的元素；
构造四个包含平移和旋转平移关系的矩阵G1，G2，G3和G4，如式(22)至(25)所示：
<mrow> <msub> <mi>G</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>R</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>G</mi> <mn>2</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>R</mi> <mn>11</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>T</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>G</mi> <mn>3</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>R</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>G</mi> <mn>4</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>R</mi> <mn>12</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>T</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> </mrow>
设向量I＝[1,1,1,1]T，并计算K1＝G1I，K2＝G2I，K3＝G3I，K4＝G4I；
当Kt满足公式(26)时，t＝1,2,3,4，取Kt对应的旋转矩阵作为最终的查询图像P1 Q对应的旋转矩阵R1；
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mi>t</mi> </msub> <mrow> <mo>(</mo> <mn>3</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>&gt;</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mi>t</mi> </msub> <mrow> <mo>(</mo> <mn>4</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>&gt;</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>26</mn> <mo>)</mo> </mrow> </mrow>
式中，Kt(3,1)为在向量Kt中，位置位于第三行第一列的元素；Kt(4,1)为在向量Kt中，位置位于第四行第一列的元素；
步骤三五：重复步骤三一至步骤三四，计算出查询图像对应的旋转矩阵R2。
3.根据权利要求2所述一种基于视觉特征匹配与拍摄角度估计的室内定位方法，其特征在于，所述步骤四中根据R1和R2计算查询图像摄像机的位置；具体过程为：
步骤四一：计算查询图像P1 Q摄像机坐标系与室内坐标系之间的旋转角度α1：
根据查询图像P1 Q对应的旋转矩阵R1，由式(27)得到查询图像摄像机坐标系与室内坐标系之间的旋转矩阵RD1：
RD1＝R1RC1 (27)
式中，矩阵RC1为数据库图像摄像机坐标系与室内坐标系之间的旋转关系，RD1为查询图像P1 Q摄像机坐标系与室内坐标系之间的旋转矩阵，矩阵RD1中位于第一行第一列的元素可以表示为式(28)：
<mrow> <msubsup> <mi>r</mi> <mrow> <mi>D</mi> <mn>1</mn> </mrow> <mn>11</mn> </msubsup> <mo>=</mo> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;alpha;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>28</mn> <mo>)</mo> </mrow> </mrow>
式中，α1为摄像机坐标系ZC轴与室内坐标系ZD轴之间的旋转角度，通过可以计算出角度α1，如式(29)所示：
<mrow> <msub> <mi>&amp;alpha;</mi> <mn>1</mn> </msub> <mo>=</mo> <mi>arccos</mi> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mrow> <mi>D</mi> <mn>1</mn> </mrow> <mn>11</mn> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>29</mn> <mo>)</mo> </mrow> </mrow>
步骤四二：计算查询图像摄像机坐标系与室内坐标系之间的旋转角度α2：
根据查询图像对应的旋转矩阵R2，可以得到查询图像摄像机坐标系与室内坐标系之间的旋转矩阵RD2：
RD2＝R2RC2 (30)
式中，矩阵RC2为数据库图像摄像机坐标系与室内坐标系之间的旋转关系，RD2为查询图像摄像机坐标系与室内坐标系之间的旋转矩阵，矩阵RD2中位于第一行第一列的元素表示为：
<mrow> <msubsup> <mi>r</mi> <mrow> <mi>D</mi> <mn>2</mn> </mrow> <mn>11</mn> </msubsup> <mo>=</mo> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;alpha;</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>31</mn> <mo>)</mo> </mrow> </mrow>
式中，α2为摄像机坐标系ZC轴与室内坐标系ZD轴之间的旋转角度，通过计算出角度α2，如式所示：
<mrow> <msub> <mi>&amp;alpha;</mi> <mn>2</mn> </msub> <mo>=</mo> <mi>arc</mi> <mi> </mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mrow> <mi>D</mi> <mn>2</mn> </mrow> <mn>11</mn> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>32</mn> <mo>)</mo> </mrow> </mrow>
步骤四三：计算视觉特征的投影点坐标：
在室内坐标系中，将位于位置的视觉特征投影到XDODZD平面上，记为特征投影位置点Qt1，可得该视觉特征在XDODZD平面上的投影点坐标Pt1＝[xt1,zt1]，
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>x</mi> <mrow> <mi>m</mi> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mrow> <mi>t</mi> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>z</mi> <mrow> <mi>m</mi> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>33</mn> <mo>)</mo> </mrow> </mrow>
式中，xt1为投影点坐标Pt1的X轴坐标值，zt1为为投影点坐标Pt1的Z轴坐标值，xm1为位置的X轴坐标值，zm1为位置的Z轴坐标值；
在室内坐标系中，将位于位置的视觉特征投影到XDODZD平面上，记为特征投影位置点Qt2，可得该视觉特征在XDODZD平面上的投影点坐标Pt2＝[xt2,zt2]；
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mn>2</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>x</mi> <mrow> <mi>m</mi> <mn>2</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mrow> <mi>t</mi> <mn>2</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>z</mi> <mrow> <mi>m</mi> <mn>2</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>34</mn> <mo>)</mo> </mrow> </mrow>
式中，xt2为投影点坐标Pt2的X轴坐标值，zt2为投影点坐标Pt2的Z轴坐标值，xm2为位置的X轴坐标值，zm2为位置的Z轴坐标值；
步骤四四：计算查询图像摄像机的位置：
设查询图像摄像机在XDODZD平面上的投影为点Q，则点Q与特征投影位置点Qt1的连线lt1的斜率为投影为点Q与特征投影位置点Qt2的连线lt2的斜率为设投影点Q的位置坐标为PQ＝[xQ,zQ]，连线lt1的直线方程可以表示为式(35)：
<mrow> <msub> <mi>z</mi> <mi>Q</mi> </msub> <mo>-</mo> <msub> <mi>z</mi> <mrow> <mi>t</mi> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>Q</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mi>&amp;pi;</mi> <mn>2</mn> </mfrac> <mo>-</mo> <msub> <mi>&amp;alpha;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>35</mn> <mo>)</mo> </mrow> </mrow>
式中，zQ为投影点Q的Z轴坐标值，zt1为投影点坐标Pt1的Z轴坐标值，xQ为投影点Q的X轴坐标值，xt1为投影点坐标Pt1的X轴坐标值；
连线lt2的直线方程可以表示为式(36)：
<mrow> <msub> <mi>z</mi> <mi>Q</mi> </msub> <mo>-</mo> <msub> <mi>z</mi> <mrow> <mi>t</mi> <mn>2</mn> </mrow> </msub> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>Q</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mn>2</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>tan</mi> <mrow> <mo>(</mo> <mfrac> <mi>&amp;pi;</mi> <mn>2</mn> </mfrac> <mo>-</mo> <msub> <mi>&amp;alpha;</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>36</mn> <mo>)</mo> </mrow> </mrow>
式中，zt2为投影点坐标Pt2的Z轴坐标值，xt2为投影点坐标Pt2的X轴坐标值；
联立公式(35)与公式(36)求得投影点Q的位置坐标[xQ,zQ]，投影点Q位于地平面上，该点在地平面XDODZD上的坐标即为查询图像摄像机的位置。
CN201510622949.XA 2015-09-25 2015-09-25 一种基于视觉特征匹配与拍摄角度估计的室内定位方法 Active CN105225240B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510622949.XA CN105225240B (zh) 2015-09-25 2015-09-25 一种基于视觉特征匹配与拍摄角度估计的室内定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510622949.XA CN105225240B (zh) 2015-09-25 2015-09-25 一种基于视觉特征匹配与拍摄角度估计的室内定位方法

Publications (2)

Publication Number Publication Date
CN105225240A CN105225240A (zh) 2016-01-06
CN105225240B true CN105225240B (zh) 2017-10-03

Family

ID=54994189

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510622949.XA Active CN105225240B (zh) 2015-09-25 2015-09-25 一种基于视觉特征匹配与拍摄角度估计的室内定位方法

Country Status (1)

Country Link
CN (1) CN105225240B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107798701A (zh) * 2017-10-12 2018-03-13 唐宓 一种机器人的平面空间定位方法

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106228538B (zh) * 2016-07-12 2018-12-11 哈尔滨工业大学 基于logo的双目视觉室内定位方法
CN106382930B (zh) * 2016-08-18 2019-03-29 广东工业大学 一种室内agv无线导航方法及装置
CN107036593A (zh) * 2016-11-18 2017-08-11 中国矿业大学 一种基于特征匹配与人机交互组合的室内定位方法
CN107376360B (zh) * 2017-06-19 2021-01-01 深圳市铂岩科技有限公司 游戏直播方法及游戏直播系统
CN107689063A (zh) * 2017-07-27 2018-02-13 南京理工大学北方研究院 一种基于天花板图像的机器人室内定位方法
CN108709558B (zh) * 2018-05-24 2021-10-08 郑州辰维科技股份有限公司 一种大尺寸厂房高精度定位的方法
CN108871314B (zh) * 2018-07-18 2021-08-17 江苏实景信息科技有限公司 一种定位定姿方法及装置
CN109141442B (zh) * 2018-09-07 2022-05-17 高子庆 基于uwb定位与图像特征匹配的导航方法和移动终端
CN112348885A (zh) * 2019-08-09 2021-02-09 华为技术有限公司 视觉特征库的构建方法、视觉定位方法、装置和存储介质
CN110986916A (zh) * 2019-11-21 2020-04-10 拉扎斯网络科技（上海）有限公司 室内定位方法、装置及电子设备和存储介质
CN111189440B (zh) * 2019-12-31 2021-09-07 中国电建集团华东勘测设计研究院有限公司 一种基于空间信息模型与实时图像比对的定位导航方法
WO2022250605A1 (en) * 2021-05-24 2022-12-01 Hitachi, Ltd. Navigation guidance methods and navigation guidance devices

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104484881A (zh) * 2014-12-23 2015-04-01 哈尔滨工业大学 基于图像采集的Visual Map数据库建立方法及利用该数据库的室内定位方法
CN104596519A (zh) * 2015-02-17 2015-05-06 哈尔滨工业大学 基于ransac算法的视觉定位方法
CN104616035A (zh) * 2015-03-12 2015-05-13 哈尔滨工业大学 基于图像全局特征及SURF算法的Visual Map快速匹配方法
CN104820718A (zh) * 2015-05-22 2015-08-05 哈尔滨工业大学 基于地理位置特征与全局视觉特征的图像分类和检索方法
CN104866873A (zh) * 2015-04-10 2015-08-26 长安大学 一种基于手机图像匹配的室内定位方法
CN104899603A (zh) * 2015-06-03 2015-09-09 孙思宇 一种基于图像匹配室内定位的优化算法
CN104936283A (zh) * 2014-03-21 2015-09-23 中国电信股份有限公司 室内定位方法、服务器和系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9075826B2 (en) * 2013-05-16 2015-07-07 Hewlett-Packard Development Company, L.P. Image matching

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104936283A (zh) * 2014-03-21 2015-09-23 中国电信股份有限公司 室内定位方法、服务器和系统
CN104484881A (zh) * 2014-12-23 2015-04-01 哈尔滨工业大学 基于图像采集的Visual Map数据库建立方法及利用该数据库的室内定位方法
CN104596519A (zh) * 2015-02-17 2015-05-06 哈尔滨工业大学 基于ransac算法的视觉定位方法
CN104616035A (zh) * 2015-03-12 2015-05-13 哈尔滨工业大学 基于图像全局特征及SURF算法的Visual Map快速匹配方法
CN104866873A (zh) * 2015-04-10 2015-08-26 长安大学 一种基于手机图像匹配的室内定位方法
CN104820718A (zh) * 2015-05-22 2015-08-05 哈尔滨工业大学 基于地理位置特征与全局视觉特征的图像分类和检索方法
CN104899603A (zh) * 2015-06-03 2015-09-09 孙思宇 一种基于图像匹配室内定位的优化算法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107798701A (zh) * 2017-10-12 2018-03-13 唐宓 一种机器人的平面空间定位方法

Also Published As

Publication number Publication date
CN105225240A (zh) 2016-01-06

Similar Documents

Publication Publication Date Title
CN105225240B (zh) 一种基于视觉特征匹配与拍摄角度估计的室内定位方法
CN106296812B (zh) 同步定位与建图方法
Zhang et al. Photogrammetric processing of low‐altitude images acquired by unpiloted aerial vehicles
CN104596519B (zh) 基于ransac算法的视觉定位方法
CN105608421B (zh) 一种人体动作的识别方法及装置
CN108592787B (zh) 3d旋转扫描系统的转轴标定方法与系统
CN106780459A (zh) 一种三维点云数据自动配准方法
CN110068270A (zh) 一种基于多线结构光图像识别的单目视觉箱体体积测量方法
Liu et al. Ground feature oriented path planning for unmanned aerial vehicle mapping
CN107871327A (zh) 基于特征点线的单目相机位姿估计和优化方法及系统
Lu et al. High-performance visual odometry with two-stage local binocular BA and GPU
CN105425231B (zh) 一种基于分层投影和泰勒展开的多传感器多目标定位方法
CN103745498A (zh) 一种基于图像的快速定位方法
CN104457758B (zh) 基于视频采集的Visual Map数据库建立方法及利用该数据库的室内视觉定位方法
CN105004337B (zh) 基于直线匹配的农用无人机自主导航方法
CN103218812A (zh) 基于摄影测量的树木形态模型参数快速获取方法
CN108627798A (zh) 基于线性判别分析和梯度提升树的wlan室内定位算法
CN107133986A (zh) 一种基于二维标定物的相机标定方法
CN104318566B (zh) 可返回多个高程值的新型多视影像铅垂线轨迹匹配方法
Jiang et al. Learned local features for structure from motion of uav images: A comparative evaluation
Gong et al. Point cloud and digital surface model generation from high resolution multiple view stereo satellite imagery
CN105910586B (zh) 基于具有时间属性的像片获取实际地理信息的方法
CN109857895A (zh) 基于多环路视图卷积神经网络的立体视觉检索方法与系统
CN104964669B (zh) 类柱面文物对象正射影像生成方法
CN103606189B (zh) 一种面向非刚体三维重建的轨迹基选择方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20190107

Address after: 236800 Rose Road, Bozhou Bowu Modern Industrial Park, Bozhou City, Anhui Province

Patentee after: Anhui Tiannuo Mechanical and Electrical Technology Co.,Ltd.

Address before: 150001 No. 92 West straight street, Nangang District, Heilongjiang, Harbin

Patentee before: Harbin Institute of Technology

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20200109

Address after: 610225 Chengdu Weihan technology partnership (limited partnership), Chengdu, Sichuan Province

Patentee after: Chengdu Weihan technology partnership (L.P.)

Address before: 236800 Rose Road, Bozhou Bowu Modern Industrial Park, Bozhou City, Anhui Province

Patentee before: Anhui Tiannuo Mechanical and Electrical Technology Co.,Ltd.

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20240110

Address after: 150090 At the intersection of Dalian North Road and Xingkai Road in the concentrated area of Haping Road, Economic Development Zone, Harbin City, Heilongjiang Province

Patentee after: HARBIN XINZHIDA AUTOMATION COMPLETE EQUIPMENT Co.,Ltd.

Address before: 610225 Chengdu Weihan technology partnership (limited partnership), Chengdu City, Sichuan Province

Patentee before: Chengdu Weihan technology partnership (L.P.)