CN105474033A - 智能机姿态测定、全景影像生成及目标识别方法 - Google Patents

Abstract

本发明公开了一种智能机姿态测定、全景影像生成及目标识别方法，其中智能机姿态测定方法包括以下步骤：定义局部坐标系；测定智能机姿态矩阵Rg，Rg是一个相对于局部坐标系的3x3单位正交矩阵，其根据智能机内部的不同传感器可以通过多种方法得到。根据智能机的姿态测定可实现多种应用，包括智能机的虚拟现实漫游方法、智能机的多目视觉定位、智能机的单目单点定位、智能机的全景影像生成、智能机中的目标方向选择、视频影像实时纠正、智能机单像定位以及多个相机间的相对定位等。

Description

智能机姿态测定、全景影像生成及目标识别方法 技术领域

本发明涉及信息技术领域，尤其涉及一种智能机姿态测定、全景影像生成及目标识别方法。

背景技术

地理信息位置服务一直只强调空间定位和空间距离，却忽视了一个非常重要的数据和概念——姿态，实际上用户更关心目的地或目标相对于自身的方向，没有姿态的位置服务是不完整的位置服务。比如一款的士打车软件，司机在抢单的时候如果只考虑距离，就可能选择位于自身后方的乘客，由于车道不能逆行，导致司机无法接到乘客，乘客也无法乘坐的士。作战的时候如果只知道敌人在接近而不知道敌人从哪个方向接近，这必然会导致混乱和恐慌。因此智能机姿态的测定能实现一种真正意义上的位置服务，使得基于智能机姿态的位置服务成为可能，具有非常重要的意义，并且必将形成一种新的数据新标准而被广泛应用。

姿态传感器的应用涉及方方面面，在很多领域都需要用到智能机自身姿态的信息,如影像、视频纠正、单目定位，3D遥控、室内导航、导弹制导。传统的姿态测量非常笨重和昂贵，一般安装在大型飞机，车船上，主要用于航天军事领域。近年来随着传感器技术的提高，各种廉价、轻小型号的用于姿态测定的传感器得以安装在小型智能机上，这为智能机姿态传感数据新标准的建立和广泛应用创造了硬件条件。

一旦这些传感器数据通过数据融合处理得到智能机姿态数据之后就能实现各种重要的应用。表示姿态的传感数据用得最多、最传统的是航位角、俯仰角和翻滚角，主要用于无人机姿态表述和地面移动测量系统MMS，但是这样表示缺点很多：

首先需要用大量三角函数重构姿态矩阵，导致计算速度慢；

三个角度相对于坐标系的转角不统一；

三个角度的转换顺序没有一个统一的标准，不同导航设备上的三个角的顺序定义是不一样的，没有统一的标准，导致针对不同导航设备的姿态数据软件不能统一接口和通用共 享，因此缺少统一的姿态数据标准极大地阻碍了导航技术的发展。

有些软件内部采用的定位算法不是基于图像的，在无GPS和wifi环境下软件是不能正常定位的，软件中的摄像头扫描界面效果给人一种图像识别定位的感觉。传统的图像定位方法是不知道姿态数据的，这样就要求至少需要三对以上的物像同名点对应关系才能进行定位计算，事实上三对同名点除了进行定位还要计算姿态，因此得到的结果很不稳定，很不准确，特别是在三点位一条直线或者三点之间距离很小都会出现很不稳定，不精确的问题，原因就是这三点除了进行定位计算以外还要负担姿态运算。

发明内容

本发明的目的在于，根据智能机的姿态测定实现多种应用，包括智能显示屏的姿态测定、智能机的虚拟现实漫游方法、智能机的多目视觉定位、智能机的单目单点定位、智能机的全景影像生成、智能机中的目标方向选择、视频影像实时纠正、智能机单像定位以及多个智能机的相机间的相对定位等。

本发明解决其技术问题所采用的技术方案是：

提供一种智能机姿态测定方法，其特征在于，包括以下步骤：

1)定义局部坐标系：局部坐标系是指原点在智能机，Z轴指向地球表面法方向,Y轴或X轴与纬线相平行的坐标系；或局部坐标系原点位于地球上任意一点，Z轴与重力方向一致，北代表局部坐标系X轴，西代表局部坐标系Y轴；

2)测定智能机姿态矩阵R_g，R_g是一个相对于局部坐标系的3x3单位正交矩阵，

R_g由下述方法之一获得：

方法(1)采用加速度传感器与磁力计传感器或方向传感器

如果智能机有加速度传感器，且有磁力计传感器或方向传感器，用以下公式(1)计算R_g

其中

v_ax,v_ay,v_az是智能机加速度传感器测出的x,y,z三个方向的值，或是这三个值经滤波的结果；R_a是加速度传感器相对于智能机的姿态转换矩阵；

当a_z≤0时，s＝-1，a_z>0时,s＝1；

若采用方向传感器：通过方向传感器获取方位角将其带入公式(1)计算R_g；

若采用磁力计传感器：计算

其中R_m是磁力传感器相对于智能机的姿态转换矩阵；{v_mx,v_my,v_mz}是磁力传感器检测到的三个值或这三个值经滤波的结果；

再计算

利用磁力传感器计算方位角的三角函数为：

带入公式(1)计算R_g；

方法(2)采用旋转矢量传感器

如果智能机配置有旋转矢量传感器，若检测到旋转矢量传感器只有3个数据values[0],values[1],values[2]，

则采用以下公式得到

q₁＝values[0],q₂＝values[1],q₃＝values[2],

若检测到旋转矢量传感器有4个数据values[0],values[1],values[2],values[3]

则采用以下公式得到

q₁＝values[0],q₂＝values[1],q₃＝values[2],q₀＝values[3]

则R_g0矩阵为:

根据智能机旋转矢量传感器坐标系定义的不同，得到R_g＝R_g0R_L ^T，其中R_L ^T是局部坐标系与旋转矢量传感器坐标系之间的变换矩阵；

方法(3)采用陀螺仪、加速度传感器与磁力传感器迭代

第1步选用方法(1)或方法(2)计算R_g的初值，将R_g转换成4元数q0,q1,q2,q3作为以下第2-7步迭代的初值；

第2步设定exInt，eyInt，ezInt原始值为0，即exInt＝0，eyInt＝0，ezInt＝0；

第3步根据接收到的磁力计矢量{mx,my,mz},得到正确磁场矢量{wx,wy,wz}，

先将矢量{mx,my,mz}替换成将其单位化以后的矢量

得到局部坐标系下磁场的正确的磁场方向矢量{bx,0,bz}，bz＝hz；其中：hx＝2×mx×(0.5-q2×q2-q3×q3)+2×my×(q1×q2-q0×q3)+2×mz×(q1×q3+q0×q2)；

hy＝2×mx×(q1×q2+q0×q3)+2×my×(0.5-q1×q1-q3×q3)+2×mz×(q2×q3-q0×q1)；

hz＝2×mx×(q1×q3-q0×q2)+2×my×(q2×q3+q0×q1)+2×mz×(0.5-q1×q1-q2×q2)；

再转换到正确磁场矢量{wx,wy,wz}，其中

wx＝2×bx×(0.5-q2×q2-q3×q3)+2×bz×(q1×q3-q0×q2)；

wy＝2×bx×(q1×q2-q0×q3)+2×bz×(q0×q1+q2×q3)；

wz＝2×bx×(q0×q2+q1×q3)+2×bz×(0.5-q1×q1-q2×q2)；

第4步根据接收到的加速度传感器数据ax,ay,az,和{wx,wy,wz}得到误差矢量{ex,ey,ez}，并计算其累计值exInt，eyInt，ezInt；

具体为：先将矢量{ax,ay,az}替换成将其单位化以后的矢量计算vx＝2*(q1*q3-q0*q2)；vy＝2*(q0*q1+q2*q3)；vz＝q0*q0-q1*q1-q2*q2+q3*q3；

ex＝(ay×vz-az×vy)+(my×wz-mz×wy)；

ey＝(az×vx-ax×vz)+(mz×wx-mx×wz)；

ez＝(ax×vy-ay×vx)+(mx×wy-my×wx)；

计算误差累计值：将exInt替换为exInt+ex×Ki；eyInt替换为eyInt+ey×Ki；ezInt替换为ezInt+ez×Ki；其中Ki为一可调节的正系数，Ki在0.00001至0.5中任意选取；

若没有磁力计，则第3步不执行，同时将该第4步中的

ex＝(ay×vz-az×vy)+(my×wz-mz×wy)；

ey＝(az×vx-ax×vz)+(mz×wx-mx×wz)；

ez＝(ax×vy-ay×vx)+(mx×wy-my×wx)；

改为

ex＝ay×vz-az×vy；

ey＝az×vx-ax×vz；

ez＝ax×vy-ay×vx；

第5步根据误差矢量{ex,ey,ez}及其累计值纠正陀螺仪数据{gx0,gy0,gz0}

假设智能机读出当前的一组陀螺仪数据为{gx0,gy0,gz0}，gx＝gx0+Kp×ex+exInt；gy＝gy0+Kp×ey+eyInt；gz＝gz0+Kp×ez+ezInt；其中Kp为一可调节的正系数，Kp在0.000001至20.0中任意选取；

第6步根据陀螺仪数据gx,gy,gz修正四元数

随着不断接收到陀螺仪数据gx,gy,gz，对4元数按如下方式修正：

q₀替换为q₀+(-q₁×gx-q₂×gy–q₃×gz)×halfT；q₁替换为q₁+(q₀×gx-q₃×gy+q₂×gz)×halfT；q₂替换为q₂+(q₃×gx+q₀×gy-q₁×gz)×halfT；q₃替换为q₃+(-q₂×gx+q₁×gy+q₀×gz)×halfT；其中halfT为修正周期，halfT＝0.00001～-10.0；

第7步输出R_g矩阵和四元数

将四元数{q0,q1,q2,q3}单位化成输出；

4元数转R_g矩阵公式如下

第8步回到第3步继续接收陀螺仪运动数据更新姿态4元数q₀～q₃，在循环的过程中每次到第7步都能输出当前的R_g矩阵和四元数。

本发明还提供了一种智能机显示屏的姿态测定方法，包括以下步骤：

智能机显示屏的姿态矩阵为

r_x是智能机显示屏像素阵列x轴方向在局部坐标系中的分量；

r_y是智能机显示屏像素阵列y轴方向在局部坐标系中的分量；

r_z是垂直于智能机显示屏表面方向向智能机显示屏内部朝前看的视线方向在局部坐标系中的分量；

1)按上述方法测定智能机姿态，并得到智能机的姿态矩阵R_g；

2)计算R＝ΔR×R_g，ΔR为智能机显示屏姿态矩阵R相对于智能机姿态矩阵R_g的一个固定的方向的转换，智能机自身Z轴定义为智能机加速度传感器的z轴方向，

当显示屏观察方向和智能机自身Z轴方向刚好相反时

当显示屏观察方向和智能机自身Z轴方向完全一致时

否则采用摄影测量方法标定ΔR＝R_AR_g ^-1，R_A是摄影测量后方交汇法算得的摄像头姿态矩阵，R_g是上述方法测得此时智能机的姿态矩阵。

本发明还提供了一种基于智能机姿态的虚拟现实漫游方法，包括以下步骤：

1)采用上述方法得到智能机显示屏的姿态矩阵R；

2)根据姿态矩阵R移动控制漫游观察者在局部坐标系中的位置：

定义漫游观察者是在局部坐标系中的一个具备自身位置、姿态且用于观察周边场景的对象；智能机显示屏所渲染的场景就是漫游观察者在局部坐标系中所应看到的场景图像；定义漫游观察者在局部坐标系中的坐标为(Xd,Yd,Zd)，

采用以下方法控制漫游观察者在虚拟场景中移动：

a)获取智能机漫游者手指在屏幕本体上的滑动数据；

b)在滑动数据是漫游者单指相对于智能机屏幕本体滑动矢量△X,△Y的情况下，如果|r₃₃|>|r₁₃|且|r₃₃|>|r₂₃|，其中r_ij是R矩阵的i行j列元素值，则单指滑动后，在局部坐标系中的定位坐标(Xd,Yd)的改正数(v_x，v_y)，v_x＝ΔXr₁₁+ΔYr₂₁v_y＝ΔXr₁₂+ΔYr₂₂；否则：

c)在滑动数据是漫游者双指做拉开或合拢操作所获得的情况下，则得到拉开和合拢之前双指之间的屏幕距离，以及拉开和合拢之后双指之间的屏幕距离；通过前后两次距离的差异修正Z_d；

3)设置漫游观察者在局部坐标系里的位置和方向：

根据漫游观察者的局部坐标系坐标(Xd,Yd,Zd)，以及步骤1)测定得到的姿态矩阵R，利用OpenGL，OpenES，OS X OpenGL，Direct3D或osg绘图软件库中用于设定相机位姿的图形库函数，设置漫游观察者在局部坐标系里的位置和方向：

需要设定以下几个量:

(1)漫游观察者自身观察的垂直视场角θ，就是智能机显示屏上边缘中点的视线方向与下边缘中点的视线方向的夹角；

(2)漫游观察者所在的位置(X_d,Y_d,Z_d)，并根据步骤2)的漫游方法进行修正；

(3)漫游者观察方向{r₃₁,r₃₂r₃₃}或者用户注视点坐标(X_d+r₃₁,Y_d+r₃₂,Z_d+r₃₃)

(4)漫游者自身向上方向的矢量{-r₂₁,-r₂₂,-r₂₃}

其中步骤(3)和(4)中的r_ij是智能机显示屏姿态R矩阵中的第i行j列，i＝2或3，j＝1、2或3；

4)在局部坐标系下渲染绘制三维场景

预先在智能机的显示屏内设置一个3维渲染图层进行三维场景渲染；

