LBS AR开发实录(1)：手机位姿数据的实时获取
转载请声明出处：https://blog.csdn.net/AndrExpert/article/details/80253875
1. 计算机视觉中的坐标系
计算机视觉中，有四大坐标系：世界坐标系、摄像机坐标系、图像坐标系以及像素坐标系。在此之前，还是很有必要先了解下传说中的笛卡尔坐标系，因为，这是接下来阐述相关原理的基础。
(1) 笛卡尔坐标系
在笛卡尔坐标系下，无论是二维(平面)坐标系还是三维坐标系，通过变换坐标轴的正向方向，都能够得到两种不同的坐标系，即左手坐标系和右手坐标系。
- 右手坐标系(以三维为例)
在空间直角坐标系中，让右手拇指指向x轴的正方向，食指指向y轴的正方向，如果中指能指向z轴的正方向，则称这个坐标系为右手直角坐标系。
- 左手坐标系
在空间直角坐标系中，让左手拇指指向x轴的正方向，食指指向y轴的正方向，如果中指能指向z轴的正方向，则称这个坐标系为左手直角坐标系。
(2) 计算机视觉中的坐标系
图像坐标系
图像坐标系是以摄像机拍摄的二维照片为基准建立的坐标系，用于指定物体在照片中的位置，它表征物体从摄像机坐标系向图像坐标系的透视投影关系。该坐标系的原点位于摄像机光轴和成像平面的交点O上，通常为照片的中心处，X轴为水平向右方向，Y轴为垂直向上方向。图像坐标系示意图如下：像素坐标系
由于每张数字图像在计算机中是以一个二维数组(矩阵，M行xN列)的形式存储，数组(矩阵)中每一个元素称为像素。引入像素坐标系的目的是为了便于访问图像中的像素，该坐标系以图像最左上角的像素(点)为原点O，以水平向右为xo轴，垂直向下为yo轴。在图像的像素坐标系中，每一个像素的坐标为(xo,yo)，其中，xo,yo分别表示该像素在数组中的列数和行数。像素坐标系示意图如下：摄像机坐标系(右手系坐标)
摄像机坐标系是其站在自身的角度上衡量物体的坐标系，它的原点为摄像机的光心，Xc轴、Yc轴与图像坐标系的x轴和y轴平行，Zc轴为摄像机光轴，它与图像平面垂直，且经过图像坐标系的原点(摄像机光轴与图像平面垂直交点)。摄像机坐标系示意图如下：
- 世界坐标系(右手系坐标)
世界坐标系现实空间中的所有坐标系的参考坐标系，在计算机视觉中可以用来描述摄像机和物体的位置，并且不会因摄像机或物体状态变化而变化，它永远是客观存在的。世界坐标系以地球质心为原点Ow，Yw轴指向地磁北极(向下)，Z轴与重力方向相反(指向天空)、X轴是Y与Z的叉积(可由右手法则确定)。示意图如下：
假设现实中有一个点P(x,y,z)，它位于世界坐标系中，那么，p(x,y)则为在图像中成像的点，即位于图像坐标系中。OO’为相机的焦距。
摄像机坐标系和世界坐标系之间的关系可用旋转矩阵R与平移向量t来描述
2. 位姿数据获取及其原理剖析
位姿是指一个物体的位置和方向（The pose of an object refers to its location and orientation）其中， 位置数据指纬度、经度、海拔高度；方向为方向角、仰俯角、横滚角。一个物体的位置可以用(x,y,z)来表示。而方向可以用(α,β,γ)来表示，它们是表示围绕三个坐标轴旋转的角度。
2.1 传感器坐标系统
传感器坐标系，也称设备(摄像机)坐标系或屏幕坐标系，是指当设备处于自然放置状态(即手机竖屏portrait或平板横屏landspace)，对于大多数传感器(加速度、重力、陀螺仪、磁场传感器)来说，它相对于设备屏幕的坐标系以手机屏幕中心为原点O，X轴指向水平向右方向，Y轴指向垂直向上方向(即设备顶端)，Z轴指向设备屏幕由里向外方向。传感器坐标系是设备的传感器框架用来展示传感器数据值的三轴坐标系统，它的三个轴方向是客观存在，不会因为设备摆放方向或状态的变化而变化，即传感器坐标系是基于设备自然方向设定的，这与OpenGL的坐标系统原理一致。传感器坐标系示意图如下：
注意：由于设备在使用过程中，我们并不能保证设备总是处于自然放置状态，且处于非自然方向的设备屏幕显示的是基于传感器坐标系设定的数据，而非传感器实际的数据，这就需要我们将传感器实际坐标数据映射到标准的屏幕传感器坐标系中。通常，我们使用getRotation()方法(手机)屏幕的方向，使用remapCoordinateSystem()方法实现传感器实际坐标数据到屏幕传感器坐标系系统的映射。
2.2 位姿数据获取及其原理分析
/**
* 传感器数据获取
* Created by jiangdongguo on 2018/5/9.
*/
public class SensorActivity extends AppCompatActivity implements SensorEventListener {
// 加速传感器
private Sensor mGravitySensor;
// 磁场传感器
private Sensor mMagneticSensor;
private SensorManager mSensorManager;
// 加速传感器数据
private float[] mGravityValues = new float[3];
// 磁场传感器数据
private float[] mMagneticValues = new float[3];
// 方向结果
private float[] orientationValues = new float[3];
private boolean isPhoneVertical = true;
@Override
protected void onCreate(@Nullable Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
List<Sensor> accelers = mSensorManager.getSensorList(Sensor.TYPE_ACCELEROMETER);
if (accelers != null) {
mGravitySensor = accelers.get(0);
}
List<Sensor> magnetics = mSensorManager.getSensorList(Sensor.TYPE_MAGNETIC_FIELD);
if (magnetics != null) {
mMagneticSensor = magnetics.get(0);
}
}
@Override
protected void onStart() {
super.onStart();
// 注册传感器数据监听器
// 设置数据采样频率为SENSOR_DELAY_UI
mSensorManager.registerListener(this, mGravitySensor, SENSOR_DELAY_UI);
mSensorManager.registerListener(this, mMagneticSensor, SENSOR_DELAY_UI);
}
@Override
protected void onStop() {
super.onStop();
// 注销传感器数据监听器
mSensorManager.unregisterListener(this, mGravitySensor);
mSensorManager.unregisterListener(this, mMagneticSensor);
}
@Override
protected void onDestroy() {
super.onDestroy();
}
@Override
public void onSensorChanged(SensorEvent event) {
// 缓存加速传感器数据
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
mGravityValues = event.values;
}
// 缓存磁场传感器数据
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
mMagneticValues = event.values;
}
// 通过加速传感器和磁场传感器计算方位
calculateOrientation();
}
private void calculateOrientation() {
float[] rotateTmp = new float[9];
float[] outR = new float[9];
// 将机身坐标映射到世界坐标系，rotateTmp为旋转矩阵
SensorManager.getRotationMatrix(rotateTmp, null, mGravityValues, mMagneticValues);
// 将机身坐标系映射到世界坐标系
// 如果不处理，获得是当手机水平放置的值，即手机屏幕与地平线平行
if (isPhoneVertical) {
// 竖屏方向
SensorManager.remapCoordinateSystem(rotateTmp, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
} else {
// 横屏方向
SensorManager.remapCoordinateSystem(rotateTmp, SensorManager.AXIS_Z, SensorManager.AXIS_MINUS_X, outR);
}
// 在世界坐标系里，机器绕Z轴、X轴、Y轴旋转的角度。
SensorManager.getOrientation(outR, orientationValues);
float azimuth = (float) Math.toDegrees(orientationValues[0]);
if(azimuth < 0) {
azimuth += 360;
}
float pitch = (float)Math.toDegrees(orientationValues[1]);
float roll = (float)Math.toDegrees(orientationValues[2]);
}
@Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {
}
}● 核心代码讲解
1. SensorManager.getRotationMatrix方法
// 将设备坐标转换成世界坐标
// R：9维float类型矩阵，当将设备坐标系调整到与世界坐标一致时，
// R为一个单位矩阵: [ 1,0,0 ]
// [ 0,1,0 ]
// [ 0,0,1 ]
// I：9维float类型矩阵，当将地磁矢量转换成与重力相同的坐标空间(世界坐标空间)，
// I为一个沿X轴旋转的旋转矩阵，倾斜的角度可通过geiInclination(float[])计算
// gravity：加速传感器在传感器坐标系中的重力加速度数值(x,y,z)
// geomagnetic：磁场传感器在传感器坐标系中的磁场数值(x,y,z)
boolean getRotationMatrix (float[] R,
float[] I,
float[] gravity,
float[] geomagnetic) getRotationMatrix方法的作用是将传感器坐标转换成为世界坐标系，即通过加速传感器和磁场传感器来计算相对于世界坐标系的旋转矩阵，进而通过getOrientation方法获取手机的方位数据。其中，X轴是Y和Z轴的矢量积，它与大地表面相切且方向大致指向正东；Y轴与大地表面相切，方向指向磁场北极；Z轴与大地表面相垂直，方向指向天空。需要注意的是，该方法只有在设备不处于加速状态和强磁场中计算出来的值才会有意义。下图为世界坐标系示意图：
◇ 函数源码及其原理分析
public static boolean getRotationMatrix(float[] R, float[] I,
float[] gravity, float[] geomagnetic) {
// 重力加速度三个分量
float Ax = gravity[0];
float Ay = gravity[1];
float Az = gravity[2];
final float normsqA = (Ax*Ax + Ay*Ay + Az*Az);
final float g = 9.81f;
final float freeFallGravitySquared = 0.01f * g * g;
if (normsqA < freeFallGravitySquared) {
// 手机加速度达到*落体的10%时计算失效
return false;
}
// 磁场三个分量
final float Ex = geomagnetic[0];
final float Ey = geomagnetic[1];
final float Ez = geomagnetic[2];
//磁感应和重力叉乘得到水平向西的方向，在手机坐标系下的坐标值
float Hx = Ey*Az - Ez*Ay;
float Hy = Ez*Ax - Ex*Az;
float Hz = Ex*Ay - Ey*Ax;
final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
// 手机太靠近磁场北极，计算失败
if (normH < 0.1f) {
return false;
}
// 水平方向坐标值归一化
final float invH = 1.0f / normH;
Hx *= invH;
Hy *= invH;
Hz *= invH;
// 重力方向坐标值归一化
final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
Ax *= invA;
Ay *= invA;
Az *= invA;
// 再次做叉乘
final float Mx = Ay*Hz - Az*Hy;
final float My = Az*Hx - Ax*Hz;
final float Mz = Ax*Hy - Ay*Hx;
// 获得旋转矩阵
// R=((Hx,Hy,Hz),(Mx,My,Mz),(Ax,Ay,Az))
if (R != null) {
if (R.length == 9) {
//x轴：水平向西方向坐标值，归一化后的值
R[0] = Hx; R[1] = Hy; R[2] = Hz;
// y轴：新得到的南向北坐标值(方向由叉乘后得知)
R[3] = Mx; R[4] = My; R[5] = Mz;
// z轴：垂直地平面向下重力方向坐标值，归一化的值
R[6] = Ax; R[7] = Ay; R[8] = Az;
} else if (R.length == 16) {
R[0] = Hx; R[1] = Hy; R[2] = Hz; R[3] = 0;
R[4] = Mx; R[5] = My; R[6] = Mz; R[7] = 0;
R[8] = Ax; R[9] = Ay; R[10] = Az; R[11] = 0;
R[12] = 0; R[13] = 0; R[14] = 0; R[15] = 1;
}
}
// 获得倾斜角矩阵
if (I != null) {
final float invE = 1.0f / (float)Math.sqrt(Ex*Ex + Ey*Ey + Ez*Ez);
// 磁感应计单位向量与新得到的南北方向做点乘
final float c = (Ex*Mx + Ey*My + Ez*Mz) * invE;
// 磁感应计单位向量与重力向量做点乘
final float s = (Ex*Ax + Ey*Ay + Ez*Az) * invE;
if (I.length == 9) {
I[0] = 1; I[1] = 0; I[2] = 0;
I[3] = 0; I[4] = c; I[5] = s;
I[6] = 0; I[7] =-s; I[8] = c;
} else if (I.length == 16) {
I[0] = 1; I[1] = 0; I[2] = 0;
I[4] = 0; I[5] = c; I[6] = s;
I[8] = 0; I[9] =-s; I[10]= c;
I[3] = I[7] = I[11] = I[12] = I[13] = I[14] = 0;
I[15] = 1;
}
}
return true;
} 从源码可知，getRotationMatrix方法首先使用磁感应器的方向和重力的方向(均为传感器坐标系)做叉乘，当手机水平放置(屏幕与地面平行，方向朝向天空)时，此时磁感应方向由水平南指向北方向和重力方向垂直地面指向地心，根据叉乘右手规则，会得到一个新的水平指向西的方向；接着，对重力方向和水平向西的方向做归一化变为单位向量，然后再用重力方向和水平向西的方向做叉乘得到由水平南向北的方向与地球相切。过程如下：
经过两次叉乘后，我们最终由一个平面的向量，获得三个三维立体平面的向量，同时将一个矢量从设备坐标系转换到世界坐标系统，从而获得倾斜矩阵I和旋转矩阵R。关于旋转矩阵，我们在下一波继续讲解，这里你只需要只要设备的方向就是通过这个旋转矩阵计算出来的。
2. SensorManager.getOrientation()方法
// 使用旋转矩阵计算设备的方位
// R：旋转矩阵
// values：方位值
float[] getOrientation (float[] R,
float[] values) getOrientation方法的作用是将getRotationMatrix方法得到的旋转矩阵(以世界坐标为参考系，即相对于地球)来计算设备在的方位值(注：以下提到到X、Y、Z坐标系均以地球为参考系，即设备的世界坐标)。假设手机水平放置在地球表面，在世界坐标系中，方位角、仰俯角和转动角示意图如下：
valuse[0]：方位角(Azimuth) ，设备围绕Z轴旋转的角度，范围为-π~π 。方位角表示的是设备坐标系的Y轴(相对于地球)与磁场北极之间的夹角。当设备指向北(地理北极)时，方位角为0度；指向东，方位角为π/2度；指向南，方位角为π度；指向西，方位角为-π/2度；
valuse[1]：仰俯角(Pitch)，设备围绕X轴旋转的角度，范围为-π/2~π/2。仰俯角表示的是平行于设备屏幕的平面和与地面平行的平面之间的夹角。 假设设备平行于地面水平放置、底部边缘面向用户且屏幕是朝上，将设备的顶部边缘向地面倾斜会产生一个正的俯仰角度，将设备的底部边缘向地面倾斜产生一个负的仰俯角度。以手机为例：当手机顶部固定在地面(相切)，尾部慢慢向上翘起来直到手机屏幕与地面垂直，此时仰俯角从0~π/2度之间变动；当手机尾部固定在地面(相切)，顶部慢慢向上翘起来直到手机屏幕与地面垂直，此时仰俯角从0~-π/2度之间变动。
valuse[2]：衡倾角(Roll)，设备围绕Y轴旋转的角度，范围为-π~π。转动角表示垂直于设备屏幕的平面与垂直于地面的平面之间的夹角。假设设备平行于地面水平放置、底部边缘面向用户且屏幕是朝上，将设备的左边缘向地面倾斜会产生一个正横倾角，将设备右边缘向地面倾斜会产生一个负衡倾角。以手机为例：当手机左边框不动，右边框慢慢向上翘起来直到翻转180度，横倾角从0~-π度之间变动；当手机右边框不懂，左边框慢慢向上翘起来翻转180度，横倾角从0~π度之间变动。
注意： 如果使用方向传感器(Sensor.TYPE_ORIENTATION)这种老方式，三种角度范围和变化趋势与上述新方法会有一定的出入。
◇ 函数源码及其原理分析
public static float[] getOrientation(float[] R, float values[]) {
/* 齐次坐标
* 4x4 (length=16) case:
* / R[ 0] R[ 1] R[ 2] 0 \
* | R[ 4] R[ 5] R[ 6] 0 |
* | R[ 8] R[ 9] R[10] 0 |
* \ 0 0 0 1 /
*
* 3x3 (length=9) case:
* / R[ 0] R[ 1] R[ 2] \
* | R[ 3] R[ 4] R[ 5] |
* \ R[ 6] R[ 7] R[ 8] /
*
*/
if (R.length == 9) {
values[0] = (float)Math.atan2(R[1], R[4]);
values[1] = (float)Math.asin(-R[7]);
values[2] = (float)Math.atan2(-R[6], R[8]);
} else {
values[0] = (float)Math.atan2(R[1], R[5]);
values[1] = (float)Math.asin(-R[9]);
values[2] = (float)Math.atan2(-R[8], R[10]);
}
return values;
} 从源码中可知，getOrientation方法基于旋转矩阵R计算得到设备的方位角values[0]、仰俯角values[0]以及横滚角values[1]，至于上述结果是如何计算出来的，这里还是有必要讲解下旋转矩阵。在这篇文章中，介绍了旋转矩阵的相关概念和性质，所谓旋转矩阵，即假设有一个三维笛卡尔坐标系，当以它的X轴为轴逆时针旋转ω度时，通过计算可以得到绕X轴旋转矩阵分量Rrotx(或Rω)；当以它的Y轴为轴逆时针旋转δ度时，通过计算可以得到绕Y轴旋转矩阵分量Rroty(或Rδ)；当以它的Z轴为轴逆时针旋转κ度时，通过计算可以得到绕Z轴旋转矩阵分量Rrotz(或κ)。最后，得到旋转矩阵R=Rrotx*Rroty*Rrotz，计算公式如下：
再进一步计算，得到各分量的旋转角度值：
最后，根据上面对设备方位角、仰俯角以及横滚角的描述，它们分别为设备绕Z轴、X轴、Y轴旋转的角度，即为κ、ω、δ，与getOrientation源码一致。
旋转矩阵有一个很重要的特性就是它是一个正交矩阵，即矩阵的逆等于矩阵的转置，矩阵的逆*矩阵的转置等于单位矩阵。
3 SensorManager.remapCoordinateSystem()方法
// 变换输入的旋转矩阵，使其能够在不同坐标系统中表示
// inR：要变换的旋转矩阵；
// X：定义新坐标系中与原坐标系X轴一致(重合)的轴线
// Y：定义新坐标系中与原坐标系Y轴一致(重合)的轴线
// outR：变换后的矩阵
boolean remapCoordinateSystem (float[] inR,
int X,
int Y,
float[] outR) 由于手机在使用过程中，我们并不能保证设备总是处于竖屏方向(自然方向)，当旋转手机屏幕后，由于手机屏幕显示的传感器数据是以标准传感器坐标系(默认手持设备永远处于自然方向，是客观存在的且不会因设备状态的改变而变换)为准的，而不是传感器实际的数据，这就需要我们结合getRotation()方法和remapCoordinateSystem()方法实现传感器实际坐标数据到屏幕传感器坐标系系统的映射，其中，getRotation()用于获取手机屏幕当前的旋转状态(角度)。
◇ 函数源码及其原理分析
public static boolean remapCoordinateSystem(float[] inR, int X, int Y,
float[] outR){
if (inR == outR) {
final float[] temp = mTempMatrix;
synchronized(temp) {
// we don't expect to have a lot of contention
if (remapCoordinateSystemImpl(inR, X, Y, temp)) {
final int size = outR.length;
for (int i=0 ; i<size ; i++)
outR[i] = temp[i];
return true;
}
}
}
return remapCoordinateSystemImpl(inR, X, Y, outR);
}◇ 旋转
- getRotation屏幕旋转方向：“顺时针”方向旋转，每次递增90度
getRotation方法只有在Activity没有被android:screenOrientation=”portrait
/landspace”且手机设置中开启自动旋转时才有效，并且该方法只能检测水平方向与垂直方向的切换，无法检测180度的旋转。当我们通过screenOrientation固定屏幕方向时，这里建议使用传感器来检测屏幕旋转的方向，即OrientationEventListener。
switch (mScreenRotation) {
case Surface.ROTATION_0:
axisX = SensorManager.AXIS_X;
axisY = SensorManager.AXIS_Y;
break;
case Surface.ROTATION_90:
axisX = SensorManager.AXIS_Y;
axisY = SensorManager.AXIS_MINUS_X;
break;
case Surface.ROTATION_180:
axisX = SensorManager.AXIS_MINUS_X;
axisY = SensorManager.AXIS_MINUS_Y;
break;
case Surface.ROTATION_270:
axisX = SensorManager.AXIS_MINUS_Y;
axisY = SensorManager.AXIS_X;
break;
default:
break;
}- Camera预览旋转方向： “顺时针”方向旋转，每次递增90度
- 传感器获取屏幕旋转方向： OrientationEventListener
对于Sensor，默认的手机方向是：竖屏Home键在下面，这个是Sensor的0度方向。