task5.cpp

#include "task5.h"
#include "task1.h"
#include "task3.h"
#include "config.h"
#include <iostream>

// ── projectOutsideCorners ─────────────────────────────────────────────────────

void projectOutsideCorners(cv::Mat& frame,
                            const cv::Mat& cameraMatrix,
                            const cv::Mat& distCoeffs,
                            const cv::Mat& rvec,
                            const cv::Mat& tvec)
{
    // Four extreme internal corners in world (board) coordinates.
    // Using the same convention as buildPointSet():
    //   col → +X,   row → -Y,   Z = 0 (flat on board)
    const float maxC = static_cast<float>(BOARD_COLS - 1) * SQUARE_SIZE;
    const float maxR = static_cast<float>(BOARD_ROWS - 1) * SQUARE_SIZE;

    std::vector<cv::Point3f> worldPts = {
        { 0.f,   0.f,   0.f },   // TL — top-left
        { maxC,  0.f,   0.f },   // TR — top-right
        { 0.f,  -maxR,  0.f },   // BL — bottom-left
        { maxC, -maxR,  0.f },   // BR — bottom-right
    };

    // Project all four points onto the image plane in one call
    std::vector<cv::Point2f> imgPts;
    cv::projectPoints(worldPts, rvec, tvec, cameraMatrix, distCoeffs, imgPts);

    // ── Draw the board outline ────────────────────────────────────────────────
    // Connect TL→TR→BR→BL→TL  (indices 0,1,3,2)
    const cv::Scalar outlineColor(0, 220, 255);   // yellow-ish
    constexpr int    outlineThick = 2;
    cv::line(frame, imgPts[0], imgPts[1], outlineColor, outlineThick);  // TL→TR
    cv::line(frame, imgPts[1], imgPts[3], outlineColor, outlineThick);  // TR→BR
    cv::line(frame, imgPts[3], imgPts[2], outlineColor, outlineThick);  // BR→BL
    cv::line(frame, imgPts[2], imgPts[0], outlineColor, outlineThick);  // BL→TL

    // ── Draw each corner dot and label ────────────────────────────────────────
    const std::vector<std::string> labels = { "TL", "TR", "BL", "BR" };
    const std::vector<cv::Scalar>  colors = {
        cv::Scalar(0,   255, 255),   // TL — yellow
        cv::Scalar(0,   255,   0),   // TR — green
        cv::Scalar(255, 100,   0),   // BL — blue-ish
        cv::Scalar(0,   100, 255),   // BR — orange
    };

    for (int i = 0; i < 4; ++i) {
        cv::circle(frame, imgPts[i], 8, colors[i], cv::FILLED);
        cv::circle(frame, imgPts[i], 8, cv::Scalar(0, 0, 0), 1);   // black outline

        // Offset label slightly so it doesn't sit on top of the dot
        cv::Point2f labelPos = imgPts[i] + cv::Point2f(10.f, -10.f);
        cv::putText(frame, labels[i], labelPos,
                    cv::FONT_HERSHEY_SIMPLEX, 0.65, cv::Scalar(0, 0, 0), 3);  // shadow
        cv::putText(frame, labels[i], labelPos,
                    cv::FONT_HERSHEY_SIMPLEX, 0.65, colors[i], 2);
    }
}

// ── runTask5 ──────────────────────────────────────────────────────────────────

void runTask5(int argc, char* argv[])
{
    // Load calibration produced by Task 3
    cv::Mat cameraMatrix, distCoeffs;
    if (!loadCalibration("calibration.yml", cameraMatrix, distCoeffs))
        return;

    cv::VideoCapture cap = openVideoSource(argc, argv);
    if (!cap.isOpened()) {
        std::cerr << "[Task5][ERROR] Cannot open video source.\n";
        return;
    }

    std::cout << "=== Task 5: Project Outside Corners ===\n"
              << "Reprojecting TL / TR / BL / BR board corners onto image plane.\n"
              << "Press 's' to save snapshot | 'q'/ESC to quit\n\n";

    cv::Mat frame, gray;
    int savedCount = 0;

    while (true) {
        cap >> frame;
        if (frame.empty()) { std::cout << "[Task5] End of stream.\n"; break; }

        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);

        std::vector<cv::Point2f> corners;
        bool found = detectCorners(gray, corners);

        if (found) {
            cv::Mat rvec, tvec;
            if (estimatePose(corners, cameraMatrix, distCoeffs, rvec, tvec)) {
                // Reproject and draw the four outside corners + board outline
                projectOutsideCorners(frame, cameraMatrix, distCoeffs, rvec, tvec);
            }
        }

        drawStatusBar(frame,
                      "Task 5 | Outside corners: " +
                      std::string(found ? "PROJECTED" : "SEARCHING..."),
                      found);

        cv::imshow("Task 5 - Outside Corners", frame);
        int key = cv::waitKey(1) & 0xFF;

        if (key == 'q' || key == 27) break;

        if (key == 's') {
            std::string fn = "task5_snapshot_" + std::to_string(++savedCount) + ".png";
            cv::imwrite(fn, frame);
            std::cout << "[Task5][SAVED] " << fn << "\n";
        }
    }

    cap.release();
    cv::destroyAllWindows();
}