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

src/marker_tracker/CircularMarkerDetector.cpp
511 строк20 KB
k k
fix(high): 6 high-severity bugs
13 июл 2026, 23:43
13 июл 2026, 23:43b439ddb
#include "CircularMarkerDetector.h"
#include <opencv2/features2d.hpp>
#include <QDebug>

namespace {
constexpr bool kVerboseMarkerLogs = false;
}

CircularMarkerDetector::CircularMarkerDetector()
    : m_min_radius(10.0f), m_max_radius(100.0f),
      m_min_circularity(0.6f), m_min_convexity(0.7f), m_min_inertia_ratio(0.4f),
      m_disk_diameter_mm(300.0f), m_marker_count(7),
      m_min_area(80.0f), m_max_area(500.0f)
{
}

CircularMarkerDetector::~CircularMarkerDetector()
{
}

void CircularMarkerDetector::setDetectionParams(float min_circularity, float min_convexity, float min_inertia_ratio)
{
    m_min_circularity = min_circularity;
    m_min_convexity = min_convexity;
    m_min_inertia_ratio = min_inertia_ratio;
}

void CircularMarkerDetector::setDiskParams(float disk_diameter_mm, int marker_count)
{
    m_disk_diameter_mm = disk_diameter_mm;
    m_marker_count = marker_count;
}

std::vector<cv::Point2f> CircularMarkerDetector::detectMarkers(const cv::Mat& image)
{
    std::vector<cv::Point2f> centers;

    // Проверка корректности изображения
    if (image.empty()) {
        return centers;
    }
    
    cv::Mat processed;
    if (image.channels() == 4) {
        // RGBA -> RGB
        cv::cvtColor(image, processed, cv::COLOR_RGBA2BGR);
    } else if (image.channels() == 1) {
        // Grayscale already OK
        processed = image;
    } else if (image.channels() == 3) {
        // BGR
        processed = image;
    } else {
        // Unsupported format
        return centers;
    }

    cv::Mat gray;
    if (processed.channels() == 1) {
        gray = processed;
    } else {
        cv::cvtColor(processed, gray, cv::COLOR_BGR2GRAY);
    }

    // Бинаризация: чёрные маркеры на белом фоне
    cv::Mat binary;
    cv::threshold(gray, binary, 100, 255, cv::THRESH_BINARY_INV);

    // Морфологическое замыкание — закрывает дырки в кольцах
    // cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7));
    // cv::morphologyEx(binary, binary, cv::MORPH_CLOSE, kernel);

    // Настройка параметров детектора
    cv::SimpleBlobDetector::Params params;
    
    // Фильтрация по площади (размеру маркера)
    params.filterByArea = true;
    params.minArea = m_min_area;   // Должно быть ~1000
    params.maxArea = m_max_area;   // Должно быть ~2500

    // Фильтрация по круглости (circularity)
    params.filterByCircularity = true;
    params.minCircularity = m_min_circularity;

    // Фильтрация по выпуклости (convexity)
    params.filterByConvexity = true;
    params.minConvexity = m_min_convexity;

    // Фильтрация по инерции (соотношение осей)
    params.filterByInertia = true;
    params.minInertiaRatio = m_min_inertia_ratio;

    // Создаем детектор
    cv::Ptr<cv::SimpleBlobDetector> detector = cv::SimpleBlobDetector::create(params);

    // Исправление #12: SimpleBlobDetector внутри делает многоуровневую бинаризацию,
    // поэтому подаем grayscale, а не binary
    std::vector<cv::KeyPoint> keypoints;
    detector->detect(gray, keypoints);  // Исправление: binary -> gray

    // Отладочный вывод
    if (kVerboseMarkerLogs) {
        qDebug() << "[MarkerDetector] found" << keypoints.size() << "blobs";
        for (auto& kp : keypoints) 
            qDebug() << "   at" << kp.pt.x << kp.pt.y << "size" << kp.size;
    }

    // Исправление #3: масштабируем координаты из RGB в depth-пространство
    float scale_x = static_cast<float>(m_depth_width) / static_cast<float>(image.cols);
    float scale_y = static_cast<float>(m_depth_height) / static_cast<float>(image.rows);
    
    centers.reserve(keypoints.size());
    for (const auto& kp : keypoints) {
        const float radius = kp.size * 0.5f;
        if (radius < m_min_radius || radius > m_max_radius) continue;
        cv::Point2f scaled_point(kp.pt.x * scale_x, kp.pt.y * scale_y);
        centers.push_back(scaled_point);
    }

    // Отладочный вывод для 3D-реконструкции
    if (kVerboseMarkerLogs) {
        qDebug() << "[MarkerDetector] 2D centers ready for 3D reconstruction:";
        for (const auto& c : centers) {
            qDebug() << "  2D center:" << c.x << c.y;
        }
    }
    
    // Сохраняем центры для getReliableCount()
    m_last_centers = centers;

    return centers;
}

