lab8_2/lab8_cub_2.cpp - master - RDA/STZ

lab8_2/lab8_cub_2.cpp
106 строк4 KB
daniil rybyakov
19 мар 2025, 19:03
19 мар 2025, 19:0386214d0
#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 false;
    }

    fs["camera_matrix"] >> cameraMatrix;
    fs["distortion_coefficients"] >> distCoeffs;
    fs.release();

    if (cameraMatrix.empty() || distCoeffs.empty()) {
        std::cout << "Ошибка при загрузке параметров калибровки!" << std::endl;

        return false;
    }    

    std::cout << "Матрица камеры:\n" << cameraMatrix << std::endl;
    std::cout << "Коэффициенты искажения:\n" << distCoeffs << std::endl;

    return true;
}

// Функция для рисования 3D куба на маркере
void drawCube(cv::Mat &image, std::vector<cv::Point2f> markerCorners, cv::Mat &cameraMatrix, cv::Mat &distCoeffs, cv::Vec3d rvec, cv::Vec3d tvec, float cubeSize) 
{
    // положение куба в пространстве
    std::vector<cv::Point3f> cubePoints = {
        {0, 0, 0}, {cubeSize, 0, 0}, {cubeSize, cubeSize, 0}, {0, cubeSize, 0},
        {0, 0, cubeSize}, {cubeSize, 0, cubeSize}, {cubeSize, cubeSize, cubeSize}, {0, cubeSize, cubeSize}
    };

    // центрированире на центр маркера
    for (auto& point : cubePoints) 
        point -= cv::Point3f(cubeSize / 2, cubeSize / 2, 0);

    // проекци на изображение
    std::vector<cv::Point2f> imagePoints;
    cv::projectPoints(cubePoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);

    // ребра куба
    std::vector<std::pair<int, int>> edges = {
        {0, 1}, {1, 2}, {2, 3}, {3, 0}, {4, 5}, {5, 6}, {6, 7}, {7, 4}, 
        {0, 4}, {1, 5}, {2, 6}, {3, 7}
    };

    // рисует ребра
    for (const auto& edge : edges) 
        cv::line(image, imagePoints[edge.first], imagePoints[edge.second], cv::Scalar(0, 0, 0), 4);
}

int main() {
    int markersX = 5, markersY = 7;
    float markerLength = 0.032f, markerSeparation = 0.008f;

    // создание словаря и сетки для обнаружения
    auto dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
    auto gridboard = cv::aruco::GridBoard::create(markersX, markersY, markerLength, markerSeparation, dictionary);
    auto detectorParams = cv::aruco::DetectorParameters::create();

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

    // параметры камеры
    cv::Mat cameraMatrix, distCoeffs;
    if (!loadCamParameters(cameraMatrix, distCoeffs)) 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::drawDetectedMarkers(image, markerCorners, markerIds);

        std::vector<cv::Vec3d> rvecs, tvecs;

        if (!markerIds.empty()) {
            cv::aruco::estimatePoseSingleMarkers(markerCorners, markerLength, cameraMatrix, distCoeffs, rvecs, tvecs);
            for (size_t i = 0; i < markerIds.size(); i++)
                drawCube(image, markerCorners[i], cameraMatrix, distCoeffs, rvecs[i], tvecs[i], markerLength);
        }

        cv::imshow("Frame with Cube", image);
        if ((char)cv::waitKey(1) == 27) break;
    }

    inputVideo.release();
    cv::destroyAllWindows();
    
    return 0;
}