android 地磁传感器中Rotate Matrix

1)处理建筑平面图，去掉一些不必要的描述或杂物，确定导航关键点，即最能体现建筑地地形位置特征的点，同时完成建筑地地磁数据采样及存储，建立地磁数据地图；具体操作如下，首先用智能手机注册最高精度的地磁数据变化监听，获取机身坐标系下的三个方向的地磁数据分量；然后建立在机身坐标系下三个方向的地磁数据在世界坐标系下的映射；步行完成建筑地地磁数据的采样，并通过与建筑平面图的对应关系整合地磁数据和建筑地物理坐标建立地磁数据地图，对获取的地磁数据进行滤波，过滤掉单位采样点单位时间内获取的奇异点数据，获取单位时间内单位采样点的特征地磁数据，存储于地磁数据库中；

(2)使用与采样建立地磁数据地图时相同的方式获取用户当前位置点的实时地磁数据，获取用户当前位置点的实时地磁特征数据；

(3)使用坐标点查找-轮廓匹配-惯性定位算法将实时地磁与地磁数据地图中的地磁进行匹配定位；

(4)根据地磁定位获取的用户当前位置点在地磁数据地图中寻找最近导航关键点，并根据导航算法生成最近导航关键点到目标点的导航路线。

android 硬件调用：

java.lang.Object
↳	android.hardware.SensorManager

 public class SensorActivity extends Activity, implements SensorEventListener {
     private final SensorManager mSensorManager;
     private final Sensor mAccelerometer;

     public SensorActivity() {
         mSensorManager = (SensorManager)getSystemService(SENSOR_SERVICE);
         mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
     }

     protected void onResume() {
         super.onResume();
         mSensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_NORMAL);
     }

     protected void onPause() {
         super.onPause();
         mSensorManager.unregisterListener(this);
     }

     public void onAccuracyChanged(Sensor sensor, int accuracy) {
     }

     public void onSensorChanged(SensorEvent event) {
     }
 }

　先说一下什么是旋转矩阵？如图1所示，我们假设最开始空间的坐标系X_A，Y_A，Z_A就是笛卡尔坐标系，这样我们得到空间A的矩阵V_A={X_A，Y_A，Z_A}^T，其实也可以看做是单位阵E。进过旋转后，空间A的三个坐标系变成了图1中红色的三个坐标系X_B，Y_B，Z_B，得到空间B的矩阵V_B={X_B，Y_B，Z_B}^T。我们将两个空间联系起来可以得到V_B=R•V_A，这里R就是我们所说的旋转矩阵。 

图1

　　由于X_A={1,0,0}^T，Y_A={0,1,0}^T，Z_A={0,0,1}^T，结合图2可以看出，旋转矩阵R就是由X_B，Y_B，Z_B 三个向量组成的。讲到这里，大家应该会发现旋转矩阵R满足第一个条件，因为单位向量无论怎么旋转长度肯定不会变而且向量之间的正交性质也不会变。那么旋转矩阵就是正交阵！不过这还不能说明问题，下面我更进一步利用数学公式进行证明。

图2

---

　　进一步讨论之前，我们先说两点数学知识。（1）点乘（dot product）的几何意义：如图3，我们从点乘的公式可以得到α•β相当与β的模乘上α在β上投影的模，所以当|β|=1时，α•β就是指α在β上投影的模。这一点在下面的内容中非常重要。（2）旋转矩阵逆的几何意思：这个比较抽象，不过也好理解。旋转矩阵相当于把一个向量（空间）旋转成新的向量（空间），那么逆可以理解为由新的向量（空间）转回原来的向量（空间）。

图3

