src/services/CloudFiltersService.cpp - master - klischa/AstraScanner2

src/services/CloudFiltersService.cpp
305 строк11 KB
k k
refactor: remove MainWindow dead code + fix VoxelGrid warnings
14 июл 2026, 23:40
14 июл 2026, 23:401459aca
#include "CloudFiltersService.h"
#include <QDebug>
#include <limits>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/search/kdtree.h>
#include <cmath>

namespace {
constexpr float kPi = 3.14159265358979323846f;
}

#ifdef ASTRA_ENABLE_CUDA
#include "../filters/PointCloudFiltersCUDA.h"
#endif

CloudFiltersService::CloudFiltersService(QObject *parent) : QObject(parent)
{
#ifdef ASTRA_ENABLE_CUDA
    m_cudaFilters = new PointCloudFiltersCUDA(this);
    if (m_cudaFilters->isAvailable()) {
        qInfo() << "[CloudFiltersService] CUDA acceleration enabled";
    } else {
        qInfo() << "[CloudFiltersService] CUDA acceleration not available, using CPU only";
        delete m_cudaFilters;
        m_cudaFilters = nullptr;
    }
#else
    m_cudaFilters = nullptr;
    qInfo() << "[CloudFiltersService] CUDA support not compiled, using CPU only";
#endif
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr CloudFiltersService::applyStatisticalOutlierRemoval(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    int meanK, double stddevMulThresh)
{
    if (!cloud || cloud->empty()) return cloud;

    int pointsBefore = cloud->size();

#ifdef ASTRA_ENABLE_CUDA
    if (m_cudaFilters && m_cudaFilters->isAvailable()) {
        auto gpuResult = m_cudaFilters->applySORCUDA(cloud, meanK, stddevMulThresh);
        if (gpuResult && !gpuResult->empty()) {
            int pointsAfter = gpuResult->size();
            emit filterCompleted("Statistical Outlier Removal (GPU)", pointsBefore, pointsAfter);
            qInfo() << "SOR (GPU):" << pointsBefore << "->" << pointsAfter << "points";
            return gpuResult;
        }
    }
#endif

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZRGB>);

    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
    sor.setInputCloud(cloud);
    sor.setMeanK(meanK);
    sor.setStddevMulThresh(stddevMulThresh);
    sor.filter(*filtered);

    int pointsAfter = filtered->size();
    emit filterCompleted("Statistical Outlier Removal (CPU)", pointsBefore, pointsAfter);
    qInfo() << "SOR (CPU):" << pointsBefore << "->" << pointsAfter << "points";
    return filtered;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr CloudFiltersService::applyRadiusOutlierRemoval(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    double radius, int minNeighbors)
{
    if (!cloud || cloud->empty()) return cloud;

    int pointsBefore = cloud->size();

#ifdef ASTRA_ENABLE_CUDA
    if (m_cudaFilters && m_cudaFilters->isAvailable()) {
        auto gpuResult = m_cudaFilters->applyRORCUDA(cloud, radius, minNeighbors);
        if (gpuResult && !gpuResult->empty()) {
            int pointsAfter = gpuResult->size();
            emit filterCompleted("Radius Outlier Removal (GPU)", pointsBefore, pointsAfter);
            qInfo() << "ROR (GPU):" << pointsBefore << "->" << pointsAfter << "points";
            return gpuResult;
        }
    }
#endif

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZRGB>);

    pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> ror;
    ror.setInputCloud(cloud);
    ror.setRadiusSearch(radius);
    ror.setMinNeighborsInRadius(minNeighbors);
    ror.filter(*filtered);

    int pointsAfter = filtered->size();
    emit filterCompleted("Radius Outlier Removal (CPU)", pointsBefore, pointsAfter);
    qInfo() << "ROR (CPU):" << pointsBefore << "->" << pointsAfter << "points";
    return filtered;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr CloudFiltersService::applyVoxelGrid(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    float leafSize)
{
    if (!cloud || cloud->empty()) return cloud;

    int pointsBefore = cloud->size();
#ifdef ASTRA_ENABLE_CUDA
    if (m_cudaFilters && m_cudaFilters->isAvailable()) {
        auto cudaResult = m_cudaFilters->applyVoxelGridCUDA(cloud, leafSize);
        if (cudaResult) {
            int pointsAfter = cudaResult->size();
            emit filterCompleted("Voxel Grid (CUDA)", pointsBefore, pointsAfter);
            qInfo() << "Voxel (CUDA):" << pointsBefore << "->" << pointsAfter << "points";
            return cudaResult;
        }
    }
#endif

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::VoxelGrid<pcl::PointXYZRGB> voxel;
    voxel.setLeafSize(leafSize, leafSize, leafSize);
    voxel.setSaveLeafLayout(false);
    voxel.setInputCloud(cloud);
    // setFilterFieldName удалён: 3D-сканеру нужна равномерная XYZ-децимация
    voxel.filter(*filtered);

    int pointsAfter = filtered->size();
    emit filterCompleted("Voxel Grid (CPU)", pointsBefore, pointsAfter);
    qInfo() << "Voxel (CPU):" << pointsBefore << "->" << pointsAfter << "points";
    return filtered;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr CloudFiltersService::applyMagicWand(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    float leafSize, int meanK, double stddevMulThresh)
{
    if (!cloud || cloud->empty()) return cloud;
    int pointsBefore = cloud->size();
    auto voxelized = applyVoxelGrid(cloud, leafSize);
    auto filtered = applyStatisticalOutlierRemoval(voxelized, meanK, stddevMulThresh);
    int pointsAfter = filtered->size();
    emit filterCompleted("Magic Wand", pointsBefore, pointsAfter);
    qInfo() << "Magic Wand:" << pointsBefore << "->" << pointsAfter << "points";
    return filtered;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr CloudFiltersService::removeBackgroundPlane(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    float distanceThreshold,
    int maxIterations)
{
    if (!cloud || cloud->empty()) {
        qWarning() << "[removeBackgroundPlane] Empty input cloud";
        return cloud;
    }

    emit progressUpdated(5);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
    *filtered = *cloud;

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());

    pcl::SACSegmentation<pcl::PointXYZRGB> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(maxIterations);
    seg.setDistanceThreshold(distanceThreshold);
    seg.setInputCloud(filtered);
    seg.segment(*inliers, *coefficients);

    if (inliers->indices.empty()) {
        qWarning() << "[removeBackgroundPlane] No plane found";
        emit progressUpdated(100);
        return cloud;
    }

    emit progressUpdated(30);

    std::vector<bool> mask(filtered->size(), false);
    for (int idx : inliers->indices) {
        if (idx >= 0 && idx < static_cast<int>(filtered->size())) {
            mask[idx] = true;
        }
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
    for (size_t i = 0; i < filtered->size(); ++i) {
        if (!mask[i]) result->push_back(filtered->points[i]);
    }

    emit progressUpdated(100);
    qInfo() << "[removeBackgroundPlane] Removed" << inliers->indices.size()
            << "points (plane)," << result->size() << "points remaining";
    return result;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr CloudFiltersService::applyRegionGrowingSegmentation(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    float clusterTolerance,
    int minClusterSize,
    int maxClusterSize)
{
    if (!cloud || cloud->empty()) {
        qWarning() << "[applyRegionGrowingSegmentation] Empty input cloud";
        return cloud;
    }

    emit progressUpdated(5);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    ne.setInputCloud(cloud);
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(std::max(0.01f, clusterTolerance));  // используем переданный параметр
    ne.compute(*normals);

    if (normals->empty()) {
        qWarning() << "[applyRegionGrowingSegmentation] Normal estimation failed";
        emit progressUpdated(100);
        return cloud;
    }

    emit progressUpdated(25);
    pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
    reg.setMinClusterSize(minClusterSize);
    reg.setMaxClusterSize(maxClusterSize);
    reg.setSearchMethod(tree);
    reg.setNumberOfNeighbours(50);
    reg.setSmoothnessThreshold(3.0f * kPi / 180.0f);
    reg.setCurvatureThreshold(1.0f);
    reg.setResidualThreshold(1.0f);
    reg.setInputCloud(cloud);
    reg.setInputNormals(normals);

    std::vector<pcl::PointIndices> cluster_indices;
    reg.extract(cluster_indices);
    emit progressUpdated(75);

    int largest_cluster_idx = -1;
    size_t largest_cluster_size = 0;
    for (size_t i = 0; i < cluster_indices.size(); ++i) {
        if (cluster_indices[i].indices.size() > largest_cluster_size) {
            largest_cluster_size = cluster_indices[i].indices.size();
            largest_cluster_idx = static_cast<int>(i);
        }
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
    if (largest_cluster_idx >= 0) {
        for (int idx : cluster_indices[largest_cluster_idx].indices) {
            if (idx >= 0 && idx < static_cast<int>(cloud->size())) {
                result->push_back(cloud->points[idx]);
            }
        }
    }

    emit progressUpdated(100);
    if (largest_cluster_idx < 0 || result->empty()) {
        qWarning() << "[applyRegionGrowingSegmentation] No suitable cluster found, returning source cloud";
        return cloud;
    }

    qInfo() << "[applyRegionGrowingSegmentation] Found" << cluster_indices.size()
            << "clusters, largest has" << result->size() << "points";
    return result;
}

pcl::PointCloud<pcl::Normal>::Ptr CloudFiltersService::estimateNormals(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    float searchRadius)
{
    if (!cloud || cloud->empty()) {
        return pcl::make_shared<pcl::PointCloud<pcl::Normal>>();
    }

#ifdef ASTRA_ENABLE_CUDA
    if (m_cudaFilters && m_cudaFilters->isAvailable()) {
        auto gpuResult = m_cudaFilters->estimateNormalsCUDA(cloud, searchRadius);
        if (gpuResult) {
            qInfo() << "[NormalEstimation] GPU:" << cloud->size() << "->" << gpuResult->size() << "normals";
            return gpuResult;
        }
    }
#endif

    // CPU fallback
    auto normals = pcl::make_shared<pcl::PointCloud<pcl::Normal>>();
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    ne.setInputCloud(cloud);
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(searchRadius);
    ne.compute(*normals);
    qInfo() << "[NormalEstimation] CPU:" << cloud->size() << "->" << normals->size() << "normals";
    return normals;
}