对极几何——本质矩阵E和基础矩阵F

转载：http://blog.youkuaiyun.com/tanmengwen/article/details/7409886

（1）Essential Matrix

如上图所示，给定一个目标点P，以左摄像头光心O_l为原点。点P相对于光心O_l的观察位置为P_l，相对于光心O_r的观察位置为P_r。点P在左摄像头成像平面上的位置为p_l，在右摄像头成像平面上的位置为p_r。注意P_l、P_r、p_l、p_r 都处于摄像机坐标系，其量纲是与平移向量T相同的（p_l、p_r 在图像坐标系中对应的像素坐标为 q_l、q_r ）。

假设右摄像头相对于左摄像头的相对位置关系由旋转矩阵R和平移向量T表示，则可得：P_r = R(P_l-T)。

现在我们要寻找由点P、O_l和O_r确定的对极平面的表达式。注意到平面上任意一点x与点a的连线垂直于平面法向量n，即向量 (x-a) 与向量 n 的点积为0：(x-a)·n = 0。在Ol坐标系中，光心Or的位置为T，则P、O_l和O_r确定的对极平面可由下式表示：(P_l-T)^T·(P_l×T) = 0。

由P_r = R(P_l-T) 和 R^T=R^-1 可得：(R^TP_r)^T·(P_l×T) = 0。

另一方面，向量的叉积又可表示为矩阵与向量的乘积，记向量T的矩阵表示为S，得：P_l×T = SP_l。

图11

那么就得到：(P_r)^TRSP_l = 0。这样，我们就得到Essential Matrix：E = RS。

通过矩阵E我们知道P_l和P_r的关系满足：(P_r)^TEP_l = 0。进一步地，由 p_l = f_l*P_l/Z_l 和 p_r = f_r*P_r/Z_r 我们可以得到点P在左右两个摄像机坐标系中的观察点 p_l 和 p_r应满足的极线约束关系为：(p_r)^TEp_l = 0。

注意到 E 是不满秩的，它的秩为2，那么 (p_r)^TEp_l = 0 表示的实际上是一条直线，也就是对极线。

（2）Fundamental Matrix

由于矩阵E并不包含摄像头内参信息，且E是面向摄像头坐标系的。实际上我们更感兴趣的是在图像像素坐标系上去研究一个像素点在另一视图上的对极线，这就需要用到摄像机的内参信息将摄像头坐标系和图像像素坐标系联系起来。在（1）中，p_l和p_r是物理坐标值，对应的像素坐标值为q_l和q_r，摄像头内参矩阵为M，则有：p=M^-1q。从而：(p_r)^TEp_l = 0 à q_r^T(M_r^-1)^TE M_l^-1q_l = 0。这里，我们就得到Fundamental Matrix：F = (M_r^-1)^TE M_l^-1。并有 q_r^TFq_l = 0。

（3）OpenCV的相关计算

由上面的分析可见，求取矩阵E和F关键在于旋转矩阵R和平移向量T的计算，而cvStereoCalibrate的代码中大部分（cvcalibration.cpp的第1886-2180行）也是计算和优化R和T的。在cvcalibration.cpp的第1913-1925行给出了计算R和T初始估计值的基本方法：

    /*
       Compute initial estimate of pose

       For each image, compute:
          R(om) is the rotation matrix of om
          om(R) is the rotation vector of R
          R_ref = R(om_right) * R(om_left)'
          T_ref_list = [T_ref_list; T_right - R_ref * T_left]
          om_ref_list = {om_ref_list; om(R_ref)]

       om = median(om_ref_list)
       T = median(T_ref_list)
    */

具体的计算过程比较繁杂，不好理解，这里就不讨论了，下面是计算矩阵E和F的代码：

 

    if( matE || matF )
    {
        double* t = T_LR.data.db;
        double tx[] =
        {
            0, -t[2], t[1],
            t[2], 0, -t[0],
            -t[1], t[0], 0
        };
        CvMat Tx = cvMat(3, 3, CV_64F, tx);
        double e[9], f[9];
        CvMat E = cvMat(3, 3, CV_64F, e);
        CvMat F = cvMat(3, 3, CV_64F, f);
        cvMatMul( &Tx, &R_LR, &E );
        if( matE )
            cvConvert( &E, matE );
        if( matF )
        {
            double ik[9];
            CvMat iK = cvMat(3, 3, CV_64F, ik);
            cvInvert(&K[1], &iK);
            cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T );
            cvInvert(&K[0], &iK);
            cvMatMul(&E, &iK, &F);
            cvConvertScale( &F, matF, fabs(f[8]) > 0 ? 1./f[8] : 1 );
        }
    }