　　接下来就是重点了，我们结合图4进行分析。上面已经说明了，旋转矩阵R就是由X_B，Y_B，Z_B 三个向量组成的。我们来看看X_B，Y_B，Z_B究竟是什么？由于图中所有的向量均是单位向量，所以X_B与X_A点乘的结果可以看成X_B在X_A上的投影的模，也就是X_B在空间A中x轴的分量！！图中中间的位置列出了X_B向量中的三个分量分别为X_B在X_A上的投影的模、X_B在Y_A上的投影的模和X_B在Z_A上的投影的模。这从几何角度很好理解。以此类推，可以得出的旋转矩阵R的表达形式。我们根据图4可以惊喜的发现，矩阵R的第一行就是X_A在X_B，Y_B，Z_B上的投影的模，也就是X_A^T。

 

图4

　　这个发现有什么用呢？图5做出解释。根据上面公式可以推出A到B的旋转矩阵等于B到A的旋转矩阵的转置。根据我们上一段所说的A到B的旋转矩阵的逆就是等于B到A的旋转矩阵，因此很容易推出R^_-1等于R^T！这满足正交矩阵的第二个条件，又一次证明了旋转矩阵就是正交阵。在平时的工作中，我也测试过所有的旋转矩阵的行列式的值都是为1的，所以旋转矩阵满足正交阵的一切性质，可以说是很完美的矩阵。

图5

---

 　　现在以三个欧拉角中的RotX为例（其余两个欧拉角以此类推），验证一下以上说的结论。

　　首先结合图5的公式，计算出RotX的旋转矩阵R_rotx。

• 由于X轴是垂直于YoZ平面的，所以X_A和Y_B，Z_B的点乘结果为0，同时X_B和Y_A，Z_A的点乘结果也为0。
• 由于X_A，X_B都是单位向量，所以X_A和X_B的点乘结果为1。
• 由于绕x轴旋转，所以我们观察Y_B和Z_B分别在Y_A和Z_A上的投影情况，如图6，我已经将坐标标注了。蓝色是机身坐标，红色是大地坐标。
  

  

  图6

 

　　这样就完成旋转矩阵R_rotx，我们接下来验证一下。

1. 我们计算每一行每一列的模，都为1；并且任意两个列向量或者任意两个行向量都是正交的。所以满足上文列出的第一个性质。
2. 我们计算R_rotx的行列式，很简单可以算出为1。这时我们计算一下该矩阵的逆和转置，这里我不写出来了是相等的。所以满足上文列出的第三个性质。

https://blog.csdn.net/u012763833/article/details/52605747

SensorManager是Android中的一个类，其有一个函数getRotationMatrix，可以计算出旋转矩阵，进而通过getOrientation求得设备的方向（航向角、俯仰角、横滚角）。

    float[] mR = new float[16];    //旋转矩阵

float[] remapR = new float[16];

float[] orientationValues = new float[3];

SensorManager.getRotationMatrix(mR, null, accAverager.returnAverage(), magAverager.returnAverage());

 SensorManager.remapCoordinateSystem(mR, SensorManager.AXIS_Z, SensorManager.AXIS_MINUS_X, remapR);//机身坐标系mR和世界坐标系rempap的映射

  sensorManager.getOrientation(remapR, orientationValues);// 在世界坐标系里，机器绕Z轴、X轴、Y轴旋转的角度。

函数getRotationMatrix的源码如下所示，源码中虽然对该函数整体进行了解释，但是对代码中各个参数的计算没有说明，如为什么加速度的数值要和磁力计的数值做差乘。在网上各种搜索后，找到一段老外对这个问题的英文解释，很好的回答了上述问题。大意翻译（包括自己的理解）如下：加速度数值和磁力计数值均是向量，手机水平放置时，加速度读数实际上就是重力向量，方向是竖直朝下的；磁力计表示本地的磁场，不考虑环境影响及磁偏角的话，认为磁场方向是水平南北朝向的。因此，代码中首先对加速度和磁力计数据做了一个差乘，得出一个水平东西方向的向量（差乘的定义）。经过这个运算，本来只有一个平面的向量，变成了三个三维立体平面的向量，从而可以用来计算设备的方向。源码中后面又做了一次差乘，是用计算出的水平东西方向的向量和重力向量做的差乘，这次运算重新得出一个水平南北方向的向量，最后旋转矩阵中用这三个向量（两个计算出的水平向量、一个重力向量）构成。   

