最近在做三維重建的工作，三角測量原理是實作三維重建的基石，具體的算法推導可以參考之前的三角測量計算點的三維坐标一文。在之前的文章中，已知一對比對點，可以計算出場景的三維坐标。
[u, v] 和 [u’, v’] 為相機的歸一化坐标， P為待求的世界坐标。經過SVD分解，總能求得最優解。那這個解是否一定滿足重建的精度，換句話說，如何對求得結果進行精度判斷。
圖檔來自視覺SLAM14講
由于相機标定，圖像特征點比對的位置等，都會存在一定的誤差，實際上O1p1 與O2p2不相交，結果P就在他們之間，理論上最優解應該在這兩條線的中垂線的中間。是以可以根據P點到兩條射線的距離之和滿足一定的門檻值就可以進行過濾。
第一種做法：
- 計算三角化，求出P點的三維坐标。
- 計算兩條線的方程
- 計算點到直線的距離
關鍵是如何推導這兩條直線的方程呢？
由對極幾何知道，O1設定為原點[0, 0, 0] , O1到O2的轉換關系為[R, t] ，他們可以通過雙目立體标定計算得到。
我們都以左相機的坐标系為基準，O2坐标為:
因為在右相機中，O2本來就是原點是以Pro2=0，代入上式，O2在左相機的坐标為：
兩個相機光心的坐标有了，開始計算p1和p2點在左相機坐标系的坐标。p1很簡單，坐标值不變，p2的值經過轉換為：
空間直角坐标系中平面方程為Ax+By+Cz+D=0；可以分别代入O1 - p2，O2-pl2 求得L1 和 L2。最後計算P點到L1和L2的距離之和。
第二種方法：
前面已經計算出來4個點在左相機的坐标，其實可以根據另外的直線表示方法，計算出三維點P。
具體推導：直線方程即可以用兩點式表達，也可以用空間中點和向量表示
lambda1, lambda2可以任意取值，則點p 肯定都在它們各自的線上。v1, v2為向量。
建構的目标函數為：
寫成矩陣的表達形式：
即：
則在L1上的點為P1, L2上的點為P2
距離計算更簡單了，計算向量P1P2向量的模即可
然後根據門檻值就可以判斷是否滿足精度要求。
公式有點多，難免有打錯，請勿怪~~~
參考：http://mesh.brown.edu/desktop3dscan/ch1-intro.html