最近在做三维重建的工作，三角测量原理是实现三维重建的基石，具体的算法推导可以参考之前的三角测量计算点的三维坐标一文。在之前的文章中，已知一对匹配点，可以计算出场景的三维坐标。
[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