tests/test_circular_marker_detector.cpp - master - klischa/AstraScanner2

tests/test_circular_marker_detector.cpp
419 строк13 KB
Astra2
fix: harden project state and tests
20 июн 2026, 08:27
20 июн 2026, 08:274fd91fd
#include <gtest/gtest.h>
#include <QCoreApplication>
#include <QDebug>
#include <cstdint>
#include <opencv2/opencv.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "../src/marker_tracker/CircularMarkerDetector.h"

class CircularMarkerDetectorTest : public ::testing::Test {
protected:
    void SetUp() override {
        if (!QCoreApplication::instance()) {
            static int argc = 1;
            static char arg0[] = "test_circular_marker_detector";
            static char* argv[] = { arg0 };
            new QCoreApplication(argc, argv);
        }
    }
    
    cv::Mat createTestImage() {
        // Создаем изображение с белым фоном
        cv::Mat image(480, 640, CV_8UC3, cv::Scalar(255, 255, 255));
        return image;
    }
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr createTestCloud(float z = 0.5f) {
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
        
        for (int i = 0; i < 100; ++i) {
            pcl::PointXYZRGB pt;
            pt.x = 0.01 * (i % 10);
            pt.y = 0.01 * (i / 10);
            pt.z = z;
            pt.r = 255; pt.g = 255; pt.b = 255;
            cloud->push_back(pt);
        }
        
        cloud->width = 10;
        cloud->height = 10;
        
        return cloud;
    }
};

// Тест: Конструктор и инициализация
TEST_F(CircularMarkerDetectorTest, ConstructorAndInit)
{
    CircularMarkerDetector detector;
    
    EXPECT_TRUE(true); // Проверка, что конструктор сработал без ошибок
}

// Тест: Установка параметров фильтрации
TEST_F(CircularMarkerDetectorTest, SetDetectionParams)
{
    CircularMarkerDetector detector;
    
    detector.setDetectionParams(0.6f, 0.7f, 0.4f);
    
    EXPECT_TRUE(true);
}

// Тест: Установка параметров диска
TEST_F(CircularMarkerDetectorTest, SetDiskParams)
{
    CircularMarkerDetector detector;
    
    detector.setDiskParams(300.0f, 7);
    
    EXPECT_TRUE(true);
}

// Тест: Установка фильтра радиусов
TEST_F(CircularMarkerDetectorTest, SetRadiusFilter)
{
    CircularMarkerDetector detector;
    
    detector.setRadiusFilter(5.0f, 60.0f);
    
    EXPECT_TRUE(true);
}

// Тест: Установка фильтра площади
TEST_F(CircularMarkerDetectorTest, SetAreaFilter)
{
    CircularMarkerDetector detector;
    
    detector.setAreaFilter(80.0f, 500.0f);
    
    EXPECT_TRUE(true);
}

// Тест: Установка интринсик
TEST_F(CircularMarkerDetectorTest, SetIntrinsics)
{
    CircularMarkerDetector detector;
    
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    EXPECT_TRUE(true);
}

// Тест: Детекция на пустом изображении
TEST_F(CircularMarkerDetectorTest, DetectEmpty)
{
    CircularMarkerDetector detector;
    
    detector.setDiskParams(300.0f, 7);
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    cv::Mat empty;
    std::vector<cv::Point2f> centers = detector.detectMarkers(empty);
    
    EXPECT_EQ(centers.size(), 0u);
}

// Тест: Детекция на пустом фоне
TEST_F(CircularMarkerDetectorTest, DetectNoMarkers)
{
    CircularMarkerDetector detector;
    
    detector.setDiskParams(300.0f, 7);
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    cv::Mat image = createTestImage();
    std::vector<cv::Point2f> centers = detector.detectMarkers(image);
    
    EXPECT_GE(centers.size(), 0u);
}

// Тест: Reconstruct3DPositions
TEST_F(CircularMarkerDetectorTest, Reconstruct3DPositions)
{
    CircularMarkerDetector detector;
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = createTestCloud(0.5f);
    
    // Создаем 2D центры
    std::vector<cv::Point2f> centers_2d;
    centers_2d.push_back(cv::Point2f(320.0f, 240.0f));
    
    // Реконструируем 3D позиции
    std::vector<Eigen::Vector3f> positions = detector.reconstruct3DPositions(
        centers_2d, cloud, 0.1f, 10.0f);
    
    EXPECT_LE(positions.size(), centers_2d.size());
    
    // Проверяем валидность позиций
    for (const auto& pos : positions) {
        EXPECT_TRUE(std::isfinite(pos.x())) << "Invalid X: " << pos.x();
        EXPECT_TRUE(std::isfinite(pos.y())) << "Invalid Y: " << pos.y();
        EXPECT_TRUE(std::isfinite(pos.z())) << "Invalid Z: " << pos.z();
    }
}

