cmvr-es/cmvr-es/common/utils/visualization/image_display_test.cpp

#include "common/utils/visualization/image_display.h"
#include "common/utils/image/image_process.h"
#include "devices/camera/realsense_camera/include/realsense_camera.h"
#include "perception/include/tag_relative_target_3d.h"

#include <gtest/gtest.h>

#include <cmath>
#include <iostream>
#include <opencv2/imgproc.hpp>
#include <string>

namespace {

cv::Mat makeBlankImage(int width = 160, int height = 120) {
    return cv::Mat(height, width, CV_8UC3, cv::Scalar(0, 0, 0));
}

constexpr const char* kRsSerial = "243122074587";
constexpr double kTagSizeM = 0.01975;
constexpr int kRsWidth = 1280;
constexpr int kRsHeight = 720;
constexpr int kRsFps = 30;
constexpr int kTargetU = -1; // negative means image center
constexpr int kTargetV = -1; // negative means image center
constexpr int kDisplayFrames = 1200;
constexpr const char* kTrackingWindowName = "ImageDisplayTagTrackingTest";

int countNonZeroPixelsInRoi(const cv::Mat& image, const cv::Rect& roi) {
    const cv::Rect image_rect(0, 0, image.cols, image.rows);
    const cv::Rect clipped = roi & image_rect;
    if (clipped.width <= 0 || clipped.height <= 0) {
        return 0;
    }

    cv::Mat gray;
    cv::cvtColor(image(clipped), gray, cv::COLOR_BGR2GRAY);
    return cv::countNonZero(gray);
}

bool projectTargetToPixel(const Eigen::Vector3d& p_c,
                          const cmvr::device::Rs2Intrinsics& intrinsics,
                          cmvr::common::Pixel& pixel_out) {
    Eigen::Vector2d uv = Eigen::Vector2d::Zero();
    if (!cmvr::ImageProcess::projectCameraPointToPixel(intrinsics, p_c, uv)) {
        return false;
    }

    pixel_out.u = static_cast<int>(std::lround(uv.x()));
    pixel_out.v = static_cast<int>(std::lround(uv.y()));
    return true;
}

} // namespace

TEST(OpenCvImageCvDisplayTest, RenderFailsWithoutImage) {
    cmvr::common::OpenCvImageCvDisplay display;

    cv::Mat rendered;
    EXPECT_FALSE(display.render(rendered));
}

TEST(OpenCvImageCvDisplayTest, RenderPointChangesCenterRegion) {
    cmvr::common::OpenCvImageCvDisplay display;
    display.setImage(makeBlankImage());
    display.showPoint(cmvr::common::Pixel{60, 40},
                      cmvr::common::Color::red(),
                      5,
                      -1,
                      "target");

    cv::Mat rendered;
    ASSERT_TRUE(display.render(rendered));
    ASSERT_FALSE(rendered.empty());

    const cv::Vec3b center = rendered.at<cv::Vec3b>(40, 60);
    EXPECT_GT(static_cast<int>(center[2]), 150);
    EXPECT_LT(static_cast<int>(center[1]), 80);
    EXPECT_LT(static_cast<int>(center[0]), 80);
}

TEST(OpenCvImageCvDisplayTest, RenderCircleChangesExpectedArcRegion) {
    cmvr::common::OpenCvImageCvDisplay display;
    display.setImage(makeBlankImage());
    display.showCircle(cmvr::common::Pixel{60, 40},
                       12,
                       cmvr::common::Color::yellow(),
                       2);

    cv::Mat rendered;
    ASSERT_TRUE(display.render(rendered));
    ASSERT_FALSE(rendered.empty());

    EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(58, 26, 5, 5)), 0);
    EXPECT_EQ(countNonZeroPixelsInRoi(rendered, cv::Rect(59, 39, 3, 3)), 0);
}

TEST(OpenCvImageCvDisplayTest, RenderCrossChangesHorizontalAndVerticalArms) {
    cmvr::common::OpenCvImageCvDisplay display;
    display.setImage(makeBlankImage());
    display.showCross(cmvr::common::Pixel{60, 40},
                      10,
                      cmvr::common::Color::blue(),
                      2);

    cv::Mat rendered;
    ASSERT_TRUE(display.render(rendered));
    ASSERT_FALSE(rendered.empty());

    EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(49, 39, 5, 3)), 0);
    EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(59, 29, 3, 5)), 0);
}

