findEssentialMat

基于opencv3的本质矩阵估计算例

/**************************************************************
* 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 
#include 

using namespace cv;
using namespace std;


int main（）
{
	int i point_count = 54;
	vector rpoints（point_count）;
	vector lpoints（point_count）;
	
	Mat Ml（33CV_64F）;
	Mat Mr（33CV_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（69） rpoints
                    CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE）;
	findChessboardCorners（ lim Size（69） lpoints
                    CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE）;
/*	cout << “right:“ << endl;
	for （i=0;i		cout << rpoints[i] << endl;
	cout << “left:“ << endl;
	for （i=0;i		cout << lpoints[i] << endl;*/
	cornerSubPix（ rim rpoints Size（88）
            Size（-1-1） TermCriteria（ TermCriteria::EPS+TermCriteria::COUNT 30 0.1 ））;
	cornerSubPix（ lim lpoints Size（88）
            Size（-1-1） TermCriteria（ TermCriteria::EPS+TermCriteria::COUNT 30 0.1 ））;
			
	for （i=0;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（31CV_64F）;
	Mat ll（13CV_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 <<

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2016-05-02 08:39  ex_findEssentialMat\
     目录           0  2016-05-02 08:39  ex_findEssentialMat\CameraCalibration\
     文件       95208  2001-11-13 16:01  ex_findEssentialMat\CameraCalibration\Calib_Results_left.mat
     文件       94424  2001-11-13 16:05  ex_findEssentialMat\CameraCalibration\Calib_Results_right.mat
     文件         503  2016-05-02 08:30  ex_findEssentialMat\CameraCalibration\Makefile
     文件       61108  2016-05-02 08:30  ex_findEssentialMat\CameraCalibration\calibration
     文件       20721  2016-05-01 21:40  ex_findEssentialMat\CameraCalibration\calibration.cpp
     文件       62968  2016-05-02 08:30  ex_findEssentialMat\CameraCalibration\calibration.o
     文件       30322  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\camera_l.yml
     文件       30325  2016-05-01 21:52  ex_findEssentialMat\CameraCalibration\camera_r.yml
     文件       87612  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft01.jpg
     文件       89129  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft02.jpg
     文件       91320  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft03.jpg
     文件       79951  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft04.jpg
     文件       89192  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft05.jpg
     文件       89394  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft06.jpg
     文件       92238  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft07.jpg
     文件       89522  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft08.jpg
     文件       86155  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft09.jpg
     文件       85327  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft10.jpg
     文件       87419  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft11.jpg
     文件       81721  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft12.jpg
     文件       87796  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft13.jpg
     文件       87444  2016-05-01 21:50  ex_findEssentialMat\CameraCalibration\feft14.jpg
     文件       82765  2016-05-02 08:34  ex_findEssentialMat\CameraCalibration\fight01.jpg
     文件       84254  2016-05-02 08:34  ex_findEssentialMat\CameraCalibration\fight02.jpg
     文件       86891  2016-05-02 08:34  ex_findEssentialMat\CameraCalibration\fight03.jpg
     文件       77002  2016-05-02 08:34  ex_findEssentialMat\CameraCalibration\fight04.jpg
     文件       81363  2016-05-02 08:34  ex_findEssentialMat\CameraCalibration\fight05.jpg
     文件       84926  2016-05-02 08:34  ex_findEssentialMat\CameraCalibration\fight06.jpg
     文件       86331  2016-05-02 08:34  ex_findEssentialMat\CameraCalibration\fight07.jpg
............此处省略60个文件信息