Monocular slam 中的理论基础(2)

三角法求深度（triangulation）

   在知道了相机的轨迹以后，使用三角法就能计算某个点的深度，在Hartley的《Multiple view Geometry》一书中第10章、第12章都是讲的这个，这里只讲解线性求解方法。 

   对于三维空间中的一点  P ，我们假设第一个摄像机坐标系 C1 就是世界坐标系，P在世界坐标系下的表示为 P=(x,y,z,1)T ，这时，摄像机坐标系 C1 的外参数矩阵 M1 为单位矩阵。 点 P 和光心的连线交第一个图像平面于点 p1  ,注意这里的 p1 是在摄像机坐标系的坐标表示，不是在图像坐标系下，这在上一篇博客中已经强调。同理，和第二个摄像机光心连线交第二幅图像于点 p2  。  pi 在各自摄像机坐标系中的表示为： 
p1=⎛⎝⎜x1y11⎞⎠⎟c1   和   p2=⎛⎝⎜x2y21⎞⎠⎟c2 摄像机坐标系 C2 的外参数矩阵为 M2 ，由于摄像机坐标系 C1 就是世界坐标系，所以有 在推导本征矩阵 E 的时候，我们说 R 是从坐标系 C2 到坐标系 C1 的旋转变换矩阵，即 12R 。t是平移，更确却的说是光心 C2 在 C1 中的坐标表示,即  1t2 。所以在通过8点法求出的R,T以后，得到的从摄像机坐标2变换到到摄像机坐标系1的变换矩阵为 
12H=[R3×30t3×11] 而这里的外参数矩阵 M2 是将世界坐标系中的一点P投影到摄像机坐标系 C2 。所以 M2=21H=12H−1 ，只需去掉H矩阵的最下面一行齐次坐标就行了。所以，通过本征矩阵得到R,t以后，要计算摄像机坐标系 C2 外参数矩阵的程序如下：   有了外参矩阵，我们就可以得到这些点坐标的关系： 由于光心 Ci ，三维坐标点P，以及 pi 三点共线，所以向量 Cipi、CiP 的叉乘应该为0，上述方程又可以转化为： 这又是一个要用最小二乘求解的线性方程方程组 ,和求本征矩阵一样，计算矩阵A的SVD分解，然后奇异值最小的那个奇异向量就是三维坐标P的解。程序如下:   计算出来的P的坐标就是P在世界坐标系中的坐标，这里就是P在摄像机坐标系 C1 中的表示。并且注意上篇博客中强调过的： p1=K−1⎛⎝⎜u1v11⎞⎠⎟ 、 p2=K−1⎛⎝⎜u2v21⎞⎠⎟  
上篇博客中还提到本征矩阵恢复的R,T组合有四种组合形式，我们需要通过计算点的深度来判断R,T的哪种组合是正确的，和这篇博客结合起来，获得R,T正确组合的流程和代码如下： 

  在以上计算P三维坐标的推导过程中，可以看到和本征矩阵E是息息相关的，E和我们的尺度紧密相连，所以计算出来的深度和尺度scale也是直接相关的。同时，根据这种三角法(triangulation )计算的深度，其实是不怎么靠谱的，一般只是拿这个做一个初始值。并且，我们还可以初略的看看深度估计误差和什么有关。 
 
从两幅图中可以看出，两个射线夹角越小，误差协方差越大。所以点到光心连线组成的射线向量在orbslam中是有明确记录的。 
  在理顺了这系列流程，有了基本的视觉基础以后，就可以开始向svo，orb_slam，lsd_slam前进了。但是这些算法的深度估计都是用概率模型来更新深度，不用怕，不变应万变，在后续的博客中，我们将一一剖析。最后，再推荐下这本书《Mastering OpenCV with Practical Computer Vision Projects》，里面有一章专门教一步步用opencv写structure from motion的程序，内容实在是牛的飞，赶紧去看看动手自己写写程序吧，祝好。

（转载请注明作者和出处： http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途）
reference： 
博客主要参考了 professor William Hoff 的课件《structure from motion》