被绘制的三维场景中的点、线、面和物体模型的几何坐标都要转换成局部坐标系下的坐标[Xi Yi Zi]^T,即与漫游者的坐标(X_d,Y_d,Z_d)在同一个局部坐标系下；用OpenGL，OpenES，OS X OpenGL，Direct3D或osg绘图软件库实现局部坐标系下的三维绘图；

智能机摄像头的拍摄方向与屏幕观察方向一致时，可以智能机摄像头拍摄的视频图像为底图，叠加绘制三维场景中的点线面。

本发明还提供了一种基于智能机姿态数据的多目视觉定位方法，包括以下步骤：

步骤1：

在N个地理位置点拍摄同一个物体X，其中N≥2，测第i个观察点的以下几个量，其中i∈(1，2，…，N)：

(1)在视频帧图像中显示的位置(ui,vi)；

(2)拍摄点的地理位置分别是第i个拍摄点的经度，纬度，高度；

(3)按上述方法测得第i个拍摄点的摄像头姿态矩阵R_i；

(4)第i个拍摄点的内部参数焦距长f_i，像主点坐标(c_xi,c_yi)；

步骤2：

计算相对于地心的N个姿态矩阵，i＝1～N，R_0i＝R_iR_vi

其中

步骤3：

利用公式得到N个拍摄点摄像头光心的地心坐标系坐标T_0i＝{T_xi,T_yi,T_zi}^T i＝1～N；其中{ΔT_xi,ΔT_xi,ΔT_zi}^T＝R_vi ^TΔT_i；ΔT_i是摄像头光心相对于GPS接收天线的偏移矢量；是将经度，纬度，高度转换成地心坐标系的X坐标的函数；是将经度，纬度，高度转换成地心坐标系的Y坐标的函数；是将经度，纬度，高度转换成地心坐标系的Z坐标的函数；

步骤4

根据拍摄点N的姿态矩阵R_0N，位置[T_xN T_yN T_zN]，在图像中显示的位置坐标(u_N,v_N)，摄像头焦距像素长f_N,像主点坐标(c_xN，c_yN)都满足以下方程

其中r_0iab表示姿态矩阵R_0i的第a行第b列的元素值；i＝1～N一共列2N个方程，解(X_x,X_y,X_z)三个未知数；或采用摄影测量前方交汇方法计算出物体X得到地心坐标系坐标(X_x,X_y,X_z)；最后由大地测量学公式得到地心坐标(X_x,X_y,X_z)对应的经纬度。

本发明还提供了一种基于智能机姿态数据的单目单点定位方法，包括步骤：

假设智能机上的相机内部参数矩阵为:

被观察物体是一个直径＝D大小的物体,

在智能机上成像的宽度是Δu,高度是Δv,

成像中心点像坐标(u,v),

采用上述方法测得智能机姿态矩阵R_g,并测得智能机上的摄像头姿态矩阵R＝△RR_g包括以下两种情况：

(3)若已知智能机摄像头光心局部坐标系坐标是T,

可单目估算出物体的局部坐标系下三维坐标:

其中

(4)若已知物体的局部坐标系下三维坐标X，可以求相机坐标T

该情况实现了相机的单目定位。

本发明还提供了一种基于智能机姿态数据的全景影像生成方法，包括以下步骤：

步骤1、智能机在某一固定点用不同姿态拍摄n张图像，根据上述方法计算智能相机的姿态矩阵，根据智能相机的姿态矩阵再计算智能机摄像头姿态矩阵，分别为R_0i，i为0～n‐1之间的整数，n为大于等于1的整数；

步骤2、根据影像重叠区域迭代求解进行精确纠正

(2)提取图像间重合区域的同名点对

定义H_ij＝K_jR_jR_i ^TK_i ^-1

对N张图像之间任意两张i,j进行匹配寻找图像上的同名点对，可以将第i张图像I_i经过透视H_ij变换后得到图像I_i’再与第j张图像I_j作细致匹配，最后将图像I_i上的匹配点坐标经过H_ij ^‐1逆变换还原成I_i图原始的坐标；

(2)根据上一步得到的同名点对匹配关系建立误差e相对于参数集

φ＝[... f_i q_i0 q_i1 q_i2 q_i3 ... f_j q_j0 q_j1 q_j2 q_j3 ...]^T

的数学模型e(φ)，

前面得到的智能机摄像头姿态矩阵R_0i转换成4元数q_i00,q_i01,q_i02,q_i03,i＝0,…,n‐1

以φ＝φ₀＝[... f_0i q_i00 q_i01 q_i02 q_i03 ... f_0j q_j00 q_j01 q_j02 q_j03 ...]^T作为初值,

采用LM法通过减小误差e，逐步得到精确化相机姿态q_i0,q_i1,q_i2,q_i3和焦距参数f_i；

最后将精确化的φ中的四元数q_i0,q_i1,q_i2,q_i3转化成姿态矩阵R_i,i＝0,…,n-1；

步骤3、开辟一张宽w高h大小的全景影像，扫描全景影像上每一个像素，针对全景图的每一个像素(x,y)进行如下(1)(2)处理：

(1)横坐标x转换成纵坐标y转换成θ；

其中w，h是全景图像的宽，高；

(2)依据小孔成像公式

其中f_i为相机i焦距像素长度，(c_ix,c_iy)是相机i的像主点；

利用R_i将转换成映射到每个相机i的像坐标(u_i,v_i)，若(u_i,v_i)在图像范围内，则将相机i像坐标(u_i,v_i)处颜色取出，加权渲染到全景图(x,y)处；权重与(u_i,v_i)到该图像边缘最短距离有关，距离越短权重越小；若无一个相机的(u_i,v_i)在其图像范围内，渲染透明色或背景色到(x,y)处；扫描完全景图像上所有点填充相应的颜色，完成全景图像的渲染；

或者，在步骤2中先将全景图像颜色值全部清0，在步骤3中针对n张图像将全景图像上的相应区每个像素扫描n轮，分n轮依次将加权颜色值累加在全景图的像素上；

步骤4全景图像的三维浏览和检索定位

(B)智能机姿态控制浏览三维全景图像

在虚拟场景中以球心为观察点，球体内壁以全景图作为纹理渲染一个球；设定智能机摄像头观察方向{r₃₁,r₃₂r₃₃}，自身向上方向{-r₂₁,-r₂₂,-r₂₃}；r_ij是姿态矩阵R中第i行第j列元素；

(B)全景图建库、检索和定位

为有效利用存储空间，除采用步骤3(1)中的公式外还可用墨卡托转换公式，

转换成全景图像，保持建库和检索的转换方式一致；

建库

提取全景图像的特征点集合，建立与全景图像关联的特征点数据库，库中每个特征点指向其所在的全景图像；

检索

同一地点c≥1张图像合成待检索全景图像并提取渲染区特征点，对每个特征点，查询库中与其最相似特征点所指向的全景图像，按特征点相似程度对库中指向的全景图像进行加权投票，按投票总和大小排序；

将待检索全景图中已渲染区和投票得到的库中全景图对应区域的像素集进行相关匹配计算，相关系数越大越相似；输出投票排序靠前且相关系数大于预设值的结果；如果c＝1,也可根据单张拍摄姿态矩阵R，将库中全景图像映射到单张拍摄图像域与单张拍摄图像进行相关匹配；

定位

在已知GPS情况下，检索GPS附近区域的全景图像组成的子集特征库；

服务器数据库将全景图像与其地理位置坐标绑定在一起存储，检索结果是编号为i*的图像，即可在数据库中查找出i*所对应的地理位置。

本发明还提供了一种基于智能机姿态数据的目标方向选择方法，包括以下步骤：

1.定义智能机主方向或载体主方向相对于智能机自身的矢量单位p；

2.采用上述方法获取智能机当前姿态矩阵R_g；

3.计算p_┴＝R_g ^Tp为智能机主方向或载体主方向相对于指北坐标系的矢量；

4.计算局部坐标系中的目标矢量v_o＝O-P，假设局部坐标系中智能机自身坐标为P,目标的坐标为O，则目标矢量v_o＝O-P；

5.计算矢量p_┴与目标矢量v_o之间夹角的余弦，或计算两个矢量之间的夹角；

如果夹角的余弦>0.9，或者夹角偏差的绝对值|dφ|很小则触发智能机与该目标O之间的某一关联事件；

通过两矢量的点积除以|v_o|得到夹角dφ的余弦，

cos(dφ)＝(p_┴点积v_o)/|v_o|

如果该余弦cos(dφ)>cos(φa)，φa为预设值，则该目标方向选择事件可触发；

6.在需要与用户的可视化交互情况下，在指北坐标系中实时绘制目标、智能机和矢量p_┴；

计算局部坐标系中智能机主方向矢量与目标矢量之间夹角的方向余弦，根据余弦值大小判定夹角，并且在三维空间或二维空间中绘制出智能机主方向矢量与目标矢量之间的方向关系，便于人机交互。

本发明还提供了一种基于智能机姿态数据的视频影像实时纠正方法，包括以下步骤：

1.采用上述方法计算智能机拍摄视频的每一帧图像对应的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R，△R是摄像头相对于智能机的转换矩阵；

2.将摄像头的姿态矩阵R分解为R＝R_τR_从而得到矩阵R_τ，相机的拍摄姿态R不规则,期待得到用虚拟姿态R_作为拍摄姿态矩阵得到的标准图像；

3.对智能机拍摄视频的每一帧图像I经过H＝K₂R_τ ^TK^-1的Homograph变换得到I’；

其中是拍摄相机的内部参数矩阵，f是焦距的像素长度，(c_x,c_y)是相机主点坐标；对于理想相机(c_x,c_y)是图像的中心点，c_x＝w/2,c_y＝h/2,w,h是原始图像I的宽和 高；

是纠正以后虚拟相机的内部参数矩阵，f2是纠正以后虚拟相机的焦距的像素长度，(c_x2,c_y2)是纠正以后虚拟相机主点坐标。

本发明还提供了一种基于姿态数据的智能机单像定位方法，包括以下步骤：

假设相机或摄像机内参数据f和(c_x,c_y)已知，f是焦距的像素长度，(c_x,c_y)是相机主点坐标，

内参矩阵

单像目标定位整体实施步骤如下：

1.搜集采集物方点，转化成局部坐标系坐标；

2.采用上述方法获取智能机的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R，△R是摄像头相对于智能机的转换矩阵；

3.拍摄一张图像，自动提取其中的多个特征点，通过匹配算法得到与物点对应的n对物像同名点；n个物方点的坐标是X_i＝[X_ix,X_iy,X_iz]^T i＝1,…,n，对应n个像点坐标是[u_i v_i]^T i＝1,…,n；

4.分无错和容错2种情况计算相机在局部坐标系下的坐标T：

(1)若确认多个特征点无错误，则

采用数学模型解方程得到自身在局部坐标系下坐标Tx，Ty，Tz

其中

进而得到相机位置T，最大集ζ为n个点对组成的全集；

(2)若不确认多个特征点是否全部正确，采用Ransac容错算法，通过共线方程模型随机选取两对点n＝2,根据模型式(HH2)计算相机位置T，并通过共线方程z_i[u’_i v’_i 1]^T＝KR(X_i-T)验证T，

通过KR(X_i-T)算出的u’_i,v’_I与u_i,v_i之间的误差距离小于error像素认为是符合共线方程，选取符合共线方程点数最大集ζ并计算最大集ζ对应的T*，单目多点Ransac容错算法流程如下：

2.从n个像点中随机挑选出2个距离足够大的像点，至少大于容忍误差error的2倍；

3.挑选出来的2个像点与物点对应形成2对物象同名点，按式(HH2)计算智能机位置T；

4.用T验证所有n对物象同名点的共线方程对应关系，共线方程KR(X_i‐T)算出的u’_i,v’_I与u_i,v_i之间的误差距离小于error像素认为是符合共线方程，统计符合共线方程的个数i_in,如果i_in数量超过前期的数量n_max则记录本组满足共线方程容忍误差数量最大的点集ζ_max，并刷新n_max＝i_in；

5.更新迭代次数k+1→k

6.回到第1步，直到满足Ransac概率收敛条件(1‐(n_max/n)²)^k<p_e，p_e是Ransac容忍错误概率，为10^‐2～10^‐20之间的一个数值；

7.将前面搜索得到的最大的点集ζ_max搜集起来进行最后一次多点最小2乘计算，按公式(HH2)计算得到T_best得到最终结果；

本发明还提供了一种基于智能机姿态数据的相对定位算法，包括以下步骤：

a)两相机之间相对定位

假设相机1，2之间有i＝1..n≥2对同名点u_1i,v_1i～u_2i,v_2i；

其中R₁，R₂是智能机相机在拍摄的1,2两个地点测出来的姿态矩阵数据；

已知相机1的内参矩阵相机2的内参矩阵其中(c_xi,c_yi)是相机i的像素主点坐标，f_i是相机i的焦距像素长；

已知两相机之间的距离是L；

计算相机2相对于相机1的位移矢量T包括以下步骤：

2.用上述方法计算智能机上相机的姿态R_g1,R_g2

计算R₁＝△RR_g1 R₂＝△RR_g2

进一步得到R＝R₂R₁ ^T

3.构建矩阵A

其中

u_ji*＝K_i ^-1u_i＝[u_ji*x u_ji*y u_ji*z]^T，

u_jiR*＝Ru_ji*＝[u_jiR*x u_jiR*y u_jiR*z]^T，i＝1,2，j＝1..n；

3.如果确认n对点都是正确的，则

求约束||t_3x1||＝1下A_nx3t_3x1＝0的解

或采用数学SVD分解得到A_nx3＝U_nxnW_nx3V^T _3x3,

SVD分解出来的V矩阵的最后1列的矢量，即对应于最小奇异值的特征矢量为t，

T＝-LR^Tt得到结果，结束

否则继续进行以下步骤；

4.设置迭代初值n_max＝0，k＝1；

