tests/test_marker_map.cpp - master - klischa/AstraScanner2

tests/test_marker_map.cpp
353 строки13 KB
Astra2
fix: harden project state and tests
20 июн 2026, 08:27
20 июн 2026, 08:274fd91fd
#include <gtest/gtest.h>
#include <QCoreApplication>
#include <Eigen/Dense>
#include <cmath>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "../src/marker_tracker/MarkerMap.h"

class MarkerMapTest : public ::testing::Test {
protected:
    void SetUp() override {
        if (!QCoreApplication::instance()) {
            static int argc = 1;
            static char arg0[] = "test_marker_map";
            static char* argv[] = { arg0 };
            new QCoreApplication(argc, argv);
        }
    }
    
    // Создает тестовый DetectedMarker с ID
    DetectedMarker createTestMarker(int id, float x, float y, float z) {
        DetectedMarker m;
        m.id = id;
        m.center2d = cv::Point2f(320.0f + x, 240.0f + y);
        m.center3d = Eigen::Vector3f(x, y, z);
        return m;
    }
};

// Тест: Инициализация карты с DetectedMarker (только с ID)
TEST_F(MarkerMapTest, InitializeWithDetectedMarkers)
{
    MarkerMap markerMap;
    
    // Создаем 5 маркеров с ID
    std::vector<DetectedMarker> initialMarkers;
    for (int i = 0; i < 5; ++i) {
        initialMarkers.push_back(createTestMarker(i, 0.1 * i, 0.0, 0.0));
    }
    
    bool result = markerMap.initialize(initialMarkers, 5);
    
    ASSERT_TRUE(result);
    EXPECT_TRUE(markerMap.isInitialized());
}

// Тест: Инициализация с недостаточным количеством маркеров с ID
TEST_F(MarkerMapTest, InitializeWithDetectedMarkersNotEnough)
{
    MarkerMap markerMap;
    
    // Создаем только 3 маркера (минимум 5)
    std::vector<DetectedMarker> initialMarkers;
    for (int i = 0; i < 3; ++i) {
        initialMarkers.push_back(createTestMarker(i, 0.1 * i, 0.0, 0.0));
    }
    
    bool result = markerMap.initialize(initialMarkers, 5);
    
    ASSERT_FALSE(result);
    EXPECT_FALSE(markerMap.isInitialized());
}

// Тест: Инициализация игнорирует маркеры без ID
TEST_F(MarkerMapTest, InitializeFiltersMarkersWithoutID)
{
    MarkerMap markerMap;
    
    // Создаем маркеры: некоторые с ID, некоторые без (-1)
    std::vector<DetectedMarker> initialMarkers;
    initialMarkers.push_back(createTestMarker(0, 0.0, 0.0, 0.0));    // с ID
    initialMarkers.push_back(createTestMarker(-1, 0.1, 0.0, 0.0));   // без ID (должен быть проигнорирован)
    initialMarkers.push_back(createTestMarker(1, 0.2, 0.0, 0.0));    // с ID
    initialMarkers.push_back(createTestMarker(-1, 0.3, 0.0, 0.0));   // без ID (должен быть проигнорирован)
    initialMarkers.push_back(createTestMarker(2, 0.4, 0.0, 0.0));    // с ID
    
    // Должно инициализироваться только с 3 маркерами с ID
    bool result = markerMap.initialize(initialMarkers, 3);
    
    ASSERT_TRUE(result);
    EXPECT_TRUE(markerMap.isInitialized());
    
    // Проверяем количество соответствий (должно быть 3, а не 5)
    EXPECT_EQ(markerMap.getLastCorrespondenceCount(), 3);
}

// Тест: Обновление карты с DetectedMarker
TEST_F(MarkerMapTest, UpdateWithDetectedMarkers)
{
    MarkerMap markerMap;
    
    // Инициализируем с маркерами с ID
    std::vector<DetectedMarker> initialMarkers;
    for (int i = 0; i < 7; ++i) {
        initialMarkers.push_back(createTestMarker(i, 0.1 * i, 0.0, 0.0));
    }
    
    markerMap.initialize(initialMarkers, 5);
    
    // Обновляем с теми же позициями (с небольшим шумом)
    std::vector<DetectedMarker> currentMarkers;
    for (int i = 0; i < 7; ++i) {
        DetectedMarker m = createTestMarker(i, 0.1 * i, 0.0, 0.0);
        m.center3d += Eigen::Vector3f(0.001f, 0.0, 0.0);
        currentMarkers.push_back(m);
    }
    
    Eigen::Affine3f transform = markerMap.update(currentMarkers, 0.01f);
    
    // Проверяем, что трансформация валидна
    EXPECT_EQ(transform.matrix().rows(), 4);
    EXPECT_EQ(transform.matrix().cols(), 4);
    
    // Проверяем количество соответствий
    int correspondenceCount = markerMap.getLastCorrespondenceCount();
    EXPECT_GT(correspondenceCount, 0);
}

