tests/test_neural_tracking_guards.cpp - master - klischa/AstraScanner2

tests/test_neural_tracking_guards.cpp
133 строки4 KB
Arena Agent
tracking: harden neural pose runtime validation
17 июн 2026, 19:47
17 июн 2026, 19:4747a6097
#include <gtest/gtest.h>
#include <cstdint>
#include <limits>
#include <cmath>

#include "../src/tracking/NeuralTrackingGuards.h"

namespace {

pcl::PointCloud<pcl::PointXYZRGB>::Ptr makeCloud(std::initializer_list<Eigen::Vector3f> points)
{
    auto cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    for (const auto& p : points) {
        pcl::PointXYZRGB pt;
        pt.x = p.x();
        pt.y = p.y();
        pt.z = p.z();
        pt.r = 255;
        pt.g = 255;
        pt.b = 255;
        cloud->push_back(pt);
    }
    cloud->width = static_cast<std::uint32_t>(cloud->size());
    cloud->height = 1;
    cloud->is_dense = true;
    return cloud;
}

} // namespace

TEST(NeuralTrackingGuardsTest, PoseVectorToMatrixCheckedAcceptsIdentityPose)
{
    const std::vector<float> pose = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
    auto metrics = poseVectorToMatrixChecked(pose);

    ASSERT_TRUE(metrics.has_value());
    EXPECT_NEAR(metrics->quaternionNorm, 1.0f, 1e-6f);
    EXPECT_NEAR(metrics->translationNorm, 0.0f, 1e-6f);
    EXPECT_NEAR(metrics->rotationAngleRad, 0.0f, 1e-6f);
    EXPECT_TRUE(metrics->transform.isApprox(Eigen::Matrix4f::Identity(), 1e-6f));
}

TEST(NeuralTrackingGuardsTest, PoseVectorToMatrixCheckedRejectsNan)
{
    const float nan = std::numeric_limits<float>::quiet_NaN();
    const std::vector<float> pose = {0.0f, 0.0f, 0.0f, nan, 0.0f, 0.0f, 1.0f};
    EXPECT_FALSE(poseVectorToMatrixChecked(pose).has_value());
}

TEST(NeuralTrackingGuardsTest, PoseVectorToMatrixCheckedRejectsZeroQuaternion)
{
    const std::vector<float> pose = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    EXPECT_FALSE(poseVectorToMatrixChecked(pose).has_value());
}

TEST(NeuralTrackingGuardsTest, RotationMatrixValidation)
{
    EXPECT_TRUE(isRotationMatrixValid(Eigen::Matrix3f::Identity()));

    Eigen::Matrix3f scaled = Eigen::Matrix3f::Identity();
    scaled(0, 0) = 2.0f;
    EXPECT_FALSE(isRotationMatrixValid(scaled));
}

TEST(NeuralTrackingGuardsTest, AnalyzeNeuralCloudComputesStats)
{
    auto cloud = makeCloud({
        Eigen::Vector3f(0.0f, 0.0f, 0.0f),
        Eigen::Vector3f(1.0f, 0.0f, 0.0f),
        Eigen::Vector3f(0.0f, 2.0f, 0.0f)
    });

    pcl::PointXYZRGB invalid;
    invalid.x = std::numeric_limits<float>::quiet_NaN();
    invalid.y = 0.0f;
    invalid.z = 0.0f;
    cloud->push_back(invalid);
    cloud->width = static_cast<std::uint32_t>(cloud->size());

    auto stats = analyzeNeuralCloud(cloud);
    ASSERT_TRUE(stats.has_value());
    EXPECT_EQ(stats->totalPoints, 4);
    EXPECT_EQ(stats->finitePoints, 3);
    EXPECT_NEAR(stats->bboxDiagonal, std::sqrt(5.0f), 1e-6f);
    EXPECT_TRUE(stats->centroid.isApprox(Eigen::Vector3f(1.0f / 3.0f, 2.0f / 3.0f, 0.0f), 1e-6f));
}

TEST(NeuralTrackingGuardsTest, AnalyzeNeuralCloudRejectsEmptyCloud)
{
    auto empty = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    EXPECT_FALSE(analyzeNeuralCloud(empty).has_value());
}

TEST(NeuralTrackingGuardsTest, EvaluateAlignmentResidualIsLowForIdentity)
{
    auto cloud = makeCloud({
        Eigen::Vector3f(0.0f, 0.0f, 0.0f),
        Eigen::Vector3f(0.01f, 0.0f, 0.0f),
        Eigen::Vector3f(0.0f, 0.01f, 0.0f),
        Eigen::Vector3f(0.01f, 0.01f, 0.0f)
    });

    const auto metrics = evaluateAlignmentResidual(
        cloud,
        cloud,
        Eigen::Matrix4f::Identity(),
        1,
        0.02f);

    EXPECT_GT(metrics.samplesEvaluated, 0);
    EXPECT_NEAR(metrics.meanResidual, 0.0f, 1e-6f);
    EXPECT_NEAR(metrics.medianResidual, 0.0f, 1e-6f);
    EXPECT_NEAR(metrics.inlierRatio, 1.0f, 1e-6f);
}

TEST(NeuralTrackingGuardsTest, EvaluateAlignmentResidualDetectsBadShift)
{
    auto cloud = makeCloud({
        Eigen::Vector3f(0.0f, 0.0f, 0.0f),
        Eigen::Vector3f(0.01f, 0.0f, 0.0f),
        Eigen::Vector3f(0.0f, 0.01f, 0.0f),
        Eigen::Vector3f(0.01f, 0.01f, 0.0f)
    });

    Eigen::Matrix4f shifted = Eigen::Matrix4f::Identity();
    shifted(0, 3) = 0.10f;

    const auto metrics = evaluateAlignmentResidual(cloud, cloud, shifted, 1, 0.02f);

    EXPECT_GT(metrics.samplesEvaluated, 0);
    EXPECT_GT(metrics.medianResidual, 0.05f);
    EXPECT_LT(metrics.inlierRatio, 0.5f);
}