5.随机挑选A矩阵中的2行,构建A_2x3

就是求约束||t₂||＝1下A_2x3t₂＝0的解

或采用SVD分解得到A_2x3＝U_2x2W_2x3V^T _3x3,

分解出来的V的最后1列的矢量即为t₂；

6.将t₂＝[t_tx t_ty t_tz]^T代入第j＝1..n个点，采用式(AB)验证点j是否是内点,

(u_j2*zu_j1R*y-u_j2*yu_j1R*z)t_tx+(u_j2*xu_j1R*z-u_j2*zu_j1R*x)t_ty+(u_j2*yu_j1R*x-u_j2*xu_j1R*y)t_tz＜ε

                                          (AB)

其中ε表示最大容忍误差物方长度，相对于两相机之间的距离＝1而言；

满足式(AB)则点j是内点，如果满足内点的数量n_t超过最佳值n_max则ζ_t-->ζ n_t-->n_max

7.如果(1-(n_max/n)²)^k<0.00001错误率足够小，退出迭代进入步骤9；

8迭代次数k增加1，回到步骤5反复迭代；

9.最后采用最小二乘法针对最大内点集ζ重新计算一次最优t,

求约束||t_3x1||＝1下A_n’x3t_3x1＝0的解，其中n’＝n_max

或采用SVD分解得到A_n’x3＝U_n’xn’W_n’x3V^T _3x3,

分解出来的V的最后1列矢量,即对应于最小奇异值的特征矢量，即为t最终得到T＝-LR^Tt；

b)多个相机间的相对定位

构建矩阵

U(ij)_k＝[U(ij)_k1 U(ij)_k2 U(ij)_k3]k＝1,2

r_c(ij)ab是指第i个点对应的第j个图像的相机的姿态矩阵的a行b列，u_c(ij)是第i个点对应的第j个图像上的像点坐标，(c_x,c_y)是相机主点坐标，f是相机的焦距；

对矩阵A进行SVD分解得到A＝UWV^T,V中最后一个非零矢量

X＝[T_1x T_1y T_1z ... T_cx T_cy T_cz X₁ Y₁ Z₁ ... X_N Y_N Z_N]^T的每一个分量对应C个相机的位置T_1x,T_1y,T_1z,…,T_cx,T_cy,T_cz和N个点的坐标X₁,Y₁,Z₁,…,X_N,Y_N,Z_N。

本发明还提供了基于智能机姿态的目标识别算法，包括以下步骤：

根据上述方法，通过H＝K₂R_τ ^TK^-1对智能机拍摄视频的图像I进行纠正得到I'；

对纠正后的图像I'进行目标检测、识别处理，得到I'图像上目标的外接多边形点坐标集x_i,y_i,i＝1,...,N；

通过反纠正H^-1的方法得到坐标集在原图上映射的坐标x_i',y_i'，即

传感器纠正目标识别算法的具体实施方式：

首先根据传感器得到的R矩阵对原始图I进行H＝K₂R_τ ^TK^-1标准化纠正得到标准纠正图I’，然后对I’图像进行目标检测、跟踪，得到它对应的目标框r，目标框r相对于标准模板的平移是r_x,r_y，

目标框r首先经过平移变换H_t映射到标准图上,然后再经过H^-1变换映射到原始图上，因此目标框r相对于原始图像I的H变换为

r*＝H^-1H_tr。

本发明产生的有益效果是：本发明的方法能实时得到手机在局部地理坐标系中的姿态矩阵的方法，姿态矩阵准确描述了智能机在客观三维场景中的姿态数据对于导航，方向选择，便携式控制都有及其广泛的应用；可实现3D漫游控制，游戏场景漫游，虚拟现实，增强现实，3D仿真等领域；实现多张图像对某一物体空间3维定位，或者物体3维坐标与相机3维坐标之间的相互定位。

另外本发明还可以实现便携式智能机拍摄合成全景图像，以及根据全景图像进行空间 定位以及空间目标方向选择、控制、人机交互操作；还可实现基于姿态矩阵的智能机的视频图像实时纠正，能保持生成纠正的图像中的水平线始终对应客观场景的地平线，可用于颠簸的路面，海面场景视频图像的纠正。对于接近垂直于地面拍摄的情况，生成纠正的图像中的水平朝右方向始终对应客观场景中的局部坐标系的x轴(北)，可用于无人机地面图像拼接。

本发明还可根据姿态矩阵和图像上2个点的像坐标对相机自身进行定位以及基于姿态矩阵的双目图像或多目图像之间相对定位，解决了任意姿态拍摄视频仍能准确识别目标的难题，不必考虑目标大角度旋转，仍能实现高效准确的目标检测。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例位置服务中同时符合位置和方向的目标位示意图；

图2是本发明实施例的单像双点定位示意图；

图3是本发明实施例传感器纠正目标识别算法原理示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明实施例智能机姿态测定方法，包括以下步骤：

1)定义局部坐标系：局部坐标系是指原点在智能机，Z轴指向地球表面法方向,Y轴或X轴与纬线相平行的坐标系；或局部坐标系原点位于地球上任意一点，Z轴与重力方向一致，北代表局部坐标系X轴，西代表局部坐标系Y轴；

2)测定智能机姿态矩阵R_g。R_g是一个相对于局部坐标系的3x3单位正交矩阵，R_g由下法之一获得：

方法(1)采用加速度传感器与磁力计或方向传感器

如果智能机有加速度传感器，且有磁力计或方向传感器，用以下公式(1)计算R_g

其中

v_ax,v_ay,v_az是智能机测出来的智能机加速度传感器的x,y,z三个方向的值，或是这三个值经滤波的结果；

R_a是加速度传感器芯片相对于智能机的姿态转换矩阵；

当a_z≤0时，s＝-1，a_z>0时,s＝1；

若采用方向传感器：带入公式(1)计算R_g；

若采用磁力计传感器：计算

其中R_m是磁力传感器芯片相对于智能机的姿态转换矩阵；{v_mx,v_my,v_mz}是智能机上磁力传感器检测到的三个值或这三个值经滤波的结果；

再计算

利用磁力计计算方位角的三角函数为：

带入公式(1)即可算出R_g；

方法(2)用旋转矢量传感器

如果智能机配置有旋转矢量传感器检测到智能机上的旋转矢量传感器只有3个数据values[0],values[1],values[2]，

采用以下公式得到

q₁＝values[0],q₂＝values[1],q₃＝values[2],

若检测到旋转矢量传感器有4个数据values[0],values[1],values[2],values[3]

则采用以下公式得到

q₁＝values[0],q₂＝values[1],q₃＝values[2],q₀＝values[3]

则R_g0矩阵为:

根据智能机旋转矢量传感器坐标系定义的不同，得到R_g＝R_g0R_L ^T，其中R_L ^T是局部坐标系与旋转矢量传感器坐标系之间的变换矩阵；

根据智能机旋转矢量传感器坐标系定义的不同，采取以下几种情况之一得到R_g:

若旋转矢量传感器的X轴指向东,Y轴指向北,则

若旋转矢量传感器的X轴指向北,Y轴指向西,则

R_g＝R_g0；                   (3)

若旋转矢量传感器的X轴指向南,Y轴指向东,则

方法(3)采用陀螺仪、加速度计与磁力计迭代

第1步选用方法(1)或方法(2)计算R_g的初值，将R_g转换成4元数q0,q1,q2,q3作为以下第2-7步迭代的初值

第2步设定exInt，eyInt，ezInt原始值为0

exInt＝0，eyInt＝0，ezInt＝0

第3步根据接收到的磁力计矢量{mx,my,mz},得到正确磁场矢量{wx,wy,wz}

先将矢量{mx,my,mz}替换成将其单位化以后的矢量

得到局部坐标系下磁场的正确的磁场方向矢量{bx,0,bz}

hx＝2×mx×(0.5-q2×q2-q3×q3)+2×my×(q1×q2-q0×q3)+2×mz×(q1×q3+q0×q2)；

hy＝2×mx×(q1×q2+q0×q3)+2×my×(0.5-q1×q1-q3×q3)+2×mz×(q2×q3-q0×q1)；

hz＝2×mx×(q1×q3-q0×q2)+2×my×(q2×q3+q0×q1)+2×mz×(0.5-q1×q1-q2×q2)；

bz＝hz

再转换到正确磁场矢量{wx,wy,wz}

wx＝2×bx×(0.5-q2×q2-q3×q3)+2×bz×(q1×q3-q0×q2)；

wy＝2×bx×(q1×q2-q0×q3)+2×bz×(q0×q1+q2×q3)；

wz＝2×bx×(q0×q2+q1×q3)+2×bz×(0.5-q1×q1-q2×q2)；

第4步根据接收到的加速度传感器数据ax,ay,az,和{wx,wy,wz}

得到误差矢量{ex,ey,ez}并计算其累计值exInt，eyInt，ezInt

先将矢量{ax,ay,az}替换成将其单位化以后的矢量

vx＝2*(q1*q3-q0*q2)；vy＝2*(q0*q1+q2*q3)；vz＝q0*q0-q1*q1-q2*q2+q3*q3；

ex＝(ay×vz-az×vy)+(my×wz-mz×wy)；

ey＝(az×vx-ax×vz)+(mz×wx-mx×wz)；

ez＝(ax×vy-ay×vx)+(mx×wy-my×wx)；

计算误差累计值

exInt替换为exInt+ex×Ki；eyInt替换为eyInt+ey×Ki；ezInt替换为ezInt+ez×Ki；

其中Ki为一可调节的正系数，Ki在0.00001至0.5中任意选取；

第5步根据误差矢量{ex,ey,ez}及其累计值纠正陀螺仪数据{gx0,gy0,gz0}

假设智能机读出当前的一组陀螺仪数据为{gx0,gy0,gz0}gx＝gx0+Kp×ex+exInt；gy＝gy0+Kp×ey+eyInt；gz＝gz0+Kp×ez+ezInt；

其中Kp为一可调节的正系数

Kp在0.000001至20.0中任意选取；

第6步根据陀螺仪数据gx,gy,gz修正四元数

随着不断接收到陀螺仪数据gx,gy,gz，对4元数按如下方式修正，halfT为修正周期，

halfT＝0.00001～-10.0，

q₀替换为q₀+(-q₁×gx-q₂×gy–q₃×gz)×halfT；q₁替换为q₁+(q₀×gx-q₃×gy+q₂×gz)×halfT；q₂替换为q₂+(q₃×gx+q₀×gy-q₁×gz)×halfT；q₃替换为q₃+(-q₂×gx+q₁×gy+q₀×gz)×halfT；

第7步输出R_g矩阵和四元数

将四元数{q0,q1,q2,q3}单位化成输出

4元数转R_g矩阵公式如下

第8步回到第3步继续接收陀螺仪运动数据更新姿态4元数q₀～q₃，在循环的过程中每次到第7步都能输出当前的R_g矩阵和四元数。

注：在没有磁力计的时候，可去掉第3步，同时第4步中的

ex＝(ay×vz-az×vy)+(my×wz-mz×wy)；

ey＝(az×vx-ax×vz)+(mz×wx-mx×wz)；

ez＝(ax×vy-ay×vx)+(mx×wy-my×wx)；

改为

ex＝ay×vz-az×vy；

ey＝az×vx-ax×vz；

ez＝ax×vy-ay×vx；

本发明实施例的智能机显示屏的姿态测定方法，可实现各种可视化增强现实应用，包括以下步骤：

智能机显示屏的姿态矩阵为

r_x是智能机显示屏像素阵列x轴方向在局部坐标系中的分量；

r_y是智能机显示屏像素阵列y轴方向在局部坐标系中的分量；

r_z是垂直于智能机显示屏表面方向向智能机显示屏内部朝前看的视线方向在局部坐标系中的分量；

1)按上述方法测定智能机姿态，并得到智能机的姿态矩阵R_g；

2)计算R＝ΔR×R_g；得到智能机显示屏的姿态矩阵；

ΔR为智能机显示屏姿态矩阵R相对于智能机姿态矩阵R_g的一个固定的方向的转换，智能机自身Z轴定义为智能机加速度计的z轴方向，

当显示屏观察方向和智能机自身Z轴方向刚好相反时

当显示屏观察方向和智能机自身Z轴方向完全一致时

否则采用摄影测量方法标定ΔR＝R_AR_g ^-1，R_A是摄影测量后方交汇法算得的摄像头姿态矩阵，R_g是根据上述方法测得此时智能机的姿态矩阵。

本发明实施例基于智能机姿态的虚拟现实漫游方法，可实现3D漫游控制，游戏场景漫游，虚拟现实，增强现实，3D仿真等领域，包括以下步骤：

1)采用上述方法得到智能机显示屏的姿态矩阵R

2)移动控制漫游观察者在局部坐标系中的位置

定义漫游观察者是在局部坐标系中的一个具备自身位置、姿态且用于观察周边场景的对象；智能机显示屏所渲染的场景就是漫游观察者在局部坐标系中所应看到的场景图像；定义漫游观察者在局部坐标系中的坐标为Xd,Yd,Zd

用以下三种模式之一控制漫游观察者在虚拟场景中的移动：

(1)模式1:普通二维移动

当手指按{ΔX,ΔY}方向滑过智能机显示屏平面的时候，相当于局部坐标系下,

Xd增加Vx＝ΔXr₁₁+ΔYr₂₁；Yd增加Vy＝ΔXr₁₂+ΔYr₂₂；Zd不变；

(2)模式2:智能二维移动

当手指按{ΔX,ΔY}方向滑过智能机显示屏平面的时候，采用如下方法：

首先选择R矩阵右侧一列中的r13,r23,r33中哪一个分量的绝对值最大，

如果|r33|最大，Xd增加Vx＝ΔXr₁₁+ΔYr₂₁Yd增加Vy＝ΔXr₁₂+ΔYr₂₂