// Тест: Обновление игнорирует маркеры без ID
TEST_F(MarkerMapTest, UpdateFiltersMarkersWithoutID)
{
    MarkerMap markerMap;
    
    // Инициализируем с маркерами с ID
    std::vector<DetectedMarker> initialMarkers;
    for (int i = 0; i < 5; ++i) {
        initialMarkers.push_back(createTestMarker(i, 0.1 * i, 0.0, 0.0));
    }
    
    markerMap.initialize(initialMarkers, 5);
    
    // Обновляем с маркерами: некоторые с ID, некоторые без
    std::vector<DetectedMarker> currentMarkers;
    currentMarkers.push_back(createTestMarker(0, 0.0, 0.0, 0.0));    // с ID (должен быть найден)
    currentMarkers.push_back(createTestMarker(-1, 0.1, 0.0, 0.0));   // без ID (должен быть проигнорирован)
    currentMarkers.push_back(createTestMarker(1, 0.2, 0.0, 0.0));    // с ID (должен быть найден)
    currentMarkers.push_back(createTestMarker(-1, 0.3, 0.0, 0.0));   // без ID (должен быть проигнорирован)
    currentMarkers.push_back(createTestMarker(2, 0.4, 0.0, 0.0));    // с ID (должен быть найден)
    
    Eigen::Affine3f transform = markerMap.update(currentMarkers, 0.01f);
    
    // Проверяем количество соответствий (должно быть 3, а не 5)
    int correspondenceCount = markerMap.getLastCorrespondenceCount();
    EXPECT_EQ(correspondenceCount, 3);
}

// Тест: Сопоставление по ID (а не по расстоянию)
TEST_F(MarkerMapTest, MatchByIDNotByDistance)
{
    MarkerMap markerMap;
    
    // Инициализируем с маркерами
    std::vector<DetectedMarker> initialMarkers;
    initialMarkers.push_back(createTestMarker(0, 0.0, 0.0, 0.0));    // ID 0 на позиции 0
    initialMarkers.push_back(createTestMarker(1, 0.2, 0.0, 0.0));    // ID 1 на позиции 1
    initialMarkers.push_back(createTestMarker(2, 0.4, 0.0, 0.0));    // ID 2 на позиции 2
    
    markerMap.initialize(initialMarkers, 3);
    
    // Обновляем с переставленными позициями, но теми же ID
    std::vector<DetectedMarker> currentMarkers;
    currentMarkers.push_back(createTestMarker(0, 0.4, 0.0, 0.0));    // ID 0, но на позиции 2
    currentMarkers.push_back(createTestMarker(1, 0.0, 0.0, 0.0));    // ID 1, но на позиции 0
    currentMarkers.push_back(createTestMarker(2, 0.2, 0.0, 0.0));    // ID 2, но на позиции 1
    
    Eigen::Affine3f transform = markerMap.update(currentMarkers, 0.1f);
    
    // Проверяем, что трансформация не Identity (ID-сопоставление работает)
    Eigen::Matrix4f diff = (transform.matrix() - Eigen::Matrix4f::Identity()).array().abs();
    bool hasTransform = diff.sum() > 0.001;
    
    EXPECT_TRUE(hasTransform);
}

// Тест: Обновление без инициализации
TEST_F(MarkerMapTest, UpdateWithoutInitialize)
{
    MarkerMap markerMap;
    
    std::vector<DetectedMarker> markers;
    markers.push_back(createTestMarker(0, 0.0, 0.0, 0.0));
    markers.push_back(createTestMarker(1, 0.1, 0.0, 0.0));
    
    // Вызываем update без инициализации
    Eigen::Affine3f transform = markerMap.update(markers, 0.01f);
    
    // Должен вернуть Identity
    Eigen::Affine3f expected = Eigen::Affine3f::Identity();
    
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            EXPECT_NEAR(transform(i, j), expected(i, j), 1e-6);
        }
    }
}

// Тест: Многократное обновление с ID
TEST_F(MarkerMapTest, RepeatedUpdateWithIDs)
{
    MarkerMap markerMap;
    
    // Инициализируем
    std::vector<DetectedMarker> markers;
    for (int i = 0; i < 7; ++i) {
        markers.push_back(createTestMarker(i, 0.1 * i, 0.0, 0.0));
    }
    markerMap.initialize(markers, 5);
    
    // Многократное обновление
    for (int i = 0; i < 10; ++i) {
        std::vector<DetectedMarker> current;
        for (int j = 0; j < 7; ++j) {
            DetectedMarker m = createTestMarker(j, 0.1 * j, 0.0, 0.0);
            m.center3d += Eigen::Vector3f(0.001f * i, 0.0, 0.0);
            current.push_back(m);
        }
        
        Eigen::Affine3f transform = markerMap.update(current, 0.01f);
        EXPECT_EQ(transform.matrix().rows(), 4);
    }
}

