轉載請聲明出處：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;@Overrideprotected 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);}}@Overrideprotected void onStart() {super.onStart();// 注冊傳感器數據監聽器// 設置數據采樣頻率為SENSOR_DELAY_UImSensorManager.registerListener(this, mGravitySensor, SENSOR_DELAY_UI);mSensorManager.registerListener(this, mMagneticSensor, SENSOR_DELAY_UI);}@Overrideprotected void onStop() {super.onStop();// 注銷傳感器數據監聽器mSensorManager.unregisterListener(this, mGravitySensor);mSensorManager.unregisterListener(this, mMagneticSensor);}@Overrideprotected void onDestroy() {super.onDestroy();}@Overridepublic 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]);}@Overridepublic 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 contentionif (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度方向。

總結

以上是生活随笔為你收集整理的LBS AR开发实录(1)：手机位姿数据的实时获取的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。