TEST(OpenCvImageCvDisplayTest, RenderTextChangesRequestedRegion) {
    cmvr::common::OpenCvImageCvDisplay display;
    display.setImage(makeBlankImage());

    cmvr::common::TextOverlay overlay;
    overlay.text = "tracked tag: 7";
    overlay.position_px = cmvr::common::Pixel{10, 30};
    overlay.color = cmvr::common::Color::white();
    overlay.draw_background = true;

    display.showText(overlay);

    cv::Mat rendered;
    ASSERT_TRUE(display.render(rendered));
    ASSERT_FALSE(rendered.empty());

    EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(5, 5, 120, 35)), 0);
}

TEST(OpenCvImageCvDisplayTest, ClearOverlaysRemovesAllRenderedGeometry) {
    cmvr::common::OpenCvImageCvDisplay display;
    display.setImage(makeBlankImage());
    display.showPoint(cmvr::common::Pixel{60, 40},
                      cmvr::common::Color::red(),
                      5,
                      -1,
                      "target");
    display.showCircle(cmvr::common::Pixel{60, 40},
                       12,
                       cmvr::common::Color::yellow(),
                       2);
    display.showCross(cmvr::common::Pixel{60, 40},
                      10,
                      cmvr::common::Color::blue(),
                      2);
    display.showText("tracked tag: 7",
                     cmvr::common::Pixel{10, 30},
                     cmvr::common::Color::white(),
                     0.7,
                     2);

    display.clearOverlays();

    cv::Mat rendered;
    ASSERT_TRUE(display.render(rendered));
    ASSERT_FALSE(rendered.empty());
    EXPECT_EQ(countNonZeroPixelsInRoi(rendered, cv::Rect(0, 0, rendered.cols, rendered.rows)), 0);
}

TEST(OpenCvImageCvDisplayTest, ShowDisplaysWindowIfHighGuiIsAvailable) {
    constexpr const char* kWindowName = "OpenCvImageCvDisplayTest";
    cmvr::common::OpenCvImageCvDisplay display(kWindowName);

    bool window_enabled = true;
    try {
        cv::namedWindow(kWindowName, cv::WINDOW_NORMAL);
        cv::imshow(kWindowName, cv::Mat(120, 160, CV_8UC3, cv::Scalar(20, 20, 20)));
        cv::waitKey(1);
    } catch (const cv::Exception& e) {
        window_enabled = false;
        std::cout << "[OpenCvImageCvDisplayTest] window disabled: " << e.what() << "\n";
    }

    if (!window_enabled) {
        GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test.";
    }

    for (int frame = 0; frame < 1200; ++frame) {
        cv::Mat image(240, 320, CV_8UC3, cv::Scalar(20, 20, 20));
        display.setImage(image);
        display.clearOverlays();

        const int center_u = 80 + frame;
        const int center_v = 120;

        display.showPoint(cmvr::common::Pixel{center_u, center_v},
                          cmvr::common::Color::red(),
                          5,
                          -1,
                          "target");
        display.showCircle(cmvr::common::Pixel{center_u, center_v},
                           18,
                           cmvr::common::Color::yellow(),
                           2);
        display.showCross(cmvr::common::Pixel{center_u, center_v},
                          10,
                          cmvr::common::Color::blue(),
                          2);
        display.showText("tracked tag: 7",
                         cmvr::common::Pixel{20, 30},
                         cmvr::common::Color::white(),
                         0.8,
                         2);

        const int key = display.show(30);
        if (key == 27 || key == 'q' || key == 'Q') {
            break;
        }
    }

    display.close();
}

