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

src/services/MeshProcessingService.cpp
320 строк12 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 "MeshProcessingService.h"
#include <QDebug>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <vtkSmartPointer.h>
#include <vtkSmoothPolyDataFilter.h>
#include <vtkFillHolesFilter.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkDecimatePro.h>
#include <vtkIdList.h>
#include <vtkNew.h>

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

pcl::PolygonMesh MeshProcessingService::applyMeshSmoothing(
    const pcl::PolygonMesh &mesh,
    int numIterations,
    float /*convergence*/)
{
    pcl::PolygonMesh result = mesh;
    if (mesh.cloud.data.empty() || mesh.polygons.empty()) {
        qWarning() << "[applyMeshSmoothing] Empty input mesh";
        return result;
    }

    emit progressUpdated(5);

    // Build vtkPolyData from the PCL mesh. This respects mesh topology, so
    // smoothing is done along edges (true Laplacian) rather than across spatial
    // neighbors — the previous KdTree-based implementation would pull
    // non-adjacent-but-close vertices together and could glue distinct parts.
    vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromPCLPointCloud2(mesh.cloud, cloud);
    for (const auto& pt : cloud.points) {
        points->InsertNextPoint(pt.x, pt.y, pt.z);
    }
    polyData->SetPoints(points);

    vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
    for (const auto& polygon : mesh.polygons) {
        if (polygon.vertices.size() >= 3) {
            std::vector<vtkIdType> cell(polygon.vertices.size());
            for (size_t i = 0; i < polygon.vertices.size(); ++i) {
                cell[i] = polygon.vertices[i];
            }
            cells->InsertNextCell(static_cast<vtkIdType>(polygon.vertices.size()), cell.data());
        }
    }
    polyData->SetPolys(cells);
    emit progressUpdated(25);

    vtkSmartPointer<vtkSmoothPolyDataFilter> smooth =
        vtkSmartPointer<vtkSmoothPolyDataFilter>::New();
    smooth->SetInputData(polyData);
    smooth->SetNumberOfIterations(std::max(1, numIterations));
    smooth->SetRelaxationFactor(0.2);
    smooth->FeatureEdgeSmoothingOn();
    smooth->BoundarySmoothingOn();
    smooth->SetFeatureAngle(45.0);
    smooth->Update();
    emit progressUpdated(80);

    vtkPolyData* out = smooth->GetOutput();
    if (!out) {
        qWarning() << "[applyMeshSmoothing] vtkSmoothPolyDataFilter returned null";
        return result;
    }
    vtkPoints* outPoints = out->GetPoints();
    if (!outPoints || outPoints->GetNumberOfPoints() == 0) {
        qWarning() << "[applyMeshSmoothing] vtkSmoothPolyDataFilter returned empty points";
        return result;
    }

    pcl::PointCloud<pcl::PointXYZ> outCloud;
    outCloud.points.resize(outPoints->GetNumberOfPoints());
    for (vtkIdType i = 0; i < outPoints->GetNumberOfPoints(); ++i) {
        double pt[3];
        outPoints->GetPoint(i, pt);
        outCloud.points[i].x = static_cast<float>(pt[0]);
        outCloud.points[i].y = static_cast<float>(pt[1]);
        outCloud.points[i].z = static_cast<float>(pt[2]);
    }
    outCloud.width = outCloud.points.size();
    outCloud.height = 1;
    outCloud.is_dense = true;

    pcl::PCLPointCloud2 outCloud2;
    pcl::toPCLPointCloud2(outCloud, outCloud2);
    result.cloud = outCloud2;

    emit progressUpdated(100);
    qInfo() << "[applyMeshSmoothing] Applied vtkSmoothPolyDataFilter,"
            << numIterations << "iterations (topology-aware Laplacian)";
    return result;
}

