CN104750969B

CN104750969B - 智能机全方位增强现实信息叠加方法

Info

Publication number: CN104750969B
Application number: CN201410043385.XA
Authority: CN
Inventors: 刘进; 陶重芝
Original assignee: Individual
Current assignee: Wuhan Xiong Chu Gao Jing Technology Co Ltd
Priority date: 2013-12-29
Filing date: 2014-01-29
Publication date: 2018-01-26
Anticipated expiration: 2034-01-29
Also published as: CN104748728A; CN104750969A; CN104748728B

Abstract

本发明提供一种实景智能机全方位增强现实信息叠加方案，用户在三维空间中以任意姿态摆放智能机都能在叠加信息窗口内观察到周边地理目标的正确方位和距离。用户查找的地名可以在实景视频图像上作标注，智能机用户客户端之间也可方便的观察相互之间位置、方位关系，商铺也能借助服务平台广泛发布其广告信息。本发明中智能机包括一切含显示窗口和姿态传感的智能设备，如手机，平板电脑，笔记本电脑，带姿态传感的摄像头，相机，可穿戴设备，如眼镜，手套等设备。可在智能机Andriod，苹果系统上实现增强现实电子商务、查找好友，商铺，商铺广告信息叠加发布，摄影测量测绘，实景游戏等功能。本发明包含2种智能机姿态测定方法用于视频信息叠加，方法1利用旋转矢量传感器实现智能机姿态测定；方法2需要加速度计和磁力计以及陀螺仪配合，精度和稳定性都很高。

Description

智能机全方位增强现实信息叠加方法

技术领域

本发明涉及信息技术领域，具体涉及一种智能机全方位增强现实信息叠加方法。

背景技术

增强现实技术是将虚拟的目标标签叠加到现实场景中,提示信息给用户，并能支持用户与其进行交互。

由于智能机姿态不一样，同样位置看到的现实场景不一样，智能机姿态的精度影响导航精度和增强现实信息叠加精度，因此智能机姿态的精确测定具有重要意义。

增强现实应用中对智能机姿态精度提出了更高要求。传统的智能机姿态测定，主要利用姿态传感器的单轴航向角数据信息，系统对智能机姿态摆放有特定要求，如要求智能机平行于地面，或垂直于地面摆放，而增强现实应用中用户手持智能机的姿态千变万化，并不一定严格平行或者垂直于地面，这将导致导航方位不准。此外智能机姿态测定的误差将带来信息叠加的误差，影响增强现实用户的体验，因此迫切需要一种考虑智能机X-Y-Z三轴旋转的姿态测定算法。

基于位置的服务LBS日益受关注，但用户更关心的不仅是位置，而是自己与目标间的方向和距离。很多老年人和方向感不强的人很难将实际看到的生活场景与地图中的东南西北联系在一起，因此智能机任意角度摆放情况下，叠加在实景视频上的目标信息及其相关服务将给用户带来所见即所指的极大便利，将成为未来信息服务的发展新趋势，我们将这种服务称为“智能机全方位信息叠加服务”。

应用：

1.为商铺以及供需双方发布相对于其他海量用户的方位和距离提供视觉叠加；

2.为用户相对于指定地名，指定单位的方位与距离提供视觉叠加服务；

3.为用户之间相互联络提供方位和距离视觉叠加；

4.为车、船、物流提供货物相对于目的地的方位和距离视觉叠加。

传统的导航服务是地图导航服务，用户看到的是地图，用户需要费力的去把实景与地图联想起来才能得到正确的方向。另外传统的地图服务的更新主要是服务上定期下载，缺少用户的参与，不是动态的。

传统的姿态检测技术主要用于飞机，汽车等大型设备，追求很高精度，用于军事、工业等领域，而且非常贵重，而本发明主要考虑用廉价的智能机终端实现普通个人周边地区的视频信息叠加服务。

本发明不仅考虑方向传感器的value[0]得到的航向角，在有其他传感器，如重力加速度磁力计和陀螺仪情况下，还考虑了其他几种姿态传感器捕获到的数据，使得检测到的姿态更加精确，在没有这些传感器条件下也能得到粗略姿态。智能机的姿态对于全方位信息叠加有着重要作用。本发明可以在智能机，诸如Andriod，苹果系统上方便地实现查找好友，商铺的位置，商铺广告信息的叠加发布等功能。

本发明提供的是基于视频的实景智能机全方位信息叠加服务，用户看到的是智能机载相机拍摄的实景图像，地名，方向、用户需要查找的地名都可以在实景视频图像上作标注，智能机用户（手机、平板电脑或相机）客户端之间也可以进行方便的用视频实景预览观察相互之间的位置、方位关系。这样就为用户提供了一种非常便捷、直观的可视化信息服务，同时商铺也能借助服务平台广泛发布其广告信息。

本发明中智能机的含义包括内置相机、视频的手机系统，内置网络IP的摄像头系统，内置网络IP的相机系统，IPhone,平板电脑，以及各种同时植入摄像机和网络IP的系统等。

发明内容

本发明所要解决的问题是提供一种智能机全方位信息叠加方法，该方法使用户在智能机上获得便捷、直观的可视化信息。

