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::fourcc('M', 'J', 'P', 'G'), fps, cv::Size(frameWidth, frameHeight));
    if (!videoWriter.isOpened()) {
        std::cerr << "Error: Could not open the output video file for writing." << std::endl;
        return -1;
    }
    */
    /*
    cv::Mat image1, image2;

    Mat distort1 = (cv::Mat_<double>(4, 1) << 0.359491204591065, 42.5099982392409,  0, 0);
    Mat distort2 = (cv::Mat_<double>(4, 1) << 0.444058777698131, 34.8489892235698, 0, 0);
    undistort(dist_img1, image1, K1, distort1);
    undistort(dist_img2, image2, K2, distort2);

    */
    Mat gray1, gray2;
    cvtColor(image1, gray1, COLOR_BGR2GRAY);
    cvtColor(image2, gray2, COLOR_BGR2GRAY);

    // 用于存储检测到的角点
    std::vector<cv::Point2f> corners1, corners2;
    // 尝试在图像中检测棋盘格角点
    bool patternFound1 = cv::findChessboardCorners(image1, cv::Size(chessboardCols, chessboardRows), corners1, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
    bool patternFound2 = cv::findChessboardCorners(image2, cv::Size(chessboardCols, chessboardRows), corners2, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
    int t1 = 0;
    //for (int i = 5; i >= 1; i--)
    for(int i=1;i<=total_img;i++)
    {
    cv::Mat image3 = cv::imread(l_path+to_string(i)+".bmp");
    cv::Mat image4 = cv::imread(r_path + to_string(i) + ".bmp");
    /*cv::Mat image3, image4;
    undistort(dist_img3, image3, K1, distort1);
    undistort(dist_img4, image4, K2, distort2);
    */
    Mat  gray3, gray4;
    cvtColor(image3, gray3, COLOR_BGR2GRAY);
    cvtColor(image4, gray4, COLOR_BGR2GRAY);

    // 检查图像是否成功加载
    if (image1.empty() || image2.empty() || image3.empty() || image4.empty()) {
        std::cerr << "无法加载图像文件!" << std::endl;
        return -1;
    }

    // 计时开始
    double time1 = static_cast<double>(getTickCount());
    // 用于存储检测到的角点
    std::vector<cv::Point2f> corners3, corners4;
    bool patternFound3 = cv::findChessboardCorners(image3, cv::Size(chessboardCols, chessboardRows), corners3, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
    bool patternFound4 = cv::findChessboardCorners(image4, cv::Size(chessboardCols, chessboardRows), corners4, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);

    // 计时结束
    time1 = ((double)getTickCount() - time1) / getTickFrequency();

    t1 += time1;
    // 如果找到棋盘格角点
    if (patternFound1 && patternFound2 && patternFound3 && patternFound4) {
        /*
        // 在图像中绘制检测到的角点
        cv::drawChessboardCorners(image1, cv::Size(chessboardCols, chessboardRows), corners1, patternFound1);
        cv::drawChessboardCorners(image2, cv::Size(chessboardCols, chessboardRows), corners2, patternFound2);
        // 在图像中绘制检测到的角点
        cv::drawChessboardCorners(image3, cv::Size(chessboardCols, chessboardRows), corners3, patternFound3);
        cv::drawChessboardCorners(image4, cv::Size(chessboardCols, chessboardRows), corners4, patternFound4);
        */
        // 提高角点的精确度
        cornerSubPix(gray1, corners1, Size(11, 11), Size(-1, -1),
            TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 50, 0.01));
        cornerSubPix(gray2, corners2, Size(11, 11), Size(-1, -1),
            TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 50, 0.01));
        cornerSubPix(gray3, corners3, Size(11, 11), Size(-1, -1),
            TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 50, 0.01));
        cornerSubPix(gray4, corners4, Size(11, 11), Size(-1, -1),
            TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 50, 0.01));
        

        std::vector<cv::Point2d> pts1, pts2, pts3, pts4;
        for (int i = 0; i < corners1.size(); i++)
        {
            // 调用函数将像素坐标系下的点转换为相机坐标系下的点
            pts1.push_back(pixel2cam(corners1[i], K1));
            pts2.push_back(pixel2cam(corners2[i], K2));
            pts3.push_back(pixel2cam(corners3[i], K1));
            pts4.push_back(pixel2cam(corners4[i], K2));
        }
        /*
        std::vector<cv::Point2f> p1;
        p1.push_back(pixel2cam(corners1[1], K1));
        std::vector<cv::Point2f> p2;
        p2.push_back(pixel2cam(corners2[1], K2));
        */
        // 通过三角测量计算三维点坐标
        cv::Mat points4D1, points4D2;
        cv::triangulatePoints(T1, T2, pts1, pts2, points4D1);//T2 == T21 服了
        cv::triangulatePoints(T1, T2, pts3, pts4, points4D2);//T2 == T21 服了
        //cv::triangulatePoints(T2, T1, pts1, pts2, points4D);//for 2
        // 将齐次坐标转换为非齐次坐标
        cv::Mat points3D1, points3D2;
        cv::convertPointsFromHomogeneous(points4D1.t(), points3D1);
        cv::convertPointsFromHomogeneous(points4D2.t(), points3D2);
        /*
        std::vector<cv::Point3d> px1, py1, px2, py2;

        for (int i = 0; i < chessboardCols; i++)
        {
            px1.push_back(points3D1.at<cv::Vec3d>(i, 0));
            px2.push_back(points3D2.at<cv::Vec3d>(i, 0));
        }
        for (int i = 0; i < chessboardRows; i++)
        {
            py1.push_back(points3D1.at<cv::Vec3d>(i* chessboardCols, 0));
            py2.push_back(points3D2.at<cv::Vec3d>(i* chessboardCols, 0));
        }
        Vec6f linesx1, linesx2,linesy1, linesy2;
        fitLine(px1, linesx1, DIST_L2, 0.0, 1e-2, 1e-2);
        fitLine(px2, linesx2, DIST_L2, 0.0, 1e-2, 1e-2);
        fitLine(py1, linesy1, DIST_L2, 0.0, 1e-2, 1e-2);
        fitLine(py2, linesy2, DIST_L2, 0.0, 1e-2, 1e-2);
        cv::Vec3d X1, X2, Y1, Y2;


        if (linesx1[0] > 0)
        {
            X1[0] = linesx1[0];
            X1[1] = linesx1[1];
            X1[2] = linesx1[2];
        }
        else
        {
            X1[0] = -linesx1[0];
            X1[1] = -linesx1[1];
            X1[2] = -linesx1[2];
        }
        if (linesy1[1] > 0)
        {
            Y1[0] = linesy1[0];
            Y1[1] = linesy1[1];
            Y1[2] = linesy1[2];
        }
        else
        {
            Y1[0] = -linesy1[0];
            Y1[1] = -linesy1[1];
            Y1[2] = -linesy1[2];
        }
        if (linesx2[0] > 0)
        {
            X2[0] = linesx2[0];
            X2[1] = linesx2[1];
            X2[2] = linesx2[2];
        }
        else
        {
            X2[0] = -linesx2[0];
            X2[1] = -linesx2[1];
            X2[2] = -linesx2[2];
        }
        if (linesy2[1] > 0)
        {
            Y2[0] = linesy2[0];
            Y2[1] = linesy2[1];
            Y2[2] = linesy2[2];
        }
        else
        {
            Y2[0] = -linesy2[0];
            Y2[1] = -linesy2[1];
            Y2[2] = -linesy2[2];
        }
        
        */
        cv::Vec3d X0(1.0, 0.0, 0.0);
        cv::Vec3d Y0(0.0, 1.0, 0.0);
        cv::Vec3d Z0(0.0, 0.0, 1.0);
        cv::Vec3d X1(points3D1.at<cv::Vec3d>(10, 0)[0] - points3D1.at<cv::Vec3d>(0, 0)[0],
            points3D1.at<cv::Vec3d>(10, 0)[1] - points3D1.at<cv::Vec3d>(0, 0)[1],
            points3D1.at<cv::Vec3d>(10, 0)[2] - points3D1.at<cv::Vec3d>(0, 0)[2]);
        cv::Vec3d Y1(points3D1.at<cv::Vec3d>(77, 0)[0] - points3D1.at<cv::Vec3d>(0, 0)[0],
            points3D1.at<cv::Vec3d>(77, 0)[1] - points3D1.at<cv::Vec3d>(0, 0)[1],
            points3D1.at<cv::Vec3d>(77, 0)[2] - points3D1.at<cv::Vec3d>(0, 0)[2]);
        /* cv::normalize(_X1, _X1, 1.0, 0, cv::NORM_L2);
         cv::normalize(_Y1, _Y1, 1.0, 0, cv::NORM_L2);*/
        
        cv::Vec3d Z1 = X1.cross(Y1);
        //归一化
        cv::normalize(X1, X1, 1.0, 0, cv::NORM_L2);
        cv::normalize(Y1, Y1, 1.0, 0, cv::NORM_L2);
        cv::normalize(Z1, Z1, 1.0, 0, cv::NORM_L2);

        cv::Vec3d X2(points3D2.at<cv::Vec3d>(10, 0)[0] - points3D2.at<cv::Vec3d>(0, 0)[0],
            points3D2.at<cv::Vec3d>(10, 0)[1] - points3D2.at<cv::Vec3d>(0, 0)[1],
            points3D2.at<cv::Vec3d>(10, 0)[2] - points3D2.at<cv::Vec3d>(0, 0)[2]);
        cv::Vec3d Y2(points3D2.at<cv::Vec3d>(77, 0)[0] - points3D2.at<cv::Vec3d>(0, 0)[0],
            points3D2.at<cv::Vec3d>(77, 0)[1] - points3D2.at<cv::Vec3d>(0, 0)[1],
            points3D2.at<cv::Vec3d>(77, 0)[2] - points3D2.at<cv::Vec3d>(0, 0)[2]);
        /*cv::normalize(_X2, _X2, 1.0, 0, cv::NORM_L2);
        cv::normalize(_Y2, _Y2, 1.0, 0, cv::NORM_L2);*/

        cv::Vec3d Z2 = X2.cross(Y2);
        //归一化
        cv::normalize(X2, X2, 1.0, 0, cv::NORM_L2);
        cv::normalize(Y2, Y2, 1.0, 0, cv::NORM_L2);
        cv::normalize(Z2, Z2, 1.0, 0, cv::NORM_L2);





        cv::Mat R12 = (cv::Mat_<double>(3, 3) <<
            X2.dot(X1), Y2.dot(X1), Z2.dot(X1),
            X2.dot(Y1), Y2.dot(Y1), Z2.dot(Y1),
            X2.dot(Z1), Y2.dot(Z1), Z2.dot(Z1));

        // 计算欧拉角
        double sy = std::sqrt(R12.at<double>(0, 0) * R12.at<double>(0, 0) + R12.at<double>(1, 0) * R12.at<double>(1, 0));
        bool singular = sy < 1e-6; // 如果sy接近0,矩阵是奇异的

        double x, y, z;
        if (!singular) {
            x = std::atan2(R12.at<double>(2, 1), R12.at<double>(2, 2));
            y = std::atan2(-R12.at<double>(2, 0), sy);
            z = std::atan2(R12.at<double>(1, 0), R12.at<double>(0, 0));
        }
        else {
            x = std::atan2(-R12.at<double>(1, 2), R12.at<double>(1, 1));
            y = std::atan2(-R12.at<double>(2, 0), sy);
            z = 0;
        }

        double xdeg, ydeg, zdeg;
        xdeg = x * 180 / CV_PI;
        ydeg = y * 180 / CV_PI;
        zdeg = z * 180 / CV_PI;

        x_d.push_back(xdeg);
        y_d.push_back(ydeg);
        z_d.push_back(zdeg);
        /*
        // 水平拼接图像
        Mat result1;
        hconcat(image3, image4, result1);
        */
        /*
        // 构造DMatch对象
        std::vector<cv::DMatch> lrmatches;
        for (size_t i = 0; i < 3; i++) {
            lrmatches.push_back(cv::DMatch(i, i, 0));
        }

        // 转换为KeyPoint对象
        std::vector<cv::KeyPoint> keypointsl, keypointsr;
        
        keypointsl.push_back(cv::KeyPoint(corners3[0], 1.f));
        keypointsl.push_back(cv::KeyPoint(corners3[10], 1.f));
        keypointsl.push_back(cv::KeyPoint(corners3[77], 1.f));
        keypointsr.push_back(cv::KeyPoint(corners4[0], 1.f));
        keypointsr.push_back(cv::KeyPoint(corners4[10], 1.f));
        keypointsr.push_back(cv::KeyPoint(corners4[77], 1.f));
        // 绘制匹配结果
        cv::Mat img_matches;
        cv::drawMatches(image3, keypointsl, image4, keypointsr, lrmatches, img_matches);
        // 使用 putText 函数写入文字
        cv::putText(img_matches, "X:"+to_string(-xdeg), Point(2000, 50), FONT_HERSHEY_SIMPLEX, 2, Scalar(0, 0, 255), 3);
        cv::putText(img_matches, "Y:" + to_string(-ydeg), Point(2000, 100), FONT_HERSHEY_SIMPLEX, 2, Scalar(0, 0, 255), 3);
        cv::putText(img_matches, "Z:" + to_string(-zdeg), Point(2000, 150), FONT_HERSHEY_SIMPLEX, 2, Scalar(0, 0, 255), 3);
        // 将当前帧写入视频文件
        videoWriter.write(img_matches);
        */
        /*// 水平拼接图像
        Mat result2;
        hconcat(image1, image2, result2);
        Mat result3;
        vconcat(result1, result2, result3);
        */
        double dis = points3D1.at<cv::Vec3d>(0, 0)[2];


    }
    else {
        std::cerr << "未能检测到棋盘格角点!" << std::endl;
    }
    }
    //videoWriter.release();
    return 0;
}

传感器精度0.02°(不公布)

  • 9
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

残月独悬

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值