opencv双目测量棋盘格三轴转动原理与代码

来自本科毕设

1、匹配棋盘格角点

2、利用p1 p2 p3生成旋转矩阵然后分解成欧拉角

代码

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <iostream>
using namespace std;
using namespace cv;
cv::Point2d pixel2cam(const cv::Point2f& p, const cv::Mat& K) {
    return cv::Point2d(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

int main() {

    // 定义左右相机的相机矩阵

    cv::Mat T1 = (cv::Mat_<float>(3, 4) <<
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0);
    /*cv::Mat T2 = (cv::Mat_<float>(3, 4) <<
        0.953637062614699, 0.00658496811859602, -0.300887007035011, 4.458319257471509e+02,
        -0.00250704140519836, 0.999899730598709, 0.0139371263904078, 11.501725596171031,
        0.300948612807905, -0.0125366240873153, 0.953557950785106, 65.623152047724048);*/
        //cv::triangulatePoints(T2, T1, pts1, pts2, points4D);//for 2
        //cv::triangulatePoints(T1, T2, pts2, pts1, points4D);//for 2
        //cv::triangulatePoints(T2, T1, pts2, pts1, points4D);//啥也不是
        //cv::triangulatePoints(T1, T2, pts1, pts2, points4D);//啥也不是
    cv::Mat T2 = (cv::Mat_<float>(3, 4) <<
        0.995305136270557, -0.0144566356161613, 0.0957010522413938, -862.944841702106,
        0.0152041841514424, 0.999859296099956, -0.00708666259880605, -25.7816438948644,
        -0.0955851374311795, 0.00850844810537307, 0.995384894306268, 26.6371015576055);
    cv::Mat K1 = (cv::Mat_<double>(3, 3) <<
        13803.4389203154, 0, 635.162923580742,
        0, 13804.0945005903, 629.015160590977,
        0, 0, 1);
    cv::Mat K2 = (cv::Mat_<double>(3, 3) <<
        14138.2738088473, 0, 583.581246482159,
        0, 14136.9818157979, 622.304905721788,
        0, 0, 1);

    int total_img = 11;//多少张图片
    // 定义棋盘格内角点的行数和列数
    int chessboardRows = 8; // 棋盘格行数
    int chessboardCols = 11; // 棋盘格列数


    string l_path = "../../../../实验数据/图片数据/原始图片/12m5/测量/l/";
    string r_path = "../../../../实验数据/图片数据/原始图片/12m5/测量/r/";

    vector<double> x_d, y_d,z_d;


    // 读取图像
    cv::Mat image1 = cv::imread(l_path+"6.bmp");
    cv::Mat image2 = cv::imread(r_path + "6.bmp");
    /*
    // 获取帧的宽度和高度
    int frameWidth = static_cast<int>(image1.cols*2);
    int frameHeight = static_cast<int>(image2.rows);
    // 定义视频编解码器和创建 VideoWriter 对象
    int fps = 1; // 帧率
    std::string outputVideo = "E:/BaiduNetdiskDownload/毕设/12m5/output_video.avi"; // 输出视频文件路径
    cv::VideoWriter videoWriter(outputVideo, cv::VideoWriter::f