1、SensorManager.getRotationMatrix

    public static boolean getRotationMatrix(float[] R, float[] I,
            float[] gravity, float[] geomagnetic) {
        // TODO: move this to native code for efficiency
        float Ax = gravity[0];
        float Ay = gravity[1];
        float Az = gravity[2];
        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) {
            // device is close to free fall (or in space?), or close to
            // magnetic north pole. Typical values are  > 100.
            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;
        if (R != null) {
            if (R.length == 9) {
                R[0] = Hx;     R[1] = Hy;     R[2] = Hz;
                R[3] = Mx;     R[4] = My;     R[5] = Mz;
                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) {
            // compute the inclination matrix by projecting the geomagnetic
            // vector onto the Z (gravity) and X (horizontal component
            // of geomagnetic vector) axes.
            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;
    }
    

etRotationMatrix的参数有手机的（重力）加速度数据和磁传感器数据，按照我的理解重力加速度（g）的方向就是指向地心，垂直于水平面，磁力（m）的方向应该是南北方向，这两个向量叉乘得到了一个东西方向的向量（we），然后该向量重力加速的的向量都归一化，再做叉乘又得到一个水平南北方向的的向量（sn），g与we，sn两两正交，组成了大地坐标系。而这三个向量组成的矩阵自然也就是旋转矩阵。然后和三维旋转矩阵推导出的含有航向角（方向角）、倾斜角、旋转角的旋转矩阵一一对应，就可以求出相应的角度。

二、传感器获取Orientation

先注册两个传感器TYPE_MAGNETIC_FIELD和TYPE_ACCELEROMETER，在onSensorChanged事件里获取传感器值accelerometerValues和magneticFieldValues，再把两个传感器的值扔进SensorManager.getRotationMatrix(R, I, accelerometerValues, magneticFieldValues)，得到R和I矩阵，I矩阵是个地磁偏角，用不到，R是rotation矩阵，关键是这个。

三、机身坐标系和世界坐标系的映射

机身坐标系的定义是，将屏幕按照默认显示方向放置，然后X轴从左向右，Y轴从下至上，Z轴从里到外，右手坐标系。注意，当你切换显示方向，比如从portrait切到landscape，这个坐标系是不跟着变的，文档里用的词是screeen而不是display。

用来指定把机身坐标系的X和Y轴映射到世界坐标系的哪个轴。我选择的工作姿态是把手机横持在身前，屏幕朝向自己，所以使用如下的设定，这样当摄像头指向地磁北极，屏幕与重力方向平行，屏幕法线与重力方向垂直时，获得的orientaion三个值都是0。

SensorManager.remapCoordinateSystem(R, SensorManager.AXIS_Z, SensorManager.AXIS_MINUS_X, remapR);

https://ask.helplib.com/android/post_1625076

然后就可以得到orientaion了：

SensorManager.getOrientation(remapR, orientationValues);

   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;
    } 

来源：https://en.wikipedia.org/wiki/Euler_angles

orientationValues是个3个float的数组，依次表示Azimuth、Pitch、Roll，就是在世界坐标系里，机器绕Z轴、X轴、Y轴旋转的角度。

手机的自然坐标系

　　当一个设备被放在其默认的方向上时，X轴是水平指向右的，Y轴是垂直向上的，Z轴是指向屏幕正面之外的，即屏幕背面是Z的负值。

　　

 

　　当设备运动或者旋转的时候，这些坐标轴是不会改变的，即它们是跟随手机的。

　　即是说，手机坐标系是跟随设备的自然方向的（但是请记住自然方向不一定是竖直，比如平板它的自然方向就很有可能是横向）。 

屏幕旋转

　　常常要考虑屏幕的旋转，即屏幕画面相对于自然方向的旋转。

　　于是就需要使用 getRotation()方法来获取屏幕的旋转值。

　　这个方法是Display类中的，跟传感器不相关。

　　这个方法的返回值只对应0，90,180,270四种旋转情况，它说明屏幕显示区域的旋转情况。

世界坐标系

　　有一些传感器和方法使用相对于世界的坐标系，因为它们返回的数据反映设备相对于地球及真实环境的位置信息。

　　请见getOrientation() 方法，getRotationMatrix() 方法，Orientation Sensor, 和 Rotation Vector Sensor。下面各自介绍。

 

getRotationMatrix()

　　getRotationMatrix()方法说明中，定义的世界坐标系如下：

    

　　X轴平行于地面，指向东方。

　　Y轴平行于地面，指向北极方向。

　　Z轴垂直于地面，指向天空。

 

getOrientation()

　　getOrientation() 方法中所用的坐标系与上面的不同：

    

　　X轴平行于地面，指向西方；

　　Y轴平行于地面，指向地磁场北极。

　　Z轴垂直于地面，指向地心。

　　getOrientation方法的返回值表示：

　　values[0]: azimuth, rotation around the Z axis.

　　values[1]: pitch, rotation around the X axis.

　　values[2]: roll, rotation around the Y axis.

　　并且这三个角度值都是以弧度做单位，逆时针方向为正。

 

方向传感器的返回值说明

 

　　方向传感器是利用加速度计和地磁场传感器得到自己的数据。

　　方向传感器比较特殊，因为它的数值是相对于绝对方向的。它得到的是手机设备的绝对姿态值。

　　一个方向传感器得到的三维数据如下：

　　（参见http://developer.android.com/guide/topics/sensors/sensors_position.html#sensors-pos-orient）

 

　　方向传感器返回的都是角度值，以度数为单位。

　　注意下面说的x、y、z轴均是手机自身的坐标轴。

　　第一个角度：Azimuth (degrees of rotation around the z axis).

　　表示手机自身的y轴与地磁场北极方向的角度，即手机顶部朝向与正北方向的角度。

　　（This is the angle between magnetic north and the device's y axis. ）

　　当手机绕着自身的z轴旋转时，该角度值将发生改变。

　　例如该角度值为0时，表示手机顶部指向正北；该角度为90度时，代表手机顶部指向正东；该角度为180度时，代表手机顶部指向正南；该角度为270度时，代表手机顶部指向正西。

 

　　第二个角度：Pitch (degrees of rotation around the x axis).

　　表示手机顶部或尾部翘起的角度。

　　当手机绕着自身的x轴旋转，该角度会发生变化，值的范围是-180到180度。

　　当z轴正向朝着y轴正向旋转时，该角度是正值；当z轴正向朝着y轴负向旋转时，该角度是负值。

 

　　假设将手机屏幕朝上水平放在桌子上，如果桌子是完全水平的，该角度应该是0。

　　假如从手机顶部抬起，直到将手机沿x轴旋转180度（屏幕向下水平放在桌面上），这个过程中，该角度值会从0变化到-180。

　　如果从手机底部开始抬起，直到将手机沿x轴旋转180度（屏幕向下水平放在桌面上），该角度的值会从0变化到180。

 

　　第三个角度：Roll (degrees of rotation around the y axis).

　　表示手机左侧或右侧翘起的角度。

　　当手机绕着自身x轴旋转时，该角度值将会发生变化，取值范围是-90到90度。

　　当z轴正向朝着x轴正向旋转时，该角度是负值；

　　当z轴正向朝着x轴负向旋转时，该角度是正值。

 

　　（这里跟官方文档的说法有点不太一致，即第三个角度的正负号正好相反，我不知道是文档写错了，还是它用了别的什么坐标系）。

　　因为通过真机测试，结果如下：

　　将手机屏幕朝上水平放在桌子上，如果桌子是完全水平的，该角度应该是0。

　　假如将手机左侧逐渐抬起，直到将手机沿Y轴旋转90度（手机与桌面垂直），在这个旋转过程中，该角度会从0变化到-90。

　　如果从手机的右侧开始抬起，直到将手机沿Y轴旋转90度（手机与桌面垂直），该角度的值会从0变化到90度。