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

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

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

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

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

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

由于基础矩阵的自由度为7，而本质矩阵的
4000
自由度为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，通常有四对旋转平移组合，进行cheirality
check（三维点在相机前方约束），留下一组物理可实现的R,T。即完成双目的相互位置标定。

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

/**************************************************************
* 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));
}

static bool readStringList( const string& filename, vector<string>& l )
{
l.resize(0);
FileStorage fs(filename, FileStorage::READ);
if( !fs.isOpened() )
return false;
FileNode n = fs.getFirstTopLevelNode();
if( n.type() != FileNode::SEQ )
return false;
FileNodeIterator it = n.begin(), it_end = n.end();
for( ; it != it_end; ++it )
l.push_back((string)*it);
return true;
}

void calc3Dpts(vector<Point2f> &pr, vector<Point2f> &pl,Mat &MR,Mat &ML, vector<Point3f> &pts)
{

Mat cof(4,4,CV_64F), z;
double u1,v1,u2,v2;
int len, i, j;
len = pr.size();
pts.reserve(len);
for (i=0; i < len; i++)
{
u1=pr[i].x;
v1=pr[i].y;
u2=pl[i].x;
v2=pl[i].y;

*((double*)cof.data)=*((double*)MR.data+8)*u1-*((double*)MR.data);
*((double*)cof.data+1)=*((double*)MR.data+9)*u1-*((double*)MR.data+1);
*((double*)cof.data+2)=*((double*)MR.data+10)*u1-*((double*)MR.data+2);
*((double*)cof.data+3)=*((double*)MR.data+11)*u1-*((double*)MR.data+3);
*((double*)cof.data+4)=*((double*)MR.data+8)*v1-*((double*)MR.data+4);
*((double*)cof.data+5)=*((double*)MR.data+9)*v1-*((double*)MR.data+5);
*((double*)cof.data+6)=*((double*)MR.data+10)*v1-*((double*)MR.data+6);
*((double*)cof.data+7)=*((double*)MR.data+11)*v1-*((double*)MR.data+7);
*((double*)cof.data+8)=*((double*)ML.data+8)*u2-*((double*)ML.data);
*((double*)cof.data+9)=*((double*)ML.data+9)*u2-*((double*)ML.data+1);
*((double*)cof.data+10)=*((double*)ML.data+10)*u2-*((double*)ML.data+2);
*((double*)cof.data+11)=*((double*)ML.data+11)*u2-*((double*)ML.data+3);
*((double*)cof.data+12)=*((double*)ML.data+8)*v2-*((double*)ML.data+4);
*((double*)cof.data+13)=*((double*)ML.data+9)*v2-*((double*)ML.data+5);
*((double*)cof.data+14)=*((double*)ML.data+10)*v2-*((double*)ML.data+6);
*((double*)cof.data+15)=*((double*)ML.data+11)*v2-*((double*)ML.data+7);

//	cout << "cof:" << endl << cof << endl;
SVD::solveZ(cof, z);
for (j=0;j<3;j++) *((double*)z.data+j)/=*((double*)z.data+3);
pts.push_back(Point3f(float(*((double*)z.data)), float(*((double*)z.data+1)), float(*((double*)z.data+2))));
}
}

int main()
{
int i, point_count = 54;

Mat Kl, Dl, imagePointsL, rtL;
Mat Kr, Dr, imagePointsR, rtR;

FileStorage fs("out_camera_left.yml", FileStorage::READ);
if( !fs.isOpened() )
return -1;
fs["camera_matrix"] >> Kl;
fs["distortion_coefficients"] >> Dl;
fs["image_points"] >> imagePointsL;
fs["extrinsic_parameters"] >> rtL;
fs.release();

fs.open("out_camera_right.yml", FileStorage::READ);
if( !fs.isOpened() )
return -1;
fs["camera_matrix"] >> Kr;
fs["distortion_coefficients"] >> Dr;
fs["image_points"] >> imagePointsR;
fs["extrinsic_parameters"] >> rtR;
fs.release();

cout << "Kr:" << endl << Kr << endl;
cout << "Kl:" << endl << Kl << endl;
cout << "Dr:" << endl << Dr << endl;
cout << "Dl:" << endl << Dl << endl;
cout << "imagePointsL channels:" << endl << imagePointsL.channels() << endl;
cout << "imagePointsR channels:" << endl << imagePointsR.channels() << endl;

Mat rvecL(3,1,CV_64F,rtL.data);
Mat tvecL(3,1,CV_64F,rtL.data+3*sizeof(double));
Mat rvecR(3,1,CV_64F,rtR.data);
Mat tvecR(3,1,CV_64F,rtR.data+3*sizeof(double));
Mat rotL, rotR;
Rodrigues(rvecL, rotL);
Rodrigues(rvecR, rotR);

vector<vector<Point2f>> ptsL, ptsR, ptsLu, ptsRu;
ptsL.resize(1);
ptsR.resize(1);
ptsLu.resize(1);
ptsRu.resize(1);

for (int j=0;j<1;j++)
{
for (i=0;i<imagePointsL.cols;i++)
{
//		  cout << imagePointsL.at<Point2f>(j,i) << endl;
ptsL[j].push_back(imagePointsL.at<Point2f>(j,i));
ptsR[j].push_back(imagePointsR.at<Point2f>(j,i));
}
undistortPoints(ptsL[j], ptsLu[j], Kl, Dl);
undistortPoints(ptsR[j], ptsRu[j], Kr, Dr);
}
/*  FILE *fp;
fp = fopen("data.txt","w");
fprintf(fp,"L:\n");
for (i=0;i<imagePointsL.cols;i++)
{
fprintf(fp,"%f,%f\n",ptsLu[0][i].x,ptsLu[0][i].y);
cout << ptsLu[0][i] << endl;
}
cout << "distorted---undistorted:R" << endl;
fprintf(fp,"L:\n");
for (i=0;i<imagePointsL.cols;i++)
{
fprintf(fp,"%f,%f\n",ptsRu[0][i].x,ptsRu[0][i].y);
cout << ptsRu[0][i] << endl;
}
fclose(fp);*/
cout << "out" << endl;
Mat E_mat;
E_mat = findEssentialMat(ptsLu[0], ptsRu[0], 1, Point2d(0, 0), RANSAC, 0.999, 1.f);
cout << "E:" << endl << E_mat/E_mat.at<double>(2,2) << endl;