pcl::PolygonMesh MeshProcessingService::simplifyMesh(
    const pcl::PolygonMesh &mesh,
    float targetReduction)
{
    pcl::PolygonMesh result = mesh;
    if (mesh.cloud.data.empty() || mesh.polygons.empty()) {
        qWarning() << "[simplifyMesh] Empty input mesh";
        return result;
    }

    emit progressUpdated(5);
    vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromPCLPointCloud2(mesh.cloud, cloud);
    for (const auto& pt : cloud.points) {
        points->InsertNextPoint(pt.x, pt.y, pt.z);
    }
    polyData->SetPoints(points);

    vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
    for (const auto& polygon : mesh.polygons) {
        if (polygon.vertices.size() >= 3) {
            std::vector<vtkIdType> cell(polygon.vertices.size());
            for (size_t i = 0; i < polygon.vertices.size(); ++i) {
                cell[i] = polygon.vertices[i];
            }
            cells->InsertNextCell(static_cast<vtkIdType>(polygon.vertices.size()), cell.data());
        }
    }
    polyData->SetPolys(cells);

    vtkSmartPointer<vtkDecimatePro> decimate = vtkSmartPointer<vtkDecimatePro>::New();
    decimate->SetInputData(polyData);
    decimate->SetTargetReduction(targetReduction);
    decimate->PreserveTopologyOff();
    decimate->SetSplitting(false);
    decimate->SetMaximumError(1e-6);
    decimate->Update();

    vtkPolyData* outputPolyData = decimate->GetOutput();
    if (!outputPolyData) {
        qWarning() << "[simplifyMesh] VTK DecimatePro returned null output";
        return result;
    }

    vtkPoints* outPoints = outputPolyData->GetPoints();
    if (!outPoints || outPoints->GetNumberOfPoints() == 0) {
        qWarning() << "[simplifyMesh] VTK DecimatePro returned empty output";
        return result;
    }

    pcl::PointCloud<pcl::PointXYZ> outCloud;
    outCloud.points.resize(outPoints->GetNumberOfPoints());
    for (vtkIdType i = 0; i < outPoints->GetNumberOfPoints(); ++i) {
        double pt[3];
        outPoints->GetPoint(i, pt);
        outCloud.points[i].x = static_cast<float>(pt[0]);
        outCloud.points[i].y = static_cast<float>(pt[1]);
        outCloud.points[i].z = static_cast<float>(pt[2]);
    }
    outCloud.width = outCloud.points.size();
    outCloud.height = 1;
    outCloud.is_dense = true;

    pcl::PCLPointCloud2 outCloud2;
    pcl::toPCLPointCloud2(outCloud, outCloud2);
    result.cloud = outCloud2;

    result.polygons.clear();
    vtkCellArray* outCells = outputPolyData->GetPolys();
    if (outCells) {
        vtkNew<vtkIdList> cellPoints;
        for (vtkIdType cellId = 0; cellId < outCells->GetNumberOfCells(); ++cellId) {
            outCells->GetCellAtId(cellId, cellPoints);
            const vtkIdType npts = cellPoints->GetNumberOfIds();
            if (npts >= 3) {
                pcl::Vertices v;
                v.vertices.resize(npts);
                bool valid = true;
                for (vtkIdType i = 0; i < npts; ++i) {
                    vtkIdType ptId = cellPoints->GetId(i);
                    if (ptId < 0 || ptId >= outPoints->GetNumberOfPoints()) {
                        qWarning() << "[simplifyMesh] Invalid vertex index:" << ptId
                                   << "(max:" << outPoints->GetNumberOfPoints() << ")";
                        valid = false;
                        break;
                    }
                    v.vertices[i] = static_cast<uint32_t>(ptId);
                }
                if (valid && !v.vertices.empty()) {
                    result.polygons.push_back(v);
                }
            }
        }
    }

    emit progressUpdated(100);
    qInfo() << "[simplifyMesh] Simplified mesh from" << mesh.polygons.size()
            << "to" << result.polygons.size() << "polygons (target reduction:" << targetReduction << ")";
    return result;
}