void CircularMarkerDetector::setRadiusFilter(float min_radius, float max_radius)
{
    if (min_radius >= max_radius) {
        std::swap(min_radius, max_radius);
    }
    m_min_radius = min_radius;
    m_max_radius = max_radius;
}

void CircularMarkerDetector::setAreaFilter(float min_area, float max_area)
{
    if (min_area >= max_area) {
        std::swap(min_area, max_area);
    }
    m_min_area = min_area;
    m_max_area = max_area;
}

void CircularMarkerDetector::setIntrinsics(float fx, float fy, float cx, float cy)
{
    m_fx = fx;
    m_fy = fy;
    m_cx = cx;
    m_cy = cy;
    qDebug() << "[MarkerDetector] Intrinsics set:" << m_fx << m_fy << m_cx << m_cy;
}

// Исправление #3: метод для установки разрешения depth-изображения
void CircularMarkerDetector::setDepthResolution(int depth_width, int depth_height)
{
    m_depth_width = depth_width;
    m_depth_height = depth_height;
    qDebug() << "[MarkerDetector] Depth resolution set:" << depth_width << "x" << depth_height;
}

float CircularMarkerDetector::getDepthAround(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, int cx, int cy, int radius) {
    return getDepthMedian(cloud, cx, cy, radius);
}

std::vector<Eigen::Vector3f> CircularMarkerDetector::reconstruct3DPositions(
    const std::vector<cv::Point2f>& centers_2d,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& depth_cloud,
    float min_depth, float max_depth)
{
    std::vector<Eigen::Vector3f> centers_3d;

    if (!depth_cloud || depth_cloud->empty() || depth_cloud->width == 0) {
        qDebug() << "[Marker3D] Depth cloud is null, empty, or has zero width";
        return centers_3d;
    }

    int width = static_cast<int>(depth_cloud->width);
    int height = static_cast<int>(depth_cloud->height);
    qDebug() << "[Marker3D] Depth cloud resolution:" << width << "x" << height;

    for (const auto& center : centers_2d) {
        int u = static_cast<int>(center.x);
        int v = static_cast<int>(center.y);

        // Проверка границ изображения
        if (u < 0 || u >= width || v < 0 || v >= height) {
            qDebug() << "[Marker3D] Point out of bounds:" << u << v << "cloud size:" << width << height;
            continue;
        }

        // Исправление: используем правильную индексацию для плоского облака
        size_t idx = v * width + u;
        if (idx >= depth_cloud->size()) {
            qDebug() << "[Marker3D] Index out of bounds:" << idx << "for" << u << v << ", size:" << depth_cloud->size();
            continue;
        }
        
        // Получаем глубину из окрестности центра маркера
        float z = getDepthAround(depth_cloud, u, v, 5); // Используем окрестность 5x5 пикселей

        qDebug() << "[Marker3D] Marker center" << u << v << "depth (from neighborhood):" << z;

        // Фильтрация по глубине
        if (z < min_depth || z > max_depth || std::isnan(z) || z == 0.0f) {
            qDebug() << "  Skipped: depth out of range or invalid";
            continue;
        }

        // Вычисляем X и Y из 2D-координат и Z с помощью интринсик
        float x = (u - m_cx) * z / m_fx;
        float y = (v - m_cy) * z / m_fy;

        if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) {
            centers_3d.emplace_back(x, y, z);
        } else {
            qDebug() << "[Marker3D] Non-finite 3D point:" << x << y << z;
        }
    }

    qDebug() << "[Marker3D] Reconstructed" << centers_3d.size() << "3D centers";
    return centers_3d;
}

