src/tracking/NeuralTrackingPipeline.cpp - master - klischa/AstraScanner2

src/tracking/NeuralTrackingPipeline.cpp
449 строк20 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 "NeuralTrackingPipeline.h"
#include "../services/RegistrationService.h"
#include "../services/CloudFiltersService.h"
#include "../settings/SettingsManager.h"
#include <QDebug>
#include <QSettings>
#include <Eigen/Geometry>
#include <cmath>
#include <limits>

NeuralTrackingPipeline::NeuralTrackingPipeline() = default;

void NeuralTrackingPipeline::setTracker(std::unique_ptr<NeuralTracker> tracker)
{
    m_neuralTracker = std::move(tracker);
}

void NeuralTrackingPipeline::setEnabled(bool enabled)
{
    m_neuralTrackingEnabled = enabled;
}

void NeuralTrackingPipeline::reset()
{
    m_cumulativeTransform = Eigen::Matrix4f::Identity();
    m_nnRejectStreak = 0;
    m_nnAcceptStreak = 0;
    m_frameCounterNN = 0;
    QMutexLocker locker(&m_prevFrameMutex);
    m_prevFrameCloud.reset();
    m_neuralInProgress.store(false, std::memory_order_release);
}

void NeuralTrackingPipeline::loadThresholds()
{
    const auto &settings = SettingsManager::instance();
    m_nnThresholds.enableIcpConfirm = settings.neuralIcpConfirmEnabled();
    m_nnThresholds.icpConfirmMaxCorrespondenceDistance =
        static_cast<float>(settings.neuralIcpConfirmMaxCorrespondenceDistance());
    m_nnThresholds.icpConfirmMaxIterations = settings.neuralIcpConfirmMaxIterations();
    m_nnThresholds.icpConfirmMaxFitness =
        static_cast<float>(settings.neuralIcpConfirmMaxFitness());
    m_nnThresholds.maxIcpCorrectionTranslationMeters =
        static_cast<float>(settings.neuralIcpConfirmMaxCorrectionTranslation());
    m_nnThresholds.maxIcpCorrectionRotationDeg =
        static_cast<float>(settings.neuralIcpConfirmMaxCorrectionRotationDeg());

    // Cadence: settings key is optional; default is every 4th frame (~7-8 Hz at 30 FPS).
    // Read via QSettings directly because SettingsManager doesn't expose a generic value().
    QSettings s;
    int cadence = s.value(QStringLiteral("tracking/neuralInferenceStride"), 4).toInt();
    m_inferenceFrameStride = std::clamp(cadence, 1, 60);

    qInfo() << "[NeuralPipeline] Thresholds loaded: enabled=" << m_nnThresholds.enableIcpConfirm
            << "maxCorr=" << m_nnThresholds.icpConfirmMaxCorrespondenceDistance
            << "maxIter=" << m_nnThresholds.icpConfirmMaxIterations
            << "maxFitness=" << m_nnThresholds.icpConfirmMaxFitness
            << "maxCorrectionT=" << m_nnThresholds.maxIcpCorrectionTranslationMeters
            << "maxCorrectionRot=" << m_nnThresholds.maxIcpCorrectionRotationDeg
            << "stride=" << m_inferenceFrameStride;
}

void NeuralTrackingPipeline::setInferenceFrameStride(int stride)
{
    m_inferenceFrameStride = std::clamp(stride, 1, 60);
}

bool NeuralTrackingPipeline::hasValidDepthPoints(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud)
{
    if (!cloud || cloud->empty()) return false;
    for (const auto& pt : cloud->points) {
        if (std::isfinite(pt.z) && pt.z > 0.001f) return true;
    }
    return false;
}

std::optional<NeuralCloudStats> NeuralTrackingPipeline::analyzeTrackingCloud(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud) const
{
    return analyzeNeuralCloud(cloud);
}

QString NeuralTrackingPipeline::buildDepthQualityText(const NeuralCloudStats &stats) const
{
    return QString("%1 finite pts, bbox %2 m")
        .arg(stats.finitePoints)
        .arg(stats.bboxDiagonal, 0, 'f', 2);
}