本发明提供的技术方案是，一种智能机全方位增强现实信息叠加方法，包括以下步骤：

1）通过智能机获取周边目标i相对于智能机的局部坐标系坐标(Xi，Yi，Zi);

其中局部坐标系是指原点在智能机，Z轴指向地球表面法方向,Y轴或X轴与纬线相平行的坐标系；

上述三维坐标(Xi，Yi，Zi)中的Xi，Yi，Zi由下法得到：

其中R_v=

θ_a是智能机纬度，是智能机经度；

T_i是目标i的地球坐标； T_s是智能机的地球坐标；

这种情况下Z轴是沿着地球表面法向量指向地球外，也可以采用Z轴沿着地球表面法向量指向地心。这时候

Rv=

2) 计算目标i在智能机上信息叠加的位置ui,vi

确定目标i（Xi，Yi，Zi）在视频帧图像中显示的位置（u_i，v_i）；在智能机视频窗口中的目标i位置（u_i,v_i ）处叠加显示目标i的信息；

目标i（Xi，Yi，Zi）在视频帧图像中显示的位置（u_i，v_i）

按如下公式计算：

（1）

式中：

Z_Oi=

f_x,f_y为摄像头内部参数中的焦距像素长度， c_x，c_y为摄像头内部参数中的主点，

X_i，Y_i，Z_i为目标i的位置坐标

、、、、、、、和由下式得到：

其中R=∆R×R_g；

∆R为智能机的摄像头姿态矩阵R和智能机姿态矩阵R_g之间的一个固定的方向的转换，取∆R=

R_g由下法之一获得：

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

首先采用(a)(b)(c)中任何一种得到R_g0矩阵

(a)智能机上指定传感器类型为旋转矢量类型TYPE_ROTATION_VECTOR,实时得到旋转矢量传感器的观测值R_vec 利用智能机系统上的旋转矢量转矩阵函数

将R_vec转换成绘图转换矩阵 R₄₄

R₄₄是一个绘图转换矩阵，其左上角3x3矩阵就是R_g0

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

采用以下公式得到

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

q₀=

再将q₀,q₁,q₂,q₃按以下公式转换成R_g0矩阵

R_g0=

(c)如果检测智能机上的旋转矢量传感器有4个数据，

p₀=values[3], p₁=values[0], p₂=values[1], p₃=values[2]

计算

q₀=p₀/L_p

q₁=p₁/L_p

q₂=p₂ /L_p

q₃=p₃ /L_p

其中L_p=

再将q₀,q₁,q₂,q₃按以下公式转换成R_g0矩阵

R_g0=

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

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

R_g=R_g0

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

R_g=R_g0

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

R_g=R_g0

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

第1步选用以下(a)(b) 2种方法之一设定q0,q1,q2,q3的初值

(a)将方法(1)得到的R_g转换成4元数q0,q1,q2,q3作为以下第2-7步迭代的初值

(b)采取以下方法得到q0,q1,q2,q3初值

R_g=

转换为4元数q0, q1，q2，q3 其中

R_θ=

、、是智能机测出来的智能机的重力加速度的x、y、z三个方向的分量，

L= 当<0时，s = -1，a_z>0时,s = 1；

若采用方向传感器：= value[0]

若采用磁力计传感器：检测到磁力传感器矢量m={m_x,m_y,m_z}，计算

=

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

将4元数q0, q1，q2，q3作为以下迭代计算的初值，

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

exInt=0，eyInt=0，ezInt=0

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

先得到局部坐标系下磁场的正确的磁场方向矢量{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);

bx = ;

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

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中任意选取；优选Ki =0.005。

第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中任意选取；优选Kp = 2.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步输出4元数转R_g矩阵的公式如下：

R_g=

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

通过上述步骤得到R_g，进而带入公式(1)得到N个目标i=1~N的像坐标ui,vi. 就可以在ui,vi处增强显示多个目标的信息；其中最大目标数N的取值范围是1~200。

3) 根据ui,vi在智能机上叠加显示目标i的信息

如果Z_Oi≥0且u，v在图像矩形框内部，即ui≥0且ui<w且vi≥0且vi<h, w,h分别是图像宽高，则在智能机显示屏内部标示出目标的距离L_i=，广告信息等属性。

如果需要在边缘标示视野外目标才进行以下操作：

如果Z_Oi≥0且u，v在图像矩形框外部，即ui<0或ui≥w或vi<0或vi≥h, w,h分别是图像宽高，则在智能机显示屏边缘部分标示出一个指向目标的箭头。该箭头针尖坐标ex,ey正好是相机主点cx,cy到ui,vi两点之间的连线与图像边缘的相交点，箭头末端为ex-dx*s,ey--dy*s。其中：

s为箭头的像素长度

dx=(ui-cx)/L₂

dy=(vi-cy)/L₂

L₂=

ex,ey坐标可通过已有成熟技术在2维平面上计算直线交点得到。

如果Z_Oi<0时，可采用两种方法之一

方法1，反向延长法

Z_Oi<0的时候仍采用共线方程公式

