/**************************************************************
*?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?<	cout?<	
	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?<	for?（i=0;i		cout?<	cout?<	for?（i=0;i		cout?<	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?<//		cout?<	}
	
	Mat?E_mat?=findEssentialMat（rpoints?lpoints?1?Point2d（0?0）?RANSAC?0.999?1.f）;
	cout?<
/*	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?<		cout?<	}
	Mat?R1?R2?t;
	decomposeEssentialMat（E_mat?R1?R2?t）;
	cout?<	cout?<	cout?<	
	Mat?R?t;
	recoverPose（E_mat?rpoints?lpoints?R?t）;
	cout?<