src/marker_tracker/MarkerMap.cpp - master - klischa/AstraScanner2

src/marker_tracker/MarkerMap.cpp
298 строк14 KB
Arena Agent
fix: neural ICP confirm, cumulative transform direction, CMake cleanup, mesh smoothing, multi-seed BFS
02 авг 2026, 21:44
02 авг 2026, 21:4410986cc
#include "MarkerMap.h"
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <QDebug>
#include <algorithm>
#include <cmath>
#include <unordered_set>

// Добавляем объявление вспомогательной функции
static void rotationMatrixToAxisAngle(const Eigen::Matrix3f& rotation, Eigen::Vector3f& axis, float& angle);

MarkerMap::MarkerMap()
    : m_initialized(false)
{
    m_last_update_time = std::chrono::steady_clock::now();
}

MarkerMap::~MarkerMap()
{
}

bool MarkerMap::initialize(const std::vector<DetectedMarker>& initial_markers, int min_markers_count)
{
    // Отфильтровываем маркеры без ID (Circular, id = -1)
    std::vector<DetectedMarker> filtered;
    for (const auto& marker : initial_markers) {
        if (marker.id >= 0) {  // Используем только маркеры с валидным ID
            filtered.push_back(marker);
        }
    }
    
    if (filtered.size() < static_cast<size_t>(min_markers_count)) {
        qWarning() << "[MarkerMap] Инициализация не удалась: недостаточно маркеров с ID.";
        return false;
    }

    // Создаем карту ID -> 3D позиция и фиксируем стабильный порядок ID.
    // Защита от дубликатов: если маркер с таким ID уже есть — пропускаем.
    m_global_marker_ids.clear();
    m_global_marker_order.clear();
    std::unordered_set<int> seenIds;

    for (const auto& marker : filtered) {
        if (!seenIds.insert(marker.id).second) {
            qWarning() << "[MarkerMap] Duplicate marker id skipped:" << marker.id;
            continue;
        }
        m_global_marker_ids[marker.id] = marker.center3d;
        m_global_marker_order.push_back(marker.id);
    }

    if (m_global_marker_order.size() < static_cast<size_t>(min_markers_count)) {
        qWarning() << "[MarkerMap] Инициализация не удалась: после фильтрации дубликатов осталось"
                   << m_global_marker_order.size() << "уникальных маркеров (нужно >=" << min_markers_count << ")";
        return false;
    }
    
    m_initialized = true;
    m_pending_markers.clear();
    m_last_update_time = std::chrono::steady_clock::now();

    // Вычисляем центр диска как среднюю точку всех маркеров
    m_disk_center = Eigen::Vector3f::Zero();
    for (int id : m_global_marker_order) {
        m_disk_center += m_global_marker_ids[id];
    }
    m_disk_center /= static_cast<float>(m_global_marker_order.size());

    // Вычисляем нормаль к плоскости диска с помощью SVD
    Eigen::Matrix3Xf points(3, m_global_marker_order.size());
    for (size_t i = 0; i < m_global_marker_order.size(); ++i) {
        points.col(i) = m_global_marker_ids[m_global_marker_order[i]] - m_disk_center;
    }

    Eigen::JacobiSVD<Eigen::Matrix3Xf> svd(points, Eigen::ComputeFullU | Eigen::ComputeFullV);

    // Check for degenerate geometry (collinear/coplanar markers)
    const auto& singularValues = svd.singularValues();
    if (singularValues(2) < 1e-6f) {
        qWarning() << "[MarkerMap] Markers are collinear/coplanar — SVD rank deficient."
                   << "Smallest singular value:" << singularValues(2)
                   << "(normal may be inaccurate)";
    }

    m_disk_normal = svd.matrixU().col(2); // Нормаль - последний столбец U
    
    // Корректируем направление нормали (вверх по Z)
    if (m_disk_normal.z() < 0) {
        m_disk_normal = -m_disk_normal;
    }

    qInfo() << "[MarkerMap] Инициализирована с" << m_global_marker_order.size() << "маркерами с ID.";
    qInfo() << "[MarkerMap] Центр диска:" << m_disk_center.x() << m_disk_center.y() << m_disk_center.z();
    qInfo() << "[MarkerMap] Нормаль к диску:" << m_disk_normal.x() << m_disk_normal.y() << m_disk_normal.z();
    
    // Сохраняем количество соответствий (количество маркеров с ID)
    m_last_correspondence_count = static_cast<int>(m_global_marker_order.size());
    
    return true;
}