如果|r23|最大，Xd增加Vx＝ΔXr₁₁±ΔYr₃₁Yd增加Vy＝ΔXr₁₂±ΔYr₃₂

r23≥0时，±取+；r23<0时±取-或者r23≥0时，±取-；r23<0时±取+

如果|r13|最大，Xd增加Vx＝±ΔXr₃₁+ΔYr₂₁Yd增加Vy＝±ΔXr₃₂+ΔYr₂₂

r13≥0时，±取+；r13<0时±取-或者r13≥0时，±取-；r13<0时±取+；

(3)模式3:三维漫游移动

因为智能机显示屏只有二维，单个手指滑动只能进行二维操作，单指划过手机显示屏时ΔX和ΔY与滑动位移成正比；此时操作智能机的用户双指滑动触碰智能机操作实现3维操作：采用双指合拢相当于ΔZ<0,双指拉开相当于ΔZ>0，或者双指合拢相当于ΔZ>0,双指拉开相当于ΔZ<0；当智能机接收到手指控制信息{ΔX,ΔY,ΔZ}的时候,Xd增加Vx＝ΔXr₁₁+ΔYr₂₁+ΔZr₃₁ Yd增加Vy＝ΔXr₁₂+ΔYr₂₂+ΔZr₃₂ Zd增加Vz＝ΔXr₁₃+ΔYr₂₃+ΔZr₃₃；

3)设置漫游观察者在局部坐标系里的位置和方向：

根据漫游观察者的局部坐标系坐标Xd,Yd,Zd，以及步骤1)测定得到的姿态矩阵R， 利用OpenGL，OpenES，OS X OpenGL，Direct3D或osg绘图软件库中用于设定相机位姿的图形库函数，设置漫游观察者在局部坐标系里的位置和方向：

需要设定以下几个量:

(1)漫游观察者自身观察的垂直视场角θ，就是智能机显示屏上边缘中点的视线方向与下边缘中点的视线方向的夹角；

(2)漫游观察者所在的位置X_d,Y_d,Z_d，由步骤2)漫游方法修改；

(3)漫游者观察方向{r₃₁,r₃₂r₃₃}或者用户注视点坐标(X_d+r₃₁,Y_d+r₃₂,Z_d+r₃₃)

(4)漫游者自身向上方向的矢量{-r₂₁,-r₂₂,-r₂₃}

其中步骤(3)和(4)中的r_ij是智能机显示屏姿态R矩阵中的第i行j列，i＝2或3，j＝1、2或3；

4)在局部坐标系下渲染绘制三维场景

预先在智能机的显示屏内设置一个3维渲染图层进行三维场景渲染；

这里被绘制的三维场景中的点、线、面和物体模型的几何坐标都要转换成局部坐标系下的坐标[Xi Yi Zi]^T,即与漫游者的坐标X_d,Y_d,Z_d在同一个局部坐标系下；用OpenGL，OpenES，OS X OpenGL，Direct3D或osg绘图软件库实现局部坐标系下的三维绘图。

智能机摄像头的拍摄方向与屏幕观察方向一致时，可以智能机摄像头拍摄的视频图像为底图，叠加绘制三维场景中的点线面。

在本发明的一个实施例中，在上述实施例的基础上虚拟场景赋予真实的地理坐标，可用于地理信息浏览，地图漫游，虚拟旅游，3D真图游戏场景漫游控制，增强现实，数字城市,智慧城市漫游，房地产漫游等领域。

在所述步骤3)和步骤4)之间可增加如下操作：

(a)获取智能机的高度h_s,纬度θ_s和经度

θ_s是智能机的纬度，是智能机的经度,由智能机上自带的GPS接收模块获取；

(b)计算智能机所在经纬度的R_v

(c)计算智能机所在纬度θ_s经度高度h_s的地心坐标系坐标T_s

利用大地测量学中经典的转换公式

得到T_s；

其中

h_s是智能机离地面高度

N_s是随纬度变化的卯酉圈曲率半径；

a,b是地球椭球的长短轴，是大地测量学中的已知常量；

e是偏心率，是定值；

(d)根据地球上任何一个点的经度纬度θ_i和高程h_i得到该点地心坐标系坐标T_0i＝[X_0i Y_0i Z_0i]^T

利用大地测量学中经典的转换公式

得到T_0i；

其中

h_i是点离地面高度

N_i是随纬度变化的卯酉圈曲率半径；

a,b是地球椭球的长短轴，是大地测量学中的已知常量；

e是偏心率，是定值；

如果已知一个物体所在经纬度φ_i和θ_i，通过本步骤计算出它的地心坐标系坐标T_0i；

(e)地心坐标系坐标T_0i转换为局部坐标系中的坐标(X_i Y_i Z_i)^T

ΔT是局部坐标系中智能机显示屏的几何中心相对于智能机GPS接收天线的局部坐标系偏移。

在本发明的另一实施例中，可实现便携式智能机任意倾斜摆放条件下显示屏所显示的文字始终是按重力方向水平显示，这将会给用户极好的体验。具体实现为：

在步骤3)和步骤4)之间或在步骤4)之后，将智能机的显示屏设定为二维绘图模式并设置若干个二维标签，针对每一个二维标签，按如下步骤显示：

1)首先确定二维标签的中心位置u,v；

如果标签用于标注三维场景中局部坐标系坐标为(X,Y,Z)的目标，则根据以下公式确定目标显示的中心位置u,v

Z_O＝r₃₁(X-X_d)+r₃₂(Y-Y_d)+r₃₃(Z-Z_d)

式中：

c_x,c_y是智能机显示屏中心点的坐标，若显示屏是矩形,cx＝屏宽/2,cy＝屏高/2；

r₁₁～r₃₃是R矩阵对应行列的元素；

h是显示屏上下边缘垂直像素长，θ是漫游观察者自身观察的垂直视场角X_d,Y_d,Z_d是漫游观察者在局部坐标系中的坐标；

如果满足Z_O>0且u,v在显示屏范围内，则u,v保持不变,

否则需要按下法修正u,v

若Z_O＝0则将[u v]替换为从点(c_x,c_y)出发

按矢量{r₁₁(X-X_d)+r₁₂(Y-Y_d)+r₁₃(Z-Z_d),r₂₁(X-X_d)+r₂₂(Y-Y_d)+r₂₃(Z-Z_d)}

方向的射线与显示屏边缘相交的2维点；

若Z_O>0则将u,v替换成从点(c_x,c_y)到点(u,v)的连线与显示屏边缘相交的2维点；

若Z_O<0则将u,v替换成从点(u,v)到点(c_x,c_y)延长线与显示屏边缘相交的2维点；

如果二维标签不是用于标注目标，u,v为显示屏范围内的一个由界面设计者设定的像素坐标，或是用户手指触摸或鼠标点击的显示屏像素坐标；

2)按下述方法(1)或(2)计算矢量{Qx,Qy}

方法(1)Qx＝-r₂₃/La Qy＝r₁₃/La

其中r₂₃,r₁₃是上述方法中的R矩阵对应行列的元素,

方法(2)利用上述智能机姿态测定方法(1)所述加速度计的前两个输出值ax,ay得到

Qx＝-ay/La Qy＝ax/La,

3)在二维平面上经过以下矩阵旋转变换以后再绘制二维标签：

二维标签上每一点X，经过上式变换到坐标X’显示出来；

式中r取+是正放显示，取-是倒放显示，标签的水平轴始终平行于地面。

实际上就是以u,v为中心经过旋转变换显示标签。

叠加的内容可以是静止的目标如：商铺、地图上的地名，及线条围成的区域。

也可以是动态的目标如：好友，车船等交通工具。

4)得到局部坐标系坐标(Xi,Yi,Zi)的目标i的标签像坐标ui,vi之后，在智能机屏幕坐标 ui,vi区域附近设置感应区,等待接收用户触摸该区域的事件。当用户在智能机上点击ui,vi附近区域的时候，将执行与目标i相关联的操作。

包括以下操作之一：

1.弹出目标i更详细的广告信息，资费信息，需求信息,人气值；

2.向目标i发端信或者聊天或者打电话；

3.弹出与目标i关联的菜单或对话框；

4.向目标i发送请求；

5.获取目标i的数据；

6.向服务器发请求，使得目标i的人气值增加；

7.向服务器发请求，记录本次操作。

本发明实施例中，三维场景为一个空心的三维形体，三维形体为空心球，空心立方体、空心柱体、空心正多面体或空心锥体，三维形体的内壁以全景图作为纹理进行渲染；所述漫游观察者在虚拟场景中位于三维形体内部的某一点，从三维形体内部观察三维形体的内壁，实现利用智能机显示屏姿态控制全景浏览。该实施例实现了任意姿态拿放智能机时，浏览渲染全景始终保持正确的渲染效果。

本发明实施例标定智能机上的摄像头姿态矩阵的方法，包括以下步骤：

对姿态矩阵R_g进行校正，校正姿态矩阵

其中R＝ΔR×R_g；

ΔR为智能机的摄像头姿态矩阵R和智能机姿态矩阵R_g之间的一个固定的方向转换，当摄像头方向和智能机屏幕方向刚好相反时摄像头方向和智能机屏幕方向完全一致时否则采用摄影测量后方交汇法标定ΔR＝R_AR_g ^-1。R_A是摄影测量后方交汇法算得的相机姿态矩阵，R_g是根据上述方法测得此时智能机的姿态矩阵。

本发明实施例基于智能机姿态数据的多目视觉定位方法，可实现多张图像对某一物体空间3维定位。具体包括以下步骤：

步骤1：

在N个地理位置点拍摄同一个物体X，其中N≥2，测第i个观察点的以下几个量，其中i∈(1，2，…，N)：

(1)在视频帧图像中显示的位置(ui,vi)；

(2)拍摄点的地理位置(φi,θi,hi)，分别是第i个拍摄点的经度，纬度，高度；

(3)按上述方法测得第i个拍摄点的姿态矩阵R_i；

(4)第i个拍摄点的内部参数焦距长f_i，像主点坐标(c_xi,c_yi)；

步骤2：

计算相对于地心的N个姿态矩阵，i＝1～N，R_0i＝R_iR_vi

其中

步骤3：

利用公式得到N个拍摄点摄像头光心的地心坐标系坐标T_0i＝{Txi,Tyi,Tzi}^T i＝1～N；其中{ΔTxi,ΔTxi,ΔTzi}^T＝R_vi ^TΔT_i；ΔT_i是摄像头光心相对于GPS接收天线的偏移矢量；是将经度，纬度，高度转换成地心坐标系的X坐标的函数；是将经度，纬度，高度转换成地心坐标系的Y坐标的函数；是将经度，纬度，高度转换成地心坐标系的Z坐标的函数；

步骤4

根据拍摄点N的姿态矩阵R_0N，位置[T_xN T_yN T_zN]，在图像(单张图像或者视频都可以)中显示的位置坐标(u_N,v_N)，摄像头焦距像素长f_N,像主点坐标(c_xN，c_yN)都满足以下方程

其中r_0iab表示姿态矩阵R_0i的第a行第b列的元素值；i＝1～N一共列2N个方程，解(Xx,Xy,Xz)三个未知数；或采用摄影测量前方交汇方法计算出物体X得到地心坐标系坐标(Xx,Xy,Xz)；

最后由大地测量学公式得到地心坐标(Xx,Xy,Xz)对应的经纬度。

本发明基于智能机姿态数据的单目单点定位方法，利用姿态矩阵和物体像坐标实现物体3维坐标与相机3维坐标之间的相互定位，包括步骤：

假设智能机上的相机内部参数矩阵为:

被观察物体是一个直径＝D大小的物体,

在智能机上成像的宽度是Δu,高度是Δv,

成像中心点像坐标u,v,

采用上述方法测得智能机姿态矩阵R_g,进一步采用上述方法测得智能机上的相机姿态R＝△RR_g

以下分两种情况讨论：

(5)若已知智能机摄像头光心局部坐标系坐标是T,

可单目估算出物体的局部坐标系下三维坐标:

其中

(6)反过来若已知物体的局部坐标系下三维坐标X，可以求相机坐标T

这种情况实现了相机的单目定位。

本发明一种基于智能机姿态数据的全景影像生成方法，基于姿态矩阵实现便携式智能机拍摄合成全景图像，以及根据全景图像进行空间定位的方法，具体包括以下步骤：

步骤1、智能机在某一固定点用不同姿态拍摄n张图像，根据上述方法得到智能机摄像头的姿态矩阵分别为R_0i，i为0～n‐1之间的整数，n为大于等于1的整数；

步骤2、根据影像重叠区域迭代求解进行精确纠正

(3)提取图像间重合区域的同名点对。

定义H_ij＝K_jR_jR_i ^TK_i ^-1

对N张图像之间任意两张i,j进行匹配寻找图像上的同名点对，可以将第i张图像I_i经过透视变换H_ij后得到图像I_i’再与第j张图像I_j作细致匹配，这样同名点对没有考虑旋转和平移的干扰，不仅找得多而且找得准。最后将图像I_i上的匹配点坐标经过H_ij ^‐1逆变换还原成I_i图原始的坐标即可。

(2)根据上一步得到的同名点对匹配关系建立误差e相对于参数集

φ＝[... f_i q_i0 q_i1 q_i2 q_i3 ... f_j q_j0 q_j1 q_j2 q_j3 ...]^T

的数学模型e(φ)，

前面得到的R_0i转换成4元数q_i00,q_i01,q_i02,q_i03,i＝0,…,n‐1

以φ＝φ₀＝[... f_0i q_i00 q_i01 q_i02 q_i03 ... f_0j q_j00 q_j01 q_j02 q_j03 ...]^T作为初值,

采用LM法通过减小误差e，逐步得到精确化相机姿态q_i0,q_i1,q_i2,q_i3和焦距参数f_i。