int NeuralTrackingPipeline::depthQualityLevel(const NeuralCloudStats &stats) const
{
    if (!std::isfinite(stats.bboxDiagonal) || stats.finitePoints < 400 || stats.bboxDiagonal < 0.02f)
        return QualityPoor;
    if (stats.finitePoints < m_nnThresholds.minFinitePoints || stats.bboxDiagonal < m_nnThresholds.minBboxDiagonal)
        return QualityWarning;
    return QualityGood;
}

bool NeuralTrackingPipeline::isCloudSuitableForNeuralTracking(const NeuralCloudStats &stats) const
{
    return stats.finitePoints >= m_nnThresholds.minFinitePoints &&
           std::isfinite(stats.bboxDiagonal) &&
           stats.bboxDiagonal >= m_nnThresholds.minBboxDiagonal &&
           stats.bboxDiagonal <= m_nnThresholds.maxBboxDiagonal;
}

bool NeuralTrackingPipeline::areTrackingCloudsCompatible(
    const NeuralCloudStats &current, const NeuralCloudStats &previous) const
{
    if (previous.finitePoints <= 0 || current.finitePoints <= 0) return false;

    const float pointRatio = static_cast<float>(current.finitePoints) /
                             static_cast<float>(previous.finitePoints);
    if (!std::isfinite(pointRatio) ||
        pointRatio < m_nnThresholds.minPointRatio ||
        pointRatio > m_nnThresholds.maxPointRatio) return false;

    const float centroidShift = (current.centroid - previous.centroid).norm();
    if (!std::isfinite(centroidShift) ||
        centroidShift > m_nnThresholds.maxCentroidShiftMeters) return false;

    if (previous.bboxDiagonal > 1e-6f) {
        const float bboxRatio = current.bboxDiagonal / previous.bboxDiagonal;
        if (!std::isfinite(bboxRatio) ||
            bboxRatio < m_nnThresholds.minPointRatio ||
            bboxRatio > m_nnThresholds.maxPointRatio) return false;
    }
    return true;
}

float NeuralTrackingPipeline::transformTranslationNorm(const Eigen::Matrix4f &transform) const
{
    if (!transform.array().isFinite().all()) return std::numeric_limits<float>::infinity();
    return transform.block<3, 1>(0, 3).norm();
}

float NeuralTrackingPipeline::transformRotationAngleDeg(const Eigen::Matrix4f &transform) const
{
    if (!transform.array().isFinite().all()) return std::numeric_limits<float>::infinity();
    const Eigen::Matrix3f rotation = transform.block<3, 3>(0, 0);
    if (!isRotationMatrixValid(rotation)) return std::numeric_limits<float>::infinity();
    Eigen::Quaternionf q(rotation);
    q.normalize();
    if (!q.coeffs().array().isFinite().all()) return std::numeric_limits<float>::infinity();
    constexpr float kPi = 3.14159265358979323846f;
    return 2.0f * std::acos(std::clamp(std::abs(q.w()), 0.0f, 1.0f)) * 180.0f / kPi;
}

bool NeuralTrackingPipeline::isRelativeTransformPlausible(const Eigen::Matrix4f &transform) const
{
    if (!transform.array().isFinite().all()) return false;
    const Eigen::Matrix3f rotation = transform.block<3, 3>(0, 0);
    if (!isRotationMatrixValid(rotation)) return false;
    const float translationNorm = transformTranslationNorm(transform);
    if (!std::isfinite(translationNorm) || translationNorm > m_nnThresholds.maxTranslationPerStepMeters) return false;
    const float rotationAngleDeg = transformRotationAngleDeg(transform);
    if (!std::isfinite(rotationAngleDeg) || rotationAngleDeg > m_nnThresholds.maxRotationPerStepDeg) return false;
    return true;
}

RegistrationService* NeuralTrackingPipeline::resolveIcpService(RegistrationService *external) const
{
    if (external) return external;
    if (!m_nnThresholds.enableIcpConfirm) return nullptr;
    // Lazy-create a local ICP service so callers that don't wire one up
    // (e.g. direct CaptureWorker call with nullptr) still get confirm-on-neural-pose.
    // The owned services are QObjects without a QThread parent — they live and die
    // with this pipeline and are only invoked synchronously from processFrame().
    if (!m_ownedIcpService) {
        m_ownedCloudService = std::make_unique<CloudFiltersService>(nullptr);
        m_ownedIcpService = std::make_unique<RegistrationService>(m_ownedCloudService.get(), nullptr);
    }
    return m_ownedIcpService.get();
}

