tests/test_registration_service.cpp - master - klischa/AstraScanner2

tests/test_registration_service.cpp
84 строки3 KB
Arena Agent
tracking: confirm neural pose with icp refinement
17 июн 2026, 20:12
17 июн 2026, 20:12ac6f067
#include <gtest/gtest.h>
#include <QCoreApplication>
#include <cstdint>
#include <cmath>

#include "../src/services/RegistrationService.h"

namespace {

pcl::PointCloud<pcl::PointXYZRGB>::Ptr makeGridCloud(int size, const Eigen::Vector3f& offset)
{
    auto cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    for (int x = 0; x < size; ++x) {
        for (int y = 0; y < size; ++y) {
            for (int z = 0; z < size; ++z) {
                pcl::PointXYZRGB pt;
                pt.x = offset.x() + 0.01f * static_cast<float>(x);
                pt.y = offset.y() + 0.01f * static_cast<float>(y);
                pt.z = offset.z() + 0.01f * static_cast<float>(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;
}

class RegistrationServiceTest : public ::testing::Test
{
protected:
    void SetUp() override
    {
        if (!QCoreApplication::instance()) {
            static int argc = 1;
            static char arg0[] = "test_registration_service";
            static char* argv[] = { arg0 };
            new QCoreApplication(argc, argv);
        }
    }
};

} // namespace

TEST_F(RegistrationServiceTest, IcpResultRejectsEmptyInput)
{
    RegistrationService service(nullptr);
    auto empty = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    auto target = makeGridCloud(3, Eigen::Vector3f::Zero());

    const auto result = service.registerPointCloudsICPWithResult(empty, target, 0.05, 20, nullptr);

    EXPECT_FALSE(result.converged);
    EXPECT_TRUE(result.aligned == empty);
    EXPECT_TRUE(std::isinf(result.fitness));
    EXPECT_TRUE(result.transform.isApprox(Eigen::Matrix4f::Identity(), 1e-6f));
}

TEST_F(RegistrationServiceTest, IcpResultUsesInitialGuessAndReturnsTransform)
{
    RegistrationService service(nullptr);

    auto target = makeGridCloud(5, Eigen::Vector3f::Zero());
    auto source = makeGridCloud(5, Eigen::Vector3f(0.02f, -0.01f, 0.015f));

    Eigen::Matrix4f initialGuess = Eigen::Matrix4f::Identity();
    initialGuess(0, 3) = -0.018f;
    initialGuess(1, 3) = 0.009f;
    initialGuess(2, 3) = -0.013f;

    const auto result = service.registerPointCloudsICPWithResult(source, target, 0.05, 40, &initialGuess);

    ASSERT_TRUE(result.converged);
    ASSERT_TRUE(result.aligned);
    EXPECT_TRUE(std::isfinite(result.fitness));
    EXPECT_LT(result.fitness, 0.0025);
    EXPECT_NEAR(result.transform(0, 3), -0.02f, 0.01f);
    EXPECT_NEAR(result.transform(1, 3), 0.01f, 0.01f);
    EXPECT_NEAR(result.transform(2, 3), -0.015f, 0.01f);
}