最后将精确化的φ中的四元数q_i0,q_i1,q_i2,q_i3转化成姿态矩阵R_i,i＝0,…,n-1即可。

步骤3、开辟一张宽w高h大小的全景影像；

步骤4、扫描全景影像上每一个像素，针对全景图的每一个像素(x,y)进行如下(1)(2)处理(1)横坐标x转换成纵坐标y转换成θ；

其中w，h是全景图像的宽，高；

(2)依据小孔成像公式

其中f_i为相机i焦距像素长度，(c_ix,c_iy)是相机i的像主点，利用R_i将转换成映射到每个相机i的像坐标(u_i,v_i)，若(u_i,v_i)在图像范围内，则将相机i像坐标(u_i,v_i)处颜色取出，加权渲染到全景图(x,y)处；权重与(u_i,v_i)到该图像边缘最短距离有关，距离越短权重越小；若无一个相机的(u_i,v_i)在其图像范围内，渲染透明色或背景色到(x,y)处；

扫描完全景图像上所有点填充相应的颜色，即完成全景图像的渲染；

或者在步骤2中先将全景图像颜色值全部清0，在步骤3中针对n张图像将全景图像上的相应区每个像素扫描n轮，分n轮依次将加权颜色值累加在全景图的像素上；

步骤4全景图像的三维浏览和检索定位

(C)智能机姿态控制浏览三维全景图像

在虚拟场景中以球心为观察点，球体内壁以全景图作为纹理渲染一个球；设定智能机摄像头观察方向{r₃₁,r₃₂r₃₃}，自身向上方向{-r₂₁,-r₂₂,-r₂₃}；r_ij是姿态R矩阵中第i行第j列元素；方法见上文。

(B)全景图建库、检索和定位

为有效利用存储空间，除采用步骤3(1)中的公式外还可用墨卡托转换公式，

转换成全景图像，保持建库和检索的转换方式一致；

建库

提取全景图像的特征点集合，建立与全景图像关联的特征点数据库，库中每个特征点指向其所在的全景图像；

检索

同一地点c≥1张图像合成待检索全景图像并提取渲染区特征点，对每个特征点，查询库中与其最相似特征点所指向的全景图像，按特征点相似程度对库中指向的全景图像进行加权投票，按投票总和大小排序；

将待检索全景图中已渲染区和投票得到的库中全景图对应区域的像素集进行相关匹配计算，相关系数越大越相似；输出投票排序靠前且相关系数大于预设值的结果；如果c＝1, 也可根据单张拍摄姿态R，将库中全景图像映射到单张拍摄图像域与单张拍摄图像进行相关匹配；

定位

在已知GPS情况下，检索GPS附近区域的全景图像组成的子集特征库；

服务器数据库将全景图像与其地理位置坐标绑定在一起存储，检索结果是编号为i*的图像，即可在数据库中查找出i*所对应的地理位置。

基于智能机姿态数据的目标方向选择方法，可实现便携式智能机空间目标方向选择、控制、人机交互操作，具体包括以下步骤：

1.定义智能机主方向或载体主方向相对于智能机自身的矢量单位p；

2.采用上述方法获取智能机当前姿态矩阵R_g；

3.计算p_┴＝R_g ^Tp为智能机主方向或载体主方向相对于指北坐标系的矢量；

4.计算局部坐标系中的目标矢量v_o＝O-P

假设局部坐标系中智能机自身坐标为P,目标的坐标为O，则目标矢量v_o＝O-P；

5.计算矢量p_┴与目标矢量v_o之间夹角的余弦，或计算两个矢量之间的夹角。

如果夹角的余弦很大，余弦>0.9，接近1，或者夹角偏差的绝对值|dφ|很小则触发智能机与该目标O之间的某一关联事件。如选中该目标，击中该目标、通知该目标、联络该目标等。

可通过两矢量的点积除以|v_o|得到夹角dφ的余弦，

cos(dφ)＝(p_┴点积v_o)/|v_o|

如果该余弦cos(dφ)>cos(φa)则该目标方向选择事件可触发。

6.在需要与用户的可视化交互情况下，在指北坐标系中实时绘制目标，智能机，和矢量p_┴。

计算局部坐标系中智能机主方向矢量与目标矢量之间夹角的方向余弦。根据余弦值大小判定夹角。并且在三维空间或2维空间中绘制出智能机主方向矢量与目标矢量之间方向关系便于人机交互。

具体应用方式有以下几种类型：

方法1姿态指向

用户通过操作智能机所指主方向所形成的矢量与目标矢量之间的夹角保持较小角度，或夹角的余弦>cos(φa)，则将触发某一事件。

用户用智能机主方向指向某一目标，如果目标矢量与主方向矢量之间的夹角的余弦>cos(φa)可执行与该目标相关联的事件，如选中该目标，击中该目标、通知该目标、联络该目标等。Φa为最大容忍偏差角。

在三维空间中实时绘制智能机姿态矢量r_z与目标矢量v_o之间的关系示意图，以便于实现用户的交互控制。

实施方式：

假设智能机头部定义为主方向，头部相对于智能机自身坐标系的矢量是p＝{0,1,0}^T 那么p_┴＝R^Tp＝{r₂₁r₂₂r₂₃}^T其中r_ij是R矩阵第i行j列的元素。

假设智能机右侧定义为主方向，右侧相对于智能机自身坐标系的矢量是p＝{1,0,0}^T那么p_┴＝R^Tp＝{r₁₁r₁₂r₁₃}^T

方法2导航服务

将智能机与某一载体，如车船，固定起来，定义载体主方向相对于智能机自身的矢量p。

在地图中实时绘制载体主方向矢量p_┴＝R^Tp与目标矢量v_o之间的关系示意图。

计算夹角的余弦cos(dφ)>cos(φa)说明方向吻合，再结合距离约束可触发某一事件。

导航位置服务中同时符合位置和方向的目标位于图1中的阴影区。

本方法也可以与运动方向形成的轨迹矢量结合智能机主方向进行融合，进而计算与目标矢量之间的夹角。

实施方式：

摄像头拍摄方向朝前固定安装在车船上，摄像头拍摄方向相对于智能机自身坐标系的矢量是p＝{0,0,-1}^T

那么p_┴＝R^Tp＝{-r₃₁-r₃₂-r₃₃}^T其中r_ij是R矩阵第i行j列的元素。

p_┴与v_o之间的夹角的余弦＝(p_┴点积v_o)/|v_o|

方法3 2D导航服务

上述方法可简化到2维情况，在2维地图上实现2D导航。

这种方法要求智能机水平放置，利用方向传感器的第一个值value[0]得出主方向角矢量与智能机到目标矢量之间的夹角。或两者之间的点积。

在地图中实时绘制智能机主矢量{cosφ,sinφ}与目标矢量v_o＝O‐P示意图。

其中φ＝value[0]+△φvalue[0]是方向传感器的第一个值，value[0]＝0表示智能机头部指北，value[0]＝90表示智能机头部指东，△φ是在地图平面上，用户定义的主方向相对于智能机主方向之间夹角。

O是局部坐标系中X，Y平面上目标的2维坐标，P是局部坐标系中X，Y平面上的智能机2维坐标。或者建立一个以北x轴，西y轴的2维地图坐标系，O是目标在2维地图上的坐标，P是智能机在2维地图上的坐标。

{cosφ,sinφ}与目标矢量v_o之间的夹角<φa说明方向吻合。

或者{cosφ,sinφ}与目标矢量v_o点积除以|v_o|的结果>cosφa则说明方向吻合。

实施方式：

应用：导航，顺路方向的的士叫车抢单，司机将智能机固定在车上，智能机软件只可选择cos(dφ)>cos(φa)的乘客，选出来的都是顺路乘客。

2D导航的具体实施方式：

要求智能机水平放置，利用方向传感器的第一个值value[0]得出主方向角矢量与智能机到目标矢量之间的夹角，或两者之间的点积。

计算2维地图上的目标矢量v_o＝{v_ox，v_oy}的方位角φ_vo可调用反三角函数atan2(v_oy,v_ox)实现，计算φ角与φ_vo角形成的矢量在平面上的夹角

实施方法，采用但不局限于以下方法：

φ角与φ_vo之间的夹角可以按以下方式计算：

一般采用2维点积的方式计算φ角与φ_vo之间的夹角余弦＝({cosφ,sinφ}点积v_o)/|v_o|，若该余弦值>cos(φa)认定为方向一致。

本发明实施例基于智能机姿态数据的视频影像实时纠正方法，实现了基于姿态矩阵的智能机的视频图像实时纠正，能保持生成纠正的图像中的水平线始终对应客观场景的地平线，可用于颠簸的路面，海面场景视频图像的纠正。对于接近垂直于地面拍摄的情况，生成纠正的图像中的水平朝右方向始终对应客观场景中的局部坐标系的x轴(北)，可用于无人机地面图像拼接。该方法主要包括以下步骤：

1.采用上述方法获取智能机拍摄视频的每一帧对应的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R。△R是摄像头相对于智能机的转换矩阵。

2.根据实际应用需求R矩阵分解为R＝R_τR_从而得到矩阵R_τ，相机的拍摄姿态R不规则,期待得到用虚拟姿态R_作为拍摄姿态矩阵得到的标准图像。计算矩阵R_τ的方法有三类

(a)根据实际应用中R_的特性直接得到，再通过R_τ＝R R_^T得出R_τ

详见下文：标准对地坐标系下的影像正射纠正

(b)根据实际应用中R_τ和R_的特性列方程R_τ＝R R_^T或R＝R_τR_求解得到R_τ中的几个重要元素的值，从而构建R_τ。

详见下文：任意R矩阵的Z轴旋转分解出水平矩阵

(c)根据R_τ和R_的性质列方程R_τ＝R R_^T或R＝R_τR_求解得到R_中的几个重要元素的值，从而构建R_，最后通过R_τ＝R R_^T得出R_τ

详见下文：对地坐标系下的影像正射纠正

3.对图像I经过H＝K₂R_τ ^TK^-1的Homograph变换得到I’

其中是拍摄相机的内部参数矩阵，f是焦距的像素长度，c_x,c_y是相机主点坐标。对于理想相机c_x,c_y是图像的中心点，c_x＝w/2,c_y＝h/2,w,h是原始图像I的宽和高。

是纠正以后虚拟相机的内部参数矩阵，f2是纠正以后虚拟相机的焦距的像素长度，c_x2,c_y2是纠正以后虚拟相机主点坐标。

注：

-对于理想相机c_x2,c_y2是纠正以后虚拟相机图像的中心点，c_x2＝H/2,c_y2＝H/2,W,H是图像纠正以后虚拟相机的宽高。

-通常f₂选取与f相等，而W，H的大小根据实际应用需求选取，有时为避免纠正后图像出界而选取W>w H>h

-如果H＝K₂R_τ ^TK^-1中的第三行元素是[001]，那么Homograph透视变换可简化成仿射变换从而提高图像变换速度。

下面具体给出前述三类计算矩阵R_τ的应用案例和方法

(a)标准对地坐标系下的影像正射纠正

视频摄像机往下拍地面上的场景，但由于各种原因，视线不可能完全垂直于地面。为得到垂直于地面的图像，先通过以下方法计算R_τ

R＝R_τR_{_}中的

最后根据公式H＝K₂R_τ ^TK^-1实时将原始视频帧影像纠正成垂直于地面拍摄的效果，这个变换效果是图像I_┴始终是垂直于地面往下拍的效果，若姿态矩阵R是相对于局部指北坐标系的，图像I_┴的右侧始终指向北，下侧始终指东。垂直于地面的图像实际上可直接映射到地理信息系统地图平面上，因此本技术实现了从视频图像到地理信息系统地图平面上的实时映射。

(b)任意R矩阵的Z轴旋转分解出水平矩阵

对于红外末制导导引头图像获取，或者在颠簸的车、船、飞机上安装有摄像头的视频捕获到视频帧图像的同时，将该帧对应的姿态数据保留并加以处理，保证载体获取的图像始终是保持水平正方的，这样目标识别程序就不用考虑目标的旋转了，这会极大提高目标识别的效率和准确性，也能使得用户获取的视频更稳定更准确。本方法涉及第(b)类计算R的方法如下：

首先设

由R_＝R_τ ^TR列方程得到

针对矩阵第1行第3列元素等于0列方程

r₁₃cosτ+r₂₃sinτ＝0

再考虑到{cosτ,sinτ}是2维单位向量，解得

或

有两个解是正常的，因为旋转两个角度(还有一个相机底朝天，两者刚好相差180度)都能保证本体x轴保持水平于地面。因此必须保证本体y轴朝下，也就是R_的第2行第3列元素值<0,即列约束不等式：

-r₁₃sinτ+r₂₃cosτ＜0

解得代入上式得到

另一个解不满足条件舍去。

最后根据公式H＝K₂R_τ ^TK^-1实时将原始视频帧影像纠正成水平于地平面拍摄的效果。

(c)对地坐标系下的影像正射纠正

方法(a)得到的垂直纠正图像不会跟随摄像头绕垂直于地面方向轴的旋转而旋转，为了实现这个效果采用如下方法：

假设摄像头旋转了τ角，

R＝R_τR_{_}中的

列方程

解得:或

(d)最终得到：

(e)

最后根据公式H＝K₂R_τ ^TK^‐1实时将原始视频帧影像纠正成垂直于地面拍摄的效果，这

个变换效果是图像I_┴会跟随摄像头绕垂直于地面方向轴的旋转而旋转。

本发明实施例基于姿态数据的智能机单像定位方法，实现了根据姿态矩阵和图像上2个点的像坐标对相机自身进行定位，具体包括以下步骤：

假设相机或摄像机内参数据f和c_x,c_y已知，f是焦距的像素长度，c_x,c_y是相机主点坐标。内参矩阵

单像目标定位整体实施步骤如下：

1.搜集采集物方点，转化成局部坐标系坐标，如指北为x轴，指西为y轴；

