opencv 计算旋转矩阵R，平移矩阵T

问题：已知摄像机内参K ，以及两张不同角度的图片 ，求解摄像机的RT矩阵？
步骤：

           1.从两张图片中提取特征点，本文采用的SURF

           2.匹配特征点，得到相对应的匹配关系

           3.将keyPoint转化为Mat，然后计算基本矩阵F

           4.由基本矩阵F，求本质矩阵E。根据公式 E=(K‘)t * F *K

           5.对E进行SVD分解得到R 和 T 。

原理：参考文章 http://blog.csdn.net/xiao4399/article/details/48037287

#include <iostream>
#include<opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <vector>
using namespace std;
using namespace cv;

int main(int argc, char *argv[])
{
//
Mat img1 = imread("WIN_20161202_09_04_28_Pro.jpg");
Mat img2 = imread("WIN_20161202_09_04_47_Pro.jpg");

if (!img1.data || !img2.data)
return -1;

//step1: Detect the keypoints using SURF Detector
int minHessian = 400;

SurfFeatureDetector detector(minHessian);

vector<KeyPoint> keypoints1, keypoints2;

detector.detect(img1, keypoints1);
detector.detect(img2, keypoints2);

//step2: Calculate descriptors (feature vectors)
SurfDescriptorExtractor extractor;
Mat descriptors1, descriptors2;
extractor.compute(img1, keypoints1, descriptors1);
extractor.compute(img2, keypoints2, descriptors2);

//step3:Matching descriptor vectors with a brute force matcher
BFMatcher matcher(NORM_L2,true);
vector<DMatch> matches;
matcher.match(descriptors1, descriptors2,matches);

//Draw matches
Mat imgMatches;
drawMatches(img1, keypoints1, img2, keypoints2, matches, imgMatches);

resize(imgMatches,imgMatches,Size(192*5,108*5));
//  namedWindow("Matches");
//  imshow("brute force Matches", imgMatches);

int ptcount=(int) matches.size();
Mat p1(ptcount,2,CV_32F);
Mat p2(ptcount,2,CV_32F);

//change keypoint to mat
Point2f pt;
for(int i=0;i<ptcount;i++)
{
pt=keypoints1[matches[i].queryIdx].pt;
p1.at<float>(i,0)=pt.x;
p1.at<float>(i,1)=pt.y;

pt=keypoints2[matches[i].trainIdx].pt;
p2.at<float>(i,0)=pt.x;
p2.at<float>(i,1)=pt.y;
}

//use RANSAC to calculate F
Mat fundamental;
vector <uchar> RANSACStatus;
fundamental=findFundamentalMat(p1,p2,RANSACStatus,FM_RANSAC);

cout<<"F="<<fundamental<<endl;

double fx,fy,cx,cy;
fx=700.388086;
fy=700.784113;
cx=353.260055;
cy=223.483445;
//标定矩阵
Mat K= cv::Mat::eye(3,3,CV_64FC1);
K.at<double>(0,0) = fx;
K.at<double>(1,1) = fy;
K.at<double>(0,2) = cx;
K.at<double>(1,2) = cy;

cout<<"K="<<K<<endl;

//K转置
Mat Kt=K.t();

cout<<"Kt="<<Kt<<endl;

//E=K't * F * K
Mat E=Kt*fundamental*K;
cout<<"E="<<E<<endl;

SVD svd(E);
//        Matx33d  W(0,-1,0,
//                            1,0,0,
//                            0,0,1);
Mat W=Mat::eye(3,3,CV_64FC1);
W.at<double>(0,1)=-1;
W.at<double>(1,0)=1;
W.at<double>(2,2)=1;

Mat_<double> R=svd.u*W*svd.vt;
Mat_<double> t=svd.u.col(2);
cout<<"R="<<R<<endl;
cout<<"t="<<t<<endl;

//下面的代码为RANSAC优化后的特征点匹配效果
 /*
//calculate the number of outliner
int outlinerCount=0;
for(int i=0;i<ptcount;i++)
{
if(RANSACStatus[i]==0)
outlinerCount++;
}

//calculate inLiner
vector<Point2f> inliner1,inliner2;
vector<DMatch> inlierMatches;
int inlinerCount=ptcount-outlinerCount;
inliner1.resize(inlinerCount);
inliner2.resize(inlinerCount);
inlierMatches .resize(inlinerCount);

int inlinerMatchesCount=0;
for(int i=0;i<ptcount;i++)
{
if(RANSACStatus[i]!=0)
{
inliner1[inlinerMatchesCount].x=p1.at<float>(i,0);
inliner1[inlinerMatchesCount].y=p1.at<float>(i,1);
inliner2[inlinerMatchesCount].x=p2.at<float>(i,0);
inliner2[inlinerMatchesCount].y=p2.at<float>(i,1);
inlierMatches[inlinerMatchesCount].queryIdx=inlinerMatchesCount;
inlierMatches[inlinerMatchesCount].trainIdx=inlinerMatchesCount;
inlinerMatchesCount++;
}
}

vector<KeyPoint> key1(inlinerMatchesCount);
vector<KeyPoint> key2(inlinerMatchesCount);
KeyPoint::convert(inliner1,key1);
KeyPoint::convert(inliner2,key2);

Mat out;
drawMatches(img1,key1,img2,key2,inlierMatches,out);
resize(out,out,Size(192*5,108*5));
imshow("good match result",out);
*/
waitKey();
}