TEST(OpenCvImageCvDisplayTest, ShowTrackingImageWithActiveTagAndTargetPoint) {
    const std::string serial = kRsSerial;
    if (serial.empty()) {
        GTEST_SKIP() << "kRsSerial is empty, please set it in image_display_test.cpp";
    }

    cmvr::config::RealSenseCameraConfig cam_cfg;
    cam_cfg.set_id("image_display_test");
    cam_cfg.set_serialnumber(serial);
    cam_cfg.set_width(kRsWidth);
    cam_cfg.set_height(kRsHeight);
    cam_cfg.set_fps(kRsFps);
    cam_cfg.set_codec("H265");
    cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
    cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
    cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
    cam_cfg.set_buffer_size(30);
    cam_cfg.set_sync(true);
    cam_cfg.set_enable(true);

    auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
    auto perception = std::make_shared<cmvr::perception::AprilTagPerception>(camera);
    perception->setTagSize(kTagSizeM);

    cmvr::perception::TagRelativeTarget3D tracker(perception);
    tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
    tracker.setActiveTagSwitchPolicy(4, 1.2);
    tracker.setTrackingCandidateScoreWeights(1.0, 2.0, 0.08);

    cmvr::perception::AprilTagPerception::Options opt;
    opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
    opt.detect_tags = true;

    ASSERT_NO_THROW(camera->init());
    ASSERT_NO_THROW(camera->start());
    struct CameraStopGuard {
        std::shared_ptr<cmvr::device::RealsenseCamera> cam;
        ~CameraStopGuard() {
            if (!cam) return;
            try {
                cam->stop();
            } catch (...) {
            }
        }
    } stop_guard{camera};

    cmvr::common::OpenCvImageCvDisplay display(kTrackingWindowName);
    bool window_enabled = true;
    try {
        cv::namedWindow(kTrackingWindowName, cv::WINDOW_NORMAL);
        cv::resizeWindow(kTrackingWindowName, kRsWidth, kRsHeight);
        cv::imshow(kTrackingWindowName,
                   cv::Mat(kRsHeight, kRsWidth, CV_8UC3, cv::Scalar(20, 20, 20)));
        cv::waitKey(1);
    } catch (const cv::Exception& e) {
        window_enabled = false;
        std::cout << "[OpenCvImageCvDisplayTest] window disabled: " << e.what() << "\n";
    }
    if (!window_enabled) {
        GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test.";
    }

    bool target_uv_initialized = false;
    int target_u = kTargetU;
    int target_v = kTargetV;
    bool target_locked = false;

    for (int frame = 0; frame < kDisplayFrames; ++frame) {
        const bool update_ok = perception->update(opt);
        const cv::Mat& color = perception->color();

        cv::Mat display_image = color.empty()
            ? cv::Mat(kRsHeight, kRsWidth, CV_8UC3, cv::Scalar(20, 20, 20))
            : color;
        display.setImage(display_image);
        display.clearOverlays();

        if (!target_uv_initialized && !display_image.empty()) {
            target_u = (kTargetU >= 0) ? kTargetU : (display_image.cols / 2);
            target_v = (kTargetV >= 0) ? kTargetV : (display_image.rows / 2);
            target_uv_initialized = true;
        }

        if (target_uv_initialized) {
            const cmvr::common::Pixel selected_px{target_u, target_v};
            display.showCross(selected_px, 10, cmvr::common::Color::yellow(), 2);
            display.showCircle(selected_px, 14, cmvr::common::Color::yellow(), 2);
        }

        bool tracker_ok = false;
        if (target_uv_initialized && !color.empty()) {
            if (!target_locked) {
                tracker_ok = tracker.startTrackingFromPixel(target_u, target_v);
                target_locked = tracker_ok;
            } else {
                tracker_ok = tracker.track();
            }
        }

        if (tracker_ok) {
            cmvr::common::Pixel target_px{};
            if (projectTargetToPixel(tracker.lastTargetInCamera(), perception->intrinsics(), target_px)) {
                display.showPoint(target_px,
                                  cmvr::common::Color::red(),
                                  5,
                                  -1,
                                  "target");
                display.showCircle(target_px, 18, cmvr::common::Color::red(), 2);
            }
        }

        display.showText("active tag: " + std::to_string(tracker.activeTagId()),
                         cmvr::common::Pixel{20, 30},
                         cmvr::common::Color::white(),
                         0.8,
                         2);
        display.showText("perception: " +
                             std::string(cmvr::perception::AprilTagPerception::statusToString(
                                 perception->lastStatus())),
                         cmvr::common::Pixel{20, 60},
                         cmvr::common::Color::white(),
                         0.7,
                         2);
        display.showText("tracker: " +
                             std::string(cmvr::perception::TagRelativeTarget3D::statusToString(
                                 tracker.lastStatus())),
                         cmvr::common::Pixel{20, 90},
                         cmvr::common::Color::white(),
                         0.7,
                         2);
        display.showText("tags: " + std::to_string(perception->tags().size()) +
                             "  frame_ok: " + std::string(update_ok ? "true" : "false"),
                         cmvr::common::Pixel{20, 120},
                         cmvr::common::Color::white(),
                         0.7,
                         2);

        if (tracker_ok) {
            const Eigen::Vector3d& p_c_target = tracker.lastTargetInCamera();
            display.showText("target_c: [" +
                                 std::to_string(p_c_target.x()) + ", " +
                                 std::to_string(p_c_target.y()) + ", " +
                                 std::to_string(p_c_target.z()) + "]",
                             cmvr::common::Pixel{20, 150},
                             cmvr::common::Color::white(),
                             0.6,
                             2);
        }

        const int key = display.show(1);
        if (key == 27 || key == 'q' || key == 'Q') {
            break;
        }
    }

    display.close();
}