2.采用上述方法获取智能机的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R。△R是摄像头相对于智能机的转换矩阵；

3.拍摄一张图像，自动提取其中的多个特征点，通过匹配算法得到与物点对应的n对物像同名点。

n个物方点的坐标是X_i＝[X_ix,X_iy,X_iz]^T i＝1,…,n

对应n个像点坐标是[u_i v_i]^T i＝1,…,n

4.分无错和容错2种情况计算相机在局部坐标系下的坐标T：

(1)若确认多个特征点无错误，则

采用数学模型解方程得到自身在局部坐标系下坐标Tx，Ty，Tz

其中

进而得到相机位置T，最大集ζ为n个点对组成的全集。

(2)若不确认多个特征点是否全部正确，采用Ransac容错算法，通过共线方程模型随机选取两对点n＝2,根据模型式(HH2)计算相机位置T，并通过共线方程z_i[u’_i v’_i 1]^T＝KR(X_i-T)验证T，

通过KR(X_i-T)算出的u’_i,v’_I与u_i,v_i之间的误差距离小于error像素认为是符合共线方程。选取符合共线方程点数最大集ζ并计算最大集ζ对应的T*。单目多点容错RANSAC算法流程如下：

8.从n个像点中随机挑选出2个距离足够大的像点，一般至少要大于容忍误差error的2倍；

9.挑选出来的2个像点与物点对应形成2对物象同名点，按式(HH2)计算智能机位置T；

10.用T验证所有n对物象同名点的共线方程对应关系，共线方程KR(X_i‐T)算出的u’_i,v’_I与u_i,v_i之间的误差距离小于error像素认为是符合共线方程，统计符合共线方程的个数i_in,如果i_in数量超过前期的数量n_max则记录本组满足共线方程容忍误差数量最大的点集ζ_max，并刷新n_max＝i_in；

11.更新迭代次数k+1→k

12.回到第1步，直到满足ransac概率收敛条件(1‐(n_max/n)²)^k<p_e p_e是RANSAC容忍错误概率，一般设为10^‐2～10^‐20之间的一个数值。

13.将前面搜索得到的最优内点集合ζ_max搜集起来进行最后一次多点最小2乘计算，按公式(HH2)计算得到T_best得到最终结果。

5.如需进一步提高精度，可基于数学模型式(HH2)对ζ集中多点进行LM优化。以期望得到更精确的解。

LM优化中一般是固定所有物点坐标X_ix,X_iy,X_iz,i＝1,…,n优化相机的位置T和姿态R

6.如需要得出目标的经纬度，可根据局部坐标系原点的经纬度将相机在局部坐标系下的坐标Tx，Ty，Tz转换成大地经纬度。

单像双点定位如图2所示。

说明：

真正实现了基于场景图像的室内定位，用于商场室内定位，导航，基于位姿的电子商务商铺广告发布。

本发明基于智能机姿态数据的相对定位算法，实现了基于姿态矩阵的双目图像或多目图像之间相对定位，传统的方法缺少相机姿态信息需要给初值，而本方法不需要给初值直接SVD解方程得到结果。包括以下步骤：

假设相机1，2之间有i＝1..n≥2对同名点u_1i,v_1i～u_2i,v_2i。

其中R₁，R₂是智能机相机在拍摄的1,2两个地点测出来的姿态矩阵数据。

已知相机1的内参矩阵相机2的内参矩阵其中c_xi,c_yi是相机i的像素主点坐标，f_i是相机i的焦距像素长。

已知两相机之间的距离是L；

可以计算相机2相对于相机1的位移矢量T。

方法步骤如下：

4.用上述方法计算智能机上相机的姿态R_g1,R_g2

计算R₁＝△RR_g1R₂＝△RR_g2

进一步得到R＝R₂R₁ ^T

5.构建矩阵A

其中

u_ji*＝K_i ^-1u_i＝[u_ji*x u_ji*y u_ji*z]^T i＝1,2 j＝1..n

u_jiR*＝Ru_ji*＝[u_jiR*x u_jiR*y u_jiR*z]^T

3.如果确认n对点都是正确的，则

求约束||t_3x1||＝1下A_nx3t_3x1＝0的解

或采用SVD分解得到A_nx3＝U_nxnW_nx3V^T _3x3,

分解出来的V的最后1列的矢量，即对应于最小奇异值的特征矢量，即为t

T＝-LR^Tt得到结果，结束

否则继续进行以下步骤

4.设置迭代初值n_max＝0 k＝1

5.随机挑选A矩阵中的2行,构建A_2x3

就是求约束||t₂||＝1下A_2x3t₂＝0的解

或采用SVD分解得到A_2x3＝U_2x2W_2x3V^T _3x3,

分解出来的V的最后1列的矢量即为t₂

6.将t₂＝[t_tx t_ty t_tz]^T代入第j＝1..n个点，采用式(AB)验证点j是否是内点,

(u_j2*zu_j1R*y-u_j2*yu_j1R*z)t_tx+(u_j2*xu_j1R*z-u_j2*zu_j1R*x)t_ty+(u_j2*yu_j1R*x-u_j2*xu_j1R*y)t_tz＜ε

                                           (AB)

其中ε表示最大容忍误差物方长度，暂且相对于两相机之间的距离＝1而言。

满足式(AB)则点j是内点，如果满足内点的数量n_t超过最佳值n_max则ζ_t-->ζ n_t-->n_max

7.如果(1-(n_max/n)²)^k<0.00001错误率足够小，退出迭代进入步骤9.

8迭代次数k增加1，回到步骤5反复迭代；

9.最后采用最小二乘法针对最大内点集ζ重新计算一次最优t,

求约束||t_3x1||＝1下A_n’x3t_3x1＝0的解其中n’＝n_max

或采用SVD分解得到A_n’x3＝U_n’xn’W_n’x3V^T _3x3,

分解出来的V的最后1列矢量,即对应于最小奇异值的特征矢量，即为t

最终得到T＝-LR^Tt。

本方法推广到C个相机,N个点的情况，构建矩阵

U(ij)_k＝[U(ij)_k1 U(ij)_k2 U(ij)_k3]k＝1,2

R_c(ij)ab是指第i个点对应的第j个像片的相机的姿态矩阵的a行b列u_c(ij)是第i个点对应的第j个像片上的像点坐标。c_x,c_y是相机主点坐标，f是相机的焦距。

对矩阵A进行SVD分解得到A＝UWV^T,V中最后一个非零矢量

X＝[T_1x T_1y T_1z...T_cx T_cy T_cz X₁ Y₁ Z₁ ...X_N Y_N Z_N]^T的每一个分量对应C个相机的位置T_1x,T_1y,T_1z,…,T_cx,T_cy,T_cz和N个点的坐标X₁,Y₁,Z₁,…,X_N,Y_N,Z_N。

说明，或研究背景

以前在未知相机姿态数据的情况下，两相机之间相对定向计算是需要至少5对点的，而本发明中在已知相机姿态的前提下，只需要至少2对即可计算相对位置了，可应用于，序列图像三维重建，基于图像的室内外定位等领域。

具体实施方式

本算法在得到相对姿态T以后可进一步虚拟出物方点，然后通过多相机LM算法优化R，T数据，使得精度进一步提高。

本发明基于智能机姿态的目标识别算法，可解决任意姿态拍摄视频仍能准确识别目标的难题，而不必考虑目标大角度旋转，仍能实现高效准确的目标检测。

该方法包括以下步骤：

通过H＝K₂R_τ ^TK^-1对原图像I进行纠正得到I'然后对纠正图像I'进行处理，处理得到I'图像上的坐标集x_i,y_i,i＝1,...,N再通过反纠正H^-1的方法得到坐标集在原图上映射的坐标x_i',y_i,'

I'图像处理的方式一般是目标识别，摄影测量，坐标检测。

具体实施方式：

1.传感器纠正目标识别算法

首先根据传感器得到的R矩阵对原始图I进行H＝K₂R_τ ^TK^-1标准化纠正得到标准纠正图I’，然后对I’图像进行目标检测、跟踪(如采用行扫描box方法)得到它对应的目标框r，目标框r相对于标准模板的平移是r_x,r_y，

矩形块r首先经过平移变换H_t映射到标准图上,然后再经过H^-1变换映射到原始图上。

因此目标框r相对于原始图像I的H变换为

r*＝H^-1H_tr

算法原理示意图如图3所示，由此定义H＝H^-1H_t实现了从标准目标矩形框r(计量单位像素)到原始图像4边形r*。

学习的时候扣选目标一定要从纠正标准图里选择矩形框。这样就能保证匹配识别都是在纠正标准图域进行的，这不需要考虑大角度旋转，大大减轻了图像目标检测的计算负荷。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (14)

1. 一种智能机姿态测定方法，其特征在于，包括以下步骤：

   1)定义局部坐标系：局部坐标系是指原点在智能机，Z轴指向地球表面法方向,Y轴或X轴与纬线相平行的坐标系；或局部坐标系原点位于地球上任意一点，Z轴与重力方向一致，北代表局部坐标系X轴，西代表局部坐标系Y轴；

   2)测定智能机姿态矩阵R_g，R_g是一个相对于局部坐标系的3x3单位正交矩阵，

   R_g由下述方法之一获得：

   方法(1)采用加速度传感器与磁力计传感器或方向传感器

   如果智能机有加速度传感器，且有磁力计传感器或方向传感器，用以下公式(1)计算R_g

   其中

   v_ax,v_ay,v_az是智能机加速度传感器测出的x,y,z三个方向的值，或是这三个值经滤波的结果；

   R_a是加速度传感器相对于智能机的姿态转换矩阵；

   当a_z≤0时，s＝-1，a_z>0时,s＝1；

   若采用方向传感器：通过方向传感器获取方位角将其带入公式(1)计算R_g；若采用磁力计传感器：计算

   其中R_m是磁力传感器相对于智能机的姿态转换矩阵；{v_mx,v_my,v_mz}是磁力传感器检测到 的三个值或这三个值经滤波的结果；

   再计算

   利用磁力传感器计算方位角的三角函数为：

   带入公式(1)计算R_g；

   方法(2)采用旋转矢量传感器

   如果智能机配置有旋转矢量传感器，若检测到旋转矢量传感器只有3个数据values[0],

   values[1],values[2]，

   则采用以下公式得到

   q₁＝values[0],q₂＝values[1],q₃＝values[2],

   若检测到旋转矢量传感器有4个数据values[0],values[1],values[2],values[3]

   则采用以下公式得到

   q₁＝values[0],q₂＝values[1],q₃＝values[2],q₀＝values[3]

   则R_g0矩阵为:

   根据智能机旋转矢量传感器坐标系定义的不同，得到R_g＝R_g0R_L ^T，其中R_L ^T是局部坐标系与旋转矢量传感器坐标系之间的变换矩阵；

   方法(3)采用陀螺仪、加速度传感器与磁力传感器迭代

   第1步选用方法(1)或方法(2)计算R_g的初值，将R_g转换成4元数q0,q1,q2,q3作为以下第2-7步迭代的初值；

   第2步设定exInt，eyInt，ezInt原始值为0，即exInt＝0，eyInt＝0，ezInt＝0；

   第3步根据接收到的磁力计矢量{mx,my,mz},得到正确磁场矢量{wx,wy,wz}，先将矢量{mx,my,mz}替换成将其单位化以后的矢量得到局部坐标系下磁场的正确的磁场方向矢量{bx,0,bz}，bz＝hz；其中：hx＝2×mx×(0.5-q2×q2-q3×q3)+2×my×(q1×q2-q0×q3)+2×mz×(q1×q3+q0×q2)；

   hy＝2×mx×(q1×q2+q0×q3)+2×my×(0.5-q1×q1-q3×q3)+2×mz×(q2×q3-q0×q1)；

   hz＝2×mx×(q1×q3-q0×q2)+2×my×(q2×q3+q0×q1)+2×mz×(0.5-q1×q1-q2×q2)；

   再转换到正确磁场矢量{wx,wy,wz}，其中

   wx＝2×bx×(0.5-q2×q2-q3×q3)+2×bz×(q1×q3-q0×q2)；

   wy＝2×bx×(q1×q2-q0×q3)+2×bz×(q0×q1+q2×q3)；

   wz＝2×bx×(q0×q2+q1×q3)+2×bz×(0.5-q1×q1-q2×q2)；

   第4步根据接收到的加速度传感器数据ax,ay,az,和{wx,wy,wz}得到误差矢量{ex,ey,ez}，并计算其累计值exInt，eyInt，ezInt；

   具体为：先将矢量{ax,ay,az}替换成将其单位化以后的矢量计算vx＝2*(q1*q3-q0*q2)；vy＝2*(q0*q1+q2*q3)；vz＝q0*q0-q1*q1-q2*q2+q3*q3；

   ex＝(ay×vz-az×vy)+(my×wz-mz×wy)；

   ey＝(az×vx-ax×vz)+(mz×wx-mx×wz)；

   ez＝(ax×vy-ay×vx)+(mx×wy-my×wx)；

   计算误差累计值：将exInt替换为exInt+ex×Ki；eyInt替换为eyInt+ey×Ki；ezInt替换为ezInt+ez×Ki；其中Ki为一可调节的正系数，Ki在0.00001至0.5中任意选取；

   若没有磁力计，则第3步不执行，同时将该第4步中的

   ex＝(ay×vz-az×vy)+(my×wz-mz×wy)；

   ey＝(az×vx-ax×vz)+(mz×wx-mx×wz)；

   ez＝(ax×vy-ay×vx)+(mx×wy-my×wx)；

   改为

   ex＝ay×vz-az×vy；

   ey＝az×vx-ax×vz；

   ez＝ax×vy-ay×vx；

   第5步根据误差矢量{ex,ey,ez}及其累计值纠正陀螺仪数据{gx0,gy0,gz0}

   假设智能机读出当前的一组陀螺仪数据为{gx0,gy0,gz0}，gx＝gx0+Kp×ex+exInt；gy＝gy0+Kp×ey+eyInt；gz＝gz0+Kp×ez+ezInt；其中Kp为一可调节的正系数，Kp在0.000001至20.0中任意选取；

   第6步根据陀螺仪数据gx,gy,gz修正四元数

   随着不断接收到陀螺仪数据gx,gy,gz，对4元数按如下方式修正：

   q₀替换为q₀+(-q₁×gx-q₂×gy–q₃×gz)×halfT；q₁替换为q₁+(q₀×gx-q₃×gy+q₂×gz)×halfT；q₂替换为q₂+(q₃×gx+q₀×gy-q₁×gz)×halfT；q₃替换为q₃+(-q₂×gx+q₁×gy+q₀×gz)×halfT；其中halfT为修正周期，halfT＝0.00001～-10.0；

   第7步输出R_g矩阵和四元数

   将四元数{q0,q1,q2,q3}单位化成输出；

   4元数转R_g矩阵公式如下

   第8步回到第3步继续接收陀螺仪运动数据更新姿态4元数q₀～q₃，在循环的过程中每次到第7步都能输出当前的R_g矩阵和四元数。
