基于opencv3的本质矩阵（essential matrix）估计算例

至今opencv3面世快两年了，五一期间抽空看了下新增的几个关于双目视觉标定算法的使用。

关于双目的标定有一类方法是基于本质矩阵（Essential matrix）的标定方法，本质方程与基础方程都是刻画双目视觉极几何关系的两个方程，只不过它们所使用的坐标系不一样而已，本质方程是在相机光心坐标系下描述的双目成像几何关系，而基础方程是本质方程在相机像素坐标系下的表示。

由于本质矩阵是平移量t叉乘旋转矩阵R的结果。所以通过本质方程估计出本质矩阵，然后再对本质矩阵进行运动分解，即可得到双目摄像头之间的相互位置关系，即双目的标定。值得一提的是，由于本质矩阵的秩为2，分解出来的平移向量t只能得到方向，其模需要使用其它方法来获取。

理论上可以直接使用估计基础矩阵（Fundamental matrix）的方法来估计本质矩阵，如：8点算法，7点算法，最小中值法，RANSAC法等。但是实际中使用这些方法估计出的本质矩阵的结果并不太令人满意，尤其当匹配点对趋于共面时，这些方法获得的结果完全就不能用。如果用于标定的匹配点对，是在一个平面上的话，这些方法将完全失效，因为平面是一种退化结构或者叫临界曲线（Critical surfaces）。而且在opencv3之前的opencv版本只提供了求取基础矩阵F的API，虽然cv::stereoCalibrate(...)可以得到双目标定结果以及本质矩阵E，但该方法是使用平面标定版辅助的plane2plane估计出各相机的外参数，然后由外参数直接计算出旋转矩阵R和平移量t，此时获取的刚体运动参数自由度为6。

此外双目标定还有一些方法并不是基于本质矩阵来完成的，如：光束法平差（Bundle adjustment）、结构重投影法等。在此就不进行讨论了。

由于基础矩阵的自由度为7，而本质矩阵的自由度为5。直接使用基础矩阵的估计方法来估计本质矩阵并不适合，而且本质矩阵还有一种叫做5点算法的方法可以估计本质矩阵，该算法比较复杂，涉及到高次多项式方程的求解。但是对于估计本质矩阵，5点算法会得到比7点或8点算法更好的结果，而且不受匹配点集共面的影响。该方法的具体原理可见论文David
Nistér. An efficient solution to the five-point relative pose problem. Pattern Analysis and Machine Intelligence, IEEE Transactions on,
26(6):756–770, 2004.和Henrik Stewenius. Calibrated fivepoint solver.

5点算法的大概思路就是把本质方程展开，再加上本质矩阵的充要条件。化简后得到一个高次多项式方程，其实数解中的一个确定出的本质矩阵是系统的真实本质矩阵，对伪解进行剔除。然后对获得的本质矩阵进行使用SVD分解而得到旋转矩阵R和单位平移量t，即完成双目的相互位置标定。

opencv3中的cv::findEssentialMat(...)即是基于该五点算法实现的本质矩阵的估计，附上使用代码

[cpp] view
plain copy

/************************************************************** 

* an example to estimate essential matrix and relative pose of  

* binocular vision sensor using the API function of opencv3.0.0 

*   

* by jah10527@126.com 

* May, 1st, 2016 

*  

***************************************************************/  

  

  

#include <opencv2/opencv.hpp>  

#include <iostream>  

  

using namespace cv;  

using namespace std;  

  

  

static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)  

{  

    corners.resize(0);  

  

    for( int i = 0; i < boardSize.height; i++ )  

        for( int j = 0; j < boardSize.width; j++ )  

            corners.push_back(Point3f(float(j*squareSize),  

                                      float(i*squareSize), 0));  

}  

  

int main()  

