tests/test_surfel_coverage_tracker.cpp - master - klischa/AstraScanner2

tests/test_surfel_coverage_tracker.cpp
127 строк4 KB
k k
fix: SurfelCoverageTracker tests need sufficient points for normal estimation
14 июл 2026, 18:48
14 июл 2026, 18:48350f4f8
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <cmath>
#include <cstdint>

#include "../src/tracking/SurfelCoverageTracker.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(SurfelCoverageTrackerTest, ResetClearsSurfels)
{
    SurfelCoverageTracker tracker;
    // Need enough points for normal estimation (radius search)
    auto cloud = makeCloud({
        Eigen::Vector3f(0.0f, 0.0f, 0.1f),
        Eigen::Vector3f(0.005f, 0.0f, 0.1f),
        Eigen::Vector3f(0.0f, 0.005f, 0.1f),
        Eigen::Vector3f(0.01f, 0.0f, 0.1f)
    });
    tracker.integrateFrame(cloud, Eigen::Vector3f::Zero());
    EXPECT_GT(tracker.stats().surfelCount, 0);

    tracker.reset();
    EXPECT_EQ(tracker.stats().surfelCount, 0);
}

TEST(SurfelCoverageTrackerTest, ReusesMatchingSurfels)
{
    SurfelCoverageConfig cfg;
    cfg.downsampleLeaf = 0.001f;
    cfg.maxIntegrationPoints = 100;
    cfg.associationDistance = 0.02f;
    cfg.surfelRadius = 0.01f;

    SurfelCoverageTracker tracker(cfg);
    tracker.setFrameQualityWeights(0, 0, 0, 0);

    auto cloud1 = makeCloud({Eigen::Vector3f(0.0f, 0.0f, 0.1f), Eigen::Vector3f(0.005f, 0.0f, 0.1f)});
    auto cloud2 = makeCloud({Eigen::Vector3f(0.001f, 0.0f, 0.1f), Eigen::Vector3f(0.006f, 0.0f, 0.1f)});

    tracker.integrateFrame(cloud1, Eigen::Vector3f::Zero());
    const int firstCount = tracker.stats().surfelCount;
    tracker.integrateFrame(cloud2, Eigen::Vector3f::Zero());
    const int secondCount = tracker.stats().surfelCount;

    EXPECT_LE(secondCount, firstCount + 1);
}

TEST(SurfelCoverageTrackerTest, FrameQualityWeightAffectsConfidence)
{
    SurfelCoverageConfig cfg;
    cfg.downsampleLeaf = 0.001f;
    cfg.maxIntegrationPoints = 100;
    cfg.associationDistance = 0.03f;
    cfg.surfelRadius = 0.01f;

    auto cloud = makeCloud({
        Eigen::Vector3f(0.0f, 0.0f, 0.1f),
        Eigen::Vector3f(0.005f, 0.0f, 0.1f),
        Eigen::Vector3f(0.01f, 0.0f, 0.1f),
        Eigen::Vector3f(0.0f, 0.005f, 0.1f),
        Eigen::Vector3f(0.01f, 0.005f, 0.1f)
    });

    SurfelCoverageTracker poor(cfg);
    poor.setFrameQualityWeights(2, 2, 2, 2);
    poor.integrateFrame(cloud, Eigen::Vector3f::Zero());
    auto poorStats = poor.stats();

    SurfelCoverageTracker good(cfg);
    good.setFrameQualityWeights(0, 0, 0, 0);
    good.integrateFrame(cloud, Eigen::Vector3f::Zero());
    auto goodStats = good.stats();

    EXPECT_GT(goodStats.meanConfidence, poorStats.meanConfidence);
}

TEST(SurfelCoverageTrackerTest, CoverageCloudUsesConfidenceColors)
{
    SurfelCoverageConfig cfg;
    cfg.downsampleLeaf = 0.001f;
    cfg.maxIntegrationPoints = 100;
    cfg.associationDistance = 0.03f;
    cfg.surfelRadius = 0.01f;

    SurfelCoverageTracker tracker(cfg);
    tracker.setFrameQualityWeights(0, 0, 0, 0);

    auto cloud = makeCloud({
        Eigen::Vector3f(0.0f, 0.0f, 0.1f),
        Eigen::Vector3f(0.005f, 0.0f, 0.1f),
        Eigen::Vector3f(0.01f, 0.0f, 0.1f),
        Eigen::Vector3f(0.0f, 0.005f, 0.1f),
        Eigen::Vector3f(0.01f, 0.005f, 0.1f)
    });
    for (int i = 0; i < 6; ++i) {
        tracker.integrateFrame(cloud, Eigen::Vector3f(0.0f, 0.0f, 0.0f));
    }

    auto colored = tracker.buildCoverageCloud(cloud);
    ASSERT_TRUE(colored);
    ASSERT_FALSE(colored->empty());

    const auto &pt = colored->points.front();
    EXPECT_GE(static_cast<int>(pt.g), static_cast<int>(pt.r));
}