diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..79ce0e0 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,2 @@ +Checks: 'clang-diagnostic-*,clang-analyzer-*,bugprone-*,modernize-*,performance-*,readability-*,concurrency-*,-modernize-use-trailing-return-type,-readability-else-after-return,-readability-math-missing-parentheses,-bugprone-easily-swappable-parameters,-readability-identifier-length,-readability-magic-numbers,-performance-implicit-conversion-in-loop,-bugprone-exception-escape' +CheckOptions: [{key: readability-function-cognitive-complexity.Threshold, value: 35}] diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 8c66428..45ca59b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -7,11 +7,15 @@ on: - main workflow_dispatch: +env: + CI: true + jobs: ci: strategy: matrix: - os: [ubuntu-22.04, ubuntu-22.04-arm, macos-latest, macos-14] + os: [ubuntu-22.04, ubuntu-22.04-arm, macos-latest] + build: [Release, Debug] fail-fast: false runs-on: ${{ matrix.os }} steps: @@ -30,6 +34,10 @@ jobs: version: pcl-cache-v1 execute_install_scripts: true + - name: Set up Homebrew (MacOS) + if: ${{ contains(matrix.os, 'macos') }} + uses: Homebrew/actions/setup-homebrew@master + - name: Cache Homebrew downloads (macOS) if: ${{ contains(matrix.os, 'macos') }} uses: actions/cache@v4 @@ -39,10 +47,6 @@ jobs: restore-keys: | homebrew-cache-${{ runner.os }}- - - name: Set up Homebrew (MacOS) - if: ${{ contains(matrix.os, 'macos') }} - uses: Homebrew/actions/setup-homebrew@master - - name: Brew update (MacOS) if: ${{ contains(matrix.os, 'macos') }} run: brew update @@ -112,15 +116,16 @@ jobs: # Build all (Cross-platform) - name: Build all - run: make OPT=Release + run: make OPT=${{ matrix.build }} # Run tests (Cross-platform) - name: Run tests - run: make test_ply OPT=Release + run: make test_ply OPT=${{ matrix.build }} # Run benchmarks (Cross-platform) - name: Run benchmarks - run: make bench OPT=Release + if: matrix.build == 'Release' + run: make bench OPT=${{ matrix.build }} # Test install (Cross-platform) - name: Test install diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml index c160cea..7e545d5 100644 --- a/.github/workflows/lint.yaml +++ b/.github/workflows/lint.yaml @@ -1,4 +1,4 @@ -name: Code Style +name: Lint on: pull_request: @@ -6,10 +6,81 @@ on: branches: - main -jobs: - build: - runs-on: ubuntu-latest +env: + CI: true +jobs: + lint: + runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v2 + - name: Checkout + uses: actions/checkout@v4 + - uses: DoozyX/clang-format-lint-action@v0.18.1 + + - name: Add clang apt repository + run: | + wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add - + sudo add-apt-repository 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-19 main' + + - name: apt update + run: sudo apt-get update + + - name: Cache APT packages (Ubuntu) + uses: awalsh128/cache-apt-pkgs-action@latest + with: + packages: libpcl-dev libeigen3-dev libsdl2-dev mpich clang-tidy + version: lint-cache-v1 + execute_install_scripts: true + + # Install Eigen (Cross-platform) + - name: Install Eigen + run: | + sudo apt-get install -y libeigen3-dev + + # Install SDL2 (Cross-platform) + - name: Install SDL2 + run: | + sudo apt-get install -y libsdl2-dev + + - name: Install PCL + run: | + sudo apt-get install mpich + sudo apt-get install libpcl-dev + + # Install sdl-wrapper (Cross-platform) + - name: Install sdl-wrapper + run: | + git clone https://github.com/cornellev/sdl-wrapper.git + cd sdl-wrapper + sudo make install + + # Install libcmdapp2 (Cross-platform) + - name: Install libcmdapp2 + run: | + git clone https://github.com/cornellev/libcmdapp2.git + cd libcmdapp2 + sudo make install + + # Install libconfig (Cross-platform) + - name: Install libconfig + run: | + git clone https://github.com/cornellev/config + cd config + sudo make install + + # Install simple-test (Cross-platform) + - name: Install simple-test + run: | + git clone https://github.com/cornellev/simple-test + cd simple-test + sudo make install + + - name: Install clang-tidy + run: | + sudo apt-get install clang-tidy-19 + sudo update-alternatives --install /usr/bin/clang-tidy clang-tidy /usr/bin/clang-tidy-19 100 + clang-tidy --version + + - name: Run clang-tidy + run: make tidy diff --git a/CMakeLists.txt b/CMakeLists.txt index feb2b9c..3ad8807 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,28 +1,83 @@ # Copyright (C) 2024-2025 Cornell Electric Vehicles. # SPDX-License-Identifier: MIT -cmake_minimum_required(VERSION 3.14) -project(cev_icp LANGUAGES C CXX) +cmake_minimum_required(VERSION 3.15) +project(CevIcp LANGUAGES C CXX) set(PROJECT_VERSION 1.3.0) +string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +option(USE_SANITIZERS "Compile with ASan and UBSan" OFF) + +if(DEFINED ENV{CI}) + set(USE_SANITIZERS ON) + set(CMAKE_COMPILE_WARNING_AS_ERROR ON) + + # for clang thinking unused functions in headers are problematic + # and also complaining about float-store + if (CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang") + add_compile_options(-Wno-error=unused-function -Wno-error=ignored-optimization-argument) + endif() +endif() # Add compile options add_compile_options(-Wall -Wextra -g) +if(USE_SANITIZERS) + message(STATUS "Using address and UB sanitizers") + add_compile_options(-fsanitize=address,undefined) + add_link_options(-fsanitize=address,undefined) +endif() -option(LIB_ONLY "Build library only" OFF) +include_directories(/usr/local/include) +link_directories(/usr/local/lib) # Find dependencies find_package(Eigen3 REQUIRED) -if(NOT LIB_ONLY) - find_package(SDL2 REQUIRED) - find_package(PCL 1.12 REQUIRED COMPONENTS common io) +find_package(SDL2) +find_package(PCL) + +find_library(LIBCONFIG config) +find_library(LIBCMDAPP cmdapp) +find_library(LIBSDLWRAPPER sdlwrapper) + +if(${SDL2_FOUND}) + message(STATUS "SDL2 found.") +else() + message(WARNING "SDL2 not found.") +endif() +if(${PCL_FOUND}) + message(STATUS "PCL found. PCL-dependent targets can be built.") add_definitions(${PCL_DEFINITIONS}) - include_directories(${PCL_INCLUDE_DIRS} /usr/local/include) - link_directories(${PCL_LIBRARY_DIRS} /usr/local/lib) + include_directories(${PCL_INCLUDE_DIRS}) + link_directories(${PCL_LIBRARY_DIRS}) +else() + message(WARNING "PCL not found. PCL-dependent targets can NOT be built.") +endif() + +if(LIBCONFIG) + message(STATUS "libconfig found.") +else() + message(WARNING "libconfig not found.") + set(LIBCONFIG config) +endif() + +if(LIBCMDAPP) + message(STATUS "libcmdapp found.") +else() + message(WARNING "libcmdapp not found.") + set(LIBCMDAPP cmdapp) +endif() + +if(LIBSDLWRAPPER) + message(STATUS "libsdlwrapper found.") +else() + message(WARNING "libsdlwrapper not found.") + set(LIBSDLWRAPPER sdlwrapper) endif() # Library target @@ -36,16 +91,16 @@ set(LIB_SOURCES lib/icp/impl/vanilla_3d.cpp lib/icp/impl/trimmed_3d.cpp ) -add_library(cevicp STATIC ${LIB_SOURCES}) -target_include_directories(cevicp - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/include - PUBLIC - ${EIGEN3_INCLUDE_DIRS} +add_library(${PROJECT_NAME_LOWER} ${LIB_SOURCES}) +add_library(CEV::${PROJECT_NAME} ALIAS ${PROJECT_NAME_LOWER}) +target_include_directories(${PROJECT_NAME_LOWER} PUBLIC + $ + $ ) +target_link_libraries(${PROJECT_NAME_LOWER} Eigen3::Eigen) -if(NOT LIB_ONLY) - # Main executable +# Main executable +if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME}) set(MAIN_SOURCES vis/main.cpp vis/lidar_view.cpp @@ -54,11 +109,12 @@ if(NOT LIB_ONLY) ) add_executable(main ${MAIN_SOURCES}) target_link_libraries(main - cevicp + ${PROJECT_NAME_LOWER} + Eigen3::Eigen SDL2::SDL2 - libcmdapp.a - libsdlwrapper.a - libconfig.a + ${LIBCMDAPP} + ${LIBSDLWRAPPER} + ${LIBCONFIG} ) target_include_directories(main PRIVATE @@ -70,24 +126,20 @@ if(NOT LIB_ONLY) /usr/local/include/config /usr/local/include/sdlwrapper ) - + set(TEST_SOURCES tests/test.cpp) add_executable(test_suite ${TEST_SOURCES}) - target_link_libraries(test_suite - cevicp - ) + target_link_libraries(test_suite ${PROJECT_NAME_LOWER} Eigen3::Eigen) target_include_directories(test_suite PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include /usr/local/include/simple_test ) target_compile_definitions(test_suite PRIVATE TEST) - + set(TEST_SOURCES_3D tests/test3d.cpp) add_executable(test_suite_3d ${TEST_SOURCES_3D}) - target_link_libraries(test_suite_3d - cevicp - ) + target_link_libraries(test_suite_3d ${PROJECT_NAME_LOWER} Eigen3::Eigen) target_include_directories(test_suite_3d PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include @@ -99,8 +151,9 @@ if(NOT LIB_ONLY) add_executable(test_suite_ply ${TEST_SOURCES_PLY}) target_link_libraries(test_suite_ply - cevicp + ${PROJECT_NAME_LOWER} ${PCL_LIBRARIES} + Eigen3::Eigen ) target_include_directories(test_suite_ply PRIVATE @@ -114,8 +167,8 @@ if(NOT LIB_ONLY) set(BENCH_SOURCES bench/bench.cpp common/parse_scan.cpp) add_executable(bench_suite ${BENCH_SOURCES}) target_link_libraries(bench_suite - cevicp - libconfig.a + ${PROJECT_NAME_LOWER} + ${LIBCONFIG} ) target_include_directories(bench_suite PRIVATE @@ -124,63 +177,61 @@ if(NOT LIB_ONLY) ) endif() -# Installation -if(NOT LIB_ONLY) - include(CMakePackageConfigHelpers) - set(INSTALL_CONFIGDIR ${CMAKE_INSTALL_LIBDIR}/cmake/cevicp) +# Include +include(CMakePackageConfigHelpers) +set(INSTALL_CONFIGDIR ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME_LOWER}) - # Generate the CevIcpConfig.cmake file - configure_package_config_file( - ${CMAKE_CURRENT_SOURCE_DIR}/CevIcpConfig.cmake.in - ${CMAKE_CURRENT_BINARY_DIR}/CevIcpConfig.cmake - INSTALL_DESTINATION ${INSTALL_CONFIGDIR} - ) +# Generate the CevIcpConfig.cmake file +configure_package_config_file( + ${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}Config.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake + INSTALL_DESTINATION ${INSTALL_CONFIGDIR} +) - # Generate the CevIcpConfigVersion.cmake file - write_basic_package_version_file( - CevIcpConfigVersion.cmake - VERSION ${PROJECT_VERSION} - COMPATIBILITY AnyNewerVersion - ) +# Generate the CevIcpConfigVersion.cmake file +write_basic_package_version_file( + ${PROJECT_NAME}ConfigVersion.cmake + VERSION ${PROJECT_VERSION} + COMPATIBILITY AnyNewerVersion +) - export(TARGETS cevicp NAMESPACE CevIcp FILE CevIcpTargets.cmake) +export(TARGETS ${PROJECT_NAME_LOWER} NAMESPACE CEV FILE ${PROJECT_NAME}Targets.cmake) - export(PACKAGE CevIcp) +export(PACKAGE ${PROJECT_NAME}) - # Install the targets and export them - install(EXPORT CevIcpTargets - NAMESPACE CevIcp:: - DESTINATION ${INSTALL_CONFIGDIR} - ) +# Install the targets and export them +install(EXPORT ${PROJECT_NAME}Targets + NAMESPACE CEV:: + DESTINATION ${INSTALL_CONFIGDIR} +) - install(TARGETS cevicp - EXPORT CevIcpTargets - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - ) +install(TARGETS ${PROJECT_NAME_LOWER} + EXPORT ${PROJECT_NAME}Targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) - # Install headers - install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/cev_icp - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" - ) +# Install headers +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME_LOWER} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" +) - # Install configuration files - install(FILES - ${CMAKE_CURRENT_BINARY_DIR}/CevIcpConfig.cmake - ${CMAKE_CURRENT_BINARY_DIR}/CevIcpConfigVersion.cmake - DESTINATION ${INSTALL_CONFIGDIR} - ) +# Install configuration files +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake + DESTINATION ${INSTALL_CONFIGDIR} +) - # Per https://gitlab.kitware.com/cmake/community/-/wikis/FAQ#can-i-do-make-uninstall-with-cmake - # uninstall target - if(NOT TARGET uninstall) - configure_file( - "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in" - "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" - IMMEDIATE @ONLY) +# Per https://gitlab.kitware.com/cmake/community/-/wikis/FAQ#can-i-do-make-uninstall-with-cmake +# uninstall target +if(NOT TARGET uninstall) + configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" + IMMEDIATE @ONLY) - add_custom_target(uninstall - COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) - endif() + add_custom_target(uninstall + COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) endif() diff --git a/Makefile b/Makefile index 8f53ee9..f1bdcad 100644 --- a/Makefile +++ b/Makefile @@ -75,6 +75,15 @@ view: $(MAIN_TARGET) bench: $(BENCH_TARGET) DYLD_LIBRARY_PATH=/usr/local/lib ./$(BUILD_DIR)/$(BENCH_TARGET) +.PHONY: tidy +tidy: configure # needed for compile commands database + @if [ -z "$(CI)" ]; then \ + find bench common include lib vis -iname '*.h' -o -iname '*.cpp' \ + | xargs clang-tidy -p ./$(BUILD_DIR); \ + else \ + find bench common include lib vis -iname '*.h' -o -iname '*.cpp' \ + | xargs clang-tidy -p ./$(BUILD_DIR) -warnings-as-errors='*'; \ + fi .PHONY: clean clean: configure diff --git a/bench/bench.cpp b/bench/bench.cpp index 2e45b34..c4879ef 100644 --- a/bench/bench.cpp +++ b/bench/bench.cpp @@ -5,7 +5,7 @@ */ #include -#include +#include #include #include #include @@ -38,9 +38,11 @@ struct BenchmarkParams { uint32_t scan_id; }; -BenchmarkResult run_benchmark(BenchmarkParams params) { +BenchmarkResult run_benchmark(const BenchmarkParams& params) { + // NOLINTNEXTLINE(bugprone-unchecked-optional-access) auto icp = icp::ICP2::from_method(params.method, icp::Config()).value(); icp::ICPDriver driver(std::move(icp)); + driver.set_max_iterations(params.max_iter); driver.set_transform_tolerance(params.angle_tol, params.trans_tol); @@ -68,22 +70,22 @@ BenchmarkResult run_benchmark(BenchmarkParams params) { result.max_cost = final_costs.back(); result.median_cost = final_costs[final_costs.size() / 2]; result.mean_cost = std::accumulate(final_costs.begin(), final_costs.end(), 0.0) - / final_costs.size(); + / static_cast(final_costs.size()); result.min_iterations = iteration_counts.front(); result.max_iterations = iteration_counts.back(); result.median_iterations = iteration_counts[iteration_counts.size() / 2]; result.mean_iterations = std::accumulate(iteration_counts.begin(), iteration_counts.end(), 0.0) - / iteration_counts.size(); + / static_cast(iteration_counts.size()); - result.average_time_per_invocation = diff.count() / params.num_invoc; + result.average_time_per_invocation = diff.count() / static_cast(params.num_invoc); result.average_time_per_iteration = diff.count() / (std::accumulate(iteration_counts.begin(), iteration_counts.end(), 0.0)); return result; } -void print_benchmark_params(BenchmarkParams params) { +void print_benchmark_params(const BenchmarkParams& params) { std::cout << "* Method: " << params.method << "\n" << "* Number of invocations: " << params.num_invoc << "\n" << "* Angle tolerance: " << params.angle_tol << " rad\n" @@ -122,7 +124,9 @@ int main() { std::cout << "=======================================\n"; for (uint32_t scan_id = 1; scan_id <= scans; scan_id++) { - if (scan_id != 1) std::cout << "---------------------------------------\n"; + if (scan_id != 1) { + std::cout << "---------------------------------------\n"; + } params.method = method; params.scan_id = scan_id; diff --git a/common/parse_scan.cpp b/common/parse_scan.cpp index c0b0250..f9e879c 100644 --- a/common/parse_scan.cpp +++ b/common/parse_scan.cpp @@ -15,7 +15,7 @@ #include #include "icp/geo.h" -icp::PointCloud2 parse_lidar_scan(std::string path) { +icp::PointCloud2 parse_lidar_scan(const std::string& path) { std::ifstream in(path); if (!in.is_open()) { throw std::runtime_error("failed to read lidar scan: failed to open file"); @@ -43,7 +43,7 @@ icp::PointCloud2 parse_lidar_scan(std::string path) { icp::PointCloud2 result(2, temp.size()); for (size_t i = 0; i < temp.size(); i++) { - result.col(i) = temp[i]; + result.col(static_cast(i)) = temp[i]; } return result; diff --git a/common/parse_scan.h b/common/parse_scan.h index c27d79d..4ff7095 100644 --- a/common/parse_scan.h +++ b/common/parse_scan.h @@ -6,4 +6,4 @@ #include "icp/geo.h" -icp::PointCloud2 parse_lidar_scan(std::string path); +icp::PointCloud2 parse_lidar_scan(const std::string& path); diff --git a/include/algo/kdtree.h b/include/algo/kdtree.h index 7f9713f..32743f9 100644 --- a/include/algo/kdtree.h +++ b/include/algo/kdtree.h @@ -63,11 +63,10 @@ namespace icp { * @brief Internal node structure of the k-d tree. */ struct Node { - int idx; // index to the original point - std::unique_ptr next[2]; // pointers to the child nodes using unique_ptr - int axis; // dimension's axis - - Node(): idx(-1), axis(-1) {} + int idx = -1; // index to the original point + std::array, 2> + next; // pointers to the child nodes using unique_ptr + int axis = -1; // dimension's axis }; /** @@ -78,12 +77,14 @@ namespace icp { * @return Index of the nearest neighbor in the original input point vector. */ int search(const PointT& query, double* minDist = nullptr) const { - int guess; + int guess = -1; double _minDist = std::numeric_limits::max(); search_recursive(query, root_.get(), &guess, &_minDist); - if (minDist) *minDist = _minDist; + if (minDist != nullptr) { + *minDist = _minDist; + } return guess; } @@ -93,7 +94,9 @@ namespace icp { */ std::unique_ptr build_recursive(int* indices, int npoints, int depth) { - if (npoints <= 0) return nullptr; + if (npoints <= 0) { + return nullptr; + } const int axis = depth % dim_; const int mid = (npoints - 1) / 2; @@ -117,7 +120,9 @@ namespace icp { */ static double distance(const PointT& p, const PointT& q) { double dist = 0; - for (int i = 0; i < p.size(); i++) dist += (p[i] - q[i]) * (p[i] - q[i]); + for (int i = 0; i < p.size(); i++) { + dist += (p[i] - q[i]) * (p[i] - q[i]); + } return std::sqrt(dist); } @@ -126,7 +131,9 @@ namespace icp { */ void search_recursive(const PointT& query, const Node* node, int* guess, double* minDist) const { - if (node == nullptr) return; + if (node == nullptr) { + return; + } const PointT& train = points_[node->idx]; const double dist = distance(query, train); @@ -141,7 +148,9 @@ namespace icp { search_recursive(query, node->next[dir].get(), guess, minDist); const double diff = std::fabs(query[axis] - train[axis]); - if (diff < *minDist) search_recursive(query, node->next[!dir].get(), guess, minDist); + if (diff < *minDist) { + search_recursive(query, node->next[dir == 0].get(), guess, minDist); + } } std::unique_ptr root_; // root node of the k-d tree diff --git a/include/icp/config.h b/include/icp/config.h index 5b11337..9db2759 100644 --- a/include/icp/config.h +++ b/include/icp/config.h @@ -12,18 +12,18 @@ namespace icp { public: /** Constructs an empty configuration. */ - Config() {} + Config() = default; /** Associates `key` with an integer, double, or string `value`. */ template - void set(std::string key, T value) { + void set(const std::string& key, T value) { params[key] = value; } /** Retrieves the integer, double, or string value associated with * `key`. */ template - T get(std::string key, T otherwise) const { + T get(const std::string& key, T otherwise) const { if (params.find(key) == params.end()) { return otherwise; } else { diff --git a/include/icp/dim.h b/include/icp/dim.h index ca2024d..cf67ae8 100644 --- a/include/icp/dim.h +++ b/include/icp/dim.h @@ -1,3 +1,5 @@ +#include + namespace icp { - enum Dimension { TwoD = 2, ThreeD = 3 }; + enum Dimension : std::uint8_t { TwoD = 2, ThreeD = 3 }; } diff --git a/include/icp/driver.h b/include/icp/driver.h index c64a140..fc2a10b 100644 --- a/include/icp/driver.h +++ b/include/icp/driver.h @@ -43,15 +43,15 @@ namespace icp { * @brief Runs ICP to convergence based on the termination conditions set. If no conditions * are set, ICP will run indefinitely. This is rarely desirable. * - * @param a The source point cloud. - * @param b The destination point cloud. - * @param t The initial guess for the transformation. + * @param source The source point cloud. + * @param target The target point cloud. + * @param guess The initial guess for the transformation. * @return ConvergenceState */ - ConvergenceState converge(const PointCloud& a, const PointCloud& b, - RBTransform t) { + ConvergenceState converge(const PointCloud& source, const PointCloud& target, + RBTransform guess) { start_time_ = std::chrono::steady_clock::now(); - icp_->begin(a, b, t); + icp_->begin(source, target, guess); ConvergenceState state; state.iteration_count = 0; diff --git a/include/icp/icp.h b/include/icp/icp.h index a155f59..139e52b 100644 --- a/include/icp/icp.h +++ b/include/icp/icp.h @@ -8,7 +8,6 @@ #pragma once #include -#include #include #include #include @@ -61,10 +60,10 @@ namespace icp { /** A matching between `point` and `pair` at (arbitrary) cost `cost`. */ struct Match { /** An index into the source point cloud. */ - size_t point; + Eigen::Index point; /** An index into the destination point cloud. */ - size_t pair; + Eigen::Index pair; /** The (arbitrary) cost of the pair. */ double cost; @@ -116,15 +115,15 @@ namespace icp { virtual ~ICP() = default; - /** Begins the ICP process for point clouds `a` and `b` with an initial - * guess for the transform `t`.*/ - void begin(const PointCloud& a, const PointCloud& b, const RBTransform& t) { + /** Begins the ICP process for point clouds `source` and `target` with an initial + * guess for the transform `guess`.*/ + void begin(const PointCloud& source, const PointCloud& target, const RBTransform& guess) { // Initial transform guess - this->transform = t; + this->transform = guess; // Copy in point clouds - this->a = a; - this->b = b; + this->a = source; + this->b = target; // Ensure arrays are the right size matches.resize(this->a.cols()); @@ -147,7 +146,7 @@ namespace icp { * \par Efficiency: * `O(a.size())` where `a` is the source point cloud. */ - double calculate_cost() const { + [[nodiscard]] double calculate_cost() const { double sum_squares = 0.0; for (auto& match: matches) { sum_squares += match.cost; @@ -156,7 +155,7 @@ namespace icp { } /** The current transform. */ - RBTransform current_transform() const { + [[nodiscard]] RBTransform current_transform() const { return transform; } @@ -165,7 +164,7 @@ namespace icp { * * @return A reference to the matching. Invalidates if `begin` or `iterate` are called. */ - const std::vector& get_matches() const { + [[nodiscard]] const std::vector& get_matches() const { return matches; } }; diff --git a/include/icp/impl/feature_aware.h b/include/icp/impl/feature_aware.h index e062c90..6b37adf 100644 --- a/include/icp/impl/feature_aware.h +++ b/include/icp/impl/feature_aware.h @@ -5,7 +5,6 @@ #pragma once #include -#include #include "icp/icp.h" #include "icp/config.h" @@ -14,7 +13,7 @@ namespace icp { public: FeatureAware(double overlap_rate, double feature_weight, int symmetric_neighbors); FeatureAware(const Config& config); - ~FeatureAware(); + ~FeatureAware() override; void setup() override; void iterate() override; @@ -22,7 +21,7 @@ namespace icp { private: void compute_matches(); - Eigen::MatrixXd compute_features(const PointCloud& points); + [[nodiscard]] Eigen::MatrixXd compute_features(const PointCloud& points) const; /** * @brief Computes a matrix M where M_ij is the distance between the i-th vector (column) of @@ -39,8 +38,8 @@ namespace icp { const Eigen::Matrix& first, const Eigen::Matrix& second) { Eigen::MatrixXd dists(first.cols(), second.cols()); - for (ptrdiff_t i = 0; i < first.cols(); i++) { - for (ptrdiff_t j = 0; j < second.cols(); j++) { + for (Eigen::Index i = 0; i < first.cols(); i++) { + for (Eigen::Index j = 0; j < second.cols(); j++) { dists(i, j) = (first.col(i) - second.col(j)).norm(); } } diff --git a/include/icp/impl/trimmed.h b/include/icp/impl/trimmed.h index c675651..22de190 100644 --- a/include/icp/impl/trimmed.h +++ b/include/icp/impl/trimmed.h @@ -13,7 +13,7 @@ namespace icp { public: Trimmed(double overlap_rate); Trimmed(const Config& config); - ~Trimmed(); + ~Trimmed() override; void setup() override; void iterate() override; diff --git a/include/icp/impl/trimmed_3d.h b/include/icp/impl/trimmed_3d.h index 2ae1e2c..a610c7c 100644 --- a/include/icp/impl/trimmed_3d.h +++ b/include/icp/impl/trimmed_3d.h @@ -16,7 +16,7 @@ namespace icp { public: Trimmed3d(const Config& config); Trimmed3d(); - ~Trimmed3d(); + ~Trimmed3d() override; protected: void setup() override; @@ -29,10 +29,10 @@ namespace icp { double max_distance; // max distance for trimming std::unique_ptr> kdtree_; - Neighbors nearest_neighbor(const PointCloud& src, const PointCloud& dst); - float dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb); - RBTransform best_fit_transform(const PointCloud& A, const PointCloud& B); - void calculate_cost(const std::vector& distances); + Neighbors nearest_neighbor(const PointCloud& src); + static double dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb); + static RBTransform best_fit_transform(const PointCloud& A, const PointCloud& B); + void calculate_cost(const std::vector& distances); }; } // namespace icp \ No newline at end of file diff --git a/include/icp/impl/vanilla.h b/include/icp/impl/vanilla.h index 76847c7..a9d7405 100644 --- a/include/icp/impl/vanilla.h +++ b/include/icp/impl/vanilla.h @@ -15,7 +15,7 @@ namespace icp { public: Vanilla(); Vanilla(const Config& config); - ~Vanilla(); + ~Vanilla() override; void setup() override; void iterate() override; diff --git a/include/icp/impl/vanilla_3d.h b/include/icp/impl/vanilla_3d.h index 22514d5..9ed5417 100644 --- a/include/icp/impl/vanilla_3d.h +++ b/include/icp/impl/vanilla_3d.h @@ -16,7 +16,7 @@ namespace icp { public: Vanilla3d(const Config& config); Vanilla3d(); - ~Vanilla3d(); + ~Vanilla3d() override; protected: void setup() override; @@ -28,10 +28,10 @@ namespace icp { double current_cost_; std::unique_ptr> kdtree_; - Neighbors nearest_neighbor(const PointCloud& src, const PointCloud& dst); - float dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb); - RBTransform best_fit_transform(const PointCloud& A, const PointCloud& B); - void calculate_cost(const std::vector& distances); + Neighbors nearest_neighbor(const PointCloud& src); + static double dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb); + static RBTransform best_fit_transform(const PointCloud& A, const PointCloud& B); + void calculate_cost(const std::vector& distances); }; } // namespace icp \ No newline at end of file diff --git a/include/icp/neighbor.h b/include/icp/neighbor.h index 84e48ba..fea4363 100644 --- a/include/icp/neighbor.h +++ b/include/icp/neighbor.h @@ -3,11 +3,12 @@ * SPDX-License-Identifier: MIT */ +#include #include namespace icp { struct Neighbors { - std::vector distances; - std::vector indices; + std::vector distances; + std::vector indices; }; } diff --git a/lib/icp/driver.cpp b/lib/icp/driver.cpp deleted file mode 100644 index e281946..0000000 --- a/lib/icp/driver.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/** - * @copyright Copyright (C) 2025 Cornell Electric Vehicles. - * SPDX-License-Identifier: MIT - */ - -#include "icp/driver.h" -#include - -namespace icp { - ICPDriver::ICPDriver(std::unique_ptr icp) { - icp_ = std::move(icp); - } - - ICPDriver::ConvergenceState ICPDriver::converge(const std::vector& a, - const std::vector& b, RBTransform t) { - start_time_ = std::chrono::steady_clock::now(); - icp_->begin(a, b, t); - - ConvergenceState state{}; - state.iteration_count = 0; - state.cost = icp_->calculate_cost(); - state.transform = icp_->current_transform(); - - std::optional last_state{}; - - while (!should_terminate(state, last_state)) { - icp_->iterate(); - last_state = state; - state.iteration_count++; - state.cost = icp_->calculate_cost(); - state.transform = icp_->current_transform(); - } - - return state; - } - - bool ICPDriver::should_terminate(ConvergenceState current_state, - std::optional last_state) { - // absolute conditions based only on current state - if (min_iterations_ && current_state.iteration_count < min_iterations_.value()) { - return false; - } - - if (max_iterations_ && current_state.iteration_count >= max_iterations_.value()) { - return true; - } - - if (stop_cost_ && current_state.cost < stop_cost_.value()) { - return true; - } - - if (time_limit_) { - auto current_time = std::chrono::steady_clock::now(); - if (current_time - start_time_ > time_limit_.value()) { - return true; - } - } - - // end if we don't have a last state - if (!last_state) { - return false; - } - - // relative conditions based on progress - double delta_cost = current_state.cost - last_state.value().cost; - if (absolute_cost_tolerance_ && std::abs(delta_cost) < absolute_cost_tolerance_.value()) { - return true; - } - - double relative_cost_change = std::abs(delta_cost) / current_state.cost; - if (relative_cost_tolerance_ && relative_cost_change < relative_cost_tolerance_.value()) { - return true; - } - - if (angle_tolerance_rad_ && translation_tolerance_) { - icp::Vector prev_rot_vector = last_state.value().transform.rotation * icp::Vector(1, 0); - icp::Vector current_rot_vector = current_state.transform.rotation * icp::Vector(1, 0); - double dot = prev_rot_vector.dot(current_rot_vector); - double angle = std::acos(std::clamp(dot, -1.0, 1.0)); - - auto translation = current_state.transform.translation - - last_state.value().transform.translation; - - if (angle < angle_tolerance_rad_.value() - && translation.norm() < translation_tolerance_.value()) { - return true; - } - } - - return false; - } - - void ICPDriver::set_min_iterations(uint64_t min_iterations) { - assert(!max_iterations_ || min_iterations <= max_iterations_.value()); - min_iterations_ = min_iterations; - } - - void ICPDriver::set_max_iterations(uint64_t max_iterations) { - assert(!min_iterations_ || max_iterations >= min_iterations_.value()); - max_iterations_ = max_iterations; - } - - void ICPDriver::set_stop_cost(double stop_cost) { - stop_cost_ = stop_cost; - } - - void ICPDriver::set_relative_cost_tolerance(double relative_cost_tolerance) { - relative_cost_tolerance_ = relative_cost_tolerance; - } - - void ICPDriver::set_absolute_cost_tolerance(double absolute_cost_tolerance) { - absolute_cost_tolerance_ = absolute_cost_tolerance; - } - - void ICPDriver::set_transform_tolerance(double angle_tolerance, double translation_tolerance) { - angle_tolerance_rad_ = angle_tolerance; - translation_tolerance_ = translation_tolerance; - } - - void ICPDriver::set_time_limit(std::chrono::duration time_limit) { - time_limit_ = time_limit; - } -} diff --git a/lib/icp/geo.cpp b/lib/icp/geo.cpp deleted file mode 100644 index 1c59d8d..0000000 --- a/lib/icp/geo.cpp +++ /dev/null @@ -1,18 +0,0 @@ -/** - * @author Ethan Uppal - * @copyright Copyright (C) 2024 Ethan Uppal. - * SPDX-License-Identifier: MIT - */ - -#include -#include "icp/geo.h" - -namespace icp { - Vector get_centroid(const std::vector& points) { - Vector sum = Vector::Zero(); - for (const Vector& point: points) { - sum += point; - } - return sum / points.size(); - } -} diff --git a/lib/icp/impl/feature_aware.cpp b/lib/icp/impl/feature_aware.cpp index 35cc793..1d8515b 100644 --- a/lib/icp/impl/feature_aware.cpp +++ b/lib/icp/impl/feature_aware.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #include "icp/geo.h" /* #name Feature-Aware */ @@ -19,9 +18,13 @@ distance criteria, matches them based on a local "feature vector." #include "icp/impl/feature_aware.h" namespace icp { + constexpr double DEFAULT_OVERLAP_RATE = 0.9; + constexpr double DEFAULT_FEATURE_WEIGHT = 0.7; + constexpr int DEFAULT_SYMMETRIC_NEIGHBORS = 10; + constexpr double MIN_NORM_THRESHOLD = 1e-6; + FeatureAware::FeatureAware(double overlap_rate, double feature_weight, int symmetric_neighbors) - : ICP(), - overlap_rate(overlap_rate), + : overlap_rate(overlap_rate), symmetric_neighbors(symmetric_neighbors), feature_weight(feature_weight), neighbor_weight(1 - feature_weight) {} @@ -33,11 +36,11 @@ namespace icp { /* #conf "symmetric_neighbors" An `int` with default value `10`. Decides how many neighbors to * use on each side of a point when constructing the feature vector. */ FeatureAware::FeatureAware(const Config& config) - : FeatureAware(config.get("overlap_rate", 0.9), - config.get("feature_weight", 0.7), - config.get("symmetric_neighbors", 10)) {} + : FeatureAware(config.get("overlap_rate", DEFAULT_OVERLAP_RATE), + config.get("feature_weight", DEFAULT_FEATURE_WEIGHT), + config.get("symmetric_neighbors", DEFAULT_SYMMETRIC_NEIGHBORS)) {} - FeatureAware::~FeatureAware() {} + FeatureAware::~FeatureAware() = default; void FeatureAware::setup() { a_current = transform * a; @@ -49,7 +52,7 @@ namespace icp { normalized_feature_dists = compute_norm_dists(a_features, b_features); double max = normalized_feature_dists.maxCoeff(); - if (max > 1e-6) { + if (max > MIN_NORM_THRESHOLD) { normalized_feature_dists /= max; } @@ -84,13 +87,13 @@ namespace icp { */ std::sort(matches.begin(), matches.end(), [](const auto& a, const auto& b) { return a.cost < b.cost; }); - ptrdiff_t new_n = static_cast(overlap_rate * a.cols()); - new_n = std::max(new_n, 1); // TODO: bad for scans with 0 points + auto new_n = static_cast(overlap_rate * static_cast(a.cols())); + new_n = std::max(new_n, 1); // TODO: bad for scans with 0 points // yeah, i know this is inefficient. we'll get back to it later. PointCloud trimmed_a_current(2, new_n); PointCloud trimmed_b(2, new_n); - for (ptrdiff_t i = 0; i < new_n; i++) { + for (Eigen::Index i = 0; i < new_n; i++) { trimmed_a_current.col(i) = a_current.col(matches[i].point); trimmed_b.col(i) = b.col(matches[i].pair); } @@ -125,14 +128,14 @@ namespace icp { void FeatureAware::compute_matches() { Eigen::MatrixXd normalized_dists = compute_norm_dists<2>(a_current, b); double max = normalized_dists.maxCoeff(); - if (max > 1e-6) { + if (max > MIN_NORM_THRESHOLD) { normalized_dists /= max; } - for (ptrdiff_t i = 0; i < a.cols(); i++) { + for (Eigen::Index i = 0; i < a.cols(); i++) { matches[i].point = i; matches[i].cost = std::numeric_limits::infinity(); - for (ptrdiff_t j = 0; j < b.cols(); j++) { + for (Eigen::Index j = 0; j < b.cols(); j++) { double dist = normalized_dists(i, j); double feature_dist = normalized_feature_dists(i, j); double cost = neighbor_weight * dist + feature_weight * feature_dist; @@ -145,27 +148,27 @@ namespace icp { } } - Eigen::MatrixXd FeatureAware::compute_features(const PointCloud& points) { + Eigen::MatrixXd FeatureAware::compute_features(const PointCloud& points) const { Eigen::MatrixXd features(2 * symmetric_neighbors, points.cols()); features.setZero(); - Vector cm = get_centroid(points); + Vector centroid = get_centroid(points); - for (ptrdiff_t i = 0; i < points.cols(); i++) { - Vector p = points.col(i); - double cm_dist_p = (p - cm).norm(); + for (Eigen::Index i = 0; i < points.cols(); i++) { + Vector point1 = points.col(i); + double cm_dist_p = (point1 - centroid).norm(); - ptrdiff_t lower = std::max(0, i - symmetric_neighbors); - for (ptrdiff_t j = lower; j < i; j++) { - Vector q = points.col(j); - double cm_dist_q = (q - cm).norm(); + Eigen::Index lower = std::max(0, i - symmetric_neighbors); + for (Eigen::Index j = lower; j < i; j++) { + Vector point2 = points.col(j); + double cm_dist_q = (point2 - centroid).norm(); features(j - lower, i) = cm_dist_q - cm_dist_p; } - ptrdiff_t upper = std::min(points.cols() - 1, i + symmetric_neighbors); - for (ptrdiff_t j = i + 1; j <= upper; j++) { - Vector q = points.col(j); - double cm_dist_q = (q - cm).norm(); + Eigen::Index upper = std::min(points.cols() - 1, i + symmetric_neighbors); + for (Eigen::Index j = i + 1; j <= upper; j++) { + Vector point2 = points.col(j); + double cm_dist_q = (point2 - centroid).norm(); features(j - i - 1 + symmetric_neighbors, i) = cm_dist_q - cm_dist_p; } } diff --git a/lib/icp/impl/trimmed.cpp b/lib/icp/impl/trimmed.cpp index 94620aa..7856cad 100644 --- a/lib/icp/impl/trimmed.cpp +++ b/lib/icp/impl/trimmed.cpp @@ -6,7 +6,6 @@ */ #include -#include #include #include #include @@ -26,10 +25,8 @@ namespace icp { /* #conf "overlap_rate" A `double` between `0.0` and `1.0` for * the overlap rate. The default is `1.0`. */ - Trimmed::Trimmed(const Config& config) - : ICP(), overlap_rate(config.get("overlap_rate", 0.9)) {} - - Trimmed::~Trimmed() {} + Trimmed::Trimmed(const Config& config): overlap_rate(config.get("overlap_rate", 0.9)) {} + Trimmed::~Trimmed() = default; void Trimmed::setup() { a_current = transform * a; @@ -39,7 +36,7 @@ namespace icp { } void Trimmed::iterate() { - const ptrdiff_t n = a.cols(); + const Eigen::Index n = a.cols(); a_current = transform * a; @@ -57,13 +54,13 @@ namespace icp { */ std::sort(matches.begin(), matches.end(), [](const auto& a, const auto& b) { return a.cost < b.cost; }); - ptrdiff_t new_n = static_cast(overlap_rate * n); - new_n = std::max(new_n, 1); // TODO: bad for scans with 0 points + auto new_n = static_cast(overlap_rate * static_cast(n)); + new_n = std::max(new_n, 1); // TODO: bad for scans with 0 points // yeah, i know this is inefficient. we'll get back to it later. PointCloud trimmed_a_current(2, new_n); PointCloud trimmed_b(2, new_n); - for (ptrdiff_t i = 0; i < new_n; i++) { + for (Eigen::Index i = 0; i < new_n; i++) { trimmed_a_current.col(i) = a_current.col(matches[i].point); trimmed_b.col(i) = b.col(matches[i].pair); } @@ -96,10 +93,10 @@ namespace icp { } void Trimmed::compute_matches() { - for (ptrdiff_t i = 0; i < a.cols(); i++) { + for (Eigen::Index i = 0; i < a.cols(); i++) { matches[i].point = i; matches[i].cost = std::numeric_limits::infinity(); - for (ptrdiff_t j = 0; j < b.cols(); j++) { + for (Eigen::Index j = 0; j < b.cols(); j++) { // Point-to-point matching double dist_ij = (b.col(j) - a_current.col(i)).squaredNorm(); diff --git a/lib/icp/impl/trimmed_3d.cpp b/lib/icp/impl/trimmed_3d.cpp index f3ef4af..d5c87dd 100644 --- a/lib/icp/impl/trimmed_3d.cpp +++ b/lib/icp/impl/trimmed_3d.cpp @@ -4,7 +4,6 @@ */ #include -#include #include #include #include @@ -15,6 +14,7 @@ #include #include "icp/impl/trimmed_3d.h" +#include #include "algo/kdtree.h" #include "icp/geo.h" @@ -22,19 +22,18 @@ namespace icp { Trimmed3d::Trimmed3d([[maybe_unused]] const Config& config) - : ICP(), - c(3, 0), + : c(3, 0), current_cost_(std::numeric_limits::max()), max_distance(config.get("max_distance", 1.0)) {} - Trimmed3d::Trimmed3d(): ICP(), c(3, 0), current_cost_(std::numeric_limits::max()) {} - Trimmed3d::~Trimmed3d() {} + Trimmed3d::Trimmed3d(): c(3, 0), current_cost_(std::numeric_limits::max()) {} + Trimmed3d::~Trimmed3d() = default; // Euclidean distance between two points - float Trimmed3d::dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb) { + double Trimmed3d::dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb) { return (pta - ptb).norm(); } - Neighbors Trimmed3d::nearest_neighbor(const PointCloud& src, const PointCloud& dst) { + Neighbors Trimmed3d::nearest_neighbor(const PointCloud& src) { Neighbors neigh; neigh.distances.resize(src.cols()); neigh.indices.resize(src.cols()); @@ -80,7 +79,7 @@ namespace icp { current_cost_ = std::numeric_limits::max(); std::vector dst_vec(b.cols()); - for (ptrdiff_t i = 0; i < b.cols(); ++i) { + for (Eigen::Index i = 0; i < b.cols(); ++i) { dst_vec[i] = b.col(i); } @@ -89,24 +88,24 @@ namespace icp { void Trimmed3d::iterate() { // Reorder target point set based on nearest neighbor - Neighbors neighbor = nearest_neighbor(c, b); + Neighbors neighbor = nearest_neighbor(c); PointCloud dst_reordered(3, a.cols()); // Assuming PointCloud is a 3xN matrix std::vector src_valid; // Get the valid points from source and distance according to max_distance std::vector dst_valid; - for (ptrdiff_t i = 0; i < a.cols(); ++i) { + for (Eigen::Index i = 0; i < a.cols(); ++i) { if (neighbor.distances[i] <= max_distance) { - src_valid.push_back(c.col(i)); - dst_valid.push_back(b.col(neighbor.indices[i])); + src_valid.emplace_back(c.col(i)); + dst_valid.emplace_back(b.col(neighbor.indices[i])); } } Eigen::MatrixXd AA(3, src_valid.size()); Eigen::MatrixXd BB(3, dst_valid.size()); for (size_t i = 0; i < src_valid.size(); ++i) { - AA.col(i) = src_valid[i]; - BB.col(i) = dst_valid[i]; + AA.col(static_cast(i)) = src_valid[i]; + BB.col(static_cast(i)) = dst_valid[i]; } RBTransform T = best_fit_transform(AA, BB); @@ -116,13 +115,13 @@ namespace icp { calculate_cost(neighbor.distances); } - void Trimmed3d::calculate_cost(const std::vector& distances) { + void Trimmed3d::calculate_cost(const std::vector& distances) { if (distances.empty()) { current_cost_ = std::numeric_limits::max(); return; } double sum = std::accumulate(distances.begin(), distances.end(), 0.0); - current_cost_ = sum / distances.size(); + current_cost_ = sum / static_cast(distances.size()); } } diff --git a/lib/icp/impl/vanilla.cpp b/lib/icp/impl/vanilla.cpp index fb5866b..e5d6205 100644 --- a/lib/icp/impl/vanilla.cpp +++ b/lib/icp/impl/vanilla.cpp @@ -5,7 +5,6 @@ * SPDX-License-Identifier: MIT */ #include -#include #include #include #include @@ -15,9 +14,9 @@ #include "icp/impl/vanilla.h" namespace icp { - Vanilla::Vanilla([[maybe_unused]] const Config& config): ICP() {} - Vanilla::Vanilla(): ICP() {} - Vanilla::~Vanilla() {} + Vanilla::Vanilla([[maybe_unused]] const Config& config) {} + Vanilla::Vanilla() = default; + Vanilla::~Vanilla() = default; void Vanilla::setup() { a_current = transform * a; @@ -37,13 +36,13 @@ namespace icp { compute_matches(); Vector matched_b_cm = Vector::Zero(); - for (size_t i = 0; i < matches.size(); i++) { - matched_b_cm += b.col(matches[i].pair); + for (auto& match: matches) { + matched_b_cm += b.col(match.pair); } - matched_b_cm /= matches.size(); + matched_b_cm /= static_cast(matches.size()); Eigen::Matrix2d N = Eigen::Matrix2d::Zero(); - for (ptrdiff_t i = 0; i < a.cols(); i++) { + for (Eigen::Index i = 0; i < a.cols(); i++) { N += (a_current.col(i) - a_current_cm) * (b.col(matches[i].pair) - matched_b_cm).transpose(); } @@ -70,8 +69,9 @@ namespace icp { if (a.cols() == 0 || b.cols() == 0) { return; } + std::vector b_vec(b.cols()); - for (ptrdiff_t i = 0; i < b.cols(); i++) { + for (Eigen::Index i = 0; i < b.cols(); i++) { b_vec[i] = b.col(i); } icp::KdTree kdtree(b_vec, 2); diff --git a/lib/icp/impl/vanilla_3d.cpp b/lib/icp/impl/vanilla_3d.cpp index be2be41..6349681 100644 --- a/lib/icp/impl/vanilla_3d.cpp +++ b/lib/icp/impl/vanilla_3d.cpp @@ -3,7 +3,6 @@ * SPDX-License-Identifier: MIT */ -#include #include #include #include @@ -19,16 +18,16 @@ namespace icp { Vanilla3d::Vanilla3d([[maybe_unused]] const Config& config) - : ICP(), c(3, 0), current_cost_(std::numeric_limits::max()) {} - Vanilla3d::Vanilla3d(): ICP(), c(3, 0), current_cost_(std::numeric_limits::max()) {} - Vanilla3d::~Vanilla3d() {} + : c(3, 0), current_cost_(std::numeric_limits::max()) {} + Vanilla3d::Vanilla3d(): c(3, 0), current_cost_(std::numeric_limits::max()) {} + Vanilla3d::~Vanilla3d() = default; // Euclidean distance between two points - float Vanilla3d::dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb) { + double Vanilla3d::dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb) { return (pta - ptb).norm(); } - Neighbors Vanilla3d::nearest_neighbor(const PointCloud& src, const PointCloud& dst) { + Neighbors Vanilla3d::nearest_neighbor(const PointCloud& src) { Neighbors neigh; neigh.distances.resize(src.cols()); neigh.indices.resize(src.cols()); @@ -74,7 +73,7 @@ namespace icp { current_cost_ = std::numeric_limits::max(); std::vector dst_vec(b.cols()); - for (ptrdiff_t i = 0; i < b.cols(); ++i) { + for (Eigen::Index i = 0; i < b.cols(); ++i) { dst_vec[i] = b.col(i); } @@ -83,9 +82,9 @@ namespace icp { void Vanilla3d::iterate() { // Reorder target point set based on nearest neighbor - Neighbors neighbor = nearest_neighbor(c, b); + Neighbors neighbor = nearest_neighbor(c); PointCloud dst_reordered(3, a.cols()); // Assuming PointCloud is a 3xN matrix - for (ptrdiff_t i = 0; i < a.cols(); i++) { + for (Eigen::Index i = 0; i < a.cols(); i++) { dst_reordered.col(i) = b.col(neighbor.indices[i]); } RBTransform T = best_fit_transform(c, dst_reordered); @@ -96,13 +95,13 @@ namespace icp { calculate_cost(neighbor.distances); } - void Vanilla3d::calculate_cost(const std::vector& distances) { + void Vanilla3d::calculate_cost(const std::vector& distances) { if (distances.empty()) { current_cost_ = std::numeric_limits::max(); return; } double sum = std::accumulate(distances.begin(), distances.end(), 0.0); - current_cost_ = sum / distances.size(); + current_cost_ = sum / static_cast(distances.size()); } } diff --git a/tests/test.cpp b/tests/test.cpp index 8f4e9a5..36079b1 100644 --- a/tests/test.cpp +++ b/tests/test.cpp @@ -18,8 +18,9 @@ extern "C" { #include } -#define TRANS_EPS 0.5 // Translation tolerance in units -#define RAD_EPS ((double)(0.01)) // Rotation tolerance in radians +constexpr double TRANS_EPS = 0.5; // Translation tolerance (units) +constexpr double RAD_EPS = 0.01; // Rotation tolerance (radians) + // #define assert_translation_eps(expected, real, eps) \ do { \ assert_true(std::abs((real).x() - (expected).x()) < (eps)); \ @@ -35,13 +36,17 @@ extern "C" { #define assert_rotation(expected, real) assert_rotation_eps(expected, real, RAD_EPS) -void test_kdtree(void) { +void test_kdtree() { using Vector = Eigen::Vector3d; std::vector points; + points.reserve(1000); + std::mt19937 rng(123); std::uniform_real_distribution dist(-100.0, 100.0); - for (int i = 0; i < 1000; ++i) points.emplace_back(dist(rng), dist(rng), dist(rng)); + for (int i = 0; i < 1000; ++i) { + points.emplace_back(dist(rng), dist(rng), dist(rng)); + } icp::KdTree kdtree(points, 3); @@ -67,16 +72,16 @@ void test_kdtree(void) { if (kdtree_idx != brute_idx) { std::cerr << "Mismatch at point " << i << ": " - << "kdtree_idx = " << kdtree_idx << ", brute_idx = " << brute_idx - << std::endl; + << "kdtree_idx = " << kdtree_idx << ", brute_idx = " << brute_idx << "\n"; assert(false && "KDTree search mismatch brute force!"); } } - std::cout << "All KDTree tests passed!" << std::endl; + std::cout << "All KDTree tests passed!" << "\n"; } void test_icp_generic(const std::string& method, const icp::Config& config) { + // NOLINTNEXTLINE(bugprone-unchecked-optional-access) std::unique_ptr icp = icp::ICP2::from_method(method, config).value(); icp::ICPDriver driver(std::move(icp)); driver.set_max_iterations(100); @@ -200,7 +205,7 @@ void test_icp_generic(const std::string& method, const icp::Config& config) { icp::PointCloud2 b = transform * a; - for (ptrdiff_t i = 0; i < b.cols(); i++) { + for (Eigen::Index i = 0; i < b.cols(); i++) { b.col(i) += Eigen::Vector2d(noise_dist(generator), noise_dist(generator)); } diff --git a/tests/test3d.cpp b/tests/test3d.cpp index 1e62f73..94ef69c 100644 --- a/tests/test3d.cpp +++ b/tests/test3d.cpp @@ -14,8 +14,8 @@ extern "C" { #include } -#define TRANS_EPS 0.5 // Translation tolerance in units -#define RAD_EPS ((double)(0.01)) // Rotation tolerance in radians +constexpr double TRANS_EPS = 0.5; // Translation tolerance (units) +constexpr double RAD_EPS = 0.01; // Rotation tolerance (radians) #define assert_translation_eps(expected, real, eps) \ do { \ @@ -79,8 +79,8 @@ void test_icp_3d(const icp::Config& config) { auto result = driver.converge(a, b, icp::RBTransform3::Identity()); // Use RBTransform3 Eigen::Vector3d expected_t = (Eigen::Matrix3d::Identity() - rotation_matrix) * center; - std::cout << "expected translation: " << expected_t << std::endl; - std::cout << "result translation: " << result.transform.translation() << std::endl; + std::cout << "expected translation: " << expected_t << "\n"; + std::cout << "result translation: " << result.transform.translation() << "\n"; assert_translation(expected_t, result.transform.translation()); assert_rotation(rotation_matrix, result.transform.rotation()); @@ -118,8 +118,8 @@ void test_icp_3d(const icp::Config& config) { auto result = driver.converge(a, b, icp::RBTransform3::Identity()); // Use RBTransform3 Eigen::Vector3d expected_t = (Eigen::Matrix3d::Identity() - rotation_matrix) * center; - std::cout << "expected translation: " << expected_t << std::endl; - std::cout << "result translation: " << result.transform.translation() << std::endl; + std::cout << "expected translation: " << expected_t << "\n"; + std::cout << "result translation: " << result.transform.translation() << "\n"; assert_translation(expected_t, result.transform.translation()); assert_rotation(rotation_matrix, result.transform.rotation()); @@ -186,7 +186,7 @@ void test_icp_3d(const icp::Config& config) { icp::PointCloud3 b_transformed = transform * a; // Apply transform first // Add noise separately - for (ptrdiff_t i = 0; i < a.cols(); ++i) { // Iterate through columns + for (Eigen::Index i = 0; i < a.cols(); ++i) { // Iterate through columns Eigen::Vector3d noisy_point = b_transformed.col(i); noisy_point.x() += noise_dist(generator); noisy_point.y() += noise_dist(generator); diff --git a/tests/test_ply.cpp b/tests/test_ply.cpp index 10c013b..82e53bc 100644 --- a/tests/test_ply.cpp +++ b/tests/test_ply.cpp @@ -16,13 +16,11 @@ #include "icp/geo.h" #include "icp/icp.h" #include "icp/driver.h" -#include "icp/impl/vanilla_3d.h" #include "icp/impl/trimmed_3d.h" // Algorithm parameters -#define BURN_IN 0 // Minimum required iterations -#define TRANS_EPS 0.0001 // Translation tolerance (units) -#define RAD_EPS ((double)(0.0001)) // Rotation tolerance (radians) +constexpr double TRANS_EPS = 0.0001; // Translation tolerance (units) +constexpr double RAD_EPS = 0.0001; // Rotation tolerance (radians) /** * @brief Load a point cloud from PLY file @@ -41,7 +39,7 @@ icp::PointCloud3 loadPointCloud(const std::string& path) { for (size_t i = 0; i < cloud->points.size(); i++) { auto point = cloud->points[i]; - points.col(i) = Eigen::Vector3d(point.x, point.y, point.z); + points.col(static_cast(i)) = Eigen::Vector3d(point.x, point.y, point.z); } return points; @@ -103,14 +101,13 @@ int main(int argc, char* argv[]) { std::unique_ptr icp = std::make_unique(config); icp::ICPDriver driver(std::move(icp)); - driver.set_min_iterations(BURN_IN); driver.set_max_iterations(100); driver.set_transform_tolerance(RAD_EPS, TRANS_EPS); // Load point clouds icp::PointCloud3 source_points = loadPointCloud(path_a); icp::PointCloud3 target_points = loadPointCloud(path_b); - std::cout << "Starting ICP..." << std::endl; + std::cout << "Starting ICP..." << "\n"; // Run ICP auto result = driver.converge(source_points, target_points, icp::RBTransform3::Identity()); @@ -119,14 +116,14 @@ int main(int argc, char* argv[]) { saveTransformedPointCloud(source_points, result.transform, output_path); // Print results - std::cout << "\n===== ICP Results =====" << std::endl; - std::cout << " Rotation: \n" << result.transform.rotation() << std::endl; - std::cout << " Translation: " << result.transform.translation().transpose() << std::endl; - std::cout << " Iterations: " << result.iteration_count << std::endl; + std::cout << "\n===== ICP Results =====" << "\n"; + std::cout << " Rotation: \n" << result.transform.rotation() << "\n"; + std::cout << " Translation: " << result.transform.translation().transpose() << "\n"; + std::cout << " Iterations: " << result.iteration_count << "\n"; - std::cout << "ICP completed successfully" << std::endl; + std::cout << "ICP completed successfully" << "\n"; } catch (const std::exception& e) { - std::cerr << "Error: " << e.what() << std::endl; + std::cerr << "Error: " << e.what() << "\n"; return 1; } diff --git a/vis/lidar_view.cpp b/vis/lidar_view.cpp index 5702ea7..7cee48f 100644 --- a/vis/lidar_view.cpp +++ b/vis/lidar_view.cpp @@ -6,7 +6,6 @@ #include #include -#include #include #include #include @@ -17,9 +16,9 @@ #include "icp/geo.h" #include "view_config.h" -#define CIRCLE_RADIUS 3 +constexpr int CIRCLE_RADIUS = 3; -LidarView::LidarView(icp::PointCloud2 source, icp::PointCloud2 destination, +LidarView::LidarView(const icp::PointCloud2& source, const icp::PointCloud2& destination, std::unique_ptr icp) : source(source), destination(destination), @@ -72,24 +71,24 @@ void LidarView::draw_matches(SDL_Renderer* renderer) { })->cost; // the size of available points in "a" changed - for (ptrdiff_t i = 0; i < source.cols(); i++) { + for (Eigen::Index i = 0; i < source.cols(); i++) { const auto& source_point = source.col(matches[i].point); const auto& destination_point = destination.col(matches[i].pair); auto transformed_source = icp->current_transform() * source_point; - SDL_SetRenderDrawColor(renderer, 0, 255 - (uint8_t)(matches[i].cost / max_cost * 255), 0, - SDL_ALPHA_OPAQUE); + SDL_SetRenderDrawColor(renderer, 0, + 255 - static_cast(matches[i].cost / max_cost * 255), 0, SDL_ALPHA_OPAQUE); // current transform SDL_RenderDrawLine(renderer, - static_cast(view_config::view_scale * transformed_source[0]) - + view_config::x_displace, - static_cast(view_config::view_scale * transformed_source[1]) - + view_config::y_displace, - static_cast(view_config::view_scale * destination_point[0]) - + view_config::x_displace, - static_cast(view_config::view_scale * destination_point[1]) - + view_config::y_displace); + static_cast(view_config::view_scale * transformed_source[0] + + view_config::x_displace), + static_cast(view_config::view_scale * transformed_source[1] + + view_config::y_displace), + static_cast(view_config::view_scale * destination_point[0] + + view_config::x_displace), + static_cast(view_config::view_scale * destination_point[1] + + view_config::y_displace)); } } @@ -104,15 +103,19 @@ void LidarView::draw(SDL_Renderer* renderer, [[maybe_unused]] const SDL_Rect* fr SDL_SetRenderDrawColor(renderer, 0, 0, 255, SDL_ALPHA_OPAQUE); for (const icp::Vector2& point: destination.colwise()) { - SDL_DrawCircle(renderer, view_config::view_scale * point[0] + view_config::x_displace, - view_config::view_scale * point[1] + view_config::y_displace, CIRCLE_RADIUS); + SDL_DrawCircle(renderer, + static_cast(view_config::view_scale * point.x() + view_config::x_displace), + static_cast(view_config::view_scale * point.y() + view_config::y_displace), + CIRCLE_RADIUS); } SDL_SetRenderDrawColor(renderer, 255, 0, 0, SDL_ALPHA_OPAQUE); for (const icp::Vector2& point: source.colwise()) { icp::Vector2 result = icp->current_transform() * point; - SDL_DrawCircle(renderer, view_config::view_scale * result[0] + view_config::x_displace, - view_config::view_scale * result[1] + view_config::y_displace, CIRCLE_RADIUS); + SDL_DrawCircle(renderer, + static_cast(view_config::view_scale * result[0] + view_config::x_displace), + static_cast(view_config::view_scale * result[1] + view_config::y_displace), + CIRCLE_RADIUS); } // Draw a line connecting the transformed source point to the destination point (in green) @@ -120,13 +123,15 @@ void LidarView::draw(SDL_Renderer* renderer, [[maybe_unused]] const SDL_Rect* fr icp::Vector2 a_cm = icp->current_transform() * icp::get_centroid(source); SDL_SetRenderDrawColor(renderer, 255, 0, 0, SDL_ALPHA_OPAQUE); - SDL_DrawCircle(renderer, view_config::view_scale * a_cm.x() + view_config::x_displace, - view_config::view_scale * a_cm.y() + view_config::y_displace, 20); + SDL_DrawCircle(renderer, + static_cast(view_config::view_scale * a_cm.x() + view_config::x_displace), + static_cast(view_config::view_scale * a_cm.y() + view_config::y_displace), 20); icp::Vector2 b_cm = icp::get_centroid(destination); SDL_SetRenderDrawColor(renderer, 0, 0, 255, SDL_ALPHA_OPAQUE); - SDL_DrawCircle(renderer, view_config::view_scale * b_cm.x() + view_config::x_displace, - view_config::view_scale * b_cm.y() + view_config::y_displace, 20); + SDL_DrawCircle(renderer, + static_cast(view_config::view_scale * b_cm.x() + view_config::x_displace), + static_cast(view_config::view_scale * b_cm.y() + view_config::y_displace), 20); if (is_iterating) { step(); diff --git a/vis/lidar_view.h b/vis/lidar_view.h index 3eb63bc..b353e2e 100644 --- a/vis/lidar_view.h +++ b/vis/lidar_view.h @@ -26,7 +26,7 @@ class LidarView final : public View { public: /** Constructs a new lidar view visualizing ICP (by method `method`) on * the given instance (`source` and `destination`). */ - LidarView(icp::PointCloud2 source, icp::PointCloud2 destination, + LidarView(const icp::PointCloud2& source, const icp::PointCloud2& destination, std::unique_ptr icp); void on_event(const SDL_Event& event) override; diff --git a/vis/main.cpp b/vis/main.cpp index ecb02fb..39cf422 100644 --- a/vis/main.cpp +++ b/vis/main.cpp @@ -7,7 +7,6 @@ #include #include #include -#include "icp/geo.h" extern "C" { #include #include @@ -18,6 +17,13 @@ extern "C" { #include "parse_scan.h" #include "icp/icp.h" +#define ASSERT_OPT(result) \ + do { \ + if ((result) == nullptr) { \ + return 1; \ + } \ + } while (0); + void set_config_param(const char* var, const char* data, [[maybe_unused]] void* user_data) { if (strcmp(var, "x_displace") == 0) { view_config::x_displace = std::stod(data); @@ -32,28 +38,25 @@ void set_config_param(const char* var, const char* data, [[maybe_unused]] void* } } -void parse_config(const char* path) { +bool parse_config(const char* path) { FILE* file = fopen(path, "r"); - if (!file) { + if (file == nullptr) { perror("parse_config: fopen"); - std::exit(1); + fclose(file); + return false; } if (conf_parse_file(file, set_config_param, nullptr) != 0) { perror("parse_config: conf_parse_file"); - std::exit(1); + fclose(file); + return false; } fclose(file); + return true; } -void assert_opt(bool* opt_result) { - if (!opt_result) { - std::exit(1); - } -} - -void launch_gui(LidarView* view, std::string visualized = "LiDAR scans") { +void launch_gui(LidarView* view, const std::string& visualized = "LiDAR scans") { Window window("Scan Matching", view_config::window_width, view_config::window_height); window.attach_view(view); @@ -86,34 +89,40 @@ int main(int argc, const char** argv) { ca_synopsis("-S FILE -D FILE [-l]"); ca_synopsis("-b METHOD [-l]"); - bool* enable_log = NULL; - bool* read_scan_files = NULL; - bool* basic_mode = NULL; // for gbody people - const char* f_src = NULL; - const char* f_dst = NULL; + bool* enable_log = nullptr; + bool* read_scan_files = nullptr; + bool* basic_mode = nullptr; // for gbody people + const char* f_src = nullptr; + const char* f_dst = nullptr; const char* config_file = "view.conf"; const char* method = "vanilla"; - assert_opt(read_scan_files = ca_opt('S', "src", ".FILE&D", &f_src, + // NOLINTBEGIN(bugprone-assignment-in-if-condition) + ASSERT_OPT(read_scan_files = ca_opt('S', "src", ".FILE&D", &f_src, "source scan (pass with -D)")); - assert_opt(ca_opt('D', "dst", ".FILE&S", &f_dst, "destination scan (pass with -S)")); - assert_opt(ca_opt('c', "config", ".FILE", &config_file, + ASSERT_OPT(ca_opt('D', "dst", ".FILE&S", &f_dst, "destination scan (pass with -S)")); + ASSERT_OPT(ca_opt('c', "config", ".FILE", &config_file, "selects a configuration file (default: view.conf)")); - assert_opt(ca_opt('m', "method", ".METHOD", &method, "selects an ICP method")); - assert_opt(basic_mode = ca_long_opt("basic-mode", "", NULL, "uses a ligher gui background")); - assert_opt(enable_log = ca_opt('l', "log", "", NULL, "enables debug logging")); - assert_opt(ca_opt('h', "help", " icp = std::move(icp_opt.value()); - // icp::PointCloud2 a(2, 5); - // a.col(0) << 0, 0; - // a.col(1) << 100, 0; - // a.col(2) << 100, 100; - // a.col(3) << -20, 50; - // a.col(4) << 100, 120; - // icp::PointCloud2 b(2, 5); - // b.col(0) << 18.9132, 9.87803; - // b.col(1) << 105.527, 60.6843; - // b.col(2) << 57.3474, 146.636; - // b.col(3) << -22.8571, 43.3349; - // b.col(4) << 46.8032, 164.386; - - // LidarView* view = new LidarView(a, b, std::move(icp)); - // launch_gui(view, "test"); - // return 0; - - // icp::PointCloud2 a(2, 2); - // a.col(0) << 0, 0; - // a.col(1) << 100, 100; - // icp::PointCloud2 b = a; - - // LidarView* view = new LidarView(a, b, std::move(icp)); - // launch_gui(view, "test"); - // return 0; - if (*read_scan_files) { auto source = parse_lidar_scan(f_src); auto dest = parse_lidar_scan(f_dst); icp::Config config; config.set("overlap_rate", 0.9); - LidarView* view = new LidarView(source, dest, std::move(icp)); + auto* view = new LidarView(source, dest, std::move(icp)); launch_gui(view, std::string(f_src) + std::string(" and ") + std::string(f_dst)); }