反向连接ui,vi到c_x,c_y的连线延长线，直至与图像边框相交，图像边框相交处绘制箭头，标示视野范围外目标方位。

该箭头针尖坐标ex,ey正好是相机主点cx,cy到ui,vi两点之间的连线与图像边缘的相交点，箭头末端为ex+dx*s,ey+dy*s。其中：

s为箭头的像素长度

dx=(u_i-cx)/L₂

dy=(v_i-cy)/L₂

L₂=

方法2，Z映像法

Z映像法本质上和反向延长法类似，方法如下：

Z_O<0说明搜寻目标在视线之后，根本看不到，所以先将Z_O变成-Z_O，使其影像到视野前方，再计算映像点的像坐标u_,v_

不管映像点的像坐标ui_,vi_是否在象框内部，都采用以下方法标示，

图像上连接c_x,c_y与u_,v_坐标，在图像边框相交处绘制箭头，标示视野范围外目标方位。若u_,v_在相框内部，延长c_x,c_y到ui_,vi_的连线，直至与图像边框相交，图像边框相交处绘制箭头，标示视野范围外目标方位。

上述目标i的信息包括目标i的名称、符号属性、类别、与智能机的距离、角度和/或高度。

上述步骤2）中得到多个目标的坐标u₁,v₁~u_N,v_N之后

当目标数量n>N时，只在视频上叠加显示排名前N的目标子集，显示的位置为目标i的成像坐标ui,vi，否则n≤N时按实际数量n显示；

目标按照评价函数f(Mi)从大到小的顺序排序；

f(Mi)是变量Mi的单调递增函数，Mi表示目标i的重要性评价,Mi与目标i的广告费用、用户信誉度和人气相关，

叠加信息的内容包括,目标i的名字,目标到智能机的距离

Li=。

或者，上述步骤2）完成之后，执行以下步骤：

（1）将智能机上摄像头的姿态矩阵R=设置为用于叠加显示的智能机模型绘制矩阵；

（2）按目标i局部坐标Xi，Yi，Zi绘制三维目标。

本发明可使用户在三维空间中以任意姿态摆放智能机都能在叠加信息窗口内观察到周边地理目标的正确方位和距离。用户查找的地名可以在实景视频图像上作标注，智能机用户客户端之间也可方便的观察相互之间位置、方位关系，商铺也能借助服务平台广泛发布其广告信息。

本发明中智能机包括一切含显示窗口和姿态传感的智能设备，如手机，平板电脑，笔记本电脑，带姿态传感的摄像头，相机，可穿戴设备，如眼镜，手套等设备。可在智能机Andriod，苹果系统上实现增强现实电子商务、查找好友，商铺，商铺广告信息叠加发布，摄影测量测绘，实景游戏等功能。

本发明包含2种智能机姿态测定方法用于视频信息叠加，方法1利用旋转矢量传感器实现智能机姿态测定；方法2需要加速度计和磁力计以及陀螺仪配合，精度和稳定性都很高。可使用户在智能机上获得便捷、直观的可视化信息。

附图说明

图1为本发明手机坐标系示意图，其中屏幕正面z>0；

图2为本发明相机坐标系示意图，其中摄像头拍摄方向z>0。

图3为本发明在智能机显示屏内部标示出目标的示意图。

图4为本发明在智能机显示屏边缘部分标示出一个指向目标的箭头的示意图。

图5为本发明标示视野范围外目标方位图。

图6为本发明在图像边框相交处绘制箭头，标示视野范围外目标方位图。

图7为本发明在图像边框相交处绘制箭头，标示视野范围外目标方位图。

具体实施方式

智能机周边地理数据的上传

智能机将目标名称与本地经纬度，高程信息绑定在一起发送到服务器，服务器将绑定有单位名称、经纬度，高程信息的数据保存到数据库，数据库以经纬度为索引进行保存以便实现快速索引查找。

智能机周边地理数据的获取及显示

智能机有两种方法获取周边数据：

1.智能机将本地经纬度，高程信息周期性发到服务器，服务器收到以后查询改经纬度的周边目标列表信息，反馈包含GPS坐标的周边目标列表信息到客户端。

2.智能机定期从服务器下更新周边数据，在本地查找周边目标的位置信息和名称。

客户端收到周边目标列表信息中的经纬度φ_a,θ_a高程h后，通过现有技术转换成局部坐标系三维坐标∆X, ∆Y, ∆Z。

以下列举一种近似的转换方法

其中R_v=

θ_a是智能机纬度是智能机经度

，θ_a也可以采用目标的经纬度

T_i是目标i的地球坐标, 可通过地图资料预先测定得到；. T_s是智能机的地球坐标,可通过智能机上的GPS信号接收模块或北斗接收模块获取

R_earth是地球半径

h_a是目标的高度

R_earth是地球半径

h_i是目标i高度,

θ_i是目标i纬度是目标i经度。

普通智能机系统的信息叠加方法

在其摄像头视频预览中动态叠加显示周边目标的信息。每个目标在视频帧图像中显示的位置按如下公式计算：

假设

其中Z_O= f_x,f_y为摄像头内参焦距，可选取 c_x，c_y为摄像头内参主点，可近似选取图像中心。

