对极几何获取两幅图像旋转的方法（单应矩阵，本质矩阵）

#include
#include
#include 
using namespace cv;
using namespace std;

void generate3DPointCloud(std::vector& points)
{
    cv::Point3f pmin = cv::Point3f(-1, -1, 5);
    cv::Point3f pmax = cv::Point3f(1, 1, 10);
    cv::RNG rng = cv::theRNG(); // fix the seed to use "fixed" input 3D points

    for (size_t i = 0; i < points.size(); i++)
    {
        float _x = rng.uniform(pmin.x, pmax.x);
        float _y = rng.uniform(pmin.y, pmax.y);
        float _z = rng.uniform(pmin.z, pmax.z);
        points[i] = cv::Point3f(_x, _y, _z);
    }
}

void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{
    cv::Mat u,w,vt;
    cv::SVD::compute(E,w,u,vt);

    u.col(2).copyTo(t);
    t=t/cv::norm(t);

    cv::Mat W(3,3,CV_32F,cv::Scalar(0));
    W.at(0,1)=-1;
    W.at(1,0)=1;
    W.at(2,2)=1;

    R1 = u*W*vt;
    if(cv::determinant(R1)<0)
        R1=-R1;

    R2 = u*W.t()*vt;
    if(cv::determinant(R2)<0)
        R2=-R2;
}
int main()
{
    std::vector points;
    points.resize(100);
    generate3DPointCloud(points);
   // cv::Mat trueRvec, trueTvec;
    cv::Mat trueRvec2, trueTvec2;
    //cv::Mat intrinsics, distCoeffs;
    cv::RNG rng = cv::RNG();

    double fx = 200,fy=200,cx=500, cy =500;
    cv::Mat intrinsics = (Mat_(3, 3) <<
                   fx, 0, cx,
                   0, fy, cy,
                   0, 0, 1);
    cv::Mat K = (cv::Mat_(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0);
    //cv::Mat distCoeffs;
    cv::Mat distCoeffs =(cv::Mat_(4, 1) << 0.0, 0, 0, 0);
    //generatePose(trueRvec, trueTvec, rng);
    Mat trueTvec=(Mat_(3, 1)<<0.3,0.3,0);
    Mat trueRvec=(Mat_(3, 1)<<0*M_PI/180,0,0);
    Mat R_vec12= (cv::Mat_(3,1) <<0.0*M_PI/180, 0.0*M_PI/180, 50.0*M_PI/180);
    Mat t_vec12=(cv::Mat_(3,1) << 1.0, 0.0, 0.0);
    std::cout <<"true delta rvec"<<180*R_vec12/M_PI< opoints;
    //opoints = std::vector(points.begin(), points.begin() + 4);
    opoints = std::vector(points.begin(), points.end());
    std::vector projectedPoints,projectedPoints2;
    projectedPoints.resize(opoints.size());
    projectedPoints2.resize(opoints.size());
    projectPoints(cv::Mat(opoints), trueRvec, trueTvec, intrinsics, Mat(), projectedPoints);
    projectPoints(cv::Mat(opoints), trueRvec2, trueTvec2, intrinsics, Mat(), projectedPoints2);

    std::vector projectedPointsNorm;
    for (size_t i = 0; i < projectedPoints.size(); i++)
    {
        cv::Point2f p_norm;
        p_norm.x=(projectedPoints[i].x-cx)/fx;
        p_norm.y=(projectedPoints[i].y-cy)/fy;
        projectedPointsNorm.push_back(p_norm);
    }

    std::vector projectedPointsNorm2;
    for (size_t i = 0; i < projectedPoints2.size(); i++)
    {
        cv::Point2f p_norm;
        p_norm.x=(projectedPoints2[i].x-cx)/fx;
        p_norm.y=(projectedPoints2[i].y-cy)/fy;
        projectedPointsNorm2.push_back(p_norm);
    }
    //1.
    Mat essential_matrix, e_R, e_t, mask,e_rvec;
    essential_matrix = findEssentialMat(projectedPoints, projectedPoints2, intrinsics, RANSAC, 0.999, 1.0, mask);
    cout<<"essential_matrix"<