std::vector<DetectedMarker> CircularMarkerDetector::detectAndReconstruct(
    const cv::Mat& image,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& depth_cloud,
    float min_depth,
    float max_depth)
{
    std::vector<DetectedMarker> markers;
    
    if (kVerboseMarkerLogs) {
        qDebug() << "[CircularMarkerDetector] detectAndReconstruct called:";
        qDebug() << "  Image size:" << image.cols << "x" << image.rows;
        qDebug() << "  Depth cloud:" << (depth_cloud ? "valid" : "null") << "size:" << (depth_cloud ? depth_cloud->size() : 0);
        qDebug() << "  Depth range:" << min_depth << "-" << max_depth << "m";
    }
    
    // 1. Детекция маркеров
    std::vector<cv::Point2f> centers_2d = detectMarkers(image);
    
    if (centers_2d.empty()) {
        qDebug() << "[CircularMarkerDetector] No markers detected";
        return markers;
    }
    
    qDebug() << "[CircularMarkerDetector] Detected" << centers_2d.size() << "2D markers, reconstructing 3D positions...";
    
    // 2. Для каждого маркера реконструируем 3D-позицию с медианой глубины
    for (size_t i = 0; i < centers_2d.size(); ++i) {
        const auto& center = centers_2d[i];
        DetectedMarker m;
        m.id = static_cast<int>(i);  // Sequential ID so MarkerMap can initialize
        m.center2d = center;
        
        if (kVerboseMarkerLogs)
            qDebug() << "[CircularMarkerDetector] Reconstructing marker" << i << "at 2D:" << center.x << center.y;
        m.center3d = reconstruct3DPositionSingle(center, depth_cloud, min_depth, max_depth);

        // Фильтр: невалидные 3D-позиции пропускаем
        if (!std::isfinite(m.center3d.x()) || !std::isfinite(m.center3d.y()) || !std::isfinite(m.center3d.z()) ||
            m.center3d.z() <= 0.05f || m.center3d.norm() <= 0.05f) {
            continue;
        }

        if (kVerboseMarkerLogs)
            qDebug() << "[CircularMarkerDetector] Marker" << i << "3D position:" << m.center3d.x() << m.center3d.y() << m.center3d.z();
        
        markers.push_back(m);
    }
    
    if (kVerboseMarkerLogs)
        qDebug() << "[CircularMarkerDetector] Total markers reconstructed:" << markers.size();
    return markers;
}

float CircularMarkerDetector::getDepthMedian(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,
    int cx, int cy, int radius)
{
    if (!cloud || cloud->empty()) {
        return 0.0f;
    }
    
    std::vector<float> depths;
    
    if (cloud->isOrganized()) {
        // Для организованных облаков используем индексацию
        int width = static_cast<int>(cloud->width);
        int height = static_cast<int>(cloud->height);
        
        for (int v = cy - radius; v <= cy + radius; ++v) {
            for (int u = cx - radius; u <= cx + radius; ++u) {
                if (u >= 0 && u < width && v >= 0 && v < height) {
                    size_t idx = v * width + u;
                    if (idx < cloud->size()) {
                        float z = cloud->points[idx].z;
                        if (std::isfinite(z) && z > 0.0f) {
                            depths.push_back(z);
                        }
                    }
                }
            }
        }
    } else {
        // Для неорганизованных облаков используем brute-force поиск
        // Получаем 3D луч от камеры через 2D-пиксель
        float u = static_cast<float>(cx);
        float v = static_cast<float>(cy);
        Eigen::Vector3f ray_origin((u - m_cx) * 1.0f / m_fx, (v - m_cy) * 1.0f / m_fy, 1.0f);
        Eigen::Vector3f ray_dir = ray_origin.normalized();
        
        for (const auto& pt : cloud->points) {
            if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
                continue;
            }
            
            Eigen::Vector3f point(pt.x, pt.y, pt.z);
            float dist_to_origin = point.norm();
            float depth = point.z();
            
            // Проверка диапазона глубины по оси Z (а не по norm),
            // потому что возвращаемое значение дальше используется как z.
            if (depth < 0.1f || depth > 10.0f) {
                continue;
            }
            
            // Проекция на луч
            float t = point.dot(ray_dir);
            Eigen::Vector3f closest_on_ray = t * ray_dir;
            float dist_to_ray = (point - closest_on_ray).norm();
            
            // Если точка близко к лучу
            if (dist_to_ray < 0.05f) {
                depths.push_back(depth);
            }
        }
    }
    
    // Возвращаем медиану или 0.0f, если недостаточно точек
    if (depths.empty()) return 0.0f;
    
    std::sort(depths.begin(), depths.end());
    return depths[depths.size() / 2];  // Медиана
}

Eigen::Vector3f CircularMarkerDetector::reconstruct3DPositionSingle(
    const cv::Point2f& center,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& depth_cloud,
    float min_depth,
    float max_depth)
{
    if (!depth_cloud || depth_cloud->empty() || depth_cloud->width == 0) {
        qDebug() << "[reconstruct3DPositionSingle] Cloud empty";
        return Eigen::Vector3f::Zero();
    }
    
    int u = static_cast<int>(center.x);
    int v = static_cast<int>(center.y);
    int width = static_cast<int>(depth_cloud->width);
    int height = static_cast<int>(depth_cloud->height);
    
    qDebug() << "[reconstruct3DPositionSingle] Requested pixel:" << u << v << "cloud:" << width << "x" << height << "organized:" << depth_cloud->isOrganized();
    
    // Использование KdTreeFLANN для поиска ближайших точек
    float z = getDepthAroundKdTree(depth_cloud, u, v, 5);
    qDebug() << "[reconstruct3DPositionSingle] Depth from KdTree:" << z;
    
    // Фallback на медиану, если KdTree не дал результата
    if (z <= 0.0f) {
        z = getDepthMedian(depth_cloud, u, v, 3);
        qDebug() << "[reconstruct3DPositionSingle] Depth from median fallback:" << z;
    }
    
    // Фильтрация по глубине
    if (z < min_depth || z > max_depth || z <= 0.0f) {
        qDebug() << "[reconstruct3DPositionSingle] Depth out of range or invalid";
        return Eigen::Vector3f::Zero();
    }
    
    // Реконструкция 3D позиции
    float x = (u - m_cx) * z / m_fx;
    float y = (v - m_cy) * z / m_fy;
    qDebug() << "[reconstruct3DPositionSingle] Reconstructed 3D point:" << x << y << z;
    
    if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) {
        return Eigen::Vector3f(x, y, z);
    }
    
    qDebug() << "[reconstruct3DPositionSingle] Non-finite values!";
    return Eigen::Vector3f::Zero();
}