描述智能机载摄像机的姿态矩阵R=∆RR_g

其中R_g用后面“智能机姿态矩阵R_g的计算”一节描述的方法实现。

∆R在“检校相机摄像头光心与视觉叠加客户端之间的位置关系”一节中作了定义。在坐标u,v位置显示该目标X,Y,Z的信息，采用如下步骤实现：

1．得到相机的姿态矩阵R=∆RR_g

2．得到智能机相机的坐标Xs, Ys, Zs

3．逐个遍历周边数据集S中的每一个目标，根据其经纬度转换成局部坐标系3维坐标Xi，Yi，Zi，

4．利用公式计算得到第i个目标的像坐标u_i,v_i。

5．将目标i绘制在智能机显示窗口的坐标u_i,v_i处，绘制一个对应于目标i的标记，并标上对应于该目标的文字。

6．将坐标系还原到地球坐标系，回到第3步，直到周边数据集S全部绘制完。

3) 根据ui,vi在智能机上叠加显示目标i的信息

如果Z_Oi≥0且u，v在图像矩形框内部，即ui≥0且ui<w且vi≥0且vi<h,w,h分别是图像宽高，则在智能机显示屏内部标示出目标的距离L_i=，广告信息等属性（如图3所示）。

如果需要在边缘标示视野外目标才进行以下操作：

如果Z_Oi≥0且u，v在图像矩形框外部，即ui<0或ui≥w或vi<0或vi≥h,w,h分别是图像宽高，则在智能机显示屏边缘部分标示出一个指向目标的箭头（如图4所示）。该箭头针尖坐标ex,ey正好是相机主点cx,cy到ui,vi两点之间的连线与图像边缘的相交点，箭头末端为ex-dx*s,ey--dy*s。其中：

s为箭头的像素长度

dx=(ui-cx)/L₂

dy=(vi-cy)/L₂

L₂=

如果Z_Oi<0时，可采用两种方法之一

方法1，反向延长法

Z_Oi<0的时候仍采用共线方程公式

反向连接ui,vi到c_x,c_y的连线延长线，直至与图像边框相交，图像边框相交处绘制箭头，标示视野范围外目标方位如图5所示。

s为箭头的像素长度

dx=(u_i-cx)/L₂

dy=(v_i-cy)/L₂

L₂=

方法2，Z映像法

Z映像法本质上和反向延长法类似，方法如下：

图像上连接c_x,c_y与u_,v_坐标，在图像边框相交处绘制箭头，标示视野范围外目标方位（如图6所示）。若u_,v_在相框内部，延长c_x,c_y到ui_,vi_的连线，直至与图像边框相交，图像边框相交处绘制箭头，标示视野范围外目标方位如下图7所示。

周边信息数据缓存

客户端的搜集到的周边目标集合U可按照某一关于客户端特性的函数F从大到小排序的，假设显示缓存长度最大是N，显示的时候从0~N-1循环遍历U中每一个不重复的目标，判断是否在图像视野范围内，若在，则搜集到集合S，当集合S大小超过单帧最大显示总数M，或者整个N长度全部遍历完了，则刷新显示S集合里每一个目标。

上述步骤2）中得到多个目标的坐标u₁,v₁~u_N,v_N之后，给定一个最大目标数N，N取值范围1~200，当目标数量n>N时，只在视频上叠加显示排名前N的目标子集，显示的位置为目标i的成像坐标ui,vi，否则n≤N时按实际数量n显示。

目标按照某一评价函数f(Mi)进行排序。

f(Mi)是变量Mi的单调递增函数，如

f(Mi)=kMi/Li

Mi表示目标i的重要性评价,Mi与目标i的服务缴费情况、用户信誉度，人气相关。

叠加信息的内容包括,目标i的名字,目标到智能机的距离

Li=

目标发布的广告、通知，目标的等级等与目标相关的各种信息，如上铺标题，与智能机的距离，人气，等信息，根据具体应用选择标示。

有OpenGL或者高版本OpenES的智能机系统的信息叠加方法

可用”普通智能机系统的信息叠加方法”一节描述的方法，确定的目标在视频图像上叠加的位置进行叠加显示。也可以在有OpenGL或者高版本OpenES的智能机系统中采用如下步骤实现：

1．根据“智能机姿态矩阵R的计算”一节介绍的方法得到相机的姿态矩阵R

2．根据“检校相机摄像头光心与视觉叠加客户端之间的位置关系”一节介绍的方法得到智能机相机的位置T

3．将相机的R矩阵和T设置为模型绘制矩阵

4．逐个遍历周边数据集S中的每一个目标，根据其经纬度转换成3维坐标Xi，Yi，Zi，并将模型绘图矩阵转换到Xi，Yi，Zi坐标处。

5．绘制一个对应于该目标的标记，并标上对应于该目标的文字

6．将坐标系还原到地球坐标系，回到第2步，直到周边数据集S全部绘制完。

智能机姿态矩阵R的计算

姿态矩阵R的计算步骤

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

首先采用(a)(b)(c)中任何一种得到R_g0矩阵

