lab8/lab8_cub_2.cpp - master - RDA/STZ

lab8/lab8_cub_2.cpp
119 строк6 KB
daniil rybyakov
create
10 мар 2025, 23:46
10 мар 2025, 23:46da0a00e
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/calib3d.hpp>
#include <vector>
#include <iostream>

bool loadCamparameters(cv::Mat& cameraMatrix, cv::Mat& distCoeffs)
{
    cv::FileStorage fs("camera_calibration.yml", cv::FileStorage::READ);
    if (!fs.isOpened()) {
        std::cout << "Не удалось открыть файл калибровки!" << std::endl;
        return -1;
    }
    fs["camera_matrix"] >> cameraMatrix;
    fs["distortion_coefficients"] >> distCoeffs;
    fs.release();
    if (cameraMatrix.empty() || distCoeffs.empty()) {
        std::cout << "Ошибка при загрузке параметров калибровки!" << std::endl;
        return -1;
    }    
    std::cout << "Матрица камеры:\n" << cameraMatrix << std::endl;
    std::cout << "Коэффициенты искажения:\n" << distCoeffs << std::endl;

    return true;
}

// Функция для рисования 3D куба на маркере
void drawCube(cv::Mat &image, cv::Mat &cameraMatrix, cv::Mat &distCoeffs, cv::Vec3d rvec, cv::Vec3d tvec, float cubeSize) 
{
    // Определение 3D координат вершин куба
    std::vector<cv::Point3f> cubePoints = {
        cv::Point3f(0, 0, 0),                       // Точка 0
        cv::Point3f(cubeSize, 0, 0),                // Точка 1
        cv::Point3f(cubeSize, cubeSize, 0),         // Точка 2
        cv::Point3f(0, cubeSize, 0),                // Точка 3
        cv::Point3f(0, 0, cubeSize),                // Точка 4
        cv::Point3f(cubeSize, 0, cubeSize),         // Точка 5
        cv::Point3f(cubeSize, cubeSize, cubeSize),  // Точка 6
        cv::Point3f(0, cubeSize, cubeSize)          // Точка 7
    };

    for (auto& point : cubePoints)
        point += cv::Point3f(-cubeSize/2, -cubeSize/2, 0);

    // Проекция 3D точек в 2D с помощью матрицы камеры и коэффициентов искажения
    std::vector<cv::Point2f> imagePoints;
    projectPoints(cubePoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);

    line(image, imagePoints[0], imagePoints[1], cv::Scalar(0, 0, 255), 5);
    line(image, imagePoints[1], imagePoints[2], cv::Scalar(0, 0, 255), 5);
    line(image, imagePoints[2], imagePoints[3], cv::Scalar(0, 0, 255), 5);
    line(image, imagePoints[4], imagePoints[5], cv::Scalar(0, 0, 255), 5);
    line(image, imagePoints[5], imagePoints[6], cv::Scalar(0, 0, 255), 5);
    line(image, imagePoints[6], imagePoints[7], cv::Scalar(0, 0, 255), 5);
    line(image, imagePoints[1], imagePoints[5], cv::Scalar(0, 0, 255), 5);
    line(image, imagePoints[2], imagePoints[6], cv::Scalar(0, 0, 255), 5);
    line(image, imagePoints[3], imagePoints[7], cv::Scalar(0, 255, 0), 5);
    line(image, imagePoints[7], imagePoints[4], cv::Scalar(0, 255, 0), 5);
    line(image, imagePoints[0], imagePoints[4], cv::Scalar(0, 255, 0), 5);
    line(image, imagePoints[3], imagePoints[0], cv::Scalar(0, 255, 0), 5);
}

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

    
    auto dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); // Загрузка словаря маркеров    
    cv::aruco::GridBoard gridboard(cv::Size(markersX, markersY), markerLength, markerSeparation, dictionary); // Создание объекта сетки
    
    cv::aruco::DetectorParameters detectorParams; // Параметры детектора    
    cv::aruco::ArucoDetector detector(dictionary, detectorParams); // Создание детектора

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

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

    // Загрузка коэффициентов калибровки камеры из файла
    cv::Mat cameraMatrix, distCoeffs;
    loadCamparameters(cameraMatrix, distCoeffs);

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

        std::vector<int> markerIds;
        std::vector<std::vector<cv::Point2f>> markerCorners, rejectedMarkers;
        
        detector.detectMarkers(image, markerCorners, markerIds, rejectedMarkers); // Детектирование маркеров        
        detector.refineDetectedMarkers(image, gridboard, markerCorners, markerIds, rejectedMarkers); // Стратегия для повторной детекции маркеров
        
        cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); // Отображаем маркеры на изображении
        
        std::vector<cv::Vec3d> rvecs, tvecs; // Оценка позиции и ориентации маркеров
        cv::aruco::estimatePoseSingleMarkers(markerCorners, markerLength, cameraMatrix, distCoeffs, rvecs, tvecs);
        
        for (size_t i = 0; i < markerIds.size(); i++) // Рисуем куб над каждым маркером
            drawCube(image, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], markerLength);

        imshow("Frame with Cube", image);

        char key = (char)cv::waitKey(1);
        if (key == 27) break;
    }

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

    return 0;
}