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..97555817 --- /dev/null +++ b/apps/lidar_odometry_step_1/tests/test_mlvx_calib.cpp @@ -0,0 +1,285 @@ +#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