(a)智能机上指定传感器类型为TYPE_ROTATION_VECTOR,实时得到旋转矢量传感器的观测值R_vec；直接用智能机系统上的旋转矢量转矩阵函数 SensorManager.getRotationMatrixFromVector(R₄₄, R_vec)，将检测到的旋转矢量观测值R_vec转换成绘图转换矩阵, R₄₄

R₄₄是一个绘图转换矩阵，其左上角3x3矩阵就是R_g0

采用以下公式得到

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

q₀=

再将q₀,q₁,q₂,q₃按以下公式转换成R_g0矩阵

R_g0=

(c)如果检测智能机上的旋转矢量传感器有4个数据，

p₀=values[3], p₁=values[0], p₂=values[1], p₃=values[2]

计算

q₀=p₀/L_p

q₁=p₁/L_p

q₂=p₂ /L_p

q₃=p₃ /L_p

其中L_p

再将q₀,q₁,q₂,q₃按以下公式转换成R_g0矩阵

R_g0=

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

R_g=R_g0

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

R_g=R_g0

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

R_g=R_g0

方法（2）用陀螺仪

第1步初始状态时给定手机姿态R_g矩阵

(1)在仅有方向传感器的情况下

用公式R_g=

其中φ是方向传感器的value[0],

θ_xθ_y分别对应于value[1]，value[2]

根据智能机的机型不同和纵屏、横屏显示模式的不同，value[1]，value[2]的定义有所不同，有的智能机纵屏时θ_x= value[1] θ_y= value[2]横屏时θ_x= value[2] θ_x= value[1]

有的智能机则是与上相反的，具体实现要根据不同智能机型号进行具体调整。

注：

由于value[1]，value[2]容易搞混，也可不考虑智能机的俯仰角和翻滚角，也可近似取θ_x=θ_y=0

这样以上公式变成了

R_g =

可标明智能机的航向方向，但用户体验不了俯仰角和翻滚角的叠加效果。

在有重力传感器和磁力计或方向传感器情况下

用公式

R_g=

其中

a_x,a_y,a_z是智能机测出来的加速度的三个分量。

L=

当a_z≤0时 s = 1

a_z>0时,s = -1

对于有的智能机，正好是反的，则写成

当a_z≤0时 s = -1

a_z>0时,s = 1

若配合采用方向传感器，则方向角value[0]正好就是角。

若配合采用磁力计，假设检测到本体磁力方向m={m_x,m_y,m_z}，计算

=

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

可不断的检测重力加速度{a_x,a_y,a_z}和磁力计或方向传感器，代入到公式（1）循环更新R姿态数据。

在有重力加速度计，磁力计，并配合有陀螺仪数据g_x,g_y,g_z情况下，

也可按如下步骤2-7，使得姿态转动更加平滑流畅。

根据现有技术将前面得到的R矩阵转换为4元数q₀, q₁，q₂，q₃

R矩阵转4元数是现有技术，有很多种办法实现转换，基本思想就是将R矩阵中的对称元素相加减列方程解出q₀~q₃

这里给出其中一种实施方案，一般四元数的计算矩阵运算如下:

[q₀ ²+ q₁ ²- q₂ ²- q₃ ² , 2q₁q₂-2q₀ q₃ , 2q₁q₃+2q₀q₂ ]

[ 2q₁q₂+2q₀q₃ , q₀ ²- q₁ ²- q₂ ²- q₃ ² , 2q₂q₃-2q₀q₁ ]

[ 2q₁q₃-2q₀q₂ , 2q₂q₃+2q₀ q₁ , q₀ ²- q₁ ²- q₂ ²- q₃ ² ]

但是对于范化的四元数, 可以简化上述的矩阵变换:

[ 1-2q₂ ²-2q₃ ² , 2q₁q₂-2q₀q₃ , 2q₁q₃+2q₀q₂ ]

[ 2q₁q₂+2q₀q₃ , 1-2q₁ ²-2q₃ ² , 2q₂q₃-2q₀q₁ ]

[ 2q₁q₃-2q₀q₂ , 2q₂q₃+2q₀q₁ , 1-2q₁ ²-2q₂ ² ]

R_3x3矩阵转换四元数实现C代码如下:

void MatToQuat( float m[3][3], Quat× quat )

{

float tr, s, q[4];

int i, j, k;

int nxt[3] = {1, 2, 0 };

//计算矩阵轨迹

tr = m[0][0] + m[1][1] + m[2][2];

//检查矩阵轨迹是正还是负

if(tr>0.0)

{

s = sqrt(tr + 1.0);

quat->w = s / 2.0;

s = 0.5 / s;

quat->x = (m[1][2] - m[2][1]) × s;

quat->y = (m[2][0] - m[0][2]) × s;

quat->z = (m[0][1] - m[1][0]) × s;

} else

{

//轨迹是负

i = 0;

if(m[1][1]>m[0][0]) i = 1;

if(m[2][2]>m[i][i]) i = 2;

j = nxt[i];

k = nxt[j];

s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0);

q[i] = s × 0.5;

if( s!= 0.0) s = 0.5 / s;

q[3] = (m[j][k] - m[k][j]) × s;

q[j] = (m[i][j] - m[j][i]) × s;

q[k] = (m[i][k] - m[k][i]) × s;

quat->x = q[0];

quat->y = q[1];

quat->z = q[2];

quat->w = q[3];

}