bool NeuralTrackingPipeline::confirmNeuralPoseWithICP(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &previousCloud,
    const Eigen::Matrix4f &neuralTransform,
    Eigen::Matrix4f &refinedTransform,
    QString &rejectReason,
    RegistrationService *icpService,
    float *correctionTranslationOut,
    float *correctionRotationOut,
    double *fitnessOut) const
{
    if (!icpService) {
        rejectReason = QStringLiteral("ICP confirm service is not initialized");
        return false;
    }
    const auto icpResult = icpService->registerPointCloudsICPWithResult(
        cloud, previousCloud,
        m_nnThresholds.icpConfirmMaxCorrespondenceDistance,
        m_nnThresholds.icpConfirmMaxIterations,
        &neuralTransform);
    if (!icpResult.converged) { rejectReason = QStringLiteral("ICP confirm did not converge"); return false; }
    if (!icpResult.transform.array().isFinite().all()) { rejectReason = QStringLiteral("ICP confirm returned non-finite transform"); return false; }
    if (!std::isfinite(icpResult.fitness) || icpResult.fitness > m_nnThresholds.icpConfirmMaxFitness) {
        rejectReason = QStringLiteral("ICP confirm fitness too high: %1").arg(icpResult.fitness, 0, 'f', 6);
        return false;
    }
    if (fitnessOut) *fitnessOut = icpResult.fitness;

    // ICP returns T_target_from_source when called with source=current, target=previous
    // and initialGuess=neuralTransform — i.e. T_prev_from_curr, which is exactly what we
    // accept as the per-step delta (delta = T_new_from_old inverse is T_old_from_new).
    const Eigen::Matrix4f delta = icpResult.transform.inverse();
    const float correctionTranslation = transformTranslationNorm(delta * neuralTransform.inverse());
    if (correctionTranslationOut) *correctionTranslationOut = correctionTranslation;
    if (!std::isfinite(correctionTranslation) || correctionTranslation > m_nnThresholds.maxIcpCorrectionTranslationMeters) {
        rejectReason = QStringLiteral("ICP correction translation too large: %1 m").arg(correctionTranslation, 0, 'f', 4);
        return false;
    }
    const float correctionRotation = transformRotationAngleDeg(delta * neuralTransform.inverse());
    if (correctionRotationOut) *correctionRotationOut = correctionRotation;
    if (!std::isfinite(correctionRotation) || correctionRotation > m_nnThresholds.maxIcpCorrectionRotationDeg) {
        rejectReason = QStringLiteral("ICP correction rotation too large: %1 deg").arg(correctionRotation, 0, 'f', 3);
        return false;
    }

    // Evaluate residuals using the refined delta applied to cloud→previous.
    // residual evaluation expects T: source->target mapping, so pass icpResult.transform directly.
    const auto refinedAlignment = evaluateAlignmentResidual(cloud, previousCloud, icpResult.transform, 8, m_nnThresholds.inlierThresholdMeters);
    if (refinedAlignment.samplesEvaluated <= 0 || !std::isfinite(refinedAlignment.medianResidual) ||
        !std::isfinite(refinedAlignment.inlierRatio) ||
        refinedAlignment.medianResidual > m_nnThresholds.maxMedianResidualMeters ||
        refinedAlignment.inlierRatio < m_nnThresholds.minInlierRatio) {
        rejectReason = QStringLiteral("ICP-refined residual gate failed (median=%1 m, inliers=%2, samples=%3)")
                           .arg(refinedAlignment.medianResidual, 0, 'f', 4)
                           .arg(refinedAlignment.inlierRatio, 0, 'f', 3)
                           .arg(refinedAlignment.samplesEvaluated);
        return false;
    }

    refinedTransform = delta;
    return true;
}

void NeuralTrackingPipeline::resetNeuralTrackingAnchor(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud)
{
    // Deep copy so subsequent caller-side modifications don't mutate the anchor.
    QMutexLocker locker(&m_prevFrameMutex);
    if (cloud) {
        auto copy = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>(*cloud);
        copy->is_dense = cloud->is_dense;
        m_prevFrameCloud = copy;
    } else {
        m_prevFrameCloud.reset();
    }
}