2. 一种智能机显示屏的姿态测定方法，其特征在于，包括以下步骤：

   智能机显示屏的姿态矩阵为

   r_x是智能机显示屏像素阵列x轴方向在局部坐标系中的分量；

   r_y是智能机显示屏像素阵列y轴方向在局部坐标系中的分量；

   r_z是垂直于智能机显示屏表面方向向智能机显示屏内部朝前看的视线方向在局部坐标系中的分量；

   1)按权利要求1的方法测定智能机姿态，并得到智能机的姿态矩阵R_g；

   2)计算R＝ΔR×R_g，ΔR为智能机显示屏姿态矩阵R相对于智能机姿态矩阵R_g的一个固定的方向的转换，智能机自身Z轴定义为智能机加速度传感器的z轴方向，

   当显示屏观察方向和智能机自身Z轴方向刚好相反时

   当显示屏观察方向和智能机自身Z轴方向完全一致时

   否则采用摄影测量方法标定ΔR＝R_AR_g ^-1，R_A是摄影测量后方交汇法算得的摄像头姿态矩阵，R_g是权利要求1方法测得此时智能机的姿态矩阵。
3. 一种基于智能机姿态的虚拟现实漫游方法，其特征在于，包括以下步骤：

   1)采用权利要求2所述的方法得到智能机显示屏的姿态矩阵R；

   2)根据姿态矩阵R移动控制漫游观察者在局部坐标系中的位置：

   定义漫游观察者是在局部坐标系中的一个具备自身位置、姿态且用于观察周边场景的对象；智能机显示屏所渲染的场景就是漫游观察者在局部坐标系中所应看到的场景图像；定义漫游观察者在局部坐标系中的坐标为(Xd,Yd,Zd)，

   采用以下方法控制漫游观察者在虚拟场景中移动：

   a)获取智能机漫游者手指在屏幕本体上的滑动数据；

   b)在滑动数据是漫游者单指相对于智能机屏幕本体滑动矢量△X,△Y的情况下，如果|r₃₃|>|r₁₃|且|r₃₃|>|r₂₃|，其中r_ij是R矩阵的i行j列元素值，则单指滑动后，在局部坐标系中的定位坐标(Xd,Yd)的改正数(v_x，v_y)，v_x＝ΔXr₁₁+ΔYr₂₁v_y＝ΔXr₁₂+ΔYr₂₂；否则：

   c)在滑动数据是漫游者双指做拉开或合拢操作所获得的情况下，则得到拉开和合拢之前双指之间的屏幕距离，以及拉开和合拢之后双指之间的屏幕距离；通过前后两次距离的差异修正Z_d；

   3)设置漫游观察者在局部坐标系里的位置和方向：

   根据漫游观察者的局部坐标系坐标(Xd,Yd,Zd)，以及步骤1)测定得到的姿态矩阵R，利用OpenGL，OpenES，OS X OpenGL，Direct3D或osg绘图软件库中用于设定相机位姿的图形库函数，设置漫游观察者在局部坐标系里的位置和方向：

   需要设定以下几个量:

   (1)漫游观察者自身观察的垂直视场角θ，就是智能机显示屏上边缘中点的视线方向与下边缘中点的视线方向的夹角；

   (2)漫游观察者所在的位置(X_d,Y_d,Z_d)，并根据步骤2)的漫游方法进行修正；

   (3)漫游者观察方向{r₃₁,r₃₂r₃₃}或者用户注视点坐标(X_d+r₃₁,Y_d+r₃₂,Z_d+r₃₃)

   (4)漫游者自身向上方向的矢量{-r₂₁,-r₂₂,-r₂₃}

   其中步骤(3)和(4)中的r_ij是智能机显示屏姿态R矩阵中的第i行j列，i＝2或3，j＝1、2或3；

   4)在局部坐标系下渲染绘制三维场景

   预先在智能机的显示屏内设置一个3维渲染图层进行三维场景渲染；

   被绘制的三维场景中的点、线、面和物体模型的几何坐标都要转换成局部坐标系下的坐标[Xi Yi Zi]^T,即与漫游者的坐标(X_d,Y_d,Z_d)在同一个局部坐标系下；用OpenGL，OpenES，OS X OpenGL，Direct3D或osg绘图软件库实现局部坐标系下的三维绘图；

   智能机摄像头的拍摄方向与屏幕观察方向一致时，可以智能机摄像头拍摄的视频图像为底图，叠加绘制三维场景中的点线面。
4. 根据权利要求3所述的方法，其特征在于：

   在所述步骤3)和步骤4)之间还包括步骤：

   (a)获取智能机的高度h_s,纬度θ_s和经度其中θ_s是智能机的纬度，是智能机的经度,由智能机上自带的GPS接收模块获取；

   (b)计算智能机所在经纬度的R_v

   (c)计算智能机所在纬度θ_s经度高度h_s的地心坐标系坐标T_s，

   利用大地测量学中经典的转换公式

   得到T_s，

   其中

   h_s是智能机离地面高度；

   N_s是随纬度变化的卯酉圈曲率半径；

   a,b是地球椭球的长短轴，是大地测量学中的已知常量；

   e是偏心率，是定值；

   (d)根据地球上任何一个点的经度纬度θ_i和高程h_i得到该点地心坐标系坐标T_0i＝[X_0i Y_0iZ_0i]^T

   利用大地测量学中经典的转换公式

   得到T_0i；

   其中

   h_i是点离地面高度；

   N_i是随纬度变化的卯酉圈曲率半径；

   a,b是地球椭球的长短轴，是大地测量学中的已知常量；

   e是偏心率，是定值；

   如果已知一个物体所在经纬度和θ_i，通过本步骤计算出它的地心坐标系坐标T_0i；

   (e)地心坐标系坐标T_0i转换为局部坐标系中的坐标(X_i Y_i Z_i)^T

   其中ΔT是局部坐标系中智能机显示屏的几何中心相对于智能机GPS接收天线的局部坐标系偏移。
5. 根据权利要求3所述的方法，其特征在于：

   在步骤3)和步骤4)之间或在步骤4)之后，还包括步骤：将智能机的显示屏设定为二维绘图模式并设置若干个二维标签，针对每一个二维标签，按如下步骤显示：

   1)首先确定二维标签的中心位置(u,v)；

   如果标签用于标注三维场景中局部坐标系坐标为(X,Y,Z)的目标，则根据以下公式确定目标显示的中心位置(u,v)

   Z_O＝r₃₁(X X_d)r₃₂(Y Y_d)r₃₃(Z Z_d)

   式中：

   (c_x,c_y)是智能机显示屏中心点的坐标，若显示屏是矩形,cx＝屏宽/2,cy＝屏高/2；

   r₁₁～r₃₃是R矩阵对应行列的元素；

   h是显示屏上下边缘垂直像素长，θ是漫游观察者自身观察的垂直视场角；

   (X_d,Y_d,Z_d)是漫游观察者在局部坐标系中的坐标；

   如果满足Z_O>0且中心位置(u,v)在显示屏范围内，则中心位置(u,v)保持不变,

   否则需要按下法修正(u,v)；

   若Z_O＝0则将中心位置(u，v)替换为从点(c_x,c_y)出发

   按矢量{r₁₁(X-X_d)+r₁₂(Y-Y_d)+r₁₃(Z-Z_d),r₂₁(X-X_d)+r₂₂(Y-Y_d)+r₂₃(Z-Z_d)}方向的射线与显示屏边缘相交的2维点；

   若Z_O>0则将中心位置(u，v)替换成从点(c_x,c_y)到点(u,v)的连线与显示屏边缘相交的2维点；

   若Z_O<0则将中心位置(u，v)替换成从点(u,v)到点(c_x,c_y)延长线与显示屏边缘相交的2维点；

   如果二维标签不是用于标注目标，中心位置(u,v)为显示屏范围内的一个由界面设计者设定的像素坐标，或是用户手指触摸或鼠标点击的显示屏像素坐标；

   2)按下述方法(1)或(2)计算矢量{Qx,Qy}

   方法(1)Qx＝-r₂₃/La Qy＝r₁₃/La

   其中r₂₃,r₁₃是权利要求2所述的方法中的R矩阵对应行列的元素,

   方法(2)利用权利要求1方法(1)加速度传感器的前两个输出值a_x,a_y得到

   Qx＝-a_y/La Qy＝a_x/La,

   3)在二维平面上经过以下矩阵旋转变换以后再绘制二维标签：

   二维标签上每一点X，经过上式变换到坐标X’显示出来；

   式中±取+是正放显示，取-是倒放显示，标签的水平轴始终平行于地面。

   即以二维标签的中心位置(u,v)为中心经过旋转变换显示标签；

   叠加的内容为静止的目标或者动态的目标。

   4)得到局部坐标系坐标(Xi,Yi,Zi)的目标i的标签像坐标(ui,vi)之后，在智能机屏幕坐标(ui,vi)区域附近设置感应区,等待接收用户触摸该区域的事件，当用户在智能机上点击(ui,vi)附近区域的时候，将执行与目标i相关联的操作。
6. 根据权利要求3所述的方法，其特征在于：

   所述三维场景为一个空心的三维形体，三维形体为空心球，空心立方体、空心柱体、空心正多面体或空心锥体，三维形体的内壁以全景图作为纹理进行渲染；所述漫游观察者在虚拟场景中位于三维形体内部的某一点，从三维形体内部观察三维形体的内壁，实现利用智能机显示屏姿态控制全景浏览。
7. 一种基于智能机姿态数据的多目视觉定位方法，其特征在于，包括以下步骤：

   步骤1：

   在N个地理位置点拍摄同一个物体X，其中N≥2，测第i个观察点的以下几个量，其中i∈(1，2，…，N)：

   (1)在视频帧图像中显示的位置(ui,vi)；

   (2)拍摄点的地理位置分别是第i个拍摄点的经度，纬度，高度；

   (3)按权利要求1的方法测得第i个拍摄点的摄像头姿态矩阵R_i

   (4)第i个拍摄点的内部参数焦距长f_i，像主点坐标(c_xi,c_yi)；

   步骤2：

   计算相对于地心的N个姿态矩阵，i＝1～N，R_0i＝R_iR_vi

   其中

   步骤3：

   利用公式得到N个拍摄点摄像头光心的地心坐标系坐标T_0i＝{T_xi,T_yi,T_zi}^T i＝1～N；其中{ΔT_xi,ΔT_xi,ΔT_zi}^T＝R_vi ^TΔT_i；ΔT_i是摄像头光心相对于GPS接收天线的偏移矢量；是将经度，纬度，高度转换成地心坐标系的X坐标的函数；是将经度，纬度，高度转换成地心坐标系的Y坐标的函数；是将经度，纬度，高度转换成地心坐标系的Z坐标的函数；

   步骤4

   根据拍摄点N的姿态矩阵R_0N，位置[T_xN T_yN T_zN]，在图像中显示的位置坐标(u_N,v_N)，摄像头焦距像素长f_N,像主点坐标(c_xN，c_yN)都满足以下方程

   其中r_0iab表示姿态矩阵R_0i的第a行第b列的元素值；i＝1～N一共列2N个方程，解(X_x,X_y,X_z)三个未知数；或采用摄影测量前方交汇方法计算出物体X得到地心坐标系坐标(X_x,X_y,X_z)；最后由大地测量学公式得到地心坐标(X_x,X_y,X_z)对应的经纬度。
8. 基于智能机姿态数据的单目单点定位方法，其特征在于，包括步骤：

   假设智能机上的相机内部参数矩阵为:

   被观察物体是一个直径＝D大小的物体,

   在智能机上成像的宽度是Δu,高度是Δv,

   成像中心点像坐标(u,v),

   采用权利要求1所述的方法测得智能机姿态矩阵R_g,并测得智能机上的摄像头姿态矩阵R＝△RR_g

   包括以下两种情况：

   (1)若已知智能机摄像头光心局部坐标系坐标是T,

   可单目估算出物体的局部坐标系下三维坐标:

   其中

   (2)若已知物体的局部坐标系下三维坐标X，可以求相机坐标T

   该情况实现了相机的单目定位。