作为以下迭代计算的4元数初值，若未知初值也可选取4元数初值为 q0=1,q1=q2=q3=0。

第2步根据接收到的磁力计矢量{mx,my,mz},得到本体坐标系下的正确磁场矢量{wx,wy,wz}

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

hx = 2×mx×(0.5 - q2q2 - q3q3) + 2×my×(q1q2 - q0q3) + 2×mz×(q1q3 + q0q2);

hy = 2×mx×(q1q2 + q0q3) + 2×my×(0.5 - q1q1 - q3q3) + 2×mz×(q2q3 - q0q1);

hz = 2×mx×(q1q3 - q0q2) + 2×my×(q2q3 + q0q1) + 2×mz×(0.5 -q1q1 - q2q2);

bx = sqrt((hx×hx) + (hy×hy));

bz = hz

再转换到矢量{wx,wy,wz}

wx = 2×bx×(0.5 - q2q2 - q3q3) + 2×bz×(q1q3 - q0q2);

wy = 2×bx×(q1q2 - q0q3) + 2×bz×(q0q1 + q2q3);

wz = 2×bx×(q0q2 + q1q3) + 2×bz×(0.5 - q1q1 - q2q2);

第3步根据接收到的重力加速度计数据ax,ay,az,和{wx,wy,wz}

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

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自身增加ex×Ki; eyInt自身增加 ey×Ki; ezInt自身增加 ez×Ki;

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

第4步根据误差矢量{ex,ey,ez}及其累计值纠正陀螺仪数据{gx,gy,gz} gx自身增加 Kp×ex + exInt; gy自身增加 Kp×ey + eyInt; gz自身增加 Kp×ez + ezInt;

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

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

随着不断接收到陀螺仪数据gx, gy, gz，对4元数按如下方式修正，halfT为修正周期，是一个正数，如果陀螺仪数据接收频率快则halfT较小，频率慢则halfT较大。

q0自身增加(-q1×gx - q2×gy - q3×gz)×halfT;

q1自身增加(q0×gx - q3×gy + q2×gz)×halfT;

q2自身增加(q3×gx + q0×gy - q1×gz)×halfT;

q3自身增加(-q2×gx + q1×gy + q0×gz)×halfT;

第6步输出4元数转R矩阵的公式如下：

R_g=

第7步回到第2步继续接收陀螺仪运动数据更新姿态4元数q0~q3。

检校相机摄像头光心与视觉叠加客户端之间的位置关系

由于上述方法只能测得智能机的姿态R_g，而智能机上安装的相机姿态R和R_g之间存在一个固定的方向的转换∆R。同样的智能机上安装的相机位置T和手机GPS转换坐标T_gps之间存在一个固定的方向的转换∆T,可以测得∆T和∆R

那么当以后测得叠加客户端的GPS坐标T_gps陀螺仪姿态R_g时，实际相机的位姿可表示为 T=T_gps+∆T=[Xs Ys Zs]^T R=∆RR_g=

如图1-2，近似情况下，可取∆R≈,∆T≈0,那么

T≈T_gpsR=≈R_g=

其中R_g=。

Claims

1.一种智能机全方位增强现实信息叠加方法，包括以下步骤：

1)通过智能机获取周边目标i相对于智能机的局部坐标系坐标(X_i，Y_i，Z_i)；

上述三维坐标(X_i，Y_i，Z_i)中的X_i，Y_i，Z_i由下法得到：

其中

当Z轴沿着地球表面法向量指向地球外时：

当Z轴沿着地球表面法向量指向地球心时：

上述式中θ_a是智能机纬度，是智能机经度；

T_i是目标i的地球坐标；T_s是智能机的地球坐标；

2)计算目标i在智能机上信息叠加的位置u_i,v_i

确定目标i(X_i，Y_i，Z_i)在视频帧图像中显示的位置(u_i，v_i)；在智能机视频窗口中的目标i位置(u_i,v_i)处叠加显示目标i的信息；

目标i(X_i，Y_i，Z_i)在视频帧图像中显示的位置(u_i，v_i)

按如下公式计算：

式中：

Z_Oi＝r₃₁X_i+r₃₂Y_i+r₃₃Z_i

f_x,f_y为摄像头内部参数中的焦距像素长度，c_x，c_y为摄像头内部参数中的主点，X_i，Y_i，Z_i为目标i的位置坐标

r₁₁、r₁₂、r₁₃、r₂₁、r₂₂、r₂₃、r₃₁、r₃₂和r₃₃由下式得到：

其中R＝ΔR×R_g；

ΔR为智能机的摄像头姿态矩阵R和智能机姿态矩阵R_g之间的一个固定的方向的转换，取

R_g由下法之一获得：

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

首先采用(a)(b)(c)中任何一种得到R_g0矩阵

(a)智能机上指定传感器类型为旋转矢量类型TYPE_ROTATION_VECTOR,实时得到旋转矢量传感器的观测值R_vec利用智能机系统上的旋转矢量转矩阵函数