// Тест: getMarkerIDs (для Circular должен быть пустым)
TEST_F(CircularMarkerDetectorTest, GetMarkerIDs)
{
    CircularMarkerDetector detector;
    detector.setDiskParams(300.0f, 7);
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    cv::Mat image = createTestImage();
    std::vector<cv::Point2f> centers = detector.detectMarkers(image);
    
    std::vector<int> ids = detector.getMarkerIDs();
    
    // Для Circular должен быть пустым
    EXPECT_EQ(ids.size(), 0u);
}

// Тест: getReliableCount
TEST_F(CircularMarkerDetectorTest, GetReliableCount)
{
    CircularMarkerDetector detector;
    detector.setDiskParams(300.0f, 7);
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    cv::Mat image = createTestImage();
    std::vector<cv::Point2f> centers = detector.detectMarkers(image);
    
    int reliable = detector.getReliableCount();
    
    EXPECT_GE(reliable, 0);
    EXPECT_LE(reliable, static_cast<int>(centers.size()));
}

// Тест: Множественная детекция
TEST_F(CircularMarkerDetectorTest, RepeatedDetection)
{
    CircularMarkerDetector detector;
    detector.setDiskParams(300.0f, 7);
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    cv::Mat image = createTestImage();
    
    // Детектируем несколько раз
    std::vector<cv::Point2f> centers1 = detector.detectMarkers(image);
    std::vector<cv::Point2f> centers2 = detector.detectMarkers(image);
    
    // Количество результатов должно быть стабильным
    EXPECT_EQ(centers1.size(), centers2.size());
}

// Тест: Пустое облако точек
TEST_F(CircularMarkerDetectorTest, EmptyCloud)
{
    CircularMarkerDetector detector;
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr empty_cloud;
    
    std::vector<cv::Point2f> centers_2d;
    centers_2d.push_back(cv::Point2f(320.0f, 240.0f));
    
    std::vector<Eigen::Vector3f> positions = detector.reconstruct3DPositions(
        centers_2d, empty_cloud, 0.1f, 10.0f);
    
    EXPECT_EQ(positions.size(), 0u);
}

// Тест: Out of bounds индексы
TEST_F(CircularMarkerDetectorTest, OutOfBoundsIndices)
{
    CircularMarkerDetector detector;
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    // Создаем маленькое облако
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    for (int i = 0; i < 25; ++i) {
        pcl::PointXYZRGB pt;
        pt.x = 0.01 * (i % 5);
        pt.y = 0.01 * (i / 5);
        pt.z = 0.5;
        pt.r = 255; pt.g = 255; pt.b = 255;
        cloud->push_back(pt);
    }
    cloud->width = 5;
    cloud->height = 5;
    
    // Создаем 2D центры вне границ
    std::vector<cv::Point2f> centers_2d;
    centers_2d.push_back(cv::Point2f(1000.0f, 1000.0f));  // Вне границ
    
    std::vector<Eigen::Vector3f> positions = detector.reconstruct3DPositions(
        centers_2d, cloud, 0.1f, 10.0f);
    
    // Невалидные индексы не должны вызвать crash
    EXPECT_GE(positions.size(), 0u);
}

// Тест: getDepthMedian
TEST_F(CircularMarkerDetectorTest, GetDepthMedian)
{
    CircularMarkerDetector detector;
    
    // Создаем облако с известной глубиной
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    for (int i = 0; i < 100; ++i) {
        pcl::PointXYZRGB pt;
        pt.x = 0.01 * (i % 10);
        pt.y = 0.01 * (i / 10);
        pt.z = 0.5;
        pt.r = 255; pt.g = 255; pt.b = 255;
        cloud->push_back(pt);
    }
    cloud->width = 10;
    cloud->height = 10;
    
    // Вызываем getDepthMedian
    float depth = detector.getDepthMedian(cloud, 5, 5, 2);
    
    // Должна вернуть корректное значение
    EXPECT_GT(depth, 0.0f);
    EXPECT_LT(depth, 10.0f);
}


TEST_F(CircularMarkerDetectorTest, KdTreeDepthUsesZNotEuclideanNorm)
{
    CircularMarkerDetector detector;
    detector.setIntrinsics(100.0f, 100.0f, 320.0f, 240.0f);
    detector.setDepthResolution(640, 480);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointXYZRGB pt;
    pt.x = 2.0f;   // pixel x=420 at z=2 with fx=100,cx=320
    pt.y = 0.0f;
    pt.z = 2.0f;
    pt.r = pt.g = pt.b = 255;
    cloud->push_back(pt);
    cloud->width = static_cast<uint32_t>(cloud->size());
    cloud->height = 1; // unorganized

    const float depth = detector.getDepthAroundKdTree(cloud, 420, 240, 5);
    EXPECT_NEAR(depth, 2.0f, 1e-5f);
}

// Тест: getDepthMedian с пустым облаком
TEST_F(CircularMarkerDetectorTest, GetDepthMedianEmpty)
{
    CircularMarkerDetector detector;
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr empty_cloud;
    
    float depth = detector.getDepthMedian(empty_cloud, 5, 5, 2);
    
    // Должна вернуть 0.0f
    EXPECT_EQ(depth, 0.0f);
}

// Тест: detectAndReconstruct
TEST_F(CircularMarkerDetectorTest, DetectAndReconstruct)
{
    CircularMarkerDetector detector;
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    // Создаем облако с известной глубиной
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = createTestCloud(0.5f);
    
    cv::Mat image = createTestImage();
    
    // Вызываем detectAndReconstruct
    std::vector<DetectedMarker> markers = detector.detectAndReconstruct(
        image, cloud, 0.1f, 10.0f);
    
    // Для Circular маркеров ID должен быть -1
    for (const auto& m : markers) {
        EXPECT_EQ(m.id, -1);  // Circular не имеет ID
        EXPECT_TRUE(std::isfinite(m.center3d.x()));
        EXPECT_TRUE(std::isfinite(m.center3d.y()));
        EXPECT_TRUE(std::isfinite(m.center3d.z()));
    }
}

// Тест: detectAndReconstruct с пустым изображением
TEST_F(CircularMarkerDetectorTest, DetectAndReconstructEmpty)
{
    CircularMarkerDetector detector;
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    cv::Mat empty;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = createTestCloud(0.5f);
    
    std::vector<DetectedMarker> markers = detector.detectAndReconstruct(
        empty, cloud, 0.1f, 10.0f);
    
    EXPECT_EQ(markers.size(), 0u);
}

// Тест: detectAndReconstruct с пустым облаком
TEST_F(CircularMarkerDetectorTest, DetectAndReconstructEmptyCloud)
{
    CircularMarkerDetector detector;
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    cv::Mat image = createTestImage();
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr empty_cloud;
    
    std::vector<DetectedMarker> markers = detector.detectAndReconstruct(
        image, empty_cloud, 0.1f, 10.0f);
    
    EXPECT_EQ(markers.size(), 0u);
}

// Тест: detectAndReconstruct с фильтрацией по глубине
TEST_F(CircularMarkerDetectorTest, DetectAndReconstructDepthFilter)
{
    CircularMarkerDetector detector;
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    // Создаем облако с глубиной вне диапазона
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    for (int i = 0; i < 100; ++i) {
        pcl::PointXYZRGB pt;
        pt.x = 0.01 * (i % 10);
        pt.y = 0.01 * (i / 10);
        pt.z = 0.05f;  // Глубина меньше min_depth
        pt.r = 255; pt.g = 255; pt.b = 255;
        cloud->push_back(pt);
    }
    cloud->width = 10;
    cloud->height = 10;
    
    cv::Mat image = createTestImage();
    
    std::vector<DetectedMarker> markers = detector.detectAndReconstruct(
        image, cloud, 0.1f, 10.0f);
    
    // Все маркеры должны быть отфильтрованы по глубине
    EXPECT_EQ(markers.size(), 0u);
}

// Тест: Несколько маркеров в detectAndReconstruct
TEST_F(CircularMarkerDetectorTest, DetectAndReconstructMultiple)
{
    CircularMarkerDetector detector;
    detector.setIntrinsics(570.0f, 570.0f, 320.0f, 240.0f);
    
    // Создаем облако
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = createTestCloud(0.5f);
    
    cv::Mat image = createTestImage();
    
    std::vector<DetectedMarker> markers = detector.detectAndReconstruct(
        image, cloud, 0.1f, 10.0f);
    
    // Проверяем, что все маркеры валидны
    for (const auto& m : markers) {
        EXPECT_EQ(m.id, -1);
        EXPECT_TRUE(std::isfinite(m.center2d.x));
        EXPECT_TRUE(std::isfinite(m.center2d.y));
        EXPECT_TRUE(std::isfinite(m.center3d.x()));
        EXPECT_TRUE(std::isfinite(m.center3d.y()));
        EXPECT_TRUE(std::isfinite(m.center3d.z()));
    }
}