9. 一种基于智能机姿态数据的全景影像生成方法，其特征在于，包括以下步骤：

   步骤1、智能机在某一固定点用不同姿态拍摄n张图像，根据权利要求1计算智能相机的姿态矩阵，根据智能相机的姿态矩阵再计算智能机摄像头姿态矩阵，分别为R_0i，i为0～n‐1之间的整数，n为大于等于1的整数；

   步骤2、根据影像重叠区域迭代求解进行精确纠正

   (1)提取图像间重合区域的同名点对

   定义H_ij＝K_jR_jR_i ^TK_i ^-1

   对N张图像之间任意两张i,j进行匹配寻找图像上的同名点对，可以将第i张图像I_i经

   过透视H_ij变换后得到图像I_i’再与第j张图像I_j作细致匹配，最后将图像I_i上的匹配点坐

   标经过H_ij ^‐1逆变换还原成I_i图原始的坐标；

   (2)根据上一步得到的同名点对匹配关系建立误差e相对于参数集

   φ＝[...f_iq_i0q_i1q_i2q_i3...f_j q_j0q_j1q_j2q_j3...]^T

   的数学模型e(φ)，

   前面得到的智能机摄像头姿态矩阵R_0i转换成4元数q_i00,q_i01,q_i02,q_i03,i＝0,…,n‐1

   以φ＝φ₀＝[...f_0i q_i00q_i01q_i02q_i03...f_0j q_j00q_j01q_j02q_j03...]^T作为初值,

   采用LM法通过减小误差e，逐步得到精确化相机姿态q_i0,q_i1,q_i2,q_i3和焦距参数f_i；

   最后将精确化的φ中的四元数q_i0,q_i1,q_i2,q_i3转化成姿态矩阵R_i,i＝0,…,n-1；

   步骤3、开辟一张宽w高h大小的全景影像，扫描全景影像上每一个像素，针对全景图的每一个像素(x,y)进行如下(1)(2)处理：

   (1)横坐标x转换成,纵坐标y转换成θ；

   其中w，h是全景图像的宽，高；

   (2)依据小孔成像公式

   其中f_i为相机i焦距像素长度，(c_ix,c_iy)是相机i的像主点；

   利用R_i将转换成映射到每个相机i的像坐标(u_i,v_i)，若(u_i,v_i)在图像范围内，则将相机i像坐标(u_i,v_i)处颜色取出，加权渲染到全景图(x,y)处；权重与(u_i,v_i)到该图像边缘最短距离有关，距离越短权重越小；若无一个相机的(u_i,v_i)在其图像范围内，渲染透明色或背景色到(x,y)处；扫描完全景图像上所有点填充相应的颜色，完成全景图像的渲染；

   或者，在步骤2中先将全景图像颜色值全部清0，在步骤3中针对n张图像将全景图像上的相应区每个像素扫描n轮，分n轮依次将加权颜色值累加在全景图的像素上；

   步骤4全景图像的三维浏览和检索定位

   (A)智能机姿态控制浏览三维全景图像

   在虚拟场景中以球心为观察点，球体内壁以全景图作为纹理渲染一个球；设定智能机摄像头观察方向{r₃₁,r₃₂r₃₃}，自身向上方向{-r₂₁,-r₂₂,-r₂₃}；r_ij是姿态矩阵R中第i行第j列元素；

   (B)全景图建库、检索和定位

   为有效利用存储空间，除采用步骤3(1)中的公式外还可用墨卡托转换公式，

   转换成全景图像，保持建库和检索的转换方式一致；

   建库

   提取全景图像的特征点集合，建立与全景图像关联的特征点数据库，库中每个特征点指向其所在的全景图像；

   检索

   同一地点c≥1张图像合成待检索全景图像并提取渲染区特征点，对每个特征点，查询库中与其最相似特征点所指向的全景图像，按特征点相似程度对库中指向的全景图像进行加权投票，按投票总和大小排序；

   将待检索全景图中已渲染区和投票得到的库中全景图对应区域的像素集进行相关匹配计算，相关系数越大越相似；输出投票排序靠前且相关系数大于预设值的结果；如果c＝1,也可根据单张拍摄姿态矩阵R，将库中全景图像映射到单张拍摄图像域与单张拍摄图像进行相关匹配；

   定位

   在已知GPS情况下，检索GPS附近区域的全景图像组成的子集特征库；

   服务器数据库将全景图像与其地理位置坐标绑定在一起存储，检索结果是编号为i*的图像，即可在数据库中查找出i*所对应的地理位置。
10. 一种基于智能机姿态数据的目标方向选择方法，其特征在于，包括以下步骤：

    1.定义智能机主方向或载体主方向相对于智能机自身的矢量单位p；

    2.采用权利要求1所述的方法获取智能机当前姿态矩阵R_g；

    3.计算p_┴＝R_g ^Tp为智能机主方向或载体主方向相对于指北坐标系的矢量；

    4.计算局部坐标系中的目标矢量v_o＝O-P，假设局部坐标系中智能机自身坐标为P,目标的 坐标为O，则目标矢量v_o＝O-P；

    5.计算矢量p_┴与目标矢量v_o之间夹角的余弦，或计算两个矢量之间的夹角；

    如果夹角的余弦>0.9，或者夹角偏差的绝对值|dφ|很小则触发智能机与该目标O之间的某一关联事件；

    通过两矢量的点积除以|v_o|得到夹角dφ的余弦，

    cos(dφ)＝(p_┴点积v_o)/|v_o|

    如果该余弦cos(dφ)>cos(φa)，φa为预设值，则该目标方向选择事件可触发；

    6.在需要与用户的可视化交互情况下，在指北坐标系中实时绘制目标、智能机和矢量p_┴；

    计算局部坐标系中智能机主方向矢量与目标矢量之间夹角的方向余弦，根据余弦值大小判定夹角，并且在三维空间或二维空间中绘制出智能机主方向矢量与目标矢量之间的方向关系，便于人机交互。
11. 一种基于智能机姿态数据的视频影像实时纠正方法，其特征在于，包括以下步骤：

    1.采用权利要求1的方法计算智能机拍摄视频的每一帧图像对应的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R，△R是摄像头相对于智能机的转换矩阵；

    2.将摄像头的姿态矩阵R分解为R＝R_τR_从而得到矩阵R_τ，相机的拍摄姿态R不规则,期待得到用虚拟姿态R_作为拍摄姿态矩阵得到的标准图像；

    3.对智能机拍摄视频的每一帧图像I经过H＝K₂R_τ ^TK^-1的Homograph变换得到I’；

    其中是拍摄相机的内部参数矩阵，f是焦距的像素长度，(c_x,c_y)是相机主点坐标；对于理想相机(c_x,c_y)是图像的中心点，c_x＝w/2,c_y＝h/2,w,h是原始图像I的宽和高；

    是纠正以后虚拟相机的内部参数矩阵，f2是纠正以后虚拟相机的焦距的像素长度，(c_x2,c_y2)是纠正以后虚拟相机主点坐标。
12. 一种基于姿态数据的智能机单像定位方法，其特征在于，包括以下步骤：

    假设相机或摄像机内参数据f和(c_x,c_y)已知，f是焦距的像素长度，(c_x,c_y)是相机主点坐标，

    内参矩阵

    单像目标定位整体实施步骤如下：

    1.搜集采集物方点，转化成局部坐标系坐标；

    2.采用权利要求1的方法获取智能机的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R，△R是摄像头相对于智能机的转换矩阵；

    3.拍摄一张图像，自动提取其中的多个特征点，通过匹配算法得到与物点对应的n对物像同名点；n个物方点的坐标是X_i＝[X_ix,X_iy,X_iz]^T i＝1,…,n，对应n个像点坐标是[u_i v_i]^Ti＝1,…,n；

    4.分无错和容错2种情况计算相机在局部坐标系下的坐标T：

    (1)若确认多个特征点无错误，则

    采用数学模型解方程得到自身在局部坐标系下坐标Tx，Ty，Tz

    其中

    进而得到相机位置T，最大集ζ为n个点对组成的全集；

    (2)若不确认多个特征点是否全部正确，采用Ransac容错算法，通过共线方程模型随机选取两对点n＝2,根据模型式(HH2)计算相机位置T，并通过共线方程z_i[u’_i v’_i 1]^T＝KR(X_i-T)验证T，

    通过KR(X_i-T)算出的u’_i,v’_I与u_i,v_i之间的误差距离小于error像素认为是符合共线方程，选取符合共线方程点数最大集ζ并计算最大集ζ对应的T*，单目多点Ransac容错算法流程如下：

    1)从n个像点中随机挑选出2个距离足够大的像点，至少大于容忍误差error的2倍；

    2)挑选出来的2个像点与物点对应形成2对物象同名点，按式(HH2)计算智能机位置T；

    3)用T验证所有n对物象同名点的共线方程对应关系，共线方程KR(X_i‐T)算出的u’_i,v’_I与u_i,v_i之间的误差距离小于error像素认为是符合共线方程，统计符合共线方程的个数i_in,如果i_in数量超过前期的数量n_max则记录本组满足共线方程容忍误差数量最大的点集ζ_max，并刷新n_max＝i_in；

    4)更新迭代次数k+1→k

    5)回到第1步，直到满足Ransac概率收敛条件(1‐(n_max/n)²)^k<p_e，p_e是Ransac容忍错误概率，为10^‐2～10^‐20之间的一个数值；

    6)将前面搜索得到的最大的点集ζ_max搜集起来进行最后一次多点最小2乘计算，按公式 (HH2)计算得到T_best得到最终结果；
13. 一种基于智能机姿态数据的相对定位算法，其特征在于，包括以下步骤：

    a)两相机之间相对定位

    假设相机1，2之间有i＝1..n≥2对同名点u_1i,v_1i～u_2i,v_2i；

    其中R₁，R₂是智能机相机在拍摄的1,2两个地点测出来的姿态矩阵数据；

    已知相机1的内参矩阵相机2的内参矩阵其中(c_xi,c_yi)是相机i的像素主点坐标，f_i是相机i的焦距像素长；

    已知两相机之间的距离是L；

    计算相机2相对于相机1的位移矢量T包括以下步骤：

    1.用权利要求1所述的方法计算智能机上相机的姿态R_g1,R_g2

    计算R₁＝△RR_g1R₂＝△RR_g2

    进一步得到R＝R₂R₁ ^T

    2.构建矩阵A

    其中

    u_ji*＝K_i ^-1u_i＝[u_ji*x u_ji*y u_ji*z]^T，

    u_jiR*＝Ru_ji*＝[u_jiR*x u_jiR*y u_jiR*z]^T，i＝1,2，j＝1..n；

    3.如果确认n对点都是正确的，则

    求约束||t_3x1||＝1下A_nx3t_3x1＝0的解

    或采用数学SVD分解得到A_nx3＝U_nxnW_nx3V^T _3x3,

    SVD分解出来的V矩阵的最后1列的矢量，即对应于最小奇异值的特征矢量为t，

    T＝-LR^Tt得到结果，结束

    否则继续进行以下步骤；

    4.设置迭代初值n_max＝0，k＝1；

    5.随机挑选A矩阵中的2行,构建A_2x3

    就是求约束||t₂||＝1下A_2x3t₂＝0的解

    或采用SVD分解得到A_2x3＝U_2x2W_2x3V^T _3x3,

    分解出来的V的最后1列的矢量即为t₂；

    6.将t₂＝[t_tx t_ty t_tz]^T代入第j＝1..n个点，采用式(AB)验证点j是否是内点,

    (u_j2*zu_j1R*y u_j2*yu_j1R*z)t_tx(u_j2*xu_j1R*z u_j2*zu_j1R*x)t_ty(u_j2*yu_j1R*x u_j2*xu_j1R*y)t_tz＜ε

    (AB)

    其中ε表示最大容忍误差物方长度，相对于两相机之间的距离＝1而言；

    满足式(AB)则点j是内点，如果满足内点的数量n_t超过最佳值n_max则ζ_t-->ζn_t-->n_max

    7.如果(1-(n_max/n)²)^k<0.00001错误率足够小，退出迭代进入步骤9；

    8迭代次数k增加1，回到步骤5反复迭代；

    9.最后采用最小二乘法针对最大内点集ζ重新计算一次最优t,

    求约束||t_3x1||＝1下A_n’x3t_3x1＝0的解，其中n’＝n_max

    或采用SVD分解得到A_n’x3＝U_n’xn’W_n’x3V^T _3x3,

    分解出来的V的最后1列矢量,即对应于最小奇异值的特征矢量，即为t

    最终得到T＝-LR^Tt；

    b)多个相机间的相对定位

    构建矩阵

    U(ij)_k＝[U(ij)_k1U(ij)_k2U(ij)_k3]k＝1,2

    r_c(ij)ab是指第i个点对应的第j个图像的相机的姿态矩阵的a行b列，u_c(ij)是第i个点对应的第j个图像上的像点坐标，(c_x,c_y)是相机主点坐标，f是相机的焦距；

    对矩阵A进行SVD分解得到A＝UWV^T,V中最后一个非零矢量

    X＝[T_1x T_1y T_1z...T_cx T_cy T_cz X₁Y₁Z₁...X_N Y_N Z_N]^T的每一个分量对应C个相机的位置T_1x,T_1y,T_1z,…,T_cx,T_cy,T_cz和N个点的坐标X₁,Y₁,Z₁,…,X_N,Y_N,Z_N。
14. 基于智能机姿态的目标识别算法，其特征在于，包括以下步骤：

    根据权利要求11的方法，通过H＝K₂R_τ ^TK^-1对智能机拍摄视频的图像I进行纠正得到I'；

    对纠正后的图像I'进行目标检测、识别处理，得到I'图像上目标的外接多边形点坐标集x_i,y_i,i＝1,...,N；

    通过反纠正H^-1的方法得到坐标集在原图上映射的坐标x_i',y_i'，即

    传感器纠正目标识别算法的具体实施方式：

    首先根据传感器得到的R矩阵对原始图I进行H＝K₂R_τ ^TK^-1标准化纠正得到标准纠正图I’，然后对I’图像进行目标检测、跟踪，得到它对应的目标框r，目标框r相对于标准模板的平移是r_x,r_y，

    目标框r首先经过平移变换H_t映射到标准图上,然后再经过H^-1变换映射到原始图上，因此目标框r相对于原始图像I的H变换为

    r*＝H^-1H_tr。