将R_vec转换成绘图转换矩阵R₄₄

R₄₄是一个绘图转换矩阵，其左上角3x3矩阵就是R_g0

(b)如果检测到智能机上的旋转矢量传感器只有3个数据values[0],values[1],values[2]采用以下公式得到

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

再将q₀,q₁,q₂,q₃按以下公式转换成R_g0矩阵

(c)如果检测智能机上的旋转矢量传感器有4个数据，

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

计算

q₀＝p₀/L_p

q₁＝p₁/L_p

q₂＝p₂/L_p

q₃＝p₃/L_p

其中

再将q₀,q₁,q₂,q₃按以下公式转换成R_g0矩阵

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

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

R_g＝R_g0

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

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

第1步选用以下(a)(b)2种方法之一设定q₀,q₁,q₂,q₃的初值

(a)将方法(1)得到的R_g转换成4元数q₀,q₁,q₂,q₃作为以下第2-7步迭代的初值

(b)采取以下方法得到q₀,q₁,q₂,q₃初值

转换为4元数q₀,q₁，q₂，q₃其中

<mrow> <msub> <mi>R</mi> <mi>&theta;</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>sa</mi> <mi>z</mi> </msub> </mrow> <mrow> <mi>L</mi> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <mo>/</mo> <mi>L</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mrow> </mfrac> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>sa</mi> <mi>x</mi> </msub> <msub> <mi>a</mi> <mi>y</mi> </msub> <mo>/</mo> <mo>&lsqb;</mo> <msup> <mi>L</mi> <mn>2</mn> </msup> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <mo>/</mo> <mi>L</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> <mo>&rsqb;</mo> </mrow> </mtd> <mtd> <mrow> <msub> <mi>a</mi> <mi>x</mi> </msub> <mo>/</mo> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>s</mi> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <mo>/</mo> <mi>L</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mrow> </mtd> <mtd> <mrow> <msub> <mi>a</mi> <mi>y</mi> </msub> <mo>/</mo> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <mo>-</mo> <msub> <mi>sa</mi> <mi>x</mi> </msub> </mrow> <mrow> <mi>L</mi> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <mo>/</mo> <mi>L</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mrow> </mfrac> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>sa</mi> <mi>z</mi> </msub> <msub> <mi>a</mi> <mi>y</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msup> <mi>L</mi> <mn>2</mn> </msup> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <mo>/</mo> <mi>L</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>a</mi> <mi>z</mi> </msub> <mo>/</mo> <mi>L</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

a_x、a_y、a_z是智能机测出来的智能机的重力加速度的x、y、z三个方向的分量，

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

若采用方向传感器：

若采用磁力计传感器：检测到磁力传感器矢量m＝{m_x,m_y,m_z}，计算

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>m</mi> <mrow> <mn>0</mn> <mi>x</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>m</mi> <mrow> <mn>0</mn> <mi>y</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>m</mi> <mrow> <mn>0</mn> <mi>z</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msubsup> <mi>R</mi> <mi>&theta;</mi> <mi>T</mi> </msubsup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>m</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>m</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>m</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>

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

将4元数q₀,q₁，q₂，q₃作为以下迭代计算的初值，

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

exInt＝0，eyInt＝0，ezInt＝0

第3步根据接收到的磁力计矢量{m_x,m_y,m_z},得到正确磁场矢量{w_x,w_y,w_z}

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

h_x＝2×m_x×(0.5-q₂×q₂-q₃×q₃)+2×m_y×(q₁×q₂-q₀×q₃)+2×m_z×(q₁×q₃+q₀×q₂)；

h_y＝2×m_x×(q₁×q₂+q₀×q₃)+2×m_y×(0.5-q₁×q₁-q₃×q₃)+2×m_z×(q₂×q₃-q₀×q₁)；

h_z＝2×m_x×(q₁×q₃-q₀×q₂)+2×m_y×(q₂×q₃+q₀×q₁)+2×m_z×(0.5-q₁×q1-q₂×q₂)；

b_z＝h_z

再转换到正确磁场矢量{w_x,w_y,w_z}

w_x＝2×b_x×(0.5-q₂×q₂-q₃×q₃)+2×b_z×(q₁×q₃-q₀×q₂)；

w_y＝2×b_x×(q₁×q₂-q₀×q₃)+2×b_z×(q₀×q₁+q₂×q₃)；

w_z＝2×b_x×(q₀×q₂+q₁×q₃)+2×b_z×(0.5-q₁×q₁-q₂×q₂)；

第4步根据接收到的重力加速度计数据a_x,a_y,a_z,和{w_x,w_y,w_z}

得到误差矢量{e_x,e_y,e_z}并计算其累计值exInt，eyInt，ezInt

v_x＝2*(q₁*q₃-q₀*q₂)；v_y＝2*(q₀*q₁+q₂*q₃)；v_z＝q₀*q₀-q₁*q₁-q₂*q₂+q₃*q₃；

e_x＝(a_y×v_z-a_z×v_y)+(m_y×w_z-m_z×w_y)；

e_y＝(a_z×v_x-a_x×v_z)+(m_z×w_x-m_x×w_z)；