for (i=0;i<ptsLu[0].size();i++)
{
Mat ptL, ptR, ptL64, ptR64;
ptL = Mat(ptsLu[0][i]);
ptL.push_back(1.0f);
ptR = Mat(ptsRu[0][i]);
ptR.push_back(1.0f);
ptL.convertTo(ptL64,CV_64F);
ptR.convertTo(ptR64,CV_64F);
CV_Assert(ptL64.depth() == E_mat.depth());
cout << ptR64.t()*E_mat*ptL64 << endl;
//		cout << ptL64.t()*E_mat*ptR64 << 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, ptsLu[0], ptsRu[0], R, t);
cout << "t:" << endl << t << endl;
cout << "R:" << endl << R << endl;

vector<vector<Point3f>> objpt(ptsL.size());
for (i=0;i<ptsL.size();i++)
calcChessboardCorners(Size(6,9), 0.03, objpt[i]);

Mat RR, TT, E, F;
double err = stereoCalibrate(objpt, ptsL, ptsR, Kl, Dl, Kr, Dr,
Size(640,480), RR, TT, E, F, CALIB_FIX_INTRINSIC,
TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) );
cout << "Pair calibration reprojection error = " << err << endl;
cout << "TT:" << endl << TT << endl;
cout << "RR:" << endl << RR << endl;
cout << "E:" << endl << E/E.at<double>(2,2) << endl;
cout << "R:" << rotR*rotL.t() << endl;
cout << "t:" << tvecR-rotR*rotL.t()*tvecL << endl;

Mat ML(3,4,CV_64F);
Mat MR(3,4,CV_64F);
Kl.copyTo(ML.colRange(0,3));
ML.col(3)=Mat::zeros(3,1,CV_64F);
MR.colRange(0,3)=Kr*R;
MR.col(3)=Kr*t;
cout << "ML:" << endl << ML << endl;
cout << "MR:" << endl << MR << endl;

vector<Point3f> pts;
calc3Dpts(ptsLu[0], ptsRu[0], MR, ML, pts);
cout << pts.size() << endl;
for (i=0;i<pts.size();i++) cout << pts[i] << endl;

return 0;
}

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

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



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

在实际使用中，建议尽量避免匹配点对来自于同一个平面，且必须有平移量，这样估计出的本质矩阵会好很多！如果只有旋转的存在，无平移量，可以参考http://blog.csdn.net/j10527/article/details/79077739