pcl::PolygonMesh MeshProcessingService::fillMeshHoles(
    const pcl::PolygonMesh &mesh,
    float maxHoleSize)
{
    pcl::PolygonMesh result = mesh;
    qInfo() << "[fillMeshHoles] Start filling mesh holes, maxHoleSize:" << maxHoleSize << "m";

    if (mesh.cloud.data.empty() || mesh.polygons.empty()) {
        qWarning() << "[fillMeshHoles] Empty input mesh";
        return result;
    }

    emit progressUpdated(5);
    vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromPCLPointCloud2(mesh.cloud, cloud);
    for (const auto& pt : cloud.points) {
        points->InsertNextPoint(pt.x, pt.y, pt.z);
    }
    polyData->SetPoints(points);

    vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
    for (const auto& polygon : mesh.polygons) {
        if (polygon.vertices.size() >= 3) {
            std::vector<vtkIdType> cell(polygon.vertices.size());
            for (size_t i = 0; i < polygon.vertices.size(); ++i) {
                cell[i] = polygon.vertices[i];
            }
            cells->InsertNextCell(static_cast<vtkIdType>(polygon.vertices.size()), cell.data());
        }
    }
    polyData->SetPolys(cells);

    vtkSmartPointer<vtkFillHolesFilter> fillHoles = vtkSmartPointer<vtkFillHolesFilter>::New();
    fillHoles->SetInputData(polyData);
    fillHoles->SetHoleSize(maxHoleSize);
    fillHoles->Update();

    vtkPolyData* outputPolyData = fillHoles->GetOutput();
    if (!outputPolyData) {
        qWarning() << "[fillMeshHoles] VTK FillHolesFilter returned null output";
        return result;
    }

    vtkPoints* outPoints = outputPolyData->GetPoints();
    if (!outPoints || outPoints->GetNumberOfPoints() == 0) {
        qWarning() << "[fillMeshHoles] VTK FillHolesFilter returned empty output";
        return result;
    }

    pcl::PointCloud<pcl::PointXYZ> outCloud;
    outCloud.points.resize(outPoints->GetNumberOfPoints());
    for (vtkIdType i = 0; i < outPoints->GetNumberOfPoints(); ++i) {
        double pt[3];
        outPoints->GetPoint(i, pt);
        outCloud.points[i].x = static_cast<float>(pt[0]);
        outCloud.points[i].y = static_cast<float>(pt[1]);
        outCloud.points[i].z = static_cast<float>(pt[2]);
    }
    outCloud.width = outCloud.points.size();
    outCloud.height = 1;
    outCloud.is_dense = true;

    pcl::PCLPointCloud2 outCloud2;
    pcl::toPCLPointCloud2(outCloud, outCloud2);
    result.cloud = outCloud2;

    result.polygons.clear();
    vtkCellArray* outCells = outputPolyData->GetPolys();
    if (outCells) {
        vtkNew<vtkIdList> cellPoints;
        for (vtkIdType cellId = 0; cellId < outCells->GetNumberOfCells(); ++cellId) {
            outCells->GetCellAtId(cellId, cellPoints);
            const vtkIdType npts = cellPoints->GetNumberOfIds();
            if (npts >= 3) {
                pcl::Vertices v;
                v.vertices.resize(npts);
                bool valid = true;
                for (vtkIdType i = 0; i < npts; ++i) {
                    vtkIdType ptId = cellPoints->GetId(i);
                    if (ptId < 0 || ptId >= outPoints->GetNumberOfPoints()) {
                        qWarning() << "[fillMeshHoles] Invalid vertex index:" << ptId
                                   << "(max:" << outPoints->GetNumberOfPoints() << ")";
                        valid = false;
                        break;
                    }
                    v.vertices[i] = static_cast<uint32_t>(ptId);
                }
                if (valid && !v.vertices.empty()) {
                    result.polygons.push_back(v);
                }
            }
        }
    }

    emit progressUpdated(100);
    qInfo() << "[fillMeshHoles] Filled holes in mesh with"
            << mesh.polygons.size() << "original polygons, now"
            << result.polygons.size() << "polygons (max hole size:"
            << maxHoleSize << "m)";
    qInfo() << "[fillMeshHoles] Finished: added"
            << (static_cast<int>(result.polygons.size()) - static_cast<int>(mesh.polygons.size()))
            << "new polygons";
    return result;
}