From 4100056bcf6c8f7e65034a5be1ace363060e580e Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 11 Aug 2026 01:01:55 +0200 Subject: [PATCH 1/2] Add unit tests for MLvxCalib and fix two Eigen aliasing bugs Adds a doctest suite for MLvxCalib (Livox calibration file parsing: GetIdToSnMapping, GetCalibrationFromFile, GetImuSnToUse, CombineIntoCalibration, GetImuIdToUse), compiling the real lidar_odometry_utils.cpp against core_no_gui rather than duplicating the logic. Writing the tests surfaced two self-aliasing Eigen assignments in GetCalibrationFromFile ("COLUMN" order transpose and "inverted" matrix inverse) that silently corrupt the parsed matrix in Release and abort on Eigen's aliasing assertion in Debug. Both fixed with transposeInPlace()/inverse().eval(). Also wires BUILD_TESTING=ON and a ctest step into all four CI workflows (Windows, macOS, Linux, Linux .deb) -- previously none of them actually built or ran the test suite. That in turn exposed a missing include in shared/include/HDMapping/PoseInterpolation.h, fixed here too. Co-Authored-By: Claude Sonnet 5 --- .github/workflows/cmake-linux-deb.yml | 2 +- .github/workflows/cmake-linux.yml | 2 +- .github/workflows/macos.yml | 9 +- .github/workflows/windows.yml | 7 +- CMakeLists.txt | 1 + .../lidar_odometry_utils.cpp | 4 +- .../tests/CMakeLists.txt | 49 +++ .../tests/test_mlvx_calib.cpp | 286 ++++++++++++++++++ shared/include/HDMapping/PoseInterpolation.h | 1 + 9 files changed, 355 insertions(+), 6 deletions(-) create mode 100644 apps/lidar_odometry_step_1/tests/CMakeLists.txt create mode 100644 apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp diff --git a/.github/workflows/cmake-linux-deb.yml b/.github/workflows/cmake-linux-deb.yml index 4a8ff14b..55252554 100644 --- a/.github/workflows/cmake-linux-deb.yml +++ b/.github/workflows/cmake-linux-deb.yml @@ -38,7 +38,7 @@ jobs: - name: Configure CMake # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBUILD_WITH_BUNDLED_FREEGLUT=0 -DBUILD_WITH_BUNDLED_EIGEN=0 -DBUILD_WITH_BUNDLED_LIBLASZIP=0 + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBUILD_WITH_BUNDLED_FREEGLUT=0 -DBUILD_WITH_BUNDLED_EIGEN=0 -DBUILD_WITH_BUNDLED_LIBLASZIP=0 -DBUILD_TESTING=ON - name: Build # Build your program with the given configuration diff --git a/.github/workflows/cmake-linux.yml b/.github/workflows/cmake-linux.yml index d2f0900b..0ce03bd9 100644 --- a/.github/workflows/cmake-linux.yml +++ b/.github/workflows/cmake-linux.yml @@ -41,7 +41,7 @@ jobs: - name: Configure CMake # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBUILD_TESTING=ON - name: Build # Build your program with the given configuration diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index b3f3985e..75c0dc61 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -30,11 +30,18 @@ jobs: cmake -B ${{github.workspace}}/build \ -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} \ -DFREEGLUT_COCOA=ON \ - -DHD_CPU_OPTIMIZATION=AUTO + -DHD_CPU_OPTIMIZATION=AUTO \ + -DBUILD_TESTING=ON - name: Build run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} -j$(sysctl -n hw.ncpu) + - name: Test + working-directory: ${{github.workspace}}/build + # Execute tests defined by the CMake configuration. + # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail + run: ctest -C ${{env.BUILD_TYPE}} --output-on-failure + - name: List built binaries run: | echo "Built binaries:" diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml index 8552a468..bb20014b 100644 --- a/.github/workflows/windows.yml +++ b/.github/workflows/windows.yml @@ -35,12 +35,17 @@ jobs: - name: Configure CMake # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBUILD_TESTING=ON - name: Build # Build your program with the given configuration run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} --target package + - name: Test + working-directory: ${{github.workspace}}/build + # Execute tests defined by the CMake configuration. + # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail + run: ctest -C ${{env.BUILD_TYPE}} --output-on-failure - name: Deploy mandeye package shell: cmd diff --git a/CMakeLists.txt b/CMakeLists.txt index f3af2554..2b6cf89e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -111,6 +111,7 @@ option(BUILD_TESTING "Build HDMapping unit tests" OFF) if(BUILD_TESTING) enable_testing() add_subdirectory(shared/tests) + add_subdirectory(apps/lidar_odometry_step_1/tests) endif() set(CORE_LIBRARIES core) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 149b5364..95c373e0 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -670,12 +670,12 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi std::transform(order.begin(), order.end(), order.begin(), ::toupper); if (order == "COLUMN") - value = value.transpose(); + value.transposeInPlace(); // NOTE: `value = value.transpose()` aliases and corrupts the matrix; must transpose in place. } bool inverted = JsonGetBool(calibrationEntry.value(), "inverted", false); if (inverted) - value = value.inverse(); + value = value.inverse().eval(); // `value = value.inverse()` aliases: Eigen needs the eval() to use a temporary here. Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); diff --git a/apps/lidar_odometry_step_1/tests/CMakeLists.txt b/apps/lidar_odometry_step_1/tests/CMakeLists.txt new file mode 100644 index 00000000..549b9bec --- /dev/null +++ b/apps/lidar_odometry_step_1/tests/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(lidar_odometry_step_1_tests) + +# Unit tests for the MLvxCalib namespace (lidar_odometry_utils.h/.cpp), which +# parses Livox extrinsic/IMU calibration files (.json/.mjc and .sn). Compiles +# the real lidar_odometry_utils.cpp (no GUI code path, WITH_GUI left +# undefined) instead of the whole lidar_odometry_step_1 app, so the include +# dirs/link libraries below are the subset that TU actually needs: laszip and +# TBB because load_point_cloud()/decimate() (compiled into the same TU) use +# them even though the tests never call those functions, core_no_gui for +# Core/ndt.h & Core/hash_utils.h symbols, and vqf/Fusion/unordered_dense/ +# spdlog/UTL for the rest of lidar_odometry_utils.h's includes. Uses doctest, +# same as shared/tests. +add_executable(lidar_odometry_step_1_tests + test_mlvx_calib.cpp + ../lidar_odometry_utils.cpp +) + +target_include_directories(lidar_odometry_step_1_tests PRIVATE + ${THIRDPARTY_DIRECTORY}/doctest + ${REPOSITORY_DIRECTORY}/core/include + ${THIRDPARTY_DIRECTORY} + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/json/include + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion +) + +target_link_libraries(lidar_odometry_step_1_tests PRIVATE + core_no_gui + vqf + Fusion + unordered_dense::unordered_dense + spdlog::spdlog + UTL::include + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS} +) + +if (MSVC) + target_compile_definitions(lidar_odometry_step_1_tests PRIVATE _USE_MATH_DEFINES) + target_compile_options(lidar_odometry_step_1_tests PRIVATE /bigobj) +endif() + +include(CTest) +add_test(NAME lidar_odometry_step_1_tests COMMAND lidar_odometry_step_1_tests) diff --git a/apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp b/apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp new file mode 100644 index 00000000..b0dd8f77 --- /dev/null +++ b/apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp @@ -0,0 +1,286 @@ +#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +#include + +#include "../lidar_odometry_utils.h" + +#include +#include + +namespace +{ + // Each test gets its own file under the OS temp dir, named after the + // running test case, so parallel/leftover runs don't collide. + class TempFile + { + public: + explicit TempFile(const std::string& content) + : m_path( + (std::filesystem::temp_directory_path() + / ("mlvx_calib_test_" + std::to_string(reinterpret_cast(this)) + ".tmp")) + .string()) + { + std::ofstream f(m_path); + f << content; + } + + ~TempFile() + { + std::filesystem::remove(m_path); + } + + const std::string& path() const + { + return m_path; + } + + private: + std::string m_path; + }; + + bool isIdentity(const Eigen::Affine3d& a) + { + return a.matrix().isApprox(Eigen::Matrix4d::Identity(), 1e-12); + } +} // namespace + +// --------------------------------------------------------------------------- +// GetIdToSnMapping: id<->serial-number mapping from the .sn file +// --------------------------------------------------------------------------- + +TEST_CASE("GetIdToSnMapping: nonexistent file returns an empty map") +{ + auto result = MLvxCalib::GetIdToSnMapping("/nonexistent/path/does_not_exist.sn"); + CHECK(result.empty()); +} + +TEST_CASE("GetIdToSnMapping: parses 'id serial_number' lines") +{ + TempFile file("0 47MDL9T0020193\n1 47MDL9S0020300\n"); + auto result = MLvxCalib::GetIdToSnMapping(file.path()); + + REQUIRE(result.size() == 2); + CHECK(result.at(0) == "47MDL9T0020193"); + CHECK(result.at(1) == "47MDL9S0020300"); +} + +TEST_CASE("GetIdToSnMapping: skips malformed lines but keeps parsing the valid ones") +{ + // A line missing the serial-number column fails the `iss >> key >> value` + // parse and is skipped (with a diagnostic to stderr); every other line is + // still parsed independently. + TempFile file("0 47MDL9T0020193\nnotanumber\n1 47MDL9S0020300\n"); + auto result = MLvxCalib::GetIdToSnMapping(file.path()); + + REQUIRE(result.size() == 2); + CHECK(result.at(0) == "47MDL9T0020193"); + CHECK(result.at(1) == "47MDL9S0020300"); +} + +TEST_CASE("GetIdToSnMapping: empty file returns an empty map") +{ + TempFile file(""); + auto result = MLvxCalib::GetIdToSnMapping(file.path()); + CHECK(result.empty()); +} + +// --------------------------------------------------------------------------- +// GetCalibrationFromFile: serial-number -> extrinsic calibration +// --------------------------------------------------------------------------- + +TEST_CASE("GetCalibrationFromFile: nonexistent file returns an empty map") +{ + auto result = MLvxCalib::GetCalibrationFromFile("/nonexistent/path/does_not_exist.json"); + CHECK(result.empty()); +} + +TEST_CASE("GetCalibrationFromFile: invalid JSON returns an empty map") +{ + TempFile file("{ this is not valid json"); + auto result = MLvxCalib::GetCalibrationFromFile(file.path()); + CHECK(result.empty()); +} + +TEST_CASE("GetCalibrationFromFile: missing 'calibration' key returns an empty map") +{ + TempFile file(R"({"imuToUse": "SN1"})"); + auto result = MLvxCalib::GetCalibrationFromFile(file.path()); + CHECK(result.empty()); +} + +TEST_CASE("GetCalibrationFromFile: 'identity' entry yields the identity matrix") +{ + TempFile file(R"({"calibration": {"SN_IDENTITY": {"identity": "true"}}})"); + auto result = MLvxCalib::GetCalibrationFromFile(file.path()); + + REQUIRE(result.count("SN_IDENTITY") == 1); + CHECK(isIdentity(result.at("SN_IDENTITY"))); +} + +TEST_CASE("GetCalibrationFromFile: default (ROW) order reads translation from the last column") +{ + TempFile file(R"({ + "calibration": { + "SN_ROW": { + "data": [1,0,0,1, 0,1,0,2, 0,0,1,3, 0,0,0,1] + } + } + })"); + auto result = MLvxCalib::GetCalibrationFromFile(file.path()); + + REQUIRE(result.count("SN_ROW") == 1); + const Eigen::Vector3d t = result.at("SN_ROW").translation(); + CHECK(t.isApprox(Eigen::Vector3d(1, 2, 3), 1e-12)); +} + +TEST_CASE("GetCalibrationFromFile: 'COLUMN' order transposes the raw data before use") +{ + // Filled row-major first (value(i,j) = data[i*4+j]), then transposed + // because order == COLUMN -- so the *last row* of the raw data ends up + // as the translation column after the transpose. + TempFile file(R"({ + "calibration": { + "SN_COLUMN": { + "order": "COLUMN", + "data": [1,0,0,0, 0,1,0,0, 0,0,1,0, 5,6,7,1] + } + } + })"); + auto result = MLvxCalib::GetCalibrationFromFile(file.path()); + + REQUIRE(result.count("SN_COLUMN") == 1); + const Eigen::Vector3d t = result.at("SN_COLUMN").translation(); + CHECK(t.isApprox(Eigen::Vector3d(5, 6, 7), 1e-12)); +} + +TEST_CASE("GetCalibrationFromFile: 'inverted' flag inverts the parsed matrix") +{ + TempFile file(R"({ + "calibration": { + "SN_INV": { + "inverted": "true", + "data": [1,0,0,2, 0,1,0,0, 0,0,1,0, 0,0,0,1] + } + } + })"); + auto result = MLvxCalib::GetCalibrationFromFile(file.path()); + + REQUIRE(result.count("SN_INV") == 1); + const Eigen::Vector3d t = result.at("SN_INV").translation(); + CHECK(t.isApprox(Eigen::Vector3d(-2, 0, 0), 1e-9)); +} + +TEST_CASE("GetCalibrationFromFile: blacklisted serial numbers are removed from the result") +{ + TempFile file(R"({ + "calibration": { + "SN_KEEP": {"identity": "true"}, + "SN_DROP": {"identity": "true"} + }, + "blacklist": ["SN_DROP"] + })"); + auto result = MLvxCalib::GetCalibrationFromFile(file.path()); + + CHECK(result.count("SN_KEEP") == 1); + CHECK(result.count("SN_DROP") == 0); +} + +// --------------------------------------------------------------------------- +// GetImuSnToUse: serial number of the Livox to use for IMU data +// --------------------------------------------------------------------------- + +TEST_CASE("GetImuSnToUse: nonexistent file returns an empty string") +{ + CHECK(MLvxCalib::GetImuSnToUse("/nonexistent/path/does_not_exist.json").empty()); +} + +TEST_CASE("GetImuSnToUse: invalid JSON returns an empty string") +{ + TempFile file("{ not json"); + CHECK(MLvxCalib::GetImuSnToUse(file.path()).empty()); +} + +TEST_CASE("GetImuSnToUse: missing 'imuToUse' key returns an empty string") +{ + TempFile file(R"({"calibration": {}})"); + CHECK(MLvxCalib::GetImuSnToUse(file.path()).empty()); +} + +TEST_CASE("GetImuSnToUse: non-string 'imuToUse' value returns an empty string") +{ + TempFile file(R"({"imuToUse": 123})"); + CHECK(MLvxCalib::GetImuSnToUse(file.path()).empty()); +} + +TEST_CASE("GetImuSnToUse: returns the serial number when present") +{ + TempFile file(R"({"imuToUse": "47MDL9T0020193"})"); + CHECK(MLvxCalib::GetImuSnToUse(file.path()) == "47MDL9T0020193"); +} + +// --------------------------------------------------------------------------- +// CombineIntoCalibration: (id->sn) + (sn->calibration) -> (id->calibration) +// --------------------------------------------------------------------------- + +TEST_CASE("CombineIntoCalibration: empty calibration map returns an empty result regardless of idToSn") +{ + std::unordered_map idToSn{ { 0, "SN1" } }; + std::unordered_map calibration; + auto result = MLvxCalib::CombineIntoCalibration(idToSn, calibration); + CHECK(result.empty()); +} + +TEST_CASE("CombineIntoCalibration: joins id->sn and sn->calibration by serial number") +{ + std::unordered_map idToSn{ { 0, "SN1" }, { 1, "SN2" } }; + std::unordered_map calibration{ + { "SN1", Eigen::Affine3d(Eigen::Translation3d(1, 0, 0)) }, + { "SN2", Eigen::Affine3d(Eigen::Translation3d(2, 0, 0)) }, + }; + + auto result = MLvxCalib::CombineIntoCalibration(idToSn, calibration); + + REQUIRE(result.size() == 2); + CHECK(result.at(0).translation().isApprox(Eigen::Vector3d(1, 0, 0))); + CHECK(result.at(1).translation().isApprox(Eigen::Vector3d(2, 0, 0))); +} + +TEST_CASE("CombineIntoCalibration: an id whose serial number is absent from calibration throws") +{ + // Documented current behavior: lookup uses std::unordered_map::at(), so a + // sensor id present in the .sn file but missing from the calibration + // JSON is a hard error rather than being silently skipped. + std::unordered_map idToSn{ { 0, "SN_UNKNOWN" } }; + std::unordered_map calibration{ + { "SN_OTHER", Eigen::Affine3d::Identity() }, + }; + + CHECK_THROWS_AS(MLvxCalib::CombineIntoCalibration(idToSn, calibration), std::out_of_range); +} + +// --------------------------------------------------------------------------- +// GetImuIdToUse: sensor id of the Livox to use for IMU data +// --------------------------------------------------------------------------- + +TEST_CASE("GetImuIdToUse: empty idToSn returns 0") +{ + std::unordered_map idToSn; + CHECK(MLvxCalib::GetImuIdToUse(idToSn, "SN1") == 0); +} + +TEST_CASE("GetImuIdToUse: empty snToUse returns 0") +{ + std::unordered_map idToSn{ { 5, "SN1" } }; + CHECK(MLvxCalib::GetImuIdToUse(idToSn, "") == 0); +} + +TEST_CASE("GetImuIdToUse: returns the id matching the requested serial number") +{ + std::unordered_map idToSn{ { 0, "SN1" }, { 5, "SN2" } }; + CHECK(MLvxCalib::GetImuIdToUse(idToSn, "SN2") == 5); +} + +TEST_CASE("GetImuIdToUse: serial number not present in idToSn returns 0") +{ + std::unordered_map idToSn{ { 0, "SN1" } }; + CHECK(MLvxCalib::GetImuIdToUse(idToSn, "SN_UNKNOWN") == 0); +} diff --git a/shared/include/HDMapping/PoseInterpolation.h b/shared/include/HDMapping/PoseInterpolation.h index 019068a3..4718c150 100644 --- a/shared/include/HDMapping/PoseInterpolation.h +++ b/shared/include/HDMapping/PoseInterpolation.h @@ -1,5 +1,6 @@ #pragma once #include +#include #include #include From 652bcd9df6aa7a473e7851994a7720998de29699 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 11 Aug 2026 01:05:31 +0200 Subject: [PATCH 2/2] Make clang format happy Signed-off-by: Michal Pelka --- apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp b/apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp index b0dd8f77..97555817 100644 --- a/apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp +++ b/apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp @@ -14,10 +14,9 @@ namespace { public: explicit TempFile(const std::string& content) - : m_path( - (std::filesystem::temp_directory_path() - / ("mlvx_calib_test_" + std::to_string(reinterpret_cast(this)) + ".tmp")) - .string()) + : m_path((std::filesystem::temp_directory_path() / + ("mlvx_calib_test_" + std::to_string(reinterpret_cast(this)) + ".tmp")) + .string()) { std::ofstream f(m_path); f << content;