float CircularMarkerDetector::getDepthAroundKdTree(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,
    int cx, int cy, int radius)
{
    if (!cloud || cloud->empty()) {
        return 0.0f;
    }
    
    // Для организованных облаков используем прямую индексацию
    if (cloud->isOrganized()) {
        return getDepthMedian(cloud, cx, cy, radius);
    }
    
    // Для неорганизованных облаков используем KdTreeFLANN
    typename pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr kdtree(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
    kdtree->setInputCloud(cloud);
    
    // Формируем 3D луч от камеры через 2D-пиксель
    float u = static_cast<float>(cx);
    float v = static_cast<float>(cy);
    
    // Начало луча (в центре камеры на расстоянии 1м)
    Eigen::Vector3f ray_origin((u - m_cx) * 1.0f / m_fx, (v - m_cy) * 1.0f / m_fy, 1.0f);
    // Направление луча
    Eigen::Vector3f ray_dir = ray_origin.normalized();
    
    // Создаем точку для KdTree (требуется PointT, а не Eigen::Vector3f)
    pcl::PointXYZRGB ray_point;
    ray_point.x = ray_origin.x();
    ray_point.y = ray_origin.y();
    ray_point.z = ray_origin.z();
    
    // Ищем ближайшую точку в диапазоне глубин
    float best_depth = 0.0f;
    float best_dist = std::numeric_limits<float>::max();
    
    // Используем brute-force поиск для малых облаков (быстрее, чем KdTree для < 10K точек)
    if (cloud->size() < 10000) {
        for (const auto& pt : cloud->points) {
            if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
                continue;
            }
            
            Eigen::Vector3f point(pt.x, pt.y, pt.z);
            float dist_to_origin = point.norm();
            float depth = point.z();
            
            // Проверка диапазона глубины по оси Z (а не по norm),
            // потому что возвращаемое значение дальше используется как z.
            if (depth < 0.1f || depth > 10.0f) {
                continue;
            }
            
            // Проекция на луч
            float t = point.dot(ray_dir);
            Eigen::Vector3f closest_on_ray = t * ray_dir;
            float dist_to_ray = (point - closest_on_ray).norm();
            
            // Если точка близко к лучу и ближе, чем предыдущие
            if (dist_to_ray < 0.05f && dist_to_origin < best_dist) {
                best_dist = dist_to_origin;
                best_depth = depth;
            }
        }
    } else {
        // Для больших облаков используем KdTree
        std::vector<int> indices;
        std::vector<float> squared_distances;
        
        // Ищем K ближайших соседей
        int K = 100;
        if (kdtree->radiusSearch(ray_point, 0.1, indices, squared_distances) > 0 ||
            kdtree->nearestKSearch(ray_point, K, indices, squared_distances) > 0) {
            
            for (size_t i = 0; i < indices.size(); ++i) {
                int idx = indices[i];
                if (idx >= 0 && idx < static_cast<int>(cloud->size())) {
                    const auto& pt = cloud->points[idx];
                    if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
                        continue;
                    }
                    
                    Eigen::Vector3f point(pt.x, pt.y, pt.z);
                    float dist_to_origin = point.norm();
                    float depth = point.z();
                    
                    // Проверка диапазона глубины по оси Z (а не по norm),
                    // потому что возвращаемое значение дальше используется как z.
                    if (depth < 0.1f || depth > 10.0f) {
                        continue;
                    }
                    
                    // Проекция на луч
                    float t = point.dot(ray_dir);
                    Eigen::Vector3f closest_on_ray = t * ray_dir;
                    float dist_to_ray = (point - closest_on_ray).norm();
                    
                    // Если точка близко к лучу
                    if (dist_to_ray < 0.05f) {
                        if (best_depth <= 0.0f || depth < best_depth) {
                            best_depth = depth;
                        }
                    }
                }
            }
        }
    }
    
    return best_depth;
}