OPENCV已知内参求外参

利用OPENCV，已知内参标定外参

#include <opencv2/calib3d.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <fstream>
using namespace std;
using namespace cv;
int main() {
    
    
    //相机内参矩阵
    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
    cameraMatrix.at<double>(0, 0) = 1505.891829;
    cameraMatrix.at<double>(0, 1) = 0;
    cameraMatrix.at<double>(0, 2) = 973.395;
    cameraMatrix.at<double>(1, 1) = 1505.843003;
    cameraMatrix.at<double>(1, 2) = 597.616;
    cameraMatrix.at<double>(2, 2) = 1;
    //相机畸变系数
    Mat distCoeffs = Mat::zeros(5, 1, CV_64F);
    distCoeffs.at<double>(0, 0) = 0.0019057;
    distCoeffs.at<double>(1, 0) = 0;
    distCoeffs.at<double>(2, 0) = 0;
    distCoeffs.at<double>(3, 0) = 0;
    distCoeffs.at<double>(4, 0) = 0;

    //将控制点在世界坐标系的坐标压入容器
    vector<Point3f> objP;
    objP.clear();
    objP.push_back(Point3f(0, 4, 0));
    objP.push_back(Point3f(110, 65, 0));
    objP.push_back(Point3f(220, 4, 0));
    objP.push_back(Point3f(220, 176, 0));
    objP.push_back(Point3f(110, 115, 0));
    objP.push_back(Point3f(0, 176, 0));//6

    objP.push_back(Point3f(110, 0, 0));
    objP.push_back(Point3f(110, 180, 0));//8

    objP.push_back(Point3f(220, 50, 0));
    objP.push_back(Point3f(220, 130, 0));
    objP.push_back(Point3f(0, 130, 0));
    objP.push_back(Point3f(0, 50, 0));//12


    //将之前已经检测到的角点的坐标压入容器
    std::vector<Point2f> points;
    points.clear();
    points.push_back(Point2f(309, 43));
    points.push_back(Point2f(922, 401));
    points.push_back(Point2f(1595, 46));
    points.push_back(Point2f(1558, 1077));
    points.push_back(Point2f(915, 703));
    points.push_back(Point2f(301, 1016));//6

    points.push_back(Point2f(932, 13));
    points.push_back(Point2f(909, 1076));//8

    points.push_back(Point2f(1591, 320));
    points.push_back(Point2f(1576, 805));
    points.push_back(Point2f(296, 763));
    points.push_back(Point2f(300, 305));//12

    //因为坐标系的原因可能要手动镜像
    //for (int i = 0; i < points.size(); i++)
    //{
    
    
    //    /*int j = points[i].x;
    //    points[i].x = points[i].y;*/
    //    points[i].y =  1200- points[i].y;
    //}




    //创建旋转矩阵和平移矩阵
    Mat rvecs = Mat::zeros(3, 1, CV_64FC1);
    Mat tvecs = Mat::zeros(3, 1, CV_64FC1);

    //求解pnp
    //solvePnP(objP, points, cameraMatrix, distCoeffs, rvecs, tvecs);
    solvePnPRansac(objP, points, cameraMatrix, distCoeffs, rvecs, tvecs);
    Mat rotM = Mat::eye(3, 3, CV_64F);
    Mat rotT = Mat::eye(3, 3, CV_64F);
    Rodrigues(rvecs, rotM);  //将旋转向量变换成旋转矩阵
    Rodrigues(tvecs, rotT);

    //计算相机旋转角
    double theta_x, theta_y, theta_z;
    double PI = 3.1415926;
    theta_x = atan2(rotM.at<double>(2, 1), rotM.at<double>(2, 2));
    theta_y = atan2(-rotM.at<double>(2, 0),
        sqrt(rotM.at<double>(2, 1) * rotM.at<double>(2, 1) + rotM.at<double>(2, 2) * rotM.at<double>(2, 2)));
    theta_z = atan2(rotM.at<double>(1, 0), rotM.at<double>(0, 0));
    theta_x = theta_x * (180 / PI);
    theta_y = theta_y * (180 / PI);
    theta_z = theta_z * (180 / PI);

    //计算深度
    Mat P;
    P = (rotM.t()) * tvecs;

    //输出
    cout << "角度" << endl;
    cout << theta_x << endl;
    cout << theta_y << endl;
    cout << theta_z << endl;
    cout << P << endl;

    return 0;
}