lab8/lab8_calib_2.cpp - master - RDA/STZ

lab8/lab8_calib_2.cpp
107 строк4 KB
daniil rybyakov
11 мар 2025, 17:55
11 мар 2025, 17:5504282df
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/calib3d.hpp>
#include <vector>
#include <iostream>

int main() {
    int markersX = 5;
    int markersY = 7;
    float markerLength = 0.03f;
    float markerSeparation = 0.0076f;

    auto dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
    cv::Ptr<cv::aruco::GridBoard> gridboard = cv::aruco::GridBoard::create(
        markersX, markersY, markerLength, markerSeparation, dictionary);

    cv::Ptr<cv::aruco::DetectorParameters> detectorParams = cv::aruco::DetectorParameters::create();

    std::vector<std::vector<std::vector<cv::Point2f>>> allMarkerCorners;
    std::vector<std::vector<int>> allMarkerIds;
    cv::Size imageSize;

    cv::VideoCapture inputVideo(0);
    if (!inputVideo.isOpened()) {
        std::cout << "Не удалось открыть камеру!" << std::endl;
        return -1;
    }

    while (inputVideo.grab()) {
        cv::Mat image;
        inputVideo.retrieve(image);

        std::vector<int> markerIds;
        std::vector<std::vector<cv::Point2f>> markerCorners, rejectedMarkers;
        
        cv::aruco::detectMarkers(image, dictionary, markerCorners, markerIds, detectorParams, rejectedMarkers);
        cv::aruco::refineDetectedMarkers(image, gridboard, markerCorners, markerIds, rejectedMarkers, 
                                        cv::noArray(), cv::noArray(), -1, -1, true, cv::noArray(), detectorParams);
        
        cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds);

        imshow("Frame", image);

        char key = (char)cv::waitKey(1);
        if (key == 'c' && !markerIds.empty()) {
            std::cout << "Frame captured" << std::endl;
            allMarkerCorners.push_back(markerCorners);
            allMarkerIds.push_back(markerIds);
            imageSize = image.size();
        }

        if (key == 27) break;
    }

    if (allMarkerCorners.empty()) {
        std::cout << "Не были захвачены маркеры для калибровки!" << std::endl;
        return -1;
    }

    cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
    cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64F);

    std::vector<cv::Point3f> objectPoints;
    for (int i = 0; i < markersX; i++) {
        for (int j = 0; j < markersY; j++) {
            objectPoints.push_back(cv::Point3f(
                i * (markerLength + markerSeparation),
                j * (markerLength + markerSeparation),
                0));
        }
    }

    std::vector<cv::Mat> processedObjectPoints, processedImagePoints;

    for (size_t frame = 0; frame < allMarkerCorners.size(); frame++) {
        cv::Mat currentObjPoints, currentImgPoints;
        
        // Исправленный метод
        gridboard->getObjPointsAndImgPoints(
            allMarkerCorners[frame], 
            allMarkerIds[frame], 
            currentObjPoints, 
            currentImgPoints
        );

        if (currentObjPoints.total() > 0 && currentImgPoints.total() > 0) {
            processedObjectPoints.push_back(currentObjPoints);
            processedImagePoints.push_back(currentImgPoints);
        }
    }

    calibrateCamera(processedObjectPoints, processedImagePoints, imageSize, 
                    cameraMatrix, distCoeffs, cv::noArray(), cv::noArray(),
                    cv::noArray(), cv::noArray(), cv::noArray());

    cv::FileStorage fs("camera_calibration.yml", cv::FileStorage::WRITE);
    fs << "camera_matrix" << cameraMatrix;
    fs << "distortion_coefficients" << distCoeffs;
    fs.release();

    std::cout << "Калибровка завершена!" << std::endl;

    inputVideo.release();
    cv::destroyAllWindows();

    return 0;
}