世界座標系向影象座標系轉換矩陣方程
注意1：拍攝的多張圖片中最終會計算出唯一的內參，而外參每張圖片都會對應有一個，由於我們的目的是爲了測量目標點到相機位置的橫縱向距離，所以可以將Zw值設定爲0；
重點：Zc 是通過世界座標系中 目標點的三維座標經過旋轉、平移矩陣得到的，實際將相機內外參數代入 座標轉換矩陣中時，Zc可以用Xw，Yw，Zw與R、T矩陣進行替代，替代完成後整個方程中有三個未知量，按照需求我們測距可以將Zw設定爲0，則方程中未知量個數就變成了2個，而列出的方程也只有兩個，這樣就可以實現根據影象中畫素座標（u，v）推算出世界座標系中目標點位置（Xw,Yw）,從而得到橫縱向距離
2.轉換矩陣中的Zc值說明
其中，Zc是整個矩陣能否被實際使用的關鍵點，那麼Zc如何得到的，或者來自於何處，可以用哪種方式去拿到這個值？？？
小節1中可以發現Zc來自於世界座標系向相機座標系轉換過程中得到的，在相機座標系中表現爲景深，即相機光心與目標點連線在Zc軸的投影，因此，通過分析，可以發現：
Zc=R（3，1）X+R（3，2）Y+R（3，3）Z+t（3）
其中R爲33矩陣，R（3，1）代表R矩陣中第3行第1個元素；t爲31矩陣，t（3）代表t矩陣中第3個元素；
具體計算過程圖（將Zc表示出，並用其他值進行替代，從而更加清晰使用內外參數測距過程）
注意2：matlab標定圖中的所示的X，Y軸即爲世界座標系的軸，（0，0）即爲世界座標系的原點，矩陣計算出來的X，Y即是在該座標系中的距離原點的距離
%% 根據matlab標定工具箱獲取相機的內外參數，內參數只有一個對應的矩陣，
%%而外參數（旋轉矩陣和位移矩陣）每一張標定板影象都對應有一個矩陣。
%%針對自動駕駛需求，世界座標系中目標點的Z軸並非重要參考物件，可以不考慮，提前設定Z=0；
%%綜上所述，結合各個座標系轉換方程，編寫二維影象座標系點 推導 三維世界座標系點（只包含縱向、橫向距離）程式
%%特別說明，座標系轉換矩陣中最需要注意的點就是 Zc項，該項來自於世界座標系向相機座標系轉換所產生
%%外參數的標定板應選擇合適的位置，推薦將標定板右下角放置於影象底端中間位置處，便於測距計算
%% 內參用字母 in 表示，放置於地面的標定板對應於求取外部參數，旋轉矩陣用R表示，平移矩陣用T表示
%%其中求解過程中會涉及到內參數：每一個畫素在 X 軸與 Y 軸方向上的物理尺寸爲dx、dy，用字母f_dx,f_dy表示
%%涉及到的內參數： 光心橫座標用u0表示，光心縱座標用v0表示;
%%涉及到的外參數： 旋轉矩陣爲3*3大小，具體用R1、R2、R3...R9依次表示；
%%涉及到的外參數： 平移矩陣爲3*1大小，具體用t1、t2、t3依次表示；
%% 影象座標系中的點用（u，v）表示，世界座標系的位置距離用（X，Y）表示，X代表橫向距離，Y代表縱向距離
u=input('請輸入影象中目標點的橫座標（注意畫素範圍）:');
v=input('請輸入影象中目標點的縱座標（注意畫素範圍）:');
%% 獲取matlab相機標定後的內外參數，外參數每張圖片對應有一個外參數，選擇合適的世界座標系可以有效實現測距
in = cameraParams.IntrinsicMatrix';
R = cameraParams.RotationMatrices(:,:,3);
t = cameraParams.TranslationVectors(3,:)';
u0 = in(1,3);
v0 = in(2,3);
f_dx = in(1,1);
f_dy = in(2,2);
R1=R(1,1);R2=R(1,2);R4=R(2,1);R5=R(2,2);R7=R(3,1);R8=R(3,2);
t1=t(1);t2=t(2);t3=t(3);
%%計算畫素點（u,v）對應的世界座標系距離，通過合適的擺放也可以算出距離相機的橫縱向距離
A=(u-u0)*R7-f_dx*R1; B=(u-u0)*R8-f_dx*R2; C=(u-u0)*t3-f_dx*t1;
D=(v-v0)*R7-f_dy*R4; E=(v-v0)*R8-f_dy*R5; F=(v-v0)*t3-f_dy*t2;
X=(C*E-B*F)/(B*D-A*E);
Y=(A*F-C*D)/(B*D-A*E);
disp(['橫向距離爲',num2str(X/10),'cm']);
disp(['縱向距離爲',num2str(Y/10),'cm']);
通過方程計算得到的世界座標系（X，Y）與實際測量得到的X，Y具有較小的誤差，各位讀者具備條件可以親自測試驗證，誤差主要要來源於標定板的製作精細程度，標定板務必保持在一個平面內，用A4紙列印出來的棋盤格直接貼上往往會使得精度下降！（本文中由於條件有限，後續實際測試中應當更換）