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

src/tracking/NeuralTrackingGuards.cpp
247 строк7 KB
k k
fix(high): 6 high-severity bugs
13 июл 2026, 23:43
13 июл 2026, 23:43b439ddb
#include "NeuralTrackingGuards.h"

#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <numeric>
#include <pcl/search/kdtree.h>

namespace {

bool isMatrixFinite(const Eigen::Matrix3f& matrix)
{
    return matrix.array().isFinite().all();
}

bool isMatrixFinite(const Eigen::Matrix4f& matrix)
{
    return matrix.array().isFinite().all();
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr makeFiniteCloud(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    auto finite = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    if (!cloud) {
        return finite;
    }

    bool hadNaN = false;
    finite->reserve(cloud->size());
    for (const auto& pt : cloud->points) {
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            hadNaN = true;
            continue;
        }
        finite->push_back(pt);
    }
    finite->width = static_cast<std::uint32_t>(finite->size());
    finite->height = 1;
    finite->is_dense = !hadNaN;
    return finite;
}

} // namespace

std::optional<NeuralCloudStats> analyzeNeuralCloud(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    if (!cloud || cloud->empty()) {
        return std::nullopt;
    }

    NeuralCloudStats stats;
    stats.totalPoints = static_cast<int>(cloud->size());

    bool firstFinite = true;
    Eigen::Vector3f sum = Eigen::Vector3f::Zero();

    for (const auto& pt : cloud->points) {
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            continue;
        }

        const Eigen::Vector3f current(pt.x, pt.y, pt.z);
        if (firstFinite) {
            stats.min = current;
            stats.max = current;
            firstFinite = false;
        } else {
            stats.min = stats.min.cwiseMin(current);
            stats.max = stats.max.cwiseMax(current);
        }

        sum += current;
        ++stats.finitePoints;
    }

    if (stats.finitePoints <= 0) {
        return std::nullopt;
    }

    stats.centroid = sum / static_cast<float>(stats.finitePoints);
    stats.bboxDiagonal = (stats.max - stats.min).norm();

    if (!std::isfinite(stats.bboxDiagonal)) {
        return std::nullopt;
    }

    return stats;
}

bool isRotationMatrixValid(
    const Eigen::Matrix3f& rotation,
    float detTolerance,
    float orthoTolerance)
{
    if (!isMatrixFinite(rotation)) {
        return false;
    }

    const float det = rotation.determinant();
    if (!std::isfinite(det) || std::abs(det - 1.0f) > detTolerance) {
        return false;
    }

    const Eigen::Matrix3f shouldBeIdentity = rotation.transpose() * rotation;
    const float orthoError =
        (shouldBeIdentity - Eigen::Matrix3f::Identity()).cwiseAbs().maxCoeff();
    if (!std::isfinite(orthoError) || orthoError > orthoTolerance) {
        return false;
    }

    return true;
}

std::optional<NeuralPoseMetrics> poseVectorToMatrixChecked(
    const std::vector<float>& poseVec)
{
    if (poseVec.size() < 7) {
        return std::nullopt;
    }

    for (int i = 0; i < 7; ++i) {
        if (!std::isfinite(poseVec[i])) {
            return std::nullopt;
        }
    }

    const Eigen::Vector3f translation(poseVec[0], poseVec[1], poseVec[2]);
    if (!translation.array().isFinite().all()) {
        return std::nullopt;
    }

    const float qx = poseVec[3];
    const float qy = poseVec[4];
    const float qz = poseVec[5];
    const float qw = poseVec[6];
    const float qnorm = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
    if (!std::isfinite(qnorm) || qnorm < 1e-4f) {
        return std::nullopt;
    }

    Eigen::Quaternionf q(qw, qx, qy, qz);
    q.normalize();
    if (!q.coeffs().array().isFinite().all()) {
        return std::nullopt;
    }

    const Eigen::Matrix3f rotation = q.toRotationMatrix();
    if (!isRotationMatrixValid(rotation)) {
        return std::nullopt;
    }

    NeuralPoseMetrics metrics;
    metrics.quaternionNorm = qnorm;
    metrics.translationNorm = translation.norm();
    metrics.rotationAngleRad = 2.0f * std::acos(std::clamp(std::abs(q.w()), 0.0f, 1.0f));
    metrics.transform = Eigen::Matrix4f::Identity();
    metrics.transform.block<3, 3>(0, 0) = rotation;
    metrics.transform.block<3, 1>(0, 3) = translation;

    if (!isMatrixFinite(metrics.transform)) {
        return std::nullopt;
    }

    return metrics;
}

NeuralAlignmentMetrics evaluateAlignmentResidual(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& source,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& target,
    const Eigen::Matrix4f& transform,
    int sampleStride,
    float inlierThresholdMeters)
{
    NeuralAlignmentMetrics metrics;
    if (!source || !target || source->empty() || target->empty()) {
        return metrics;
    }
    if (!isMatrixFinite(transform) || sampleStride <= 0 || inlierThresholdMeters <= 0.0f) {
        return metrics;
    }

    auto finiteTarget = makeFiniteCloud(target);
    if (!finiteTarget || finiteTarget->empty()) {
        return metrics;
    }

    pcl::search::KdTree<pcl::PointXYZRGB> tree;
    tree.setInputCloud(finiteTarget);

    std::vector<float> residuals;
    residuals.reserve(std::max<std::size_t>(1, source->size() / static_cast<std::size_t>(sampleStride)));
    int inliers = 0;

    const Eigen::Matrix3f rotation = transform.block<3, 3>(0, 0);
    const Eigen::Vector3f translation = transform.block<3, 1>(0, 3);

    std::vector<int> indices(1);
    std::vector<float> squaredDistances(1);

    for (std::size_t i = 0; i < source->size(); i += static_cast<std::size_t>(sampleStride)) {
        const auto& pt = source->points[i];
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            continue;
        }

        const Eigen::Vector3f transformed = rotation * pt.getVector3fMap() + translation;
        if (!transformed.array().isFinite().all()) {
            continue;
        }

        pcl::PointXYZRGB query;
        query.x = transformed.x();
        query.y = transformed.y();
        query.z = transformed.z();

        if (tree.nearestKSearch(query, 1, indices, squaredDistances) <= 0) {
            continue;
        }

        const float residual = std::sqrt(std::max(0.0f, squaredDistances[0]));
        if (!std::isfinite(residual)) {
            continue;
        }

        residuals.push_back(residual);
        if (residual <= inlierThresholdMeters) {
            ++inliers;
        }
    }

    if (residuals.empty()) {
        return metrics;
    }

    metrics.samplesEvaluated = static_cast<int>(residuals.size());
    metrics.meanResidual = std::accumulate(residuals.begin(), residuals.end(), 0.0f) /
                           static_cast<float>(residuals.size());

    auto middle = residuals.begin() + residuals.size() / 2;
    std::nth_element(residuals.begin(), middle, residuals.end());
    metrics.medianResidual = *middle;
    metrics.inlierRatio = static_cast<float>(inliers) / static_cast<float>(metrics.samplesEvaluated);
    return metrics;
}