e_z＝(a_x×v_y-a_y×v_x)+(m_x×w_y-m_y×w_x)；

计算误差累计值

exInt替换为exInt+e_x×Ki；eyInt替换为eyInt+e_y×Ki；ezInt替换为ezInt+e_z×Ki；

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

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

假设智能机读出当前的一组陀螺仪数据为{gx0,gy0,gz0}g_x＝gx0+Kp×e_x+exInt；g_y＝gy0+Kp×e_y+eyInt；g_z＝gz0+Kp×e_z+ezInt；

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

Kp在0.000001至20.0中任意选取；

第6步根据陀螺仪数据g_x,g_y,g_z修正四元数

随着不断接收到陀螺仪数据g_x,g_y,g_z，对4元数按如下方式修正，halfT为修正周期，halfT＝0.00001～10.0，

q₀替换为q₀+(-q₁×g_x-q₂×gy–q₃×g_z)×halfT；q₁替换为q₁+(q₀×g_x-q₃×g_y+q₂×g_z)×halfT；

q₂替换为q₂+(q₃×g_x+q₀×gy-q₁×g_z)×halfT；q₃替换为q₃+(-q₂×g_x+q₁×g_y+q₀×g_z)×halfT；

第7步输出4元数转R_g矩阵的公式如下：

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

通过上述步骤得到R_g，进而带入公式(1)得到N个目标i＝1～N的像坐标u_i,v_i.就可以在u_i,v_i处增强显示多个目标的信息；其中最大目标数N的取值范围是1～200；

3)根据u_i,v_i在智能机上叠加显示目标i的信息

假设w是智能机显示窗口分辨率的宽,h是智能机显示窗口分辨率的高；

在Z_Oi＝r₃₁X_i+r₃₂Y_i+r₃₃Z_i≥0情况下，

如果u_i，v_i在图像矩形框内部，即u_i≥0且u_i<w且v_i≥0且v_i<h,则在智能机显示屏内部以u_i，v_i为中心标出智能机到目标i的距离以及目标名称,级别,人气属性。

2.根据权利要求1所述的方法，其特征在于在窗口边缘标示视野外目标：

如果Z_Oi＝r₃₁X_i+r₃₂Y_i+r₃₃Z_i≥0

但u_i，v_i在图像矩形框外部，即u_i<0或u_i≥w或v_i<0或v_i≥h，则在智能机显示窗口边缘部分标示出一个指向目标的箭头；该箭头针尖坐标e_x0,e_y0正好是从相机主点c_x,c_y出发到点u_i,v_i形成的射线与图像方框边缘的交点，箭头末端为e_x0-dx*s,e_y0--dy*s；其中：

s为箭头的像素长度

dx＝(u_i-c_x)/L₂

dy＝(v_i-c_y)/L₂

如果Z_Oi＝r₃₁X_i+r₃₂Y_i+r₃₃Z_i<0采用反向延长法

仍采用以下公式计算u_i,v_i

该箭头针尖坐标e_x0,e_y0正好是从u_i,v_i出发到相机主点c_x,c_y的射线延长方向与图像方框边缘的交点，箭头末端为e_x0+dx*s,e_y0+dy*s；其中：

s为箭头的像素长度

dx＝(u_i-c_x)/L₂

dy＝(v_i-c_y)/L₂

或Z_Oi<0时，采用如下Z映像法：

先按以下公式计算映像点的像坐标u_{i_},v_{i_}

若u_{i_},v_{i_}在相框外部,连接c_x,c_y与u_{i_},v_{i_}形成线段，在线段与智能机窗口边框相交处e_x0,e_y0绘制箭头针尖；

若u_{i_},v_{i_}在相框内部,延长c_x,c_y到u_{i_},v_{i_}的射线，直至与图像边框相交处e_x0,e_y0绘制箭头针尖；

箭头末端为e_x0-dx*s,e_y0-dy*s；其中：

s为箭头的像素长度

dx＝(u_{i_}-c_x)/L₂

dy＝(v_{i_}-c_y)/L₂

3.根据权利要求1或2所述的方法，其特征在于：上述目标i的信息包括目标i的名称、符号属性、类别、与智能机的距离、角度和/或高度。

4.根据权利要求1或2所述的方法，其特征在于：Ki＝0.005。

5.根据权利要求1或2所述的方法，其特征在于：Kp＝2.0。

6.根据权利要求1或2所述的方法，其特征在于：步骤2)中得到多个目标的坐标u₁,v₁～u_N,v_N之后,当目标数量n>N时，只在视频上叠加显示排名前N的目标子集，显示的位置为目标i的成像坐标ui,vi，否则n≤N时按实际数量n显示；

目标按照评价函数f(Mi)从大到小的顺序排序；

叠加信息的内容包括,目标i的名字,目标到智能机的距离

7.根据权利要求1或2所述的方法，其特征在于：权利要求1的步骤2)完成之后，执行以下步骤：

(1)将智能机上摄像头的姿态矩阵设置为用于叠加显示的智能机模型绘制矩阵；

(2)按目标i局部坐标X_i，Y_i，Z_i绘制三维目标。