lab8_2/lab8_calib_2.cpp - master - RDA/STZ

lab8_2/lab8_calib_2.cpp
115 строк4 KB
daniil rybyakov
ready lab 8
12 мар 2025, 20:59
12 мар 2025, 20:59adcbdc7
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/calib3d.hpp>
#include <vector>
#include <iostream>

using namespace cv;
using namespace std;

int main() {
    // Параметры для сетки с маркерами
    int markersX = 5;   // Количество маркеров по горизонтали
    int markersY = 7;   // Количество маркеров по вертикали
    float markerLength = 0.032f;  // Длина маркера в метрах
    float markerSeparation = 0.008f;  // Расстояние между маркерами

    // Загрузка словаря маркеров
    Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::DICT_4X4_50);

    // Создание объекта сетки
    Ptr<aruco::GridBoard> gridboard = aruco::GridBoard::create(markersX, markersY, markerLength, markerSeparation, dictionary);

    // Параметры детектора
    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();

    // Вектор для хранения углов маркеров
    vector<vector<vector<Point2f>>> allMarkerCorners;
    vector<vector<int>> allMarkerIds;
    Size imageSize;

    // Открытие видеопотока
    VideoCapture inputVideo(0); // Открываем камеру по умолчанию
    if (!inputVideo.isOpened()) {
        cout << "Не удалось открыть камеру!" << endl;
        return -1;
    }

    // Захват кадров для калибровки
    while (inputVideo.grab()) {
        Mat image, imageCopy;
        inputVideo.retrieve(image);

        vector<int> markerIds;
        vector<vector<Point2f>> markerCorners, rejectedMarkers;

        // Детектирование маркеров
        aruco::detectMarkers(image, dictionary, markerCorners, markerIds, detectorParams, rejectedMarkers);

        // Отображаем маркеры на изображении
        aruco::drawDetectedMarkers(image, markerCorners, markerIds);

        imshow("Frame", image);

        // Захват кадра при нажатии клавиши 'c'
        char key = (char)waitKey(1);
        if (key == 'c' && !markerIds.empty()) {
            cout << "Frame captured" << endl;
            allMarkerCorners.push_back(markerCorners);
            allMarkerIds.push_back(markerIds);
            imageSize = image.size();
        }

        if (key == 27) { // Выход по клавише 'Esc'
            break;
        }
    }

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

    // Инициализация матрицы камеры и коэффициентов искажения
    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
    Mat distCoeffs = Mat::zeros(5, 1, CV_64F);

    // Векторы для хранения точек объекта и изображения
    vector<vector<Point3f>> processedObjectPoints;
    vector<vector<Point2f>> processedImagePoints;

    // Обработка всех кадров
    size_t nFrames = allMarkerCorners.size();
    for (size_t frame = 0; frame < nFrames; frame++) {
        vector<Point3f> currentObjPoints;
        vector<Point2f> currentImgPoints;
    
        aruco::getBoardObjectAndImagePoints(gridboard, allMarkerCorners[frame], allMarkerIds[frame], currentObjPoints, currentImgPoints);
    
        if (!currentImgPoints.empty() && !currentObjPoints.empty()) {
            processedImagePoints.push_back(currentImgPoints);
            processedObjectPoints.push_back(currentObjPoints);
        }
    }
    

    // Калибровка камеры
    double repError = calibrateCamera(processedObjectPoints, processedImagePoints, imageSize, cameraMatrix, distCoeffs, noArray(), noArray());

    cout << "Репроекционная ошибка: " << repError << endl;

    // Сохраняем параметры калибровки
    FileStorage fs("camera_calibration.yml", FileStorage::WRITE);
    fs << "camera_matrix" << cameraMatrix;
    fs << "distortion_coefficients" << distCoeffs;
    fs.release();

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

    // Закрываем видеопоток
    inputVideo.release();
    destroyAllWindows();

    return 0;
}