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 
#include
#include 
#include 
#include 
#include 
#include 
#include 
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 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 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(i,0)=pt.x;
            p1.at(i,1)=pt.y;

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

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

        cout<<"F="<(0,0) = fx;
        K.at(1,1) = fy;
        K.at(0,2) = cx;
        K.at(1,2) = cy;

         cout<<"K="<