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

src/services/RegistrationService.cpp
184 строки7 KB
k k
fix(medium): configurable normals radius and thread-safe intrinsics
13 июл 2026, 23:48
13 июл 2026, 23:48dc33f76
#include "RegistrationService.h"
#include "CloudFiltersService.h"
#include <QDebug>
#include <pcl/common/io.h>
#include <pcl/registration/icp.h>
#include <pcl/search/kdtree.h>

RegistrationService::RegistrationService(CloudFiltersService *cloudFiltersService, QObject *parent)
    : QObject(parent)
    , m_cloudFiltersService(cloudFiltersService)
{
}

bool RegistrationService::computeNormals(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
                                         pcl::PointCloud<pcl::Normal>::Ptr &normals)
{
    normals->clear();
    if (!cloud || cloud->empty()) {
        return false;
    }
    if (m_cloudFiltersService) {
        auto gpuNormals = m_cloudFiltersService->estimateNormals(cloud, static_cast<float>(m_normalsRadius));
        if (gpuNormals && gpuNormals->size() == cloud->size()) {
            *normals = *gpuNormals;
            return true;
        }
    }
    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(m_normalsRadius);
    ne.compute(*normals);
    return normals->size() == cloud->size();
}

IcpRegistrationResult RegistrationService::registerPointCloudsICPWithResult(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &source,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &target,
    double maxCorrespondenceDistance,
    int maximumIterations,
    const Eigen::Matrix4f *initialGuess)
{
    IcpRegistrationResult result;
    result.aligned = source;

    if (!source || source->empty() || !target || target->empty()) {
        return result;
    }

    emit progressUpdated(10);
    pcl::PointCloud<pcl::Normal>::Ptr sourceNormals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::Normal>::Ptr targetNormals(new pcl::PointCloud<pcl::Normal>);
    if (!computeNormals(source, sourceNormals)) {
        qWarning() << "ICP: failed to estimate source normals (" << source->size() << "pts ->" << sourceNormals->size() << "normals)";
        return result;
    }
    if (!computeNormals(target, targetNormals)) {
        qWarning() << "ICP: failed to estimate target normals (" << target->size() << "pts ->" << targetNormals->size() << "normals)";
        return result;
    }

    emit progressUpdated(30);

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr sourceWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr targetWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    pcl::concatenateFields(*source, *sourceNormals, *sourceWithNormals);
    pcl::concatenateFields(*target, *targetNormals, *targetWithNormals);

    pcl::IterativeClosestPoint<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> icp;
    icp.setInputSource(sourceWithNormals);
    icp.setInputTarget(targetWithNormals);
    icp.setMaxCorrespondenceDistance(maxCorrespondenceDistance);
    icp.setMaximumIterations(maximumIterations);
    icp.setTransformationEpsilon(1e-8);
    icp.setEuclideanFitnessEpsilon(1e-8);

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr alignedWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    if (initialGuess) {
        icp.align(*alignedWithNormals, *initialGuess);
    } else {
        icp.align(*alignedWithNormals);
    }

    emit progressUpdated(80);

    result.converged = icp.hasConverged();
    result.fitness = icp.getFitnessScore();
    result.transform = icp.getFinalTransformation();

    if (result.converged) {
        qInfo() << "ICP converged with score:" << result.fitness;
        result.aligned = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::copyPointCloud(*alignedWithNormals, *result.aligned);
    } else {
        qWarning() << "ICP did not converge";
    }

    emit progressUpdated(100);
    return result;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr RegistrationService::registerPointCloudsICP(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &source,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &target,
    double maxCorrespondenceDistance,
    int maximumIterations,
    bool *convergedOut)
{
    const auto result = registerPointCloudsICPWithResult(
        source, target, maxCorrespondenceDistance, maximumIterations, nullptr);

    if (convergedOut) {
        *convergedOut = result.converged;
    }

    return result.converged ? result.aligned : source;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr RegistrationService::mergeScans(
    const std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &scans,
    const AstraMergeParams &params)
{
    if (scans.empty()) return pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);

    // Pre-allocate: estimate total size to avoid O(n²) reallocations in operator+=
    size_t totalPoints = 0;
    for (const auto &s : scans) {
        if (s && !s->empty()) totalPoints += s->size();
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr merged(new pcl::PointCloud<pcl::PointXYZRGB>);
    merged->reserve(totalPoints);

    if (scans[0] && !scans[0]->empty()) {
        *merged = *scans[0];
        merged->reserve(totalPoints);  // re-reserve after copy (copy may not preserve reserve)
    }

    int converged = 0;
    int skipped = 0;
    int addedAsIs = 0;

    for (size_t i = 1; i < scans.size(); ++i) {
        emit progressUpdated(static_cast<int>((i * 100) / scans.size()));
        const auto &src = scans[i];
        if (!src || src->empty() || merged->empty()) continue;

        pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
        icp.setInputSource(src);
        icp.setInputTarget(merged);
        icp.setMaxCorrespondenceDistance(params.maxCorrespondenceDistance);
        icp.setMaximumIterations(params.maximumIterations);
        icp.setTransformationEpsilon(1e-8);
        icp.setEuclideanFitnessEpsilon(1e-8);

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZRGB>);
        icp.align(*aligned);

        if (icp.hasConverged()) {
            merged->insert(merged->end(), aligned->begin(), aligned->end());
            ++converged;
            qInfo() << "[mergeScans] scan" << i << "converged, score" << icp.getFitnessScore();
        } else if (params.skipNonConverged) {
            ++skipped;
            qWarning() << "[mergeScans] scan" << i << "ICP did not converge — skipped";
        } else {
            merged->insert(merged->end(), src->begin(), src->end());
            ++addedAsIs;
            qWarning() << "[mergeScans] scan" << i << "ICP did not converge — added as-is (expect misalignment)";
        }
    }

    if (params.voxelLeafOut > 0.0 && m_cloudFiltersService) {
        merged = m_cloudFiltersService->applyVoxelGrid(merged, static_cast<float>(params.voxelLeafOut));
    }

    emit progressUpdated(100);
    qInfo() << "Merged" << scans.size() << "scans into" << merged->size()
            << "points (converged=" << converged
            << ", skipped=" << skipped
            << ", added-as-is=" << addedAsIs << ")";
    return merged;
}