{  

    int i, point_count = 54;  

    vector<Point2f> rpoints(point_count);  

    vector<Point2f> lpoints(point_count);  

      

    Mat Ml(3,3,CV_64F);  

    Mat Mr(3,3,CV_64F);  

      

    FileStorage fs("camera_l.yml", FileStorage::READ);  

    if( !fs.isOpened() )  

        return -1;  

    fs["camera_matrix"] >> Ml;  

    fs.release();   

      

    fs.open("camera_r.yml", FileStorage::READ);  

    if( !fs.isOpened() )  

        return -1;  

    fs["camera_matrix"] >> Mr;  

    fs.release();   

      

    cout << "Mr:" << endl << Mr << endl;  

    cout << "Ml:" << endl << Ml << endl;  

      

    Mat rim=imread("fight01.jpg", IMREAD_GRAYSCALE);  

    Mat lim=imread("feft01.jpg", IMREAD_GRAYSCALE);  

    findChessboardCorners( rim, Size(6,9), rpoints,  

                    CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);  

    findChessboardCorners( lim, Size(6,9), lpoints,  

                    CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);  

/*  cout << "right:" << endl; 

    for (i=0;i<point_count;i++) 

        cout << rpoints[i] << endl; 

    cout << "left:" << endl; 

    for (i=0;i<point_count;i++) 

        cout << lpoints[i] << endl;*/  

    cornerSubPix( rim, rpoints, Size(8,8),  

            Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));  

    cornerSubPix( lim, lpoints, Size(8,8),  

            Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));  

              

    for (i=0;i<point_count;i++)  

    {  

        rpoints[i].x = (rpoints[i].x-*((double*)Mr.data+2))/ *((double*)Mr.data);  

        rpoints[i].y = (rpoints[i].y-*((double*)Mr.data+5))/ *((double*)Mr.data+4);  

        lpoints[i].x = (lpoints[i].x-*((double*)Ml.data+2))/ *((double*)Ml.data);  

        lpoints[i].y = (lpoints[i].y-*((double*)Ml.data+5))/ *((double*)Ml.data+4);  

//      cout << "r:" << rpoints[i] << endl;  

//      cout << "l:" << lpoints[i] << endl;  

    }  

      

    Mat E_mat = findEssentialMat(rpoints, lpoints, 1, Point2d(0, 0), RANSAC, 0.999, 1.f);  

    cout << "E:" << endl << E_mat << endl;  

  

/*  Mat rr(3,1,CV_64F); 

    Mat ll(1,3,CV_64F); 

    *((double*)rr.data+2)=1; 

    *((double*)ll.data+2)=1; 

    for (i=0;i<8;i++){ 

        *((double*)rr.data) = rpoints[i].x; 

        *((double*)rr.data+1) = rpoints[i].y; 

        *((double*)ll.data) = lpoints[i].x; 

        *((double*)ll.data+1) = lpoints[i].y; 

        cout << ll*E_mat*rr << endl; 

        cout << rr.t()*E_mat*ll.t() << endl; 

    } 

    Mat R1, R2, t; 

    decomposeEssentialMat(E_mat, R1, R2, t); 

    cout << "R1:" << endl << R1 << endl; 

    cout << "R2:" << endl << R2 << endl; 

    cout << "t:" << endl << t << endl;*/  

      

    Mat R, t;  

    recoverPose(E_mat, rpoints, lpoints, R, t);  

    cout << "t:" << endl << t << endl;  

    cout << "R:" << endl << R << endl;  

    Mat om;  

    Rodrigues(R, om);  

    cout << "om:" << endl << om << endl;  

      

      

    vector<vector<Point3f> > objpt(1);  

    vector<vector<Point2f> > imgpt(1);  

    vector<vector<Point2f> > imgpt_right(1);  

    for (i=0;i<point_count;i++)  

    {  

        imgpt[0].push_back(rpoints[i]);  

        imgpt_right[0].push_back(lpoints[i]);  

    }  

    calcChessboardCorners(Size(6,9), 0.03, objpt[0]);  

  

    Mat RR, TT, E, F;  

    Mat eye3, zeros5;  

    eye3 = Mat::eye(3,3,CV_64F);  

    zeros5 = Mat::zeros(1,5,CV_64F);  

    double err = stereoCalibrate(objpt, imgpt, imgpt_right, eye3, zeros5, eye3, zeros5,  

                                Size(640,480), RR, TT, E, F, CALIB_FIX_INTRINSIC,  

                                TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));  

    cout << "Pair calibration reprojection error = " << err << endl;  

    Rodrigues(RR, om);  

    cout << "TT:" << endl << TT << endl;  

    cout << "RR:" << endl << RR << endl;  

    cout << "om:" << endl << om <<endl;  

    cout << "E:" << endl << E << endl;  

  

    return 0;  

}  



可以看出两种标定方法获取的结果不太一样。

重构出的标定版的三维坐标如下图所示，红色为本质矩阵方法获得的标定版空间情况，蓝色为stereoCalibrate(...)的运动参数三维重构标定版角点坐标的结果。



源码及数据下载点击打开链接