Eigen::Affine3f MarkerMap::update(const std::vector<DetectedMarker>& current_markers, float position_tolerance)
{
    // Если карта не инициализирована, не можем вычислить позу
    if (!m_initialized) {
        qWarning() << "[MarkerMap] Обновление при неинициализированной карте.";
        return Eigen::Affine3f::Identity();
    }

    // Исправление #5: убрана static переменная, теперь используем член класса m_last_valid_transform
    // static Eigen::Affine3f last_valid_transform = Eigen::Affine3f::Identity();

    // Сбросим временную карту ожидающих маркеров, если прошло слишком много времени
    auto now = std::chrono::steady_clock::now();
    std::vector<size_t> to_remove;
    for (auto& [id, pending] : m_pending_markers) {
        if (std::chrono::duration<float>(now - pending.lastSeen).count() > 5.0f) {
            to_remove.push_back(id);
        }
    }
    for (auto id : to_remove) {
        m_pending_markers.erase(id);
    }

    // Обновление предсказанных позиций для сглаживания
    for (auto& [id, pending] : m_pending_markers) {
        if (!pending.observations.empty()) {
            // Простое предсказание - последняя наблюдаемая позиция
            pending.predictedPosition = pending.observations.back();
        }
    }

    // Используем только маркеры с ID для сопоставления
    std::vector<DetectedMarker> markers_with_id;
    for (const auto& marker : current_markers) {
        if (marker.id >= 0) {
            markers_with_id.push_back(marker);
        }
    }
    
    // Поиск соответствий между текущими и глобальными маркерами по ID.
    // Используем прямой lookup по unordered_map, чтобы не восстанавливать ID
    // обратно из позиции через epsilon-сравнение.
    std::vector<Eigen::Vector3f> src_points;
    std::vector<Eigen::Vector3f> dst_points;
    std::unordered_set<int> used_ids;
    src_points.reserve(markers_with_id.size());
    dst_points.reserve(markers_with_id.size());

    for (const auto& marker : markers_with_id) {
        if (used_ids.count(marker.id)) continue;
        auto it = m_global_marker_ids.find(marker.id);
        if (it == m_global_marker_ids.end()) continue;
        src_points.push_back(it->second);
        dst_points.push_back(marker.center3d);
        used_ids.insert(marker.id);
    }

    // Сохраняем количество соответствий даже при отказе, чтобы вызывающий код
    // не считал старую позу надёжной после потери маркеров.
    m_last_correspondence_count = static_cast<int>(src_points.size());

    // Достаточно ли соответствий для вычисления трансформации?
    if (src_points.size() < 3) {
        qWarning() << "[MarkerMap] Недостаточно соответствий маркеров для вычисления позы.";
        qInfo() << "[MarkerMap] Обнаружено" << markers_with_id.size() << "маркеров с ID.";
        return m_last_valid_transform;  // Исправление #5: использовать член класса
    }

    // Сборка пар точек для вычисления трансформации (3D-3D)
    Eigen::Matrix3Xf src(3, src_points.size());
    Eigen::Matrix3Xf dst(3, dst_points.size());

    for (size_t i = 0; i < src_points.size(); ++i) {
        src.col(i) = src_points[i];
        dst.col(i) = dst_points[i];
    }

    // Отладочный вывод количества найденных соответствий
    qDebug() << "[MarkerMap] Found" << src_points.size() << "correspondences out of" << m_global_marker_ids.size();
    
    // Вычисление жесткой трансформации с помощью SVD (метод Umeyama).
    //
    // Соглашение:
    //   src_points  — 3D координаты маркеров в ГЛОБАЛЬНОЙ (опорной) СК, зафиксированной
    //                 при initialize().
    //   dst_points  — 3D координаты тех же маркеров в текущей СК КАМЕРЫ.
    // Eigen::umeyama(src, dst, false) возвращает матрицу T_dst_from_src размера 4x4
    // такую что dst ≈ T * src, то есть T_cam_from_world.
    //
    // Для единообразия с нейросетевым трекингом и с интуитивным значением
    // "поза сканера в мировой системе" возвращаем обратное преобразование
    // T_world_from_cam = T_cam_from_world^{-1} = Affine3f(камера в мире).
    Eigen::Matrix4f t_cam_from_world = Eigen::umeyama(src, dst, false); // false = не масштабируем

    // Проверка корректности полученной матрицы (оборотная и не вырожденная).
    if (!t_cam_from_world.array().isFinite().all()) {
        qWarning() << "[MarkerMap] umeyama returned non-finite transform";
        return m_last_valid_transform;
    }
    const Eigen::Matrix3f rotCheck = t_cam_from_world.block<3,3>(0,0);
    if (std::abs(rotCheck.determinant()) < 0.5f) {
        qWarning() << "[MarkerMap] umeyama returned near-singular rotation";
        return m_last_valid_transform;
    }

    Eigen::Affine3f final_transform = Eigen::Affine3f::Identity();
    final_transform.matrix() = t_cam_from_world.inverse();
    
    // Исправление #5: обновляем m_last_valid_transform
    m_last_valid_transform = final_transform;

    // Отключаем добавление новых маркеров для стабильности отладки
    // Добавление новых маркеров (те, что не нашли соответствия)
    // Присваиваем им временный ID (например, индекс в current_markers)
    // for (size_t j = 0; j < current_markers.size(); ++j) {
    //     if (used_ids.count(current_markers[j].id)) continue; // Уже сопоставлен

    //     // Пытаемся найти или создать PendingMarker для этого маркера
    //     auto it = m_pending_markers.find(j);
    //     if (it == m_pending_markers.end()) {
    //         // Создаем новый PendingMarker
    //         PendingMarker pending;
    //         pending.observations.push_back(current_markers[j].center3d);
    //         pending.lastSeen = now;
    //         pending.predictedPosition = current_markers[j].center3d;
    //         m_pending_markers[j] = pending;
    //     } else {
    //         // Обновляем существующий
    //         PendingMarker& pending = it->second;
    //         pending.observations.push_back(current_markers[j].center3d);
    //         pending.lastSeen = now;

    //         // Простое сглаживание с использованием предыдущей позиции
    //         float alpha = 0.7f; // Коэффициент сглаживания
    //         Eigen::Vector3f smoothed_pos = alpha * current_markers[j].center3d + (1.0f - alpha) * pending.predictedPosition;
    //         pending.predictedPosition = smoothed_pos;

    //         // Если маркер наблюдается стабильно (например, на 3 кадрах), добавляем в глобальную карту
    //         if (pending.observations.size() >= 3) {
    //             // Проверяем, что позиции не сильно прыгают
    //             Eigen::Vector3f avg_pos = Eigen::Vector3f::Zero();
    //             for (const auto& pos : pending.observations) {
    //                 avg_pos += pos;
    //             }
    //             avg_pos /= static_cast<float>(pending.observations.size());

    //             float max_deviation = 0.0f;
    //             for (const auto& pos : pending.observations) {
    //                 max_deviation = std::max(max_deviation, (pos - avg_pos).norm());
    //             }

    //             if (max_deviation < 0.01f) { // Порог в метрах
    //                 m_global_marker_positions.push_back(avg_pos);
    //                 m_pending_markers.erase(it);
    //                 qInfo() << "[MarkerMap] Добавлен новый маркер в глобальную карту.";
    //             }
    //         }
    //     }
    // }

    m_last_update_time = now;
    
    // Вычисляем угол поворота из матрицы вращения
    Eigen::Vector3f axis;
    float angle;
    rotationMatrixToAxisAngle(final_transform.linear(), axis, angle);
    m_current_rotation_angle = angle;
    
    return final_transform;
}

// Вспомогательная функция для преобразования матрицы вращения в ось и угол
static void rotationMatrixToAxisAngle(const Eigen::Matrix3f& rotation, Eigen::Vector3f& axis, float& angle) {
    // Вычисляем угол с защитой от NaN
    float trace = rotation.trace();
    float cos_angle = (trace - 1.0f) * 0.5f;
    
    // Исправление #17: ограничиваем значение для acos, чтобы избежать NaN
    cos_angle = std::max(-1.0f, std::min(1.0f, cos_angle));
    
    angle = std::acos(cos_angle);
    
    if (angle < 1e-6) { // Если угол очень мал
        axis << 1.0f, 0.0f, 0.0f; // Произвольная ось
    } else {
        // Вычисляем ось вращения
        axis << rotation(2,1) - rotation(1,2),
                rotation(0,2) - rotation(2,0),
                rotation(1,0) - rotation(0,1);
        axis.normalize();
    }
}

bool MarkerMap::isInitialized() const
{
    return m_initialized;
}