利用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;
}