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

src/services/ReconstructionService.cpp
263 строки10 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 "ReconstructionService.h"
#ifdef ASTRA_ENABLE_CUDA
#include "../filters/PointCloudFiltersCUDA.h"
#endif
#include <QDebug>
#include <Eigen/Core>
#include <queue>
#include <vector>
#include <algorithm>
#include <pcl/common/centroid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/poisson.h>

ReconstructionService::ReconstructionService(QObject *parent) : QObject(parent)
{
}

pcl::PolygonMesh ReconstructionService::reconstructPoissonMesh(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    const AstraPoissonParams &params)
{
    pcl::PolygonMesh mesh;
    if (!cloud || cloud->empty()) {
        qWarning() << "[Poisson] Empty input cloud";
        return mesh;
    }

    emit progressUpdated(5);

    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

#ifdef ASTRA_ENABLE_CUDA
    {
        // Try GPU normal estimation (radius-only; k-nearest not implemented on GPU)
        if (params.normalSearchRadius > 0.0) {
            PointCloudFiltersCUDA cuda;
            if (cuda.isAvailable()) {
                auto gpuNormals = cuda.estimateNormalsCUDA(cloud, static_cast<float>(params.normalSearchRadius));
                if (gpuNormals && !gpuNormals->empty()) {
                    *normals = *gpuNormals;
                }
            }
        }
    }
#endif

    // Вычисляем centroid и viewpoint ДО блока нормалей —
    // они нужны и для NormalEstimation, и для consistentOrientation ниже.
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*cloud, centroid);
    float vpX = centroid[0];
    float vpY = centroid[1];
    float vpZ = centroid[2] - 1.0f;
    if (params.useCustomViewpoint) {
        vpX = params.viewpointX;
        vpY = params.viewpointY;
        vpZ = params.viewpointZ;
        qInfo() << "[Poisson] Using custom viewpoint (" << vpX << vpY << vpZ << ")";
    }

    if (normals->empty()) {
        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);
        if (params.normalSearchRadius > 0.0) {
            ne.setRadiusSearch(params.normalSearchRadius);
        } else {
            ne.setKSearch(params.kNearest);
        }
        ne.setViewPoint(vpX, vpY, vpZ);
        ne.compute(*normals);
    }

    if (normals->size() != cloud->size()) {
        qWarning() << "[Poisson] Normal estimation failed: got" << normals->size()
                   << "normals for" << cloud->size() << "points";
        return mesh;
    }
    emit progressUpdated(25);

    if (params.consistentOrientation) {
        const int k = std::max(3, params.orientationKNeighbors);
        pcl::KdTreeFLANN<pcl::PointXYZRGB> orientTree;
        orientTree.setInputCloud(cloud);

        pcl::PointXYZRGB vpPoint;
        vpPoint.x = vpX; vpPoint.y = vpY; vpPoint.z = vpZ;

        std::vector<bool> visited(cloud->size(), false);
        std::vector<int> nbIdx(k);
        std::vector<float> nbDist(k);
        const std::size_t total = cloud->size();
        std::size_t processed = 0;
        int components = 0;

        // Multi-seed BFS/MST consistent orientation:
        //   Start from the point closest to the viewpoint; after BFS exhausts
        //   its component, continue with the next unvisited finite point as a
        //   new seed (flip towards the viewpoint to keep global orientation).
        // Previously a single seed was used, which disconnected clusters with
        // unreliably oriented normals that broke Poisson reconstruction.
        const auto propagateFromSeed = [&](int seed) {
            std::queue<int> bfs;
            bfs.push(seed);
            visited[seed] = true;

            // Orient the seed normal towards the viewpoint.
            Eigen::Vector3f toVp(vpX - cloud->points[seed].x,
                                 vpY - cloud->points[seed].y,
                                 vpZ - cloud->points[seed].z);
            Eigen::Vector3f nSeed(normals->points[seed].normal_x,
                                  normals->points[seed].normal_y,
                                  normals->points[seed].normal_z);
            if (toVp.norm() > 1e-6f && nSeed.dot(toVp) < 0.0f) {
                normals->points[seed].normal_x *= -1.0f;
                normals->points[seed].normal_y *= -1.0f;
                normals->points[seed].normal_z *= -1.0f;
            }

            while (!bfs.empty()) {
                int idx = bfs.front(); bfs.pop();
                ++processed;

                int found = orientTree.nearestKSearch(cloud->points[idx], k, nbIdx, nbDist);
                if (found <= 0) continue;

                const Eigen::Vector3f parent(normals->points[idx].normal_x,
                                             normals->points[idx].normal_y,
                                             normals->points[idx].normal_z);

                for (int i = 0; i < found; ++i) {
                    const int n = nbIdx[i];
                    if (n < 0 || static_cast<std::size_t>(n) >= cloud->size()) continue;
                    if (visited[n]) {
                        // Already oriented — enforce dot>=0 with parent to
                        // resolve minor mis-flips inside the same component.
                        Eigen::Vector3f sib(normals->points[n].normal_x,
                                            normals->points[n].normal_y,
                                            normals->points[n].normal_z);
                        if (parent.dot(sib) < 0.0f) {
                            normals->points[n].normal_x *= -1.0f;
                            normals->points[n].normal_y *= -1.0f;
                            normals->points[n].normal_z *= -1.0f;
                        }
                        continue;
                    }
                    visited[n] = true;

                    Eigen::Vector3f child(normals->points[n].normal_x,
                                          normals->points[n].normal_y,
                                          normals->points[n].normal_z);
                    if (parent.dot(child) < 0.0f) {
                        normals->points[n].normal_x *= -1.0f;
                        normals->points[n].normal_y *= -1.0f;
                        normals->points[n].normal_z *= -1.0f;
                    }
                    bfs.push(n);
                }

                if ((processed & 0x3FFF) == 0 && total > 0) {
                    int pct = 25 + static_cast<int>(10 * processed / total);
                    emit progressUpdated(std::min(35, pct));
                }
            }
        };

        // First seed: nearest to viewpoint.
        std::vector<int> seedIdx(1);
        std::vector<float> seedDist(1);
        if (orientTree.nearestKSearch(vpPoint, 1, seedIdx, seedDist) >= 1 && seedIdx[0] >= 0) {
            propagateFromSeed(seedIdx[0]);
            ++components;
        } else {
            qWarning() << "[Poisson] Consistent orientation: viewpoint seed search failed";
        }

        // Sweep leftover unvisited finite points — each is a new component.
        for (std::size_t i = 0; i < cloud->size(); ++i) {
            if (visited[i]) continue;
            const auto& pt = cloud->points[i];
            if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) continue;
            propagateFromSeed(static_cast<int>(i));
            ++components;
        }
        qInfo() << "[Poisson] Consistent orientation propagated over" << processed
                << "points in" << components << "component(s)";
    }

    if (params.flipNormals) {
        for (auto &n : normals->points) {
            n.normal_x *= -1.0f;
            n.normal_y *= -1.0f;
            n.normal_z *= -1.0f;
        }
        qInfo() << "[Poisson] Normals inverted (flipNormals=true)";
    }
    emit progressUpdated(35);

    pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointNormal>);
    cloudWithNormals->reserve(cloud->size());
    for (std::size_t i = 0; i < cloud->size(); ++i) {
        pcl::PointNormal p;
        p.x = cloud->points[i].x;
        p.y = cloud->points[i].y;
        p.z = cloud->points[i].z;
        p.normal_x = normals->points[i].normal_x;
        p.normal_y = normals->points[i].normal_y;
        p.normal_z = normals->points[i].normal_z;
        p.curvature = normals->points[i].curvature;
        if (!std::isfinite(p.x) || !std::isfinite(p.normal_x)) continue;
        cloudWithNormals->push_back(p);
    }
    cloudWithNormals->width = cloudWithNormals->size();
    cloudWithNormals->height = 1;
    cloudWithNormals->is_dense = true;

    if (cloudWithNormals->empty()) {
        qWarning() << "[Poisson] No finite oriented points after filtering";
        return mesh;
    }
    emit progressUpdated(45);

    pcl::Poisson<pcl::PointNormal> poisson;
    poisson.setInputCloud(cloudWithNormals);
    poisson.setDepth(params.depth);
    poisson.setMinDepth(params.minDepth);
    poisson.setPointWeight(params.pointWeight);
    poisson.setSamplesPerNode(params.samplesPerNode);
    poisson.setScale(params.scale);
    poisson.setConfidence(params.confidence);
    poisson.setOutputPolygons(params.outputPolygons);

    if (cloudWithNormals->size() > 2'000'000 && params.depth >= 10) {
        qWarning() << "[Poisson] Very large cloud (" << cloudWithNormals->size()
                   << "points) with depth" << params.depth << "— reconstruction may be unstable";
    }

    try {
        poisson.reconstruct(mesh);
    } catch (const std::exception& e) {
        qCritical() << "[Poisson] reconstruct failed:" << e.what();
        emit progressUpdated(100);
        return {};
    } catch (...) {
        qCritical() << "[Poisson] reconstruct failed: unknown exception";
        emit progressUpdated(100);
        return {};
    }

    emit progressUpdated(100);

    const std::size_t nPoly = mesh.polygons.size();
    if (nPoly == 0) {
        qWarning() << "[Poisson] Reconstruction returned empty mesh";
    } else {
        qInfo() << "[Poisson] Reconstructed" << nPoly << "polygons from"
                << cloudWithNormals->size() << "oriented points"
                << "(depth=" << params.depth << ")";
    }
    return mesh;
}