NeuralTrackingResult NeuralTrackingPipeline::processFrame(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    RegistrationService *icpService)
{
    NeuralTrackingResult result;

    // All early-exit paths MUST clear m_neuralInProgress to avoid lock-out.
    auto clearInProgress = [this]() { m_neuralInProgress.store(false, std::memory_order_release); };

    if (!(m_neuralTrackingEnabled && m_neuralTracker && m_neuralTracker->isLoaded())) {
        clearInProgress();
        return result;
    }
    if (m_neuralInProgress.exchange(true)) return result;

    try {

    if (!hasValidDepthPoints(cloud)) { clearInProgress(); return result; }

    if (++m_frameCounterNN % m_inferenceFrameStride != 0) { clearInProgress(); return result; }

    const auto currentStats = analyzeTrackingCloud(cloud);
    if (!currentStats || !isCloudSuitableForNeuralTracking(*currentStats)) {
        m_nnRejectStreak++;
        m_nnAcceptStreak = 0;
        result.rejectReason = QStringLiteral("current cloud failed neural quality gate");
        clearInProgress();
        return result;
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr previousCloud;
    {
        QMutexLocker locker(&m_prevFrameMutex);
        previousCloud = m_prevFrameCloud;
    }
    if (!previousCloud) {
        resetNeuralTrackingAnchor(cloud);
        m_nnRejectStreak = 0; m_nnAcceptStreak = 0;
        clearInProgress();
        return result;
    }

    const auto previousStats = analyzeTrackingCloud(previousCloud);
    if (!previousStats || !isCloudSuitableForNeuralTracking(*previousStats)) {
        resetNeuralTrackingAnchor(cloud);
        m_nnRejectStreak = 0; m_nnAcceptStreak = 0;
        clearInProgress();
        return result;
    }

    if (!areTrackingCloudsCompatible(*currentStats, *previousStats)) {
        m_nnRejectStreak++;
        m_nnAcceptStreak = 0;
        result.rejectReason = QStringLiteral("current/previous clouds are not compatible");
        clearInProgress();
        return result;
    }

    // tryEstimatePose(source=curr, target=prev) returns T mapping source→target (PCL convention),
    // i.e. T_prev_from_curr. Per-step camera motion is its inverse: T_curr_from_prev.
    const auto poseOpt = m_neuralTracker->tryEstimatePose(cloud, previousCloud, 512);
    if (!poseOpt) {
        const QString trackerReason = QString::fromStdString(m_neuralTracker->lastRejectReason());
        m_nnRejectStreak++;
        m_nnAcceptStreak = 0;
        result.rejectReason = trackerReason.isEmpty()
            ? QStringLiteral("neural inference rejected")
            : QStringLiteral("neural inference rejected: %1").arg(trackerReason);
        clearInProgress();
        return result;
    }
    const Eigen::Matrix4f tPrevFromCurr = *poseOpt;
    Eigen::Matrix4f delta = tPrevFromCurr.inverse();  // T_curr_from_prev  (camera-frame relative motion)

    if (!isRelativeTransformPlausible(delta)) {
        m_nnRejectStreak++;
        m_nnAcceptStreak = 0;
        result.rejectReason = QStringLiteral("relative transform failed plausibility gate");
        clearInProgress();
        return result;
    }

    // Pre-ICP residual check: how well neural delta (inverted for src→tgt) aligns cloud to prev.
    const Eigen::Matrix4f preDeltaSrcToTgt = tPrevFromCurr;
    const auto alignmentPre = evaluateAlignmentResidual(cloud, previousCloud, preDeltaSrcToTgt, 8, m_nnThresholds.inlierThresholdMeters);
    if (alignmentPre.samplesEvaluated <= 0 || !std::isfinite(alignmentPre.medianResidual) ||
        !std::isfinite(alignmentPre.inlierRatio) ||
        alignmentPre.medianResidual > m_nnThresholds.maxMedianResidualMeters ||
        alignmentPre.inlierRatio < m_nnThresholds.minInlierRatio) {
        m_nnRejectStreak++;
        m_nnAcceptStreak = 0;
        result.rejectReason = QString("alignment residual gate failed (median=%1 m, inliers=%2, samples=%3)")
                                  .arg(alignmentPre.medianResidual, 0, 'f', 4)
                                  .arg(alignmentPre.inlierRatio, 0, 'f', 3)
                                  .arg(alignmentPre.samplesEvaluated);
        clearInProgress();
        return result;
    }

    float icpCorrectionTranslation = 0.0f;
    float icpCorrectionRotation = 0.0f;
    double icpFitness = 0.0;
    bool icpOk = false;
    if (m_nnThresholds.enableIcpConfirm) {
        RegistrationService *svc = resolveIcpService(icpService);
        Eigen::Matrix4f refinedDelta;
        QString reject;
        if (confirmNeuralPoseWithICP(cloud, previousCloud, delta, refinedDelta, reject,
                                     svc, &icpCorrectionTranslation,
                                     &icpCorrectionRotation, &icpFitness)) {
            delta = refinedDelta;
            icpOk = true;
        } else {
            m_nnRejectStreak++;
            m_nnAcceptStreak = 0;
            result.rejectReason = QStringLiteral("ICP confirm failed: %1").arg(reject);
            clearInProgress();
            return result;
        }
    }

    // Commit: cumulative is T_cam_from_world; per-frame motion (in camera coords)
    // composes on the LEFT:  new_T_cam_from_world = delta * old_T_cam_from_world.
    m_cumulativeTransform = delta * m_cumulativeTransform;

    resetNeuralTrackingAnchor(cloud);
    m_nnRejectStreak = 0;
    ++m_nnAcceptStreak;

    result.poseUpdated = true;
    // The pose emitted / stored is T_world_from_cam = T_cam_from_world^{-1},
    // which is the standard "camera pose in world frame" convention, consistent
    // with marker-based pose updates.
    result.pose = Eigen::Affine3f(m_cumulativeTransform.inverse());
    result.depthText = buildDepthQualityText(*currentStats);
    result.depthLevel = depthQualityLevel(*currentStats);
    result.trackingText = icpOk
        ? QStringLiteral("Neural + ICP: подтверждено")
        : QStringLiteral("Neural: принято");
    const auto &alignForDisplay = icpOk ? alignmentPre : alignmentPre;  // keep consistent metric
    const float usedMedian = alignForDisplay.medianResidual;
    const float usedInlier = alignForDisplay.inlierRatio;
    result.alignmentText = QString("Residual %1 m, inliers %2%3")
                               .arg(usedMedian, 0, 'f', 4)
                               .arg(usedInlier, 0, 'f', 2)
                               .arg(icpOk ? QString(", ICP fitness %1").arg(icpFitness, 0, 'f', 5) : QString());
    result.alignmentLevel = (usedMedian <= 0.01f && usedInlier >= 0.50f)
        ? QualityGood : QualityWarning;
    result.driftText = QString("step %1 m / %2°")
                           .arg(transformTranslationNorm(delta), 0, 'f', 3)
                           .arg(transformRotationAngleDeg(delta), 0, 'f', 1);
    result.driftLevel = (transformTranslationNorm(delta) <= 0.04f &&
                         transformRotationAngleDeg(delta) <= 6.0f)
        ? QualityGood : QualityWarning;

    clearInProgress();
    return result;

    } catch (const std::exception &e) {
        qCritical() << "[NeuralPipeline] Exception in processFrame:" << e.what();
        clearInProgress();
        return result;
    } catch (...) {
        qCritical() << "[NeuralPipeline] Unknown exception in processFrame";
        clearInProgress();
        return result;
    }
}

void NeuralTrackingPipeline::applyCumulativeTransform(
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud) const
{
    if (!m_neuralTrackingEnabled || !cloud) return;
    if (m_cumulativeTransform.isApprox(Eigen::Matrix4f::Identity(), 1e-6f)) return;

    // m_cumulativeTransform = T_cam_from_world; we want world positions for
    // camera points: p_world = T^{-1} * p_cam.
    const Eigen::Matrix4f camToWorld = m_cumulativeTransform.inverse();
    const Eigen::Matrix3f R = camToWorld.block<3,3>(0,0);
    const Eigen::Vector3f t = camToWorld.block<3,1>(0,3);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
    transformed->reserve(cloud->size());
    for (const auto& pt : cloud->points) {
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) continue;
        Eigen::Vector3f pos = R * pt.getVector3fMap() + t;
        pcl::PointXYZRGB tpt;
        tpt.x = pos.x(); tpt.y = pos.y(); tpt.z = pos.z();
        tpt.r = pt.r; tpt.g = pt.g; tpt.b = pt.b;
        transformed->push_back(tpt);
    }
    transformed->width = transformed->size();
    transformed->height = 1;
    transformed->is_dense = false;
    cloud = transformed;
}