// Тест: Известная трансляция и поворот должны восстанавливаться полной rigid-трансформацией
TEST_F(MarkerMapTest, RecoversKnownRigidTransform)
{
    MarkerMap markerMap;

    std::vector<DetectedMarker> initialMarkers;
    initialMarkers.push_back(createTestMarker(0, 0.0f, 0.0f, 0.0f));
    initialMarkers.push_back(createTestMarker(1, 1.0f, 0.0f, 0.0f));
    initialMarkers.push_back(createTestMarker(2, 0.0f, 1.0f, 0.0f));
    initialMarkers.push_back(createTestMarker(3, 1.0f, 1.0f, 0.0f));
    ASSERT_TRUE(markerMap.initialize(initialMarkers, 4));

    const float angle = static_cast<float>(3.14159265358979323846 / 2.0); // 90 градусов вокруг Z
    Eigen::Matrix3f R = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()).toRotationMatrix();
    Eigen::Vector3f t(1.0f, 2.0f, 3.0f);

    std::vector<DetectedMarker> transformed;
    for (const auto &m : initialMarkers) {
        DetectedMarker out = m;
        out.center3d = R * m.center3d + t;
        transformed.push_back(out);
    }

    Eigen::Affine3f pose = markerMap.update(transformed, 0.01f);

    EXPECT_NEAR(pose.translation().x(), t.x(), 1e-4f);
    EXPECT_NEAR(pose.translation().y(), t.y(), 1e-4f);
    EXPECT_NEAR(pose.translation().z(), t.z(), 1e-4f);
    EXPECT_NEAR((pose.linear() - R).norm(), 0.0f, 1e-4f);
}

// Тест: После успешного обновления при потере соответствий возвращается последняя валидная трансформация
TEST_F(MarkerMapTest, ReusesLastValidTransformWhenCorrespondencesDrop)
{
    MarkerMap markerMap;
    std::vector<DetectedMarker> initialMarkers;
    for (int i = 0; i < 4; ++i) {
        initialMarkers.push_back(createTestMarker(i, 0.5f * (i % 2), 0.5f * (i / 2), 0.0f));
    }
    ASSERT_TRUE(markerMap.initialize(initialMarkers, 4));

    std::vector<DetectedMarker> transformed;
    Eigen::Vector3f t(0.2f, 0.3f, 0.4f);
    for (const auto &m : initialMarkers) {
        DetectedMarker out = m;
        out.center3d = m.center3d + t;
        transformed.push_back(out);
    }

    Eigen::Affine3f valid = markerMap.update(transformed, 0.01f);

    std::vector<DetectedMarker> insufficient;
    insufficient.push_back(transformed[0]);
    insufficient.push_back(transformed[1]);
    Eigen::Affine3f reused = markerMap.update(insufficient, 0.01f);

    EXPECT_NEAR((reused.matrix() - valid.matrix()).norm(), 0.0f, 1e-6f);
    EXPECT_EQ(markerMap.getLastCorrespondenceCount(), 2);
}


// Тест: Инициализация с нуля ID маркеров
TEST_F(MarkerMapTest, InitializeWithZeroIDs)
{
    MarkerMap markerMap;
    
    // Создаем маркеры с ID 0, 10, 20 (не последовательные)
    std::vector<DetectedMarker> markers;
    markers.push_back(createTestMarker(0, 0.0, 0.0, 0.0));
    markers.push_back(createTestMarker(10, 0.1, 0.0, 0.0));
    markers.push_back(createTestMarker(20, 0.2, 0.0, 0.0));
    
    bool result = markerMap.initialize(markers, 3);
    
    ASSERT_TRUE(result);
    EXPECT_TRUE(markerMap.isInitialized());
}

// Тест: Инициализация с большим количеством маркеров
TEST_F(MarkerMapTest, InitializeWithMoreMarkers)
{
    MarkerMap markerMap;
    
    // Создаем 10 маркеров с ID
    std::vector<DetectedMarker> markers;
    for (int i = 0; i < 10; ++i) {
        markers.push_back(createTestMarker(i, 0.1 * (i % 5), 0.05 * (i / 5), 0.0));
    }
    
    bool result = markerMap.initialize(markers, 5);
    
    ASSERT_TRUE(result);
    EXPECT_TRUE(markerMap.isInitialized());
    
    // Проверяем центр и нормаль
    Eigen::Vector3f center = markerMap.getDiskCenter();
    Eigen::Vector3f normal = markerMap.getDiskNormal();
    
    EXPECT_TRUE(std::isfinite(center.x()));
    EXPECT_TRUE(std::isfinite(normal.x()));
}

// Тест: Инициализация с пустым вектором
TEST_F(MarkerMapTest, InitializeEmpty)
{
    MarkerMap markerMap;
    
    std::vector<DetectedMarker> markers;
    
    bool result = markerMap.initialize(markers, 5);
    
    ASSERT_FALSE(result);
    EXPECT_FALSE(markerMap.isInitialized());
}

// Тест: Инициализация с недопустимым количеством маркеров
TEST_F(MarkerMapTest, InitializeInvalidMinCount)
{
    MarkerMap markerMap;
    
    std::vector<DetectedMarker> markers;
    for (int i = 0; i < 5; ++i) {
        markers.push_back(createTestMarker(i, 0.1 * i, 0.0, 0.0));
    }
    
    // Пытаемся инициализировать с min_markers_count = 0
    bool result = markerMap.initialize(markers, 0);
    
    // Должно быть true (нельзя задать минимум 0)
    EXPECT_TRUE(result);
}