diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml index ffcbcaa..3fdd244 100644 --- a/.github/workflows/cpp.yml +++ b/.github/workflows/cpp.yml @@ -14,7 +14,7 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04] + os: [ubuntu-24.04, ubuntu-22.04, windows-2022, macos-14, macos-15] steps: - uses: actions/checkout@v3 - name: Setup cmake @@ -35,12 +35,12 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04] + os: [ubuntu-22.04, ubuntu-24.04] steps: - uses: actions/checkout@v3 - name: Cache dependencies - uses: actions/cache@v2 + uses: actions/cache@v4 with: path: ~/.apt/cache key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }} diff --git a/.github/workflows/pypi.yml b/.github/workflows/pypi.yml new file mode 100644 index 0000000..a7abc8e --- /dev/null +++ b/.github/workflows/pypi.yml @@ -0,0 +1,67 @@ +name: Publish to PyPI.org +on: + release: + types: [published] + push: + branches: ["main"] + pull_request: + branches: ["main"] + +jobs: + build_sdist: + name: Build source distribution + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + + - name: Build sdist + run: pipx run build --sdist ${{github.workspace}}/python/ + - name: Move sdist to dist + run: mkdir -p dist && mv ${{github.workspace}}/python/dist/*.tar.gz dist/ + + - uses: actions/upload-artifact@v4 + with: + path: dist/*.tar.gz + + cibuildwheel: + name: Build wheels on ${{ matrix.os }} + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-24.04, windows-2022, macos-15] + + steps: + - uses: actions/checkout@v3 + + - name: Build test wheels (only PRs) + if: github.event_name != 'release' + uses: pypa/cibuildwheel@v2.22.0 + env: # build 1 build per platform just to make sure we can do it later when releasing + CIBW_BUILD: "cp310-*" + with: + package-dir: ${{github.workspace}}/python/ + + - name: Build all wheels + if: github.event_name == 'release' + uses: pypa/cibuildwheel@v2.22.0 + with: + package-dir: ${{github.workspace}}/python/ + + - uses: actions/upload-artifact@v4 + with: + name: artifact-${{ matrix.os }} + path: ./wheelhouse/*.whl + + pypi: + if: github.event_name == 'release' + needs: [cibuildwheel, build_sdist] + runs-on: ubuntu-latest + steps: + - uses: actions/download-artifact@v4 + with: + name: artifact + path: dist + + - uses: pypa/gh-action-pypi-publish@release/v1 + with: + password: ${{ secrets.PYPI_API_TOKEN }} diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml index d89fc39..66cce23 100644 --- a/.github/workflows/python.yml +++ b/.github/workflows/python.yml @@ -10,7 +10,7 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - os: [ubuntu-24.04, ubuntu-22.04, ubuntu-20.04] + os: [ubuntu-24.04, ubuntu-22.04, windows-2022, macos-14, macos-15] steps: - uses: actions/checkout@v3 diff --git a/README.md b/README.md index 00dabb6..87a045b 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,8 @@ + +

  •   @@ -53,18 +55,8 @@ target_link_libraries(my_target PUBLIC map_closures) ``` ## Install the Python API and CLI -1. First, install the necessary system dependencies - ```sh - sudo apt-get install --no-install-recommends -y build-essential cmake pybind11-dev libeigen3-dev libopencv-dev libtbb-dev - ``` -2. To get an odometry estimate in our Python CLI we rely on [KISS-ICP](https://github.com/PRBonn/kiss-icp), you can install it using - ```sh - pip install kiss-icp - ``` -3. Then run: - ```sh - make - ``` +`pip install map-closures` + ### Usage
diff --git a/cpp/3rdparty/opencv/opencv.cmake b/cpp/3rdparty/opencv/opencv.cmake index 6650d2e..bc5dc51 100644 --- a/cpp/3rdparty/opencv/opencv.cmake +++ b/cpp/3rdparty/opencv/opencv.cmake @@ -25,6 +25,7 @@ set(BUILD_opencv_features2d ON CACHE BOOL "Build OpenCV features2d module") set(BUILD_opencv_imgproc ON CACHE BOOL "Build OpenCV imgproc module") set(BUILD_SHARED_LIBS OFF CACHE BOOL "Build shared libraries") +set(BUILD_WITH_STATIC_CRT OFF CACHE BOOL "Build with statically linked CRT") set(BUILD_JAVA OFF CACHE BOOL "Build Java bindings") set(BUILD_PERF_TESTS OFF CACHE BOOL "Build performance tests") set(BUILD_PROTOBUF OFF CACHE BOOL "Build protobuf") @@ -49,6 +50,7 @@ set(BUILD_opencv_python_tests OFF CACHE BOOL "Build OpenCV Python tests") set(BUILD_opencv_stitching OFF CACHE BOOL "Build OpenCV stitching module") set(BUILD_opencv_video OFF CACHE BOOL "Build OpenCV video module") set(BUILD_opencv_videoio OFF CACHE BOOL "Build OpenCV video IO module") +set(CMAKE_BUILD_TYPE "Release" CACHE STRING "CMake build type") message(STATUS "Fetching OpenCV from Github") include(FetchContent) diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index aa04f69..16fc213 100644 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -22,7 +22,7 @@ # SOFTWARE. cmake_minimum_required(VERSION 3.16...3.26) -project(map_closures_cpp VERSION 1.0.0 LANGUAGES CXX) +project(map_closures_cpp VERSION 1.0.1 LANGUAGES CXX) option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) option(USE_SYSTEM_TBB "Use system pre-installed TBB" ON) @@ -33,6 +33,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) include(3rdparty/find_dependencies.cmake) +include(cmake/CompilerOptions.cmake) add_subdirectory(map_closures) add_subdirectory(gt_closures) diff --git a/cpp/cmake/CompilerOptions.cmake b/cpp/cmake/CompilerOptions.cmake new file mode 100644 index 0000000..2318849 --- /dev/null +++ b/cpp/cmake/CompilerOptions.cmake @@ -0,0 +1,51 @@ +# MIT License +# +# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill +# Stachniss. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +function(set_global_target_properties target) + target_compile_features(${target} PUBLIC cxx_std_17) + target_compile_definitions(${target} PUBLIC $<$:_USE_MATH_DEFINES>) + target_compile_options( + ${target} + PRIVATE # MSVC + $<$:/W4> + # Clang/AppleClang + $<$:-fcolor-diagnostics> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wconversion> + $<$:-Wno-sign-conversion> + # GNU + $<$:-fdiagnostics-color=always> + $<$:-Wall> + $<$:-Wextra> + $<$:-pedantic> + $<$:-Wcast-align> + $<$:-Wcast-qual> + $<$:-Wconversion> + $<$:-Wdisabled-optimization> + $<$:-Woverloaded-virtual>) + set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) + get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) + target_include_directories( + ${target} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC $ $) +endfunction() diff --git a/cpp/gt_closures/CMakeLists.txt b/cpp/gt_closures/CMakeLists.txt index 6666053..c7719bd 100644 --- a/cpp/gt_closures/CMakeLists.txt +++ b/cpp/gt_closures/CMakeLists.txt @@ -25,4 +25,4 @@ add_library(gt_closures STATIC) target_sources(gt_closures PRIVATE VoxelHashSet.cpp GTClosures.cpp) target_include_directories(gt_closures PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/..) target_link_libraries(gt_closures PUBLIC Eigen3::Eigen TBB::tbb tsl::robin_map) -target_compile_features(gt_closures PUBLIC cxx_std_17) +set_global_target_properties(gt_closures) diff --git a/cpp/gt_closures/GTClosures.cpp b/cpp/gt_closures/GTClosures.cpp index 1326e4e..07cc147 100644 --- a/cpp/gt_closures/GTClosures.cpp +++ b/cpp/gt_closures/GTClosures.cpp @@ -88,7 +88,7 @@ int GTClosures::GetSegments() { last_pose = poses_.at(idx); scan_occupancies_.erase(idx); }); - return segments_.size() - n_skip_segments_; + return static_cast(segments_.size()) - n_skip_segments_; } std::vector GTClosures::ComputeClosuresForQuerySegment( diff --git a/cpp/gt_closures/VoxelHashSet.hpp b/cpp/gt_closures/VoxelHashSet.hpp index d3fce54..655613e 100644 --- a/cpp/gt_closures/VoxelHashSet.hpp +++ b/cpp/gt_closures/VoxelHashSet.hpp @@ -46,7 +46,7 @@ struct VoxelHashSet { const Eigen::Vector3d &t); void AddVoxels(const VoxelHashSet &other_set); double ComputeOverlap(const VoxelHashSet &other_set); - int size() const { return set_.size(); } + std::size_t size() const { return set_.size(); } void clear() { set_.clear(); } double voxel_size_; diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index 2c8c101..7e87457 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -35,15 +36,16 @@ namespace { Eigen::Isometry2d KabschUmeyamaAlignment2D( const std::vector &keypoint_pairs) { - auto mean = std::reduce(keypoint_pairs.cbegin(), keypoint_pairs.cend(), - map_closures::PointPair(), [](auto lhs, const auto &rhs) { - lhs.ref += rhs.ref; - lhs.query += rhs.query; - return lhs; - }); - mean.query /= keypoint_pairs.size(); - mean.ref /= keypoint_pairs.size(); - auto covariance_matrix = std::transform_reduce( + map_closures::PointPair mean = + std::reduce(keypoint_pairs.cbegin(), keypoint_pairs.cend(), map_closures::PointPair(), + [](auto lhs, const auto &rhs) { + lhs.ref += rhs.ref; + lhs.query += rhs.query; + return lhs; + }); + mean.query /= static_cast(keypoint_pairs.size()); + mean.ref /= static_cast(keypoint_pairs.size()); + Eigen::Matrix2d covariance_matrix = std::transform_reduce( keypoint_pairs.cbegin(), keypoint_pairs.cend(), Eigen::Matrix2d().setZero(), std::plus(), [&](const auto &keypoint_pair) { return (keypoint_pair.ref - mean.ref) * @@ -66,8 +68,8 @@ static constexpr double inliers_distance_threshold = 3.0; static constexpr double inliers_ratio = 0.3; static constexpr double probability_success = 0.999; static constexpr int min_points = 2; -static constexpr int __RANSAC_TRIALS__ = std::ceil( - std::log(1.0 - probability_success) / std::log(1.0 - std::pow(inliers_ratio, min_points))); +static int __RANSAC_TRIALS__ = std::ceil(std::log(1.0 - probability_success) / + std::log(1.0 - std::pow(inliers_ratio, min_points))); } // namespace namespace map_closures { @@ -106,12 +108,12 @@ std::pair RansacAlignment2D(const std::vector } } - const int num_inliers = optimal_inlier_indices.size(); + const std::size_t num_inliers = optimal_inlier_indices.size(); std::vector inlier_keypoint_pairs(num_inliers); std::transform(optimal_inlier_indices.cbegin(), optimal_inlier_indices.cend(), inlier_keypoint_pairs.begin(), [&](const auto index) { return keypoint_pairs[index]; }); - auto T = KabschUmeyamaAlignment2D(inlier_keypoint_pairs); + Eigen::Isometry2d T = KabschUmeyamaAlignment2D(inlier_keypoint_pairs); return {T, num_inliers}; } } // namespace map_closures diff --git a/cpp/map_closures/CMakeLists.txt b/cpp/map_closures/CMakeLists.txt index 477b8ce..e43c781 100644 --- a/cpp/map_closures/CMakeLists.txt +++ b/cpp/map_closures/CMakeLists.txt @@ -25,4 +25,4 @@ add_library(map_closures STATIC) target_sources(map_closures PRIVATE DensityMap.cpp AlignRansac2D.cpp MapClosures.cpp) target_include_directories(map_closures PUBLIC ${hbst_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/..) target_link_libraries(map_closures PUBLIC Eigen3::Eigen TBB::tbb ${OpenCV_LIBS}) -target_compile_features(map_closures PUBLIC cxx_std_17) +set_global_target_properties(map_closures) diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 0d5c4be..2d917f2 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -108,7 +109,8 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int const auto &ref_map_lower_bound = density_maps_.at(reference_id).lower_bound; const auto &qry_map_lower_bound = density_maps_.at(query_id).lower_bound; auto to_world_point = [](const auto &p, const auto &offset) { - return Eigen::Vector2d(p.y + offset.x(), p.x + offset.y()); + return Eigen::Vector2d(p.y + static_cast(offset.x()), + p.x + static_cast(offset.y())); }; std::vector keypoint_pairs(num_matches); std::transform( diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp index ac64b19..7fc4c31 100644 --- a/cpp/map_closures/MapClosures.hpp +++ b/cpp/map_closures/MapClosures.hpp @@ -41,8 +41,8 @@ using Tree = srrg_hbst::BinaryTree; namespace map_closures { struct Config { - float density_map_resolution = 0.5; - float density_threshold = 0.05; + float density_map_resolution = 0.5f; + float density_threshold = 0.05f; int hamming_distance_threshold = 50; }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 83d3b35..6942e29 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -21,7 +21,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. cmake_minimum_required(VERSION 3.16...3.26) -project(map_closure_pybind VERSION 1.0.0 LANGUAGES CXX) +project(map_closure_pybind VERSION 1.0.1 LANGUAGES CXX) # Set build type set(CMAKE_BUILD_TYPE Release) diff --git a/python/map_closures/__init__.py b/python/map_closures/__init__.py index 618639a..6eae826 100644 --- a/python/map_closures/__init__.py +++ b/python/map_closures/__init__.py @@ -21,4 +21,4 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. -__version__ = "1.0.0" +__version__ = "1.0.1" diff --git a/python/pyproject.toml b/python/pyproject.toml index ffd4996..d3bd914 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,11 +4,11 @@ build-backend = "scikit_build_core.build" [project] name = "map_closures" -version = "1.0.0" +version = "1.0.1" description = "Effectively Detecting Loop Closures using Point Cloud Density Maps" readme = "README.md" authors = [{ name = "Saurabh Gupta" }, { name = "Tiziano Guadagnino" }] -requires-python = ">=3.7" +requires-python = ">=3.8" keywords = ["Loop Closures", "Localization", "SLAM", "LiDAR"] dependencies = [ "kiss-icp>=1.0.0", @@ -63,3 +63,7 @@ max-line-length = "100" [tool.cibuildwheel] archs = ["auto64"] skip = ["*-musllinux*", "pp*", "cp36-*"] + +[tool.cibuildwheel.macos] +environment = "MACOSX_DEPLOYMENT_TARGET=11.00" +archs = ["auto64", "arm64"]