task4.cpp

// Sakshi B.

#include "task4.h"
#include "task1.h"
#include "task3.h"
#include "config.h"
#include <iostream>
#include <sstream>
#include <iomanip>

// ── helpers ───────────────────────────────────────────────────────────────────

// Format a 3-element column Mat as "(x.xxx, y.yyy, z.zzz)"
static std::string fmt3(const cv::Mat& v, int decimals = 3)
{
    std::ostringstream ss;
    ss << std::fixed << std::setprecision(decimals)
       << "(" << v.at<double>(0)
       << ", " << v.at<double>(1)
       << ", " << v.at<double>(2) << ")";
    return ss.str();
}

// ── runTask4 ──────────────────────────────────────────────────────────────────

void runTask4(int argc, char* argv[])
{
    // ── Load calibration from file ────────────────────────────────────────────
    cv::Mat cameraMatrix, distCoeffs;
    if (!loadCalibration("calibration.yml", cameraMatrix, distCoeffs))
        return;

    std::cout << "[Task4] Camera matrix:\n"    << cameraMatrix << "\n"
              << "[Task4] Distortion coeffs:\n" << distCoeffs  << "\n\n";

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

    std::cout << "=== Task 4: Current Camera Position ===\n"
              << "  rvec — Rodrigues rotation (radians, angle-axis)\n"
              << "  tvec — translation in board units (squares)\n"
              << "  Moving camera left/right  → tvec[0] changes\n"
              << "  Moving camera toward/away → tvec[2] changes\n"
              << "  Press 's' to save snapshot | 'q'/ESC to quit\n\n";

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

    while (true) {
        cap >> frame;
        if (frame.empty()) break;
        ++frameCount;

        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)) {

                // ── On-screen overlay ─────────────────────────────────────────
                std::string rStr = "R: " + fmt3(rvec);
                std::string tStr = "T: " + fmt3(tvec);

                cv::putText(frame, rStr, cv::Point(10, 30),
                            cv::FONT_HERSHEY_SIMPLEX, 0.6,
                            cv::Scalar(0, 255, 255), 2);
                cv::putText(frame, tStr, cv::Point(10, 58),
                            cv::FONT_HERSHEY_SIMPLEX, 0.6,
                            cv::Scalar(0, 200, 255), 2);

                // ── Terminal output every 15 frames ───────────────────────────
                if (frameCount % 15 == 0) {
                    std::cout << "[Task4] R=" << fmt3(rvec)
                              << "  T=" << fmt3(tvec) << "\n";
                }
            }
        }

        drawStatusBar(frame,
                      "Task 4 | Camera pose: " +
                      std::string(found ? "TRACKING" : "SEARCHING..."),
                      found);

        cv::imshow("Task 4 - Camera Position", frame);
        int key = cv::waitKey(1) & 0xFF;
        if (key == 'q' || key == 27) break;
        if (key == 's') {
            std::string fn = "task4_snapshot_" + std::to_string(++savedCount) + ".png";
            cv::imwrite(fn, frame);
            std::cout << "[Task4][SAVED] " << fn << "\n";
        }
    }

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