From e6d50e1ff7a6ebffd4ff589175abd3bdc883d396 Mon Sep 17 00:00:00 2001 From: ssg1002 Date: Thu, 14 Mar 2024 19:27:45 +0100 Subject: [PATCH] Initial Commit --- .clang-format | 10 + .cmake-format.yaml | 4 + .gitignore | 126 ++++++++ .gitlab-ci.yml | 37 +++ .pre-commit-config.yaml | 26 ++ LICENSE | 3 +- Makefile | 15 + config/README.md | 0 config/basic_config.yaml | 5 + cpp/map_closures/CMakeLists.txt | 48 +++ cpp/map_closures/LICENSE | 22 ++ cpp/map_closures/README.md | 0 cpp/map_closures/cmake/CompilerOptions.cmake | 52 ++++ cpp/map_closures/core/AlignRansac2D.cpp | 107 +++++++ cpp/map_closures/core/AlignRansac2D.hpp | 63 ++++ cpp/map_closures/core/CMakeLists.txt | 28 ++ cpp/map_closures/core/DensityMap.cpp | 83 ++++++ cpp/map_closures/core/DensityMap.hpp | 49 ++++ cpp/map_closures/pipeline/CMakeLists.txt | 29 ++ cpp/map_closures/pipeline/MapClosures.cpp | 125 ++++++++ cpp/map_closures/pipeline/MapClosures.hpp | 68 +++++ cpp/map_closures/thirdparty/eigen/LICENSE | 18 ++ cpp/map_closures/thirdparty/eigen/eigen.cmake | 41 +++ .../thirdparty/find_dependencies.cmake | 46 +++ cpp/map_closures/thirdparty/hbst/LICENSE | 11 + cpp/map_closures/thirdparty/hbst/hbst.cmake | 28 ++ python/CMakeLists.txt | 37 +++ python/LICENSE | 21 ++ python/MANIFEST.in | 7 + python/README.md | 0 python/map_closures/__init__.py | 24 ++ python/map_closures/config/__init__.py | 24 ++ python/map_closures/config/parser.py | 66 +++++ python/map_closures/datasets/__init__.py | 62 ++++ python/map_closures/datasets/apollo.py | 71 +++++ python/map_closures/datasets/generic.py | 108 +++++++ python/map_closures/datasets/kitti.py | 108 +++++++ python/map_closures/datasets/mcap.py | 111 +++++++ python/map_closures/datasets/mulran.py | 102 +++++++ python/map_closures/datasets/ncd.py | 106 +++++++ python/map_closures/datasets/ouster.py | 156 ++++++++++ python/map_closures/datasets/rosbag.py | 125 ++++++++ python/map_closures/map_closures.py | 61 ++++ python/map_closures/pipeline.py | 276 ++++++++++++++++++ python/map_closures/pybind/.gitignore | 2 + python/map_closures/pybind/CMakeLists.txt | 28 ++ .../pybind/map_closures_pybind.cpp | 79 +++++ python/map_closures/pybind/stl_vector_eigen.h | 117 ++++++++ python/map_closures/tools/__init__.py | 22 ++ python/map_closures/tools/cmd.py | 195 +++++++++++++ python/map_closures/tools/evaluation.py | 176 +++++++++++ python/map_closures/tools/gt_closures.py | 120 ++++++++ python/map_closures/tools/local_maps.py | 37 +++ python/pyproject.toml | 53 ++++ 54 files changed, 3337 insertions(+), 1 deletion(-) create mode 100644 .clang-format create mode 100644 .cmake-format.yaml create mode 100644 .gitignore create mode 100644 .gitlab-ci.yml create mode 100644 .pre-commit-config.yaml create mode 100644 Makefile create mode 100644 config/README.md create mode 100644 config/basic_config.yaml create mode 100644 cpp/map_closures/CMakeLists.txt create mode 100644 cpp/map_closures/LICENSE create mode 100644 cpp/map_closures/README.md create mode 100644 cpp/map_closures/cmake/CompilerOptions.cmake create mode 100644 cpp/map_closures/core/AlignRansac2D.cpp create mode 100644 cpp/map_closures/core/AlignRansac2D.hpp create mode 100644 cpp/map_closures/core/CMakeLists.txt create mode 100644 cpp/map_closures/core/DensityMap.cpp create mode 100644 cpp/map_closures/core/DensityMap.hpp create mode 100644 cpp/map_closures/pipeline/CMakeLists.txt create mode 100644 cpp/map_closures/pipeline/MapClosures.cpp create mode 100644 cpp/map_closures/pipeline/MapClosures.hpp create mode 100644 cpp/map_closures/thirdparty/eigen/LICENSE create mode 100644 cpp/map_closures/thirdparty/eigen/eigen.cmake create mode 100644 cpp/map_closures/thirdparty/find_dependencies.cmake create mode 100644 cpp/map_closures/thirdparty/hbst/LICENSE create mode 100644 cpp/map_closures/thirdparty/hbst/hbst.cmake create mode 100644 python/CMakeLists.txt create mode 100644 python/LICENSE create mode 100644 python/MANIFEST.in create mode 100644 python/README.md create mode 100644 python/map_closures/__init__.py create mode 100644 python/map_closures/config/__init__.py create mode 100644 python/map_closures/config/parser.py create mode 100644 python/map_closures/datasets/__init__.py create mode 100644 python/map_closures/datasets/apollo.py create mode 100644 python/map_closures/datasets/generic.py create mode 100644 python/map_closures/datasets/kitti.py create mode 100644 python/map_closures/datasets/mcap.py create mode 100644 python/map_closures/datasets/mulran.py create mode 100644 python/map_closures/datasets/ncd.py create mode 100644 python/map_closures/datasets/ouster.py create mode 100644 python/map_closures/datasets/rosbag.py create mode 100644 python/map_closures/map_closures.py create mode 100644 python/map_closures/pipeline.py create mode 100644 python/map_closures/pybind/.gitignore create mode 100644 python/map_closures/pybind/CMakeLists.txt create mode 100644 python/map_closures/pybind/map_closures_pybind.cpp create mode 100644 python/map_closures/pybind/stl_vector_eigen.h create mode 100644 python/map_closures/tools/__init__.py create mode 100644 python/map_closures/tools/cmd.py create mode 100644 python/map_closures/tools/evaluation.py create mode 100644 python/map_closures/tools/gt_closures.py create mode 100644 python/map_closures/tools/local_maps.py create mode 100644 python/pyproject.toml diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..bfce055 --- /dev/null +++ b/.clang-format @@ -0,0 +1,10 @@ +BasedOnStyle: Google +UseTab: Never +IndentWidth: 4 +AccessModifierOffset: -4 +ColumnLimit: 100 +BinPackParameters: false +SortIncludes: true +Standard: c++17 +DerivePointerAlignment: false +PointerAlignment: Right diff --git a/.cmake-format.yaml b/.cmake-format.yaml new file mode 100644 index 0000000..36140f3 --- /dev/null +++ b/.cmake-format.yaml @@ -0,0 +1,4 @@ +enable_markup: false +line_width: 120 +format: + max_subgroups_hwrap: 5 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..651ff07 --- /dev/null +++ b/.gitignore @@ -0,0 +1,126 @@ +build/ +install/ +log/ +results/ +wheelhouse/ +_skbuild/ +.gitlab-ci-local/ + +# Created by https://www.toptal.com/developers/gitignore/api/python,c++ +# Edit at https://www.toptal.com/developers/gitignore?templates=python,c++ +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +### Python ### +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# pytype static type analyzer +.pytype/ + +# VSCode +.vscode diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..49d608a --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,37 @@ +stages: + - format + - build +cache: + paths: + - .cache/pip + +#----- format stage -------------------------------------------------------------------------------- +black: + image: python:3.8 + stage: format + before_script: + - pip install black + script: + - black --line-length 100 --check $CI_PROJECT_DIR + +clang-format: + image: ubuntu:22.04 + stage: format + before_script: + - apt-get update && apt-get install --no-install-recommends -y clang-format + script: + - clang-format -Werror --dry-run $(find . -regextype posix-extended -regex ".*\.(cpp|hpp|h)") + +#----- build stage --------------------------------------------------------------------------------- +cpp: + image: gitlab.ipb.uni-bonn.de:4567/ssg1002/map_closures:latest + stage: build + script: + make cpp + +pip_package: + image: gitlab.ipb.uni-bonn.de:4567/ssg1002/map_closures:latest + stage: build + script: + - VERBOSE=1 pip install --verbose ./python/ + - map_closure_pipeline --version diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..22f17cb --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,26 @@ +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + - id: trailing-whitespace + - id: end-of-file-fixer + - id: check-yaml + - id: check-added-large-files + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.0 + hooks: + - id: clang-format + - repo: https://github.com/psf/black + rev: 23.1.0 + hooks: + - id: black + args: [--config=python/pyproject.toml] + - repo: https://github.com/ahans/cmake-format-precommit + rev: 8e52fb6506f169dddfaa87f88600d765fca48386 + hooks: + - id: cmake-format + - repo: https://github.com/pycqa/isort + rev: 5.12.0 + hooks: + - id: isort + args: ["--settings-path=python/pyproject.toml"] diff --git a/LICENSE b/LICENSE index dd55f52..87e174a 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,7 @@ MIT License -Copyright (c) 2024 Photogrammetry & Robotics Bonn +Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +Ignacio Vizzo, 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 diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..bf00ac6 --- /dev/null +++ b/Makefile @@ -0,0 +1,15 @@ +.PHONY: cpp + +install: + @pip install --verbose ./python/ + +uninstall: + @pip -v uninstall map-closures + +editable: + @pip install scikit-build-core pyproject_metadata pathspec pybind11 ninja cmake + @pip install --no-build-isolation -ve ./python/ + +cpp: + @cmake -Bbuild cpp/map_closures/ + @cmake --build build -j$(nproc --all) diff --git a/config/README.md b/config/README.md new file mode 100644 index 0000000..e69de29 diff --git a/config/basic_config.yaml b/config/basic_config.yaml new file mode 100644 index 0000000..d43ebf8 --- /dev/null +++ b/config/basic_config.yaml @@ -0,0 +1,5 @@ +density_map_resolution: 0.5 +density_threshold: 0.05 +hamming_distance_threshold: 50 +inliers_threshold: 10 +local_map_factor: 0.6 # Local Map size as a multiple of the maximum range of the LiDAR diff --git a/cpp/map_closures/CMakeLists.txt b/cpp/map_closures/CMakeLists.txt new file mode 100644 index 0000000..29a02fb --- /dev/null +++ b/cpp/map_closures/CMakeLists.txt @@ -0,0 +1,48 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. + +cmake_minimum_required(VERSION 3.16...3.26) +project(map_closures_cpp VERSION 0.1.0 LANGUAGES CXX) + +option(USE_CCACHE "Build using Ccache if found on the path" ON) +option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) + +# ccache setup +if(USE_CCACHE) + find_program(CCACHE_PATH ccache) + if(CCACHE_PATH) + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) + message(STATUS "Using ccache: ${CCACHE_PATH}") + endif() +endif() + +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +include(thirdparty/find_dependencies.cmake) +include(cmake/CompilerOptions.cmake) + +add_subdirectory(core) +add_subdirectory(pipeline) diff --git a/cpp/map_closures/LICENSE b/cpp/map_closures/LICENSE new file mode 100644 index 0000000..87e174a --- /dev/null +++ b/cpp/map_closures/LICENSE @@ -0,0 +1,22 @@ +MIT License + +Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +Ignacio Vizzo, 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. diff --git a/cpp/map_closures/README.md b/cpp/map_closures/README.md new file mode 100644 index 0000000..e69de29 diff --git a/cpp/map_closures/cmake/CompilerOptions.cmake b/cpp/map_closures/cmake/CompilerOptions.cmake new file mode 100644 index 0000000..2085920 --- /dev/null +++ b/cpp/map_closures/cmake/CompilerOptions.cmake @@ -0,0 +1,52 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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 # Clang + $<$:-fcolor-diagnostics> + $<$:-Werror> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wno-sign-compare> + $<$:-Wno-sign-conversion> + $<$:-Wno-ignored-qualifiers> + # GNU + $<$:-fdiagnostics-color=always> + $<$:-Werror> + $<$:-Wall> + $<$:-Wextra> + $<$:-pedantic> + $<$:-Wcast-align> + $<$:-Wno-sign-compare> + $<$:-Woverloaded-virtual> + $<$:-Wdisabled-optimization> + $<$:-Wno-ignored-qualifiers>) + 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/map_closures/core/AlignRansac2D.cpp b/cpp/map_closures/core/AlignRansac2D.cpp new file mode 100644 index 0000000..d8d2419 --- /dev/null +++ b/cpp/map_closures/core/AlignRansac2D.cpp @@ -0,0 +1,107 @@ +// MIT License +// +// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Ignacio Vizzo, 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. + +#include "AlignRansac2D.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace { +map_closures::SE2 KabschUmeyamaAlignment2D( + const std::vector &keypoint_pairs) { + auto mean = + std::reduce(keypoint_pairs.cbegin(), keypoint_pairs.cend(), map_closures::PointPair()) / + keypoint_pairs.size(); + auto 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) * + ((keypoint_pair.query - mean.query).transpose()); + }); + + Eigen::JacobiSVD svd(covariance_matrix, + Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix2d R = svd.matrixV() * svd.matrixU().transpose(); + R = R.determinant() > 0 ? R : -R; + auto t = mean.query - R * mean.ref; + + return map_closures::SE2(R, t); +} + +static constexpr double inliers_distance_threshold = 3.0; + +// RANSAC Parameters +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))); +} // namespace + +namespace map_closures { +std::pair RansacAlignment2D(const std::vector &keypoint_pairs) { + const size_t max_inliers = keypoint_pairs.size(); + + std::vector sample_keypoint_pairs(2); + std::vector inlier_indices; + inlier_indices.reserve(max_inliers); + + std::vector optimal_inlier_indices; + optimal_inlier_indices.reserve(max_inliers); + + int iter = 0; + while (iter++ < __RANSAC_TRIALS__) { + inlier_indices.clear(); + + std::sample(keypoint_pairs.begin(), keypoint_pairs.end(), sample_keypoint_pairs.begin(), 2, + std::mt19937{std::random_device{}()}); + auto T = KabschUmeyamaAlignment2D(sample_keypoint_pairs); + + int index = 0; + std::for_each(keypoint_pairs.cbegin(), keypoint_pairs.cend(), + [&](const auto &keypoint_pair) { + if ((T * keypoint_pair.ref - keypoint_pair.query).norm() < + inliers_distance_threshold) + inlier_indices.emplace_back(index); + index++; + }); + + if (inlier_indices.size() > optimal_inlier_indices.size()) { + optimal_inlier_indices = inlier_indices; + } + } + + const int 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); + return {T, num_inliers}; +} +} // namespace map_closures diff --git a/cpp/map_closures/core/AlignRansac2D.hpp b/cpp/map_closures/core/AlignRansac2D.hpp new file mode 100644 index 0000000..c78a088 --- /dev/null +++ b/cpp/map_closures/core/AlignRansac2D.hpp @@ -0,0 +1,63 @@ +// MIT License +// +// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Ignacio Vizzo, 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. + +#pragma once + +#include +#include +#include + +namespace map_closures { +struct PointPair { + PointPair() = default; + PointPair(const Eigen::Vector2d &ref, const Eigen::Vector2d &query) : ref(ref), query(query) {} + PointPair &operator+=(const PointPair &rhs) { + this->ref += rhs.ref; + this->query += rhs.query; + return *this; + } + friend PointPair operator+(PointPair lhs, const PointPair &rhs) { return lhs += rhs; } + friend PointPair operator/(PointPair lhs, const double divisor) { + lhs.ref /= divisor; + lhs.query /= divisor; + return lhs; + } + + Eigen::Vector2d ref = Eigen::Vector2d::Zero(); + Eigen::Vector2d query = Eigen::Vector2d::Zero(); +}; + +struct SE2 { + SE2() = default; + SE2(const Eigen::Matrix2d &R, const Eigen::Vector2d &t) : R(R), t(t) {} + + friend Eigen::Vector2d operator*(const SE2 &T, const Eigen::Vector2d &p) { + return T.R * p + T.t; + } + + Eigen::Matrix2d R = Eigen::Matrix2d::Identity(); + Eigen::Vector2d t = Eigen::Vector2d::Zero(); +}; + +std::pair RansacAlignment2D(const std::vector &keypoint_pairs); +} // namespace map_closures diff --git a/cpp/map_closures/core/CMakeLists.txt b/cpp/map_closures/core/CMakeLists.txt new file mode 100644 index 0000000..98f044a --- /dev/null +++ b/cpp/map_closures/core/CMakeLists.txt @@ -0,0 +1,28 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. + +add_library(core STATIC) +add_library(map_closures::core ALIAS core) +target_sources(core PRIVATE DensityMap.cpp AlignRansac2D.cpp) +target_link_libraries(core PUBLIC Eigen3::Eigen ${OpenCV_LIBS}) +set_global_target_properties(core) diff --git a/cpp/map_closures/core/DensityMap.cpp b/cpp/map_closures/core/DensityMap.cpp new file mode 100644 index 0000000..96df78f --- /dev/null +++ b/cpp/map_closures/core/DensityMap.cpp @@ -0,0 +1,83 @@ +// MIT License +// +// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Ignacio Vizzo, 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. + +#include "DensityMap.hpp" + +#include +#include +#include +#include +#include + +namespace map_closures { +std::pair GenerateDensityMap(const std::vector &pointcloud_map, + const float density_map_resolution, + const float density_threshold) { + DensityMapType density_map; + int min_x = std::numeric_limits::max(); + int min_y = std::numeric_limits::max(); + int max_x = std::numeric_limits::min(); + int max_y = std::numeric_limits::min(); + double max_points = std::numeric_limits::min(); + double min_points = std::numeric_limits::max(); + + std::for_each(pointcloud_map.cbegin(), pointcloud_map.cend(), [&](const Point3D &point) { + auto x_coord = static_cast(std::floor(point[0] / density_map_resolution)); + auto y_coord = static_cast(std::floor(point[1] / density_map_resolution)); + Pixel pixel(x_coord, y_coord); + density_map[pixel] += 1.0; + auto pixel_density = density_map[pixel]; + if (pixel_density > max_points) { + max_points = pixel_density; + } else if (pixel_density < min_points) { + min_points = pixel_density; + } + if (x_coord < min_x) { + min_x = x_coord; + } else if (x_coord > max_x) { + max_x = x_coord; + } + + if (y_coord < min_y) { + min_y = y_coord; + } else if (y_coord > max_y) { + max_y = y_coord; + } + }); + auto lower_bound_coordinates = Pixel(min_x, min_y); + + auto n_rows = max_x - min_x + 1; + auto n_cols = max_y - min_y + 1; + const double range = max_points - min_points; + cv::Mat density_img(n_rows, n_cols, CV_8UC1, 0.0); + std::for_each(density_map.cbegin(), density_map.cend(), [&](const auto &pixel) { + auto density_val = (pixel.second - min_points) * 255 / range; + density_val = density_val > density_threshold ? density_val : 0.0; + auto row_num = pixel.first.x() - min_x; + auto col_num = pixel.first.y() - min_y; + density_img.at(row_num, col_num) = static_cast(density_val); + }); + + return {density_img, lower_bound_coordinates}; +} +} // namespace map_closures diff --git a/cpp/map_closures/core/DensityMap.hpp b/cpp/map_closures/core/DensityMap.hpp new file mode 100644 index 0000000..656b6c5 --- /dev/null +++ b/cpp/map_closures/core/DensityMap.hpp @@ -0,0 +1,49 @@ +// MIT License +// +// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Ignacio Vizzo, 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. + +#pragma once + +#include +#include +#include +#include +#include + +using Pixel = Eigen::Vector2i; +using Point3D = Eigen::Vector3d; + +namespace map_closures { + +struct ComparePixels { + bool operator()(const Pixel &lhs, const Pixel &rhs) const { + return lhs.x() < rhs.x() || (lhs.x() == rhs.x() && lhs.y() < rhs.y()); + } +}; + +using DensityMapType = std::map; + +std::pair GenerateDensityMap( + const std::vector &pointcloud_map, + const float density_map_resolution, + const float density_threshold); +} // namespace map_closures diff --git a/cpp/map_closures/pipeline/CMakeLists.txt b/cpp/map_closures/pipeline/CMakeLists.txt new file mode 100644 index 0000000..18cb2a6 --- /dev/null +++ b/cpp/map_closures/pipeline/CMakeLists.txt @@ -0,0 +1,29 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. + +add_library(pipeline STATIC) +add_library(map_closures::pipeline ALIAS pipeline) +target_sources(pipeline PRIVATE MapClosures.cpp) +target_include_directories(pipeline PUBLIC "${hbst_SOURCE_DIR}") +target_link_libraries(pipeline PUBLIC map_closures::core Eigen3::Eigen ${OpenCV_LIBS}) +set_global_target_properties(pipeline) diff --git a/cpp/map_closures/pipeline/MapClosures.cpp b/cpp/map_closures/pipeline/MapClosures.cpp new file mode 100644 index 0000000..4ade05f --- /dev/null +++ b/cpp/map_closures/pipeline/MapClosures.cpp @@ -0,0 +1,125 @@ +// MIT License +// +// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Ignacio Vizzo, 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. + +#include "MapClosures.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "map_closures/core/AlignRansac2D.hpp" +#include "map_closures/core/DensityMap.hpp" +#include "srrg_hbst/types/binary_tree.hpp" + +namespace { +// fixed parameters for OpenCV ORB Features +static constexpr float scale_factor = 1.00; +static constexpr int n_levels = 1; +static constexpr int first_level = 0; +static constexpr int WTA_K = 2; +static constexpr int nfeatures = 500; +static constexpr int edge_threshold = 31; +static constexpr int score_type = 0; +static constexpr int patch_size = 31; +static constexpr int fast_threshold = 35; +} // namespace + +namespace map_closures { +MapClosures::MapClosures() : config_(Config()) { + orb_extractor_ = + cv::ORB::create(nfeatures, scale_factor, n_levels, edge_threshold, first_level, WTA_K, + cv::ORB::ScoreType(score_type), patch_size, fast_threshold); +} + +MapClosures::MapClosures(const Config &config) : config_(config) { + orb_extractor_ = + cv::ORB::create(nfeatures, scale_factor, n_levels, edge_threshold, first_level, WTA_K, + cv::ORB::ScoreType(score_type), patch_size, fast_threshold); +} + +std::pair, cv::Mat> MapClosures::MatchAndAddLocalMap( + const int map_idx, const std::vector &local_map, int top_k) { + const auto &[density_map, map_lowerbound] = + GenerateDensityMap(local_map, config_.density_map_resolution, config_.density_threshold); + density_map_lowerbounds_.emplace_back(map_lowerbound); + + cv::Mat orb_descriptors; + std::vector orb_keypoints; + orb_extractor_->detectAndCompute(density_map, cv::noArray(), orb_keypoints, orb_descriptors); + + auto hbst_matchable = Tree::getMatchables(orb_descriptors, orb_keypoints, map_idx); + hbst_binary_tree_->matchAndAdd(hbst_matchable, descriptor_matches_, + config_.hamming_distance_threshold, + srrg_hbst::SplittingStrategy::SplitEven); + + top_k = std::min(top_k, static_cast(descriptor_matches_.size())); + std::vector ref_mapclosure_indices(top_k); + if (top_k) { + std::multimap num_matches_per_ref_map; + std::for_each(descriptor_matches_.cbegin(), descriptor_matches_.cend(), + [&](const auto &matches) { + num_matches_per_ref_map.insert( + std::pair(matches.second.size(), matches.first)); + }); + + std::transform(std::next(num_matches_per_ref_map.cend(), -top_k), + num_matches_per_ref_map.cend(), ref_mapclosure_indices.begin(), + [&](const auto &num_matches_kv) { return num_matches_kv.second; }); + } + return {ref_mapclosure_indices, density_map}; +} + +std::pair MapClosures::CheckForClosure(int ref_idx, int query_idx) const { + const Tree::MatchVector &matches = descriptor_matches_.at(ref_idx); + + const size_t num_matches = matches.size(); + std::vector keypoint_pairs; + keypoint_pairs.reserve(num_matches); + + auto ref_map_lower_bound = density_map_lowerbounds_[ref_idx]; + auto qry_map_lower_bound = density_map_lowerbounds_[query_idx]; + std::for_each(matches.cbegin(), matches.cend(), [&](const Tree::Match &match) { + if (match.object_references.size() == 1) { + auto ref_match = match.object_references[0].pt; + auto qry_match = match.object_query.pt; + keypoint_pairs.emplace_back(PointPair( + {ref_match.y + ref_map_lower_bound.x(), ref_match.x + ref_map_lower_bound.y()}, + {qry_match.y + qry_map_lower_bound.x(), qry_match.x + qry_map_lower_bound.y()})); + } + }); + + int num_inliers = 0; + Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); + if (num_matches > 2) { + const auto [T, inliers_count] = RansacAlignment2D(keypoint_pairs); + pose.block<2, 2>(0, 0) = T.R; + pose.block<2, 1>(0, 3) = T.t * config_.density_map_resolution; + num_inliers = inliers_count; + } + return {pose, num_inliers}; +} +} // namespace map_closures diff --git a/cpp/map_closures/pipeline/MapClosures.hpp b/cpp/map_closures/pipeline/MapClosures.hpp new file mode 100644 index 0000000..f0f4147 --- /dev/null +++ b/cpp/map_closures/pipeline/MapClosures.hpp @@ -0,0 +1,68 @@ +// MIT License +// +// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Ignacio Vizzo, 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. + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "srrg_hbst/types/binary_tree.hpp" + +namespace { +static constexpr int descriptor_size_bits = 256; + +using Matchable = srrg_hbst::BinaryMatchable; +using Node = srrg_hbst::BinaryNode; +using Tree = srrg_hbst::BinaryTree; +} // namespace + +namespace map_closures { +struct Config { + float density_map_resolution = 0.3; + float density_threshold = 0.05; + int hamming_distance_threshold = 50; +}; + +class MapClosures { +public: + explicit MapClosures(); + explicit MapClosures(const Config &config); + ~MapClosures() = default; + +public: + std::pair, cv::Mat> MatchAndAddLocalMap( + const int map_idx, const std::vector &local_map, int top_k); + std::pair CheckForClosure(const int ref_idx, const int query_idx) const; + +private: + Config config_; + Tree::MatchVectorMap descriptor_matches_; + std::vector density_map_lowerbounds_; + std::unique_ptr hbst_binary_tree_ = std::make_unique(); + cv::Ptr orb_extractor_; +}; +} // namespace map_closures diff --git a/cpp/map_closures/thirdparty/eigen/LICENSE b/cpp/map_closures/thirdparty/eigen/LICENSE new file mode 100644 index 0000000..de5b632 --- /dev/null +++ b/cpp/map_closures/thirdparty/eigen/LICENSE @@ -0,0 +1,18 @@ +Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links: + http://www.mozilla.org/MPL/2.0/ + http://www.mozilla.org/MPL/2.0/FAQ.html + +Some files contain third-party code under BSD or LGPL licenses, whence the other +COPYING.* files here. + +All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later. +For this reason, the COPYING.LGPL file contains the LGPL 2.1 text. + +If you want to guarantee that the Eigen code that you are #including is licensed +under the MPL2 and possibly more permissive licenses (like BSD), #define this +preprocessor symbol: + EIGEN_MPL2_ONLY +For example, with most compilers, you could add this to your project CXXFLAGS: + -DEIGEN_MPL2_ONLY +This will cause a compilation error to be generated if you #include any code that is +LGPL licensed. diff --git a/cpp/map_closures/thirdparty/eigen/eigen.cmake b/cpp/map_closures/thirdparty/eigen/eigen.cmake new file mode 100644 index 0000000..8b395a9 --- /dev/null +++ b/cpp/map_closures/thirdparty/eigen/eigen.cmake @@ -0,0 +1,41 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. + +include(ExternalProject) + +ExternalProject_Add( + external_eigen + PREFIX eigen + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.bz2 + URL_HASH SHA256=b4c198460eba6f28d34894e3a5710998818515104d6e74e5cc331ce31e46e626 + UPDATE_COMMAND "" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "") + +ExternalProject_Get_Property(external_eigen SOURCE_DIR) +add_library(libEigenHelper INTERFACE) +add_dependencies(libEigenHelper external_eigen) +target_include_directories(libEigenHelper SYSTEM INTERFACE $) +set_property(TARGET libEigenHelper PROPERTY EXPORT_NAME Eigen3::Eigen) +add_library(Eigen3::Eigen ALIAS libEigenHelper) diff --git a/cpp/map_closures/thirdparty/find_dependencies.cmake b/cpp/map_closures/thirdparty/find_dependencies.cmake new file mode 100644 index 0000000..87e280a --- /dev/null +++ b/cpp/map_closures/thirdparty/find_dependencies.cmake @@ -0,0 +1,46 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. + +# CMake arguments for configuring ExternalProjects. +set(ExternalProject_CMAKE_ARGS + -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} -DCMAKE_CXX_COMPILER_LAUNCHER=${CMAKE_CXX_COMPILER_LAUNCHER} + -DCMAKE_BUILD_TYPE=Release -DCMAKE_POSITION_INDEPENDENT_CODE=ON) + +if(${USE_SYSTEM_EIGEN3}) + find_package(EIGEN3 QUIET NO_MODULE) +endif() +if(NOT ${USE_SYSTEM_EIGEN3} OR NOT TARGET Eigen3::Eigen) + set(USE_SYSTEM_EIGEN3 OFF PARENT_SCOPE) + include(${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake) +endif() + +set(SRRG_HBST_HAS_EIGEN true) +add_definitions(-DSRRG_HBST_HAS_EIGEN) + +find_package(OpenCV REQUIRED) +if(${OpenCV_FOUND}) + set(SRRG_HBST_HAS_OPENCV true) + add_definitions(-DSRRG_HBST_HAS_OPENCV) + add_definitions(-DSRRG_MERGE_DESCRIPTORS) + include(${CMAKE_CURRENT_LIST_DIR}/hbst/hbst.cmake) +endif() diff --git a/cpp/map_closures/thirdparty/hbst/LICENSE b/cpp/map_closures/thirdparty/hbst/LICENSE new file mode 100644 index 0000000..59baf4a --- /dev/null +++ b/cpp/map_closures/thirdparty/hbst/LICENSE @@ -0,0 +1,11 @@ +Copyright (c) 2018, Dominik Schlegel, Giorgio Grisetti + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/cpp/map_closures/thirdparty/hbst/hbst.cmake b/cpp/map_closures/thirdparty/hbst/hbst.cmake new file mode 100644 index 0000000..fc6aa6d --- /dev/null +++ b/cpp/map_closures/thirdparty/hbst/hbst.cmake @@ -0,0 +1,28 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. + +include(FetchContent) + +FetchContent_Declare(HBST URL https://gitlab.com/saurabh1002/srrg_hbst/-/archive/master/srrg_hbst-master.zip) + +FetchContent_Populate(HBST) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 0000000..5b72ce9 --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,37 @@ +# 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. +cmake_minimum_required(VERSION 3.16...3.26) +project(map_closure_pybind VERSION 0.1.0 LANGUAGES CXX) + +# Set build type +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +set(PYBIND11_NEWPYTHON ON) +find_package(Python COMPONENTS Interpreter Development.Module REQUIRED) +find_package(pybind11 CONFIG REQUIRED) + +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/map_closures ${CMAKE_CURRENT_BINARY_DIR}/map_closures) + +add_subdirectory(map_closures/pybind) diff --git a/python/LICENSE b/python/LICENSE new file mode 100644 index 0000000..f90acef --- /dev/null +++ b/python/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 Saurabh Gupta, Tiziano Guadagnino, 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. diff --git a/python/MANIFEST.in b/python/MANIFEST.in new file mode 100644 index 0000000..c3cc8a7 --- /dev/null +++ b/python/MANIFEST.in @@ -0,0 +1,7 @@ +recursive-include map_closures *.py +recursive-include map_closures *.cpp +recursive-include map_closures *.hpp +recursive-include map_closures *.h +global-include CMakeLists.txt +global-include LICENSE +global-include *.cmake diff --git a/python/README.md b/python/README.md new file mode 100644 index 0000000..e69de29 diff --git a/python/map_closures/__init__.py b/python/map_closures/__init__.py new file mode 100644 index 0000000..95447bf --- /dev/null +++ b/python/map_closures/__init__.py @@ -0,0 +1,24 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. + +__version__ = "0.1.0" diff --git a/python/map_closures/config/__init__.py b/python/map_closures/config/__init__.py new file mode 100644 index 0000000..36691bd --- /dev/null +++ b/python/map_closures/config/__init__.py @@ -0,0 +1,24 @@ +# MIT License +# +# Copyright (c) 2023 Saurabh Gupta, 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. + +from .parser import BaseConfig, load_config, write_config diff --git a/python/map_closures/config/parser.py b/python/map_closures/config/parser.py new file mode 100644 index 0000000..63262b9 --- /dev/null +++ b/python/map_closures/config/parser.py @@ -0,0 +1,66 @@ +# MIT License +# +# Copyright (c) 2023 Saurabh Gupta, 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. +from __future__ import annotations + +import importlib +import sys +from pathlib import Path +from typing import Dict, Optional + +from pydantic import BaseModel + + +class BaseConfig(BaseModel): + density_map_resolution: float = 0.5 + density_threshold: float = 0.05 + hamming_distance_threshold: int = 50 + inliers_threshold: int = 5 + local_map_factor: float = 0.6 + + +def load_config(config_file: Optional[Path]) -> BaseConfig: + """Load configuration from an Optional yaml file. Additionally, deskew and max_range can be + also specified from the CLI interface""" + + config = None + if config_file is not None: + try: + yaml = importlib.import_module("yaml") + except ModuleNotFoundError: + print( + "Custom configuration file specified but PyYAML is not installed on your system," + " run `pip install pyyaml`" + ) + sys.exit(1) + with open(config_file) as cfg_file: + config = yaml.safe_load(cfg_file) + return BaseConfig(**config) + + +def write_config(config: BaseConfig, filename: str): + with open(filename, "w") as outfile: + try: + yaml = importlib.import_module("yaml") + yaml.dump(config.model_dump(), outfile, default_flow_style=False) + except ModuleNotFoundError: + outfile.write(str(config.model_dump())) diff --git a/python/map_closures/datasets/__init__.py b/python/map_closures/datasets/__init__.py new file mode 100644 index 0000000..89aee2d --- /dev/null +++ b/python/map_closures/datasets/__init__.py @@ -0,0 +1,62 @@ +# 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. +from pathlib import Path +from typing import Dict, List + + +def sequence_dataloaders(): + # TODO: automatically infer this + return ["kitti"] + + +def available_dataloaders() -> List: + import os.path + import pkgutil + + pkgpath = os.path.dirname(__file__) + return [name for _, name, _ in pkgutil.iter_modules([pkgpath])] + + +def dataloader_types() -> Dict: + import ast + import importlib + + dataloaders = available_dataloaders() + _types = {} + for dataloader in dataloaders: + script = importlib.util.find_spec(f".{dataloader}", __name__).origin + with open(script) as f: + tree = ast.parse(f.read(), script) + classes = [cls for cls in tree.body if isinstance(cls, ast.ClassDef)] + _types[dataloader] = classes[0].name # assuming there is only 1 class + return _types + + +def dataset_factory(dataloader: str, data_dir: Path, *args, **kwargs): + import importlib + + dataloader_type = dataloader_types()[dataloader] + module = importlib.import_module(f".{dataloader}", __name__) + assert hasattr(module, dataloader_type), f"{dataloader_type} is not defined in {module}" + dataset = getattr(module, dataloader_type) + return dataset(data_dir=data_dir, *args, **kwargs) diff --git a/python/map_closures/datasets/apollo.py b/python/map_closures/datasets/apollo.py new file mode 100644 index 0000000..a803d24 --- /dev/null +++ b/python/map_closures/datasets/apollo.py @@ -0,0 +1,71 @@ +# 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. +import glob +import importlib +import os +import sys +from pathlib import Path + +import natsort +import numpy as np +from pyquaternion import Quaternion + + +class ApolloDataset: + def __init__(self, data_dir: Path, *_, **__): + try: + self.o3d = importlib.import_module("open3d") + except ModuleNotFoundError: + print( + 'pcd files requires open3d and is not installed on your system run "pip install open3d"' + ) + sys.exit(1) + + self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/pcds/*.pcd")) + self.gt_poses = self.read_poses(f"{data_dir}/poses/gt_poses.txt") + self.sequence_id = os.path.basename(data_dir) + + def __len__(self): + return len(self.scan_files) + + def __getitem__(self, idx): + return self.get_scan(self.scan_files[idx]) + + def get_scan(self, scan_file: str): + points = np.asarray(self.o3d.io.read_point_cloud(scan_file).points, dtype=np.float64) + return points.astype(np.float64) + + @staticmethod + def read_poses(file): + data = np.loadtxt(file) + _, _, translations, qxyzw = np.split(data, [1, 2, 5], axis=1) + rotations = np.array( + [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in qxyzw] + ) + poses = np.zeros([rotations.shape[0], 4, 4]) + poses[:, :3, -1] = translations + poses[:, :3, :3] = rotations + poses[:, -1, -1] = 1 + # Convert from global coordinate poses to local poses + first_pose = poses[0, :, :] + return np.linalg.inv(first_pose) @ poses diff --git a/python/map_closures/datasets/generic.py b/python/map_closures/datasets/generic.py new file mode 100644 index 0000000..dc85564 --- /dev/null +++ b/python/map_closures/datasets/generic.py @@ -0,0 +1,108 @@ +# 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. +import os +import sys +from pathlib import Path + +import natsort +import numpy as np +from kiss_icp.datasets import supported_file_extensions + + +class GenericDataset: + def __init__(self, data_dir: Path, *_, **__): + # Config stuff + self.sequence_id = os.path.basename(data_dir) + self.scans_dir = os.path.join(os.path.realpath(data_dir), "") + self.scan_files = np.array( + natsort.natsorted( + [ + os.path.join(self.scans_dir, fn) + for fn in os.listdir(self.scans_dir) + if any(fn.endswith(ext) for ext in supported_file_extensions()) + ] + ), + dtype=str, + ) + if len(self.scan_files) == 0: + raise ValueError(f"Tried to read point cloud files in {self.scans_dir} but none found") + self.file_extension = self.scan_files[0].split(".")[-1] + if self.file_extension not in supported_file_extensions(): + raise ValueError(f"Supported formats are: {supported_file_extensions()}") + + # Obtain the pointcloud reader for the given data folder + self._read_point_cloud = self._get_point_cloud_reader() + + def __len__(self): + return len(self.scan_files) + + def __getitem__(self, idx): + return self.read_point_cloud(self.scan_files[idx]) + + def read_point_cloud(self, file_path: str): + points = self._read_point_cloud(file_path) + return points.astype(np.float64) + + def _get_point_cloud_reader(self): + """Attempt to guess with try/catch blocks which is the best point cloud reader to use for + the given dataset folder. Supported readers so far are: + - np.fromfile + - trimesh.load + - PyntCloud + - open3d[optional] + """ + # This is easy, the old KITTI format + if self.file_extension == "bin": + print("[WARNING] Reading .bin files, the only format supported is the KITTI format") + return lambda file: np.fromfile(file, dtype=np.float32).reshape((-1, 4))[:, :3] + + print('Trying to guess how to read your data: `pip install "kiss-icp[all]"` is required') + first_scan_file = self.scan_files[0] + + # first try trimesh + try: + import trimesh + + trimesh.load(first_scan_file) + return lambda file: np.asarray(trimesh.load(file).vertices) + except: + pass + + # then try pyntcloud + try: + from pyntcloud import PyntCloud + + PyntCloud.from_file(first_scan_file) + return lambda file: PyntCloud.from_file(file).points[["x", "y", "z"]].to_numpy() + except: + pass + + # lastly with open3d + try: + import open3d as o3d + + o3d.io.read_point_cloud(first_scan_file) + return lambda file: np.asarray(o3d.io.read_point_cloud(file).points, dtype=np.float64) + except: + print("[ERROR], File format not supported") + sys.exit(1) diff --git a/python/map_closures/datasets/kitti.py b/python/map_closures/datasets/kitti.py new file mode 100644 index 0000000..197471d --- /dev/null +++ b/python/map_closures/datasets/kitti.py @@ -0,0 +1,108 @@ +# 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 mse, 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 msE OR OTHER DEALINGS IN THE +# SOFTWARE. +import glob +import os + +import numpy as np + + +class KITTIOdometryDataset: + def __init__(self, data_dir, sequence: int, *_, **__): + self.sequence_id = str(int(sequence)).zfill(2) + self.kitti_sequence_dir = os.path.join(data_dir, "sequences", self.sequence_id) + self.velodyne_dir = os.path.join(self.kitti_sequence_dir, "velodyne/") + + self.scan_files = sorted(glob.glob(self.velodyne_dir + "*.bin")) + self.calibration = self.read_calib_file(os.path.join(self.kitti_sequence_dir, "calib.txt")) + + # Load GT Poses (if available) + if sequence < 11: + self.poses_fn = os.path.join(data_dir, f"poses/{self.sequence_id}.txt") + self.gt_poses = self.load_poses(self.poses_fn) + + # Add correction for KITTI datasets, can be easilty removed if unwanted + from kiss_icp.pybind import kiss_icp_pybind + + self.correct_kitti_scan = lambda frame: np.asarray( + kiss_icp_pybind._correct_kitti_scan(kiss_icp_pybind._Vector3dVector(frame)) + ) + + def __getitem__(self, idx): + return self.scans(idx) + + def __len__(self): + return len(self.scan_files) + + def scans(self, idx): + return self.read_point_cloud(self.scan_files[idx]) + + def apply_calibration(self, poses: np.ndarray) -> np.ndarray: + """Converts from Velodyne to Camera Frame""" + Tr = np.eye(4, dtype=np.float64) + Tr[:3, :4] = self.calibration["Tr"].reshape(3, 4) + return Tr @ poses @ np.linalg.inv(Tr) + + def read_point_cloud(self, scan_file: str): + points = np.fromfile(scan_file, dtype=np.float32).reshape((-1, 4))[:, :3].astype(np.float64) + # points = points[points[:, 2] > -2.9] # Remove the annoying reflections + points = self.correct_kitti_scan(points) + return points + + def load_poses(self, poses_file): + def _lidar_pose_gt(poses_gt): + _tr = self.calibration["Tr"].reshape(3, 4) + tr = np.eye(4, dtype=np.float64) + tr[:3, :4] = _tr + left = np.einsum("...ij,...jk->...ik", np.linalg.inv(tr), poses_gt) + right = np.einsum("...ij,...jk->...ik", left, tr) + return right + + poses = np.loadtxt(poses_file, delimiter=" ") + n = poses.shape[0] + poses = np.concatenate( + (poses, np.zeros((n, 3), dtype=np.float32), np.ones((n, 1), dtype=np.float32)), axis=1 + ) + poses = poses.reshape((n, 4, 4)) # [N, 4, 4] + return _lidar_pose_gt(poses) + + def get_frames_timestamps(self) -> np.ndarray: + timestamps = np.loadtxt(os.path.join(self.kitti_sequence_dir, "times.txt")).reshape(-1, 1) + return timestamps + + @staticmethod + def read_calib_file(file_path: str) -> dict: + calib_dict = {} + with open(file_path, "r") as calib_file: + for line in calib_file.readlines(): + tokens = line.split(" ") + if tokens[0] == "calib_time:": + continue + # Only read with float data + if len(tokens) > 0: + values = [float(token) for token in tokens[1:]] + values = np.array(values, dtype=np.float32) + + # The format in KITTI's file is : ...\n -> Remove the ':' + key = tokens[0][:-1] + calib_dict[key] = values + return calib_dict diff --git a/python/map_closures/datasets/mcap.py b/python/map_closures/datasets/mcap.py new file mode 100644 index 0000000..68673bc --- /dev/null +++ b/python/map_closures/datasets/mcap.py @@ -0,0 +1,111 @@ +# MIT License +# +# Copyright (c) 2023 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. +import os +import sys + + +class McapDataloader: + def __init__(self, data_dir: str, topic: str, *_, **__): + """Standalone .mcap dataloader withouth any ROS distribution.""" + # Conditional imports to avoid injecting dependencies for non mcap users + try: + from mcap.reader import make_reader + from mcap_ros2.reader import read_ros2_messages + except ImportError as e: + print("mcap plugins not installed: 'pip install mcap-ros2-support'") + exit(1) + + from kiss_icp.tools.point_cloud2 import read_point_cloud + + # we expect `data_dir` param to be a path to the .mcap file, so rename for clarity + assert os.path.isfile(data_dir), "mcap dataloader expects an existing MCAP file" + self.sequence_id = os.path.basename(data_dir).split(".")[0] + mcap_file = str(data_dir) + + self.bag = make_reader(open(mcap_file, "rb")) + self.summary = self.bag.get_summary() + self.topic = self.check_topic(topic) + self.n_scans = self._get_n_scans() + self.msgs = read_ros2_messages(mcap_file, topics=topic) + self.read_point_cloud = read_point_cloud + self.use_global_visualizer = True + + def __del__(self): + if hasattr(self, "bag"): + del self.bag + + def __getitem__(self, idx): + msg = next(self.msgs).ros_msg + return self.read_point_cloud(msg) + + def __len__(self): + return self.n_scans + + def _get_n_scans(self) -> int: + return sum( + count + for (id, count) in self.summary.statistics.channel_message_counts.items() + if self.summary.channels[id].topic == self.topic + ) + + def check_topic(self, topic: str) -> str: + # Extract schema id from the .mcap file that encodes the PointCloud2 msg + schema_id = [ + schema.id + for schema in self.summary.schemas.values() + if schema.name == "sensor_msgs/msg/PointCloud2" + ][0] + + point_cloud_topics = [ + channel.topic + for channel in self.summary.channels.values() + if channel.schema_id == schema_id + ] + + def print_available_topics_and_exit(): + print(50 * "-") + for t in point_cloud_topics: + print(f"--topic {t}") + print(50 * "-") + sys.exit(1) + + if topic and topic in point_cloud_topics: + return topic + # when user specified the topic check that exists + if topic and topic not in point_cloud_topics: + print( + f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' + "Please select one of the following topics with the --topic flag" + ) + print_available_topics_and_exit() + if len(point_cloud_topics) > 1: + print( + "Multiple sensor_msgs/msg/PointCloud2 topics available." + "Please select one of the following topics with the --topic flag" + ) + print_available_topics_and_exit() + + if len(point_cloud_topics) == 0: + print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") + if len(point_cloud_topics) == 1: + return point_cloud_topics[0] diff --git a/python/map_closures/datasets/mulran.py b/python/map_closures/datasets/mulran.py new file mode 100644 index 0000000..f9d8e91 --- /dev/null +++ b/python/map_closures/datasets/mulran.py @@ -0,0 +1,102 @@ +# 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. +import glob +import os +from pathlib import Path + +import numpy as np + + +class MulranDataset: + def __init__(self, data_dir: Path, *_, **__): + self.sequence_id = os.path.basename(data_dir) + self.sequence_dir = os.path.realpath(data_dir) + self.velodyne_dir = os.path.join(self.sequence_dir, "Ouster/") + + self.scan_files = sorted(glob.glob(self.velodyne_dir + "*.bin")) + self.scan_timestamps = [int(os.path.basename(t).split(".")[0]) for t in self.scan_files] + self.gt_poses = self.load_gt_poses(os.path.join(self.sequence_dir, "global_pose.csv")) + + def __len__(self): + return len(self.scan_files) + + def __getitem__(self, idx): + return self.read_point_cloud(self.scan_files[idx]) + + def read_point_cloud(self, file_path: str): + points = np.fromfile(file_path, dtype=np.float32).reshape((-1, 4))[:, :3] + timestamps = self.get_timestamps() + if points.shape[0] != timestamps.shape[0]: + # MuRan has some broken point clouds, just fallback to no timestamps + return points.astype(np.float64), np.ones(points.shape[0]) + return points.astype(np.float64), timestamps + + @staticmethod + def get_timestamps(): + H = 64 + W = 1024 + return (np.floor(np.arange(H * W) / H) / W).reshape(-1, 1) + + def load_gt_poses(self, poses_file: str): + """MuRan has more poses than scans, therefore we need to match 1-1 timestamp with pose""" + + def read_csv(poses_file: str): + poses = np.loadtxt(poses_file, delimiter=",") + timestamps = poses[:, 0] + poses = poses[:, 1:] + n = poses.shape[0] + poses = np.concatenate( + (poses, np.zeros((n, 3), dtype=np.float32), np.ones((n, 1), dtype=np.float32)), + axis=1, + ) + poses = poses.reshape((n, 4, 4)) # [N, 4, 4] + return poses, timestamps + + # Read the csv file + poses, timestamps = read_csv(poses_file) + # Extract only the poses that has a matching Ouster scan + poses = poses[[np.argmin(abs(timestamps - t)) for t in self.scan_timestamps]] + + # Convert from global coordinate poses to local poses + first_pose = poses[0, :, :] + poses = np.linalg.inv(first_pose) @ poses + + T_lidar_to_base, T_base_to_lidar = self._get_calibration() + return T_lidar_to_base @ poses @ T_base_to_lidar + + def _get_calibration(self): + # Apply calibration obtainbed from calib_base2ouster.txt + # T_lidar_to_base[:3, 3] = np.array([1.7042, -0.021, 1.8047]) + # T_lidar_to_base[:3, :3] = tu_vieja.from_euler( + # "xyz", [0.0001, 0.0003, 179.6654], degrees=True + # ) + T_lidar_to_base = np.array( + [ + [-9.9998295e-01, -5.8398386e-03, -5.2257060e-06, 1.7042000e00], + [5.8398386e-03, -9.9998295e-01, 1.7758769e-06, -2.1000000e-02], + [-5.2359878e-06, 1.7453292e-06, 1.0000000e00, 1.8047000e00], + [0.0000000e00, 0.0000000e00, 0.0000000e00, 1.0000000e00], + ] + ) + T_base_to_lidar = np.linalg.inv(T_lidar_to_base) + return T_lidar_to_base, T_base_to_lidar diff --git a/python/map_closures/datasets/ncd.py b/python/map_closures/datasets/ncd.py new file mode 100644 index 0000000..dd09415 --- /dev/null +++ b/python/map_closures/datasets/ncd.py @@ -0,0 +1,106 @@ +# 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. +import importlib +import os +import re +from pathlib import Path + +import numpy as np +from pyquaternion import Quaternion + + +class NewerCollegeDataset: + def __init__(self, data_dir: Path, *_, **__): + try: + self.PyntCloud = importlib.import_module("pyntcloud").PyntCloud + except ModuleNotFoundError: + print(f'Newer College requires pnytccloud: "pip install pyntcloud"') + + self.data_source = os.path.join(data_dir, "") + self.scan_folder = os.path.join(self.data_source, "raw_format/ouster_scan") + self.pose_file = os.path.join(self.data_source, "ground_truth/registered_poses.csv") + self.sequence_id = os.path.basename(data_dir) + + # Load scan files and poses + self.scan_files = self.get_pcd_filenames(self.scan_folder) + self.gt_poses = self.load_gt_poses(self.pose_file) + + # Visualization Options + self.use_global_visualizer = True + + def __len__(self): + return len(self.scan_files) + + def __getitem__(self, idx): + file_path = os.path.join(self.scan_folder, self.scan_files[idx]) + return self.getitem(file_path) + + def getitem(self, scan_file: str): + points = self.PyntCloud.from_file(scan_file).points[["x", "y", "z"]].to_numpy() + timestamps = self.get_timestamps() + if points.shape[0] != timestamps.shape[0]: + # MuRan has some broken point clouds, just fallback to no timestamps + return points.astype(np.float64), np.ones(points.shape[0]) + return points.astype(np.float64), timestamps + + @staticmethod + def get_timestamps(): + H = 64 + W = 1024 + return (np.floor(np.arange(H * W) / H) / W).reshape(-1, 1) + + @staticmethod + def get_pcd_filenames(scans_folder): + # cloud_1583836591_182590976.pcd + regex = re.compile("^cloud_(\d*_\d*)") + + def get_cloud_timestamp(pcd_filename): + m = regex.search(pcd_filename) + secs, nsecs = m.groups()[0].split("_") + return int(secs) * int(1e9) + int(nsecs) + + return sorted(os.listdir(scans_folder), key=get_cloud_timestamp) + + @staticmethod + def load_gt_poses(file_path: str): + """Taken from pyLiDAR-SLAM/blob/master/slam/dataset/nhcd_dataset.py""" + ground_truth_df = np.genfromtxt(str(file_path), delimiter=",", dtype=np.float64) + xyz = ground_truth_df[:, 2:5] + rotations = np.array( + [ + Quaternion(x=x, y=y, z=z, w=w).rotation_matrix + for x, y, z, w in ground_truth_df[:, 5:] + ] + ) + + num_poses = rotations.shape[0] + poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0) + poses[:, :3, :3] = rotations + poses[:, :3, 3] = xyz + + T_CL = np.eye(4, dtype=np.float32) + T_CL[:3, :3] = Quaternion(x=0, y=0, z=0.924, w=0.383).rotation_matrix + T_CL[:3, 3] = np.array([-0.084, -0.025, 0.050], dtype=np.float32) + poses = np.einsum("nij,jk->nik", poses, T_CL) + poses = np.einsum("ij,njk->nik", np.linalg.inv(poses[0]), poses) + return poses diff --git a/python/map_closures/datasets/ouster.py b/python/map_closures/datasets/ouster.py new file mode 100644 index 0000000..4ec3dd1 --- /dev/null +++ b/python/map_closures/datasets/ouster.py @@ -0,0 +1,156 @@ +# MIT License +# +# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill +# Stachniss. +# Copyright (c) 2023 Pavlo Bashmakov +# +# 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. + +import glob +import os +from typing import Optional + +import numpy as np + + +def find_metadata_json(pcap_file: str) -> str: + """Attempts to resolve the metadata json file for a provided pcap file.""" + dir_path, filename = os.path.split(pcap_file) + if not filename: + return "" + if not dir_path: + dir_path = os.getcwd() + json_candidates = sorted(glob.glob(f"{dir_path}/*.json")) + if not json_candidates: + return "" + prefix_sizes = list( + map(lambda p: len(os.path.commonprefix((filename, os.path.basename(p)))), json_candidates) + ) + max_elem = max(range(len(prefix_sizes)), key=lambda i: prefix_sizes[i]) + return json_candidates[max_elem] + + +class OusterDataloader: + """Ouster pcap dataloader""" + + def __init__( + self, + data_dir: str, + meta: Optional[str] = None, + *_, + **__, + ): + """Create Ouster pcap dataloader to read scans from a pcap file. + + Ouster pcap can be recorded with a `tcpdump` command or programmatically. + Pcap file should contain raw lidar_packets and `meta` file (i.e. metadata.json) + should be a corresponding sensor metadata stored at the time of pcap recording. + + + NOTE: It's critical to have a metadata json stored in the same recording session + as a pcap file, because pcap reader checks the `init_id` field in the UDP + lidar_packets and expects it to match `initialization_id` + in the metadata json, packets with different `init_id` just skipped. + + Metadata json can be obtainer with Ouster SDK: + See examples here https://static.ouster.dev/sdk-docs/python/examples/basics-sensor.html#obtaining-sensor-metadata + + or with Sensor HTTP API endpoint GET /api/v1/sensor/metadata directly: + See doc for details https://static.ouster.dev/sensor-docs/image_route1/image_route2/common_sections/API/http-api-v1.html#get-api-v1-sensor-metadata + + Args: + data_dir: path to a pcap file (not a directory) + meta: path to a metadata json file that should be recorded together with + a pcap file. If `meta` is not provided attempts to find the best matching + json file with the longest commong prefix of the pcap file (`data_dir`) in + the same directory. + """ + + try: + import ouster.pcap as pcap + from ouster import client + except ImportError: + print(f'ouster-sdk is not installed on your system, run "pip install ouster-sdk"') + exit(1) + + # since we import ouster-sdk's client module locally, we keep it locally as well + self._client = client + + assert os.path.isfile(data_dir), "Ouster pcap dataloader expects an existing PCAP file" + + # we expect `data_dir` param to be a path to the .pcap file, so rename for clarity + pcap_file = data_dir + + metadata_json = meta or find_metadata_json(pcap_file) + if not metadata_json: + print("Ouster pcap dataloader can't find metadata json file.") + exit(1) + print("Ouster pcap dataloader: using metadata json: ", metadata_json) + + self.data_dir = os.path.dirname(data_dir) + + with open(metadata_json) as json: + self._info_json = json.read() + self._info = client.SensorInfo(self._info_json) + + # lookup table for 2D range image projection to a 3D point cloud + self._xyz_lut = client.XYZLut(self._info) + + self._pcap_file = str(data_dir) + + # read pcap file for the first pass to count scans + print("Pre-reading Ouster pcap to count the scans number ...") + self._source = pcap.Pcap(self._pcap_file, self._info) + self._scans_num = sum((1 for _ in client.Scans(self._source))) + print(f"Ouster pcap total scans number: {self._scans_num}") + + # frame timestamps array + self._timestamps = np.linspace(0, self._scans_num, self._scans_num, endpoint=False) + + # start Scans iterator for consumption in __getitem__ + self._source = pcap.Pcap(self._pcap_file, self._info) + self._scans_iter = iter(client.Scans(self._source)) + self._next_idx = 0 + + def __getitem__(self, idx): + # we assume that users always reads sequentially and do not + # pass idx as for a random access collection + assert self._next_idx == idx, ( + "Ouster pcap dataloader supports only sequential reads. " + f"Expected idx: {self._next_idx}, but got {idx}" + ) + scan = next(self._scans_iter) + self._next_idx += 1 + + self._timestamps[self._next_idx - 1] = 1e-9 * scan.timestamp[0] + + timestamps = np.tile(np.linspace(0, 1.0, scan.w, endpoint=False), (scan.h, 1)) + + # filtering our zero returns makes it substantially faster for kiss-icp + sel_flag = scan.field(self._client.ChanField.RANGE) != 0 + xyz = self._xyz_lut(scan)[sel_flag] + timestamps = timestamps[sel_flag] + + return xyz, timestamps + + def get_frames_timestamps(self): + return self._timestamps + + def __len__(self): + return self._scans_num diff --git a/python/map_closures/datasets/rosbag.py b/python/map_closures/datasets/rosbag.py new file mode 100644 index 0000000..9fdbdc3 --- /dev/null +++ b/python/map_closures/datasets/rosbag.py @@ -0,0 +1,125 @@ +# 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. +import os +import sys +from pathlib import Path +from typing import Sequence + +import natsort + + +class RosbagDataset: + def __init__(self, data_dir: Sequence[Path], topic: str, *_, **__): + """ROS1 / ROS2 bagfile dataloader. + + It can take either one ROS2 bag file or one or more ROS1 bag files belonging to a split bag. + The reader will replay ROS1 split bags in correct timestamp order. + + TODO: Merge mcap and rosbag dataloaders into 1 + """ + try: + from rosbags.highlevel import AnyReader + except ModuleNotFoundError: + print('rosbags library not installed, run "pip install -U rosbags"') + sys.exit(1) + + from kiss_icp.tools.point_cloud2 import read_point_cloud + + self.read_point_cloud = read_point_cloud + + # FIXME: This is quite hacky, trying to guess if we have multiple .bag, one or a dir + if isinstance(data_dir, Path): + self.sequence_id = os.path.basename(data_dir).split(".")[0] + self.bag = AnyReader([data_dir]) + else: + self.sequence_id = os.path.basename(data_dir[0]).split(".")[0] + self.bag = AnyReader(data_dir) + print("Reading multiple .bag files in directory:") + print("\n".join(natsort.natsorted([path.name for path in self.bag.paths]))) + self.bag.open() + self.topic = self.check_topic(topic) + self.n_scans = self.bag.topics[self.topic].msgcount + + # limit connections to selected topic + connections = [x for x in self.bag.connections if x.topic == self.topic] + self.msgs = self.bag.messages(connections=connections) + self.timestamps = [] + + # Visualization Options + self.use_global_visualizer = True + + def __del__(self): + if hasattr(self, "bag"): + self.bag.close() + + def __len__(self): + return self.n_scans + + def __getitem__(self, idx): + connection, timestamp, rawdata = next(self.msgs) + self.timestamps.append(self.to_sec(timestamp)) + msg = self.bag.deserialize(rawdata, connection.msgtype) + return self.read_point_cloud(msg) + + @staticmethod + def to_sec(nsec: int): + return float(nsec) / 1e9 + + def get_frames_timestamps(self) -> list: + return self.timestamps + + def check_topic(self, topic: str) -> str: + # Extract all PointCloud2 msg topics from the bagfile + point_cloud_topics = [ + topic[0] + for topic in self.bag.topics.items() + if topic[1].msgtype == "sensor_msgs/msg/PointCloud2" + ] + + def print_available_topics_and_exit(): + print(50 * "-") + for t in point_cloud_topics: + print(f"--topic {t}") + print(50 * "-") + sys.exit(1) + + if topic and topic in point_cloud_topics: + return topic + # when user specified the topic check that exists + if topic and topic not in point_cloud_topics: + print( + f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' + "Please select one of the following topics with the --topic flag" + ) + print_available_topics_and_exit() + if len(point_cloud_topics) > 1: + print( + "Multiple sensor_msgs/msg/PointCloud2 topics available." + "Please select one of the following topics with the --topic flag" + ) + print_available_topics_and_exit() + + if len(point_cloud_topics) == 0: + print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") + if len(point_cloud_topics) == 1: + return point_cloud_topics[0] diff --git a/python/map_closures/map_closures.py b/python/map_closures/map_closures.py new file mode 100644 index 0000000..66e1190 --- /dev/null +++ b/python/map_closures/map_closures.py @@ -0,0 +1,61 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. +from typing import Tuple + +import numpy as np +from pydantic_settings import BaseSettings + +from map_closures.pybind import map_closures_pybind + + +class MapClosures: + def __init__(self, config: BaseSettings): + self._config = config + self._pipeline = map_closures_pybind._MapClosures(self._config.model_dump()) + + def match_and_add_local_map(self, map_idx: int, local_map: np.ndarray, top_k: int): + _local_map = map_closures_pybind._VectorEigen3d(local_map) + ref_map_indices, density_map_img = self._pipeline._MatchAndAddLocalMap( + map_idx, _local_map, top_k + ) + return np.asarray(ref_map_indices, np.int32), np.asarray(density_map_img, np.uint8) + + def add_local_map_and_compute_closure(self, map_idx: int, local_map: np.ndarray, top_k: int): + matches, _ = self.match_and_add_local_map(map_idx, local_map, top_k) + best_closure = {"source_idx": -1, "target_idx": -1, "inliers_count": 0, "pose": np.eye(3)} + max_inliers_count = 0 + for ref_idx in matches: + if map_idx - ref_idx < 3: + continue + relative_tf_2d, inliers_count = self.check_for_closure(ref_idx, map_idx) + if inliers_count > max_inliers_count: + best_closure["source_idx"] = ref_idx + best_closure["target_idx"] = map_idx + best_closure["inliers_count"] = inliers_count + best_closure["pose"] = relative_tf_2d + max_inliers_count = inliers_count + return best_closure + + def check_for_closure(self, ref_idx: int, query_idx: int) -> Tuple: + T_init, inliers_count = self._pipeline._CheckForClosure(ref_idx, query_idx) + return np.asarray(T_init), inliers_count diff --git a/python/map_closures/pipeline.py b/python/map_closures/pipeline.py new file mode 100644 index 0000000..17288e0 --- /dev/null +++ b/python/map_closures/pipeline.py @@ -0,0 +1,276 @@ +# MIT License +# +# Copyright (c) 2023 Saurabh Gupta, 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. +import datetime +import os +from pathlib import Path +from typing import List, Optional + +import numpy as np +from kiss_icp.config import KISSConfig +from kiss_icp.kiss_icp import KissICP +from kiss_icp.mapping import get_voxel_hash_map +from kiss_icp.voxelization import voxel_down_sample +from tqdm.auto import trange + +from map_closures.config import BaseConfig, load_config, write_config +from map_closures.map_closures import MapClosures +from map_closures.tools.local_maps import LocalMap + + +def transform_points(pcd, T): + R = T[:3, :3] + t = T[:3, -1] + return pcd @ R.T + t + + +class MapClosurePipeline: + def __init__( + self, + dataset, + config_path: Path, + results_dir: Path, + eval: Optional[bool] = False, + ): + self._dataset = dataset + self._dataset_name = self._dataset.sequence_id + self._n_scans = len(self._dataset) + self._results_dir = results_dir + self._eval = eval + + if config_path is not None: + self.config_name = os.path.basename(config_path) + self.closure_config = load_config(config_path) + else: + self.config_name = "base_config.yaml" + self.closure_config = BaseConfig() + + self.kiss_config = KISSConfig() + self.kiss_config.mapping.voxel_size = 1.0 + self.odometry = KissICP(self.kiss_config) + self.voxel_local_map = get_voxel_hash_map(self.kiss_config) + + self._map_range = self.closure_config.local_map_factor * self.kiss_config.data.max_range + self.map_closures = MapClosures(self.closure_config) + + self.local_maps: List[LocalMap] = [] + + self.closures = [] + + if hasattr(self._dataset, "gt_poses") and self._eval: + from map_closures.tools.evaluation import EvaluationPipeline + from map_closures.tools.gt_closures import get_gt_closures + + self.gt_closures, self.gt_closures_overlap = get_gt_closures( + self._dataset, self._dataset.gt_poses, self.kiss_config + ) + self.closure_overlap_threshold = 0.5 + self.gt_closures = self.gt_closures[ + np.where(self.gt_closures_overlap > self.closure_overlap_threshold)[0] + ] + + self.closure_distance_threshold = 6.0 + self.results = EvaluationPipeline( + self.gt_closures, + self._dataset_name, + self.closure_distance_threshold, + ) + else: + self._eval = False + if not hasattr(self._dataset, "gt_poses"): + print("Cannot compute grount truth closures, no ground truth poses available\n") + + def run(self): + self._run_pipeline() + if self._eval: + self._run_evaluation() + self.results.print() + self._save_config() + self._log_to_file() + self._log_to_console() + self._write_data_to_disk() + + return self.results + + def _run_pipeline(self): + map_idx = 0 + poses_in_local_map = [] + scan_indices_in_local_map = [] + + current_map_pose = np.eye(4) + for scan_idx in trange(0, self._n_scans, ncols=8, unit=" frames", dynamic_ncols=True): + try: + frame, timestamps = self._dataset[scan_idx] + except ValueError: + frame = self._dataset[scan_idx] + timestamps = np.zeros(len(frame)) + + self.odometry.register_frame(frame, timestamps) + current_frame_pose = self.odometry.poses[-1] + + frame_downsample = voxel_down_sample(frame, self.kiss_config.mapping.voxel_size * 0.5) + frame_to_map_pose = np.linalg.inv(current_map_pose) @ current_frame_pose + self.voxel_local_map.add_points(transform_points(frame_downsample, frame_to_map_pose)) + + if np.linalg.norm(frame_to_map_pose[:3, -1]) > self._map_range or ( + scan_idx == self._n_scans - 1 + ): + print(f"Creating New Local Map") + local_map_pointcloud = self.voxel_local_map.point_cloud() + matched_map_indices, density_map = self.map_closures.match_and_add_local_map( + map_idx, local_map_pointcloud, map_idx // 2 + ) + + scan_indices_in_local_map.append(scan_idx) + poses_in_local_map.append(current_frame_pose) + + self.local_maps.append( + LocalMap( + local_map_pointcloud, + density_map, + np.copy(scan_indices_in_local_map), + np.copy(poses_in_local_map), + ) + ) + + for ref_idx in matched_map_indices: + if map_idx - ref_idx >= 3: + relative_pose, inliers_count = self.map_closures.check_for_closure( + ref_idx, map_idx + ) + if inliers_count > self.closure_config.inliers_threshold: + self.closures.append( + np.r_[ + ref_idx, + map_idx, + self.local_maps[ref_idx].scan_indices[0], + self.local_maps[map_idx].scan_indices[0], + relative_pose.flatten(), + ] + ) + if self._eval: + self.results.append( + self.local_maps[ref_idx], + self.local_maps[map_idx], + relative_pose, + self.closure_distance_threshold, + inliers_count, + ) + + self.voxel_local_map.remove_far_away_points(frame_to_map_pose[:3, -1]) + pts_to_keep = self.voxel_local_map.point_cloud() + self.voxel_local_map = get_voxel_hash_map(self.kiss_config) + self.voxel_local_map.add_points( + transform_points( + pts_to_keep, np.linalg.inv(current_frame_pose) @ current_map_pose + ) + ) + current_map_pose = np.copy(current_frame_pose) + scan_indices_in_local_map.clear() + poses_in_local_map.clear() + scan_indices_in_local_map.append(scan_idx) + poses_in_local_map.append(current_frame_pose) + map_idx += 1 + else: + scan_indices_in_local_map.append(scan_idx) + poses_in_local_map.append(current_frame_pose) + + def _run_evaluation(self): + self.results.compute_closures_and_metrics() + + def _log_to_file(self): + if self._eval: + self.results.log_to_file_pr( + os.path.join(self._results_dir, "evaluation_metrics.txt"), self.closure_config + ) + self.results.log_to_file_closures( + os.path.join(self._results_dir, "scan_level_closures.npy") + ) + np.savetxt(os.path.join(self._results_dir, "map_closures.txt"), np.asarray(self.closures)) + np.save( + os.path.join(self._results_dir, "kiss_poses.npy"), + np.asarray(self.odometry.poses), + ) + + def _log_to_console(self): + from rich import box + from rich.console import Console + from rich.table import Table + + console = Console() + table = Table(box=box.HORIZONTALS, title=f"MapClosures detected for {self._dataset_name}") + table.caption = f"Loop Closure Distance Threshold: {self.closure_distance_threshold} m" + table.add_column("# MapClosure", justify="left", style="cyan") + table.add_column("Ref Map Index", justify="left", style="magenta") + table.add_column("Query Map Index", justify="left", style="magenta") + table.add_column("Relative Translation 2D", justify="right", style="magenta") + table.add_column("Relative Rotation 2D", justify="right", style="magenta") + + for i, closure in enumerate(self.closures): + table.add_row( + f"{i+1}", + f"{closure[0]}", + f"{closure[1]}", + f"[{closure[7]:.4f}, {closure[11]:.4f}] m", + f"{(np.arctan2(closure[8], closure[9]) * 180 / np.pi):.4f} deg", + ) + console.print(table) + + def _save_config(self): + self._results_dir = self._create_results_dir() + write_config(self.closure_config, os.path.join(self._results_dir, self.config_name)) + + def _write_data_to_disk(self): + import open3d as o3d + from matplotlib import pyplot as plt + + local_map_dir = os.path.join(self._results_dir, "local_maps") + density_map_dir = os.path.join(self._results_dir, "density_maps") + os.makedirs(density_map_dir, exist_ok=True) + os.makedirs(local_map_dir, exist_ok=True) + for i, local_map in enumerate(self.local_maps): + plt.imsave( + os.path.join(density_map_dir, f"{i:06d}.png"), + 255 - local_map.density_map, + cmap="gray", + ) + o3d.io.write_point_cloud( + os.path.join(local_map_dir, f"{i:06d}.ply"), + o3d.geometry.PointCloud(o3d.utility.Vector3dVector(local_map.pointcloud)), + ) + + def _create_results_dir(self) -> Path: + def get_timestamp() -> str: + return datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + + results_dir = os.path.join( + self._results_dir, + f"{self._dataset_name}_results", + get_timestamp(), + ) + latest_dir = os.path.join(self._results_dir, f"{self._dataset_name}_results", "latest") + + os.makedirs(results_dir, exist_ok=True) + os.unlink(latest_dir) if os.path.exists(latest_dir) or os.path.islink(latest_dir) else None + os.symlink(results_dir, latest_dir) + + return results_dir diff --git a/python/map_closures/pybind/.gitignore b/python/map_closures/pybind/.gitignore new file mode 100644 index 0000000..ebc300f --- /dev/null +++ b/python/map_closures/pybind/.gitignore @@ -0,0 +1,2 @@ +include/ +share/ diff --git a/python/map_closures/pybind/CMakeLists.txt b/python/map_closures/pybind/CMakeLists.txt new file mode 100644 index 0000000..7a9b7f5 --- /dev/null +++ b/python/map_closures/pybind/CMakeLists.txt @@ -0,0 +1,28 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. + +pybind11_add_module(map_closures_pybind MODULE map_closures_pybind.cpp) +target_link_libraries(map_closures_pybind PRIVATE map_closures::pipeline) +install(TARGETS map_closures_pybind DESTINATION .) +add_custom_target(install_python_bindings ${CMAKE_COMMAND} -DCMAKE_INSTALL_COMPONENT=python_bindings -P + "${PROJECT_BINARY_DIR}/cmake_install.cmake" DEPENDS map_closures_pybind) diff --git a/python/map_closures/pybind/map_closures_pybind.cpp b/python/map_closures/pybind/map_closures_pybind.cpp new file mode 100644 index 0000000..db68a28 --- /dev/null +++ b/python/map_closures/pybind/map_closures_pybind.cpp @@ -0,0 +1,79 @@ +// MIT License +// +// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Ignacio Vizzo, 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. + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "map_closures/pipeline/MapClosures.hpp" +#include "stl_vector_eigen.h" + +PYBIND11_MAKE_OPAQUE(std::vector); + +namespace py = pybind11; +using namespace py::literals; + +namespace map_closures { +Config GetConfigFromYAML(const py::dict &yaml_cfg) { + Config cpp_config; + cpp_config.density_threshold = yaml_cfg["density_threshold"].cast(); + cpp_config.density_map_resolution = yaml_cfg["density_map_resolution"].cast(); + cpp_config.hamming_distance_threshold = yaml_cfg["hamming_distance_threshold"].cast(); + return cpp_config; +} + +PYBIND11_MODULE(map_closures_pybind, m) { + auto vector3dvector = pybind_eigen_vector_of_vector( + m, "_VectorEigen3d", "std::vector", + py::py_array_to_vectors_double); + + py::class_> map_closures(m, "_MapClosures", ""); + map_closures + .def(py::init([](const py::dict &cfg) { + auto config = GetConfigFromYAML(cfg); + return std::make_shared(config); + }), + "config"_a) + .def( + "_MatchAndAddLocalMap", + [](MapClosures &self, const int map_idx, const std::vector &local_map, + const int top_k) { + const auto &[ref_map_indices, density_map_cv] = + self.MatchAndAddLocalMap(map_idx, local_map, top_k); + Eigen::Matrix density_map_eigen; + cv::cv2eigen(density_map_cv, density_map_eigen); + return std::make_tuple(ref_map_indices, density_map_eigen); + }, + "map_idx"_a, "local_map"_a, "top_k"_a) + .def("_CheckForClosure", &MapClosures::CheckForClosure, "ref_idx"_a, "query_idx"_a); +} +} // namespace map_closures diff --git a/python/map_closures/pybind/stl_vector_eigen.h b/python/map_closures/pybind/stl_vector_eigen.h new file mode 100644 index 0000000..cf7d568 --- /dev/null +++ b/python/map_closures/pybind/stl_vector_eigen.h @@ -0,0 +1,117 @@ +// ---------------------------------------------------------------------------- +// NOTE: This fily has been adapted from the Open3D project, but copyright +// still belongs to Open3D. All rights reserved +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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. +// ---------------------------------------------------------------------------- + +#pragma once +#include +#include + +// pollute namespace with py +#include +namespace py = pybind11; +#include +#include +#include +#include +#include + +namespace pybind11 { + +template , typename... Args> +py::class_ bind_vector_without_repr(py::module &m, + const std::string &name, + Args &&...args) { + // hack function to disable __repr__ for the convenient function + // bind_vector() + using Class_ = py::class_; + Class_ cl(m, name.c_str(), std::forward(args)...); + cl.def(py::init<>()); + cl.def( + "__bool__", [](const Vector &v) -> bool { return !v.empty(); }, + "Check whether the list is nonempty"); + cl.def("__len__", &Vector::size); + return cl; +} + +// - This function is used by Pybind for std::vector constructor. +// This optional constructor is added to avoid too many Python <-> C++ API +// calls when the vector size is large using the default biding method. +// Pybind matches np.float64 array to py::array_t buffer. +// - Directly using templates for the py::array_t and py::array_t +// and etc. doesn't work. The current solution is to explicitly implement +// bindings for each py array types. +template +std::vector py_array_to_vectors_double( + py::array_t array) { + int64_t eigen_vector_size = EigenVector::SizeAtCompileTime; + if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) { + throw py::cast_error(); + } + std::vector eigen_vectors(array.shape(0)); + auto array_unchecked = array.mutable_unchecked<2>(); + for (auto i = 0; i < array_unchecked.shape(0); ++i) { + eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0)); + } + return eigen_vectors; +} +} // namespace pybind11 + +template , + typename holder_type = std::unique_ptr, + typename InitFunc> +py::class_ pybind_eigen_vector_of_vector(py::module &m, + const std::string &bind_name, + const std::string &repr_name, + InitFunc init_func) { + using Scalar = typename EigenVector::Scalar; + auto vec = py::bind_vector_without_repr>( + m, bind_name, py::buffer_protocol(), py::module_local()); + vec.def(py::init(init_func)); + vec.def_buffer([](std::vector &v) -> py::buffer_info { + size_t rows = EigenVector::RowsAtCompileTime; + return py::buffer_info(v.data(), sizeof(Scalar), py::format_descriptor::format(), 2, + {v.size(), rows}, {sizeof(EigenVector), sizeof(Scalar)}); + }); + vec.def("__repr__", [repr_name](const std::vector &v) { + return repr_name + std::string(" with ") + std::to_string(v.size()) + + std::string(" elements.\n") + std::string("Use numpy.asarray() to access data."); + }); + vec.def("__copy__", [](std::vector &v) { return std::vector(v); }); + vec.def("__deepcopy__", + [](std::vector &v) { return std::vector(v); }); + + // py::detail must be after custom constructor + using Class_ = py::class_>; + py::detail::vector_if_copy_constructible(vec); + py::detail::vector_if_equal_operator(vec); + py::detail::vector_modifiers(vec); + py::detail::vector_accessor(vec); + + return vec; +} diff --git a/python/map_closures/tools/__init__.py b/python/map_closures/tools/__init__.py new file mode 100644 index 0000000..7037915 --- /dev/null +++ b/python/map_closures/tools/__init__.py @@ -0,0 +1,22 @@ +# MIT License +# +# Copyright (c) 2023 Saurabh Gupta, 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. diff --git a/python/map_closures/tools/cmd.py b/python/map_closures/tools/cmd.py new file mode 100644 index 0000000..95ab0f2 --- /dev/null +++ b/python/map_closures/tools/cmd.py @@ -0,0 +1,195 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. +import glob +import os +from pathlib import Path +from typing import Optional + +import typer + +from map_closures.datasets import available_dataloaders, sequence_dataloaders + + +def guess_dataloader(data: Path, default_dataloader: str): + """Guess which dataloader to use in case the user didn't specify with --dataloader flag. + + TODO: Avoid having to return again the data Path. But when guessing multiple .bag files or the + metadata.yaml file, we need to change the Path specifed by the user. + """ + if data.is_file(): + if data.name == "metadata.yaml": + return "rosbag", data.parent # database is in directory, not in .yml + if data.name.split(".")[-1] in "bag": + return "rosbag", data + if data.name.split(".")[-1] == "pcap": + return "ouster", data + if data.name.split(".")[-1] == "mcap": + return "mcap", data + elif data.is_dir(): + if (data / "metadata.yaml").exists(): + # a directory with a metadata.yaml must be a ROS2 bagfile + return "rosbag", data + bagfiles = [Path(path) for path in glob.glob(os.path.join(data, "*.bag"))] + if len(bagfiles) > 0: + return "rosbag", bagfiles + return default_dataloader, data + + +def version_callback(value: bool): + if value: + try: + # Check that the python bindings are properly built and can be loaded at runtime + from map_closures.pybind import map_closures_pybind + except ImportError as e: + print(f"[ERRROR] Python bindings not properly built!") + print(f"[ERRROR] '{e}'") + raise typer.Exit(1) + + import map_closures + + print(f"MapClosures Version: {map_closures.__version__}") + raise typer.Exit(0) + + +def name_callback(value: str): + if not value: + return value + dl = available_dataloaders() + if value not in dl: + raise typer.BadParameter(f"Supported dataloaders are:\n{', '.join(dl)}") + return value + + +app = typer.Typer(add_completion=False, rich_markup_mode="rich") + +_available_dl_help = available_dataloaders() +_available_dl_help.remove("generic") +_available_dl_help.remove("mcap") +_available_dl_help.remove("ouster") +_available_dl_help.remove("rosbag") + +docstring = f""" +Effectively Detecting Loop Closures using Point Cloud Density Maps\n +\b +Examples:\n +# Use a specific dataloader: apollo, kitti, mulran, ncd +$ map_closure_pipeline --dataloader mulran --config +""" + + +@app.command(help=docstring) +def map_closure_pipeline( + data: Path = typer.Argument( + ..., + help="The data directory used by the specified dataloader", + show_default=False, + ), + dataloader: str = typer.Option( + None, + show_default=False, + case_sensitive=False, + autocompletion=available_dataloaders, + callback=name_callback, + help="[Optional] Use a specific dataloader from those supported by KISS-ICP", + ), + results_dir: Path = typer.Argument( + ..., + help="The path where results are to be stored", + show_default=False, + exists=False, + ), + config: Optional[Path] = typer.Option( + None, + "--config", + exists=True, + show_default=False, + help="[Optional] Path to the configuration file", + ), + eval: Optional[bool] = typer.Option( + False, + "--eval", + "-e", + rich_help_panel="Additional Options", + help="[Optional] Run loop closure evaluation", + ), + # Aditional Options --------------------------------------------------------------------------- + sequence: Optional[int] = typer.Option( + None, + "--sequence", + "-s", + show_default=False, + help="[Optional] For some dataloaders, you need to specify a given sequence", + rich_help_panel="Additional Options", + ), + topic: Optional[str] = typer.Option( + None, + "--topic", + "-t", + show_default=False, + help="[Optional] Only valid when processing rosbag files", + rich_help_panel="Additional Options", + ), + meta: Optional[Path] = typer.Option( + None, + "--meta", + "-m", + exists=True, + show_default=False, + help="[Optional] For Ouster pcap dataloader, specify metadata json file path explicitly", + rich_help_panel="Additional Options", + ), + version: Optional[bool] = typer.Option( + None, + "--version", + help="Show the current version of KISS-ICP", + callback=version_callback, + is_eager=True, + ), +): + if not dataloader: + dataloader, data = guess_dataloader(data, default_dataloader="generic") + + if dataloader in sequence_dataloaders() and sequence is None: + print('You must specify a sequence "--sequence"') + raise typer.Exit(code=1) + + from map_closures.datasets import dataset_factory + from map_closures.pipeline import MapClosurePipeline + + MapClosurePipeline( + dataset=dataset_factory( + dataloader=dataloader, + data_dir=data, + # Additional options + sequence=sequence, + topic=topic, + meta=meta, + ), + config_path=config, + results_dir=results_dir, + eval=eval, + ).run() + + +def run(): + app() diff --git a/python/map_closures/tools/evaluation.py b/python/map_closures/tools/evaluation.py new file mode 100644 index 0000000..1eb9134 --- /dev/null +++ b/python/map_closures/tools/evaluation.py @@ -0,0 +1,176 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. +from typing import Dict, List, Set, Tuple + +import numpy as np +from numpy.linalg import inv, norm +from rich import box +from rich.console import Console +from rich.table import Table + +from map_closures.tools.local_maps import LocalMap + + +def compute_closure_indices( + ref_indices: np.ndarray, + query_indices: np.ndarray, + ref_scan_poses: np.ndarray, + query_scan_poses: np.ndarray, + relative_tf: np.ndarray, + distance_threshold: float, +): + T_query_world = inv(query_scan_poses[0]) + T_ref_world = inv(ref_scan_poses[0]) + + # bring all poses to a common frame at the query map + query_locs = (T_query_world @ query_scan_poses)[:, :3, -1].squeeze() + ref_locs = (relative_tf @ T_ref_world @ ref_scan_poses)[:, :3, -1].squeeze() + + closure_indices = [] + query_id_start = query_indices[0] + ref_id_start = ref_indices[0] + qq, rr = np.meshgrid(query_indices, ref_indices) + distances = norm(query_locs[qq - query_id_start] - ref_locs[rr - ref_id_start], axis=2) + ids = np.where(distances < distance_threshold) + for r_id, q_id in zip(ids[0] + ref_id_start, ids[1] + query_id_start): + closure_indices.append((r_id, q_id)) + closure_indices = set(map(lambda x: tuple(sorted(x)), closure_indices)) + return closure_indices + + +class EvaluationMetrics: + def __init__(self, true_positives, false_positives, false_negatives): + self.tp = true_positives + self.fp = false_positives + self.fn = false_negatives + + try: + self.precision = self.tp / (self.tp + self.fp) + except ZeroDivisionError: + self.precision = np.nan + + try: + self.recall = self.tp / (self.tp + self.fn) + except ZeroDivisionError: + self.recall = np.nan + + try: + self.F1 = 2 * self.precision * self.recall / (self.precision + self.recall) + except ZeroDivisionError: + self.F1 = np.nan + + +class EvaluationPipeline: + def __init__( + self, + gt_closures: np.ndarray, + dataset_name: str, + closure_distance_threshold: float, + ): + self._dataset_name = dataset_name + self._closure_distance_threshold = closure_distance_threshold + + self.closure_indices_list: List[Set[Tuple]] = [] + self.inliers_count_list: List = [] + + self.predicted_closures: Dict[int, Set[Tuple[int]]] = {} + self.metrics: Dict[int, EvaluationMetrics] = {} + + gt_closures = gt_closures if gt_closures.shape[1] == 2 else gt_closures.T + self.gt_closures: Set[Tuple[int]] = set(map(lambda x: tuple(sorted(x)), gt_closures)) + + def print(self): + self.log_to_console() + + def append( + self, + ref_local_map: LocalMap, + query_local_map: LocalMap, + relative_pose: np.ndarray, + distance_threshold: float, + inliers_count: int, + ): + closure_indices = compute_closure_indices( + ref_local_map.scan_indices, + query_local_map.scan_indices, + ref_local_map.scan_poses, + query_local_map.scan_poses, + relative_pose, + distance_threshold, + ) + self.closure_indices_list.append(closure_indices) + self.inliers_count_list.append(inliers_count) + + def compute_closures_and_metrics( + self, + ): + for inliers_threshold in range(10, 15): + closures = set() + for closure_indices, inliers_count in zip( + self.closure_indices_list, self.inliers_count_list + ): + if inliers_count >= inliers_threshold: + closures = closures.union(closure_indices) + + tp = len(self.gt_closures.intersection(closures)) + fp = len(closures) - tp + fn = len(self.gt_closures) - tp + self.metrics[inliers_threshold] = EvaluationMetrics(tp, fp, fn) + self.predicted_closures[inliers_threshold] = closures + + def _rich_table_pr(self, table_format: box.Box = box.HORIZONTALS) -> Table: + table = Table(box=table_format, title=self._dataset_name) + table.caption = f"Loop Closure Distance Threshold: {self._closure_distance_threshold}m" + table.add_column("RANSAC #Inliers", justify="center", style="cyan") + table.add_column("True Positives", justify="center", style="magenta") + table.add_column("False Positives", justify="center", style="magenta") + table.add_column("False Negatives", justify="center", style="magenta") + table.add_column("Precision", justify="left", style="green") + table.add_column("Recall", justify="left", style="green") + table.add_column("F1 score", justify="left", style="green") + for [inliers, metric] in self.metrics.items(): + table.add_row( + f"{inliers}", + f"{metric.tp}", + f"{metric.fp}", + f"{metric.fn}", + f"{metric.precision:.4f}", + f"{metric.recall:.4f}", + f"{metric.F1:.4f}", + ) + return table + + def log_to_console(self): + console = Console() + console.print(self._rich_table_pr()) + + def log_to_file_pr(self, filename, config): + with open(filename, "wt") as logfile: + console = Console(file=logfile, width=100, force_jupyter=False) + table = self._rich_table_pr(table_format=box.ASCII_DOUBLE_HEAD) + console.print(table) + console.print(config) + + def log_to_file_closures(self, filename): + with open(filename, "wt") as logfile: + np.save(logfile, self.predicted_closures) diff --git a/python/map_closures/tools/gt_closures.py b/python/map_closures/tools/gt_closures.py new file mode 100644 index 0000000..b2fdc7f --- /dev/null +++ b/python/map_closures/tools/gt_closures.py @@ -0,0 +1,120 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. +import os +from typing import List + +import numpy as np +from kiss_icp.config import KISSConfig +from kiss_icp.mapping import VoxelHashMap +from tqdm import tqdm + + +def get_segment_indices(poses, sampling_distance): + segments = [] + current_segment = [] + last_pose = poses[0] + traveled_distance = 0.0 + for index, pose in enumerate(poses): + traveled_distance += np.linalg.norm(last_pose[:3, -1] - pose[:3, -1]) + if traveled_distance > sampling_distance: + segments.append(current_segment) + + # Start new segment + current_segment = [index] + traveled_distance = 0.0 + else: + current_segment.append(index) + last_pose = pose + + segments.append(current_segment) + return segments + + +def get_global_points(dataset, indices, poses): + global_points = [] + for index, pose in zip(indices, poses): + try: + points, _ = dataset[index] + except ValueError: + points = dataset[index] + R = pose[:3, :3] + t = pose[:3, -1] + global_points.append(points @ R.T + t) + return np.concatenate(global_points, axis=0) + + +def compute_overlap(query_points: np.ndarray, ref_points: np.ndarray, voxel_size: float): + # Compute Szymkiewicz–Simpson coefficient + # See https://en.wikipedia.org/wiki/Overlap_coefficient + query_map = VoxelHashMap(voxel_size, 1, 1) + query_map.add_points(query_points) + n_query_voxels = len(query_map.point_cloud()) + + ref_map = VoxelHashMap(voxel_size, 1, 1) + ref_map.add_points(ref_points) + n_ref_voxels = len(ref_map.point_cloud()) + + ref_map.add_points(query_points) + union = len(ref_map.point_cloud()) + intersection = n_query_voxels + n_ref_voxels - union + overlap = intersection / min(n_query_voxels, n_ref_voxels) + return overlap + + +def get_gt_closures(dataset, gt_poses: List[np.ndarray], config: KISSConfig): + base_dir = dataset.sequence_dir if hasattr(dataset, "sequence_dir") else "" + file_path_closures = os.path.join(base_dir, "loop_closure/gt_closures.txt") + file_path_overlaps = os.path.join(base_dir, "loop_closure/gt_overlaps.txt") + if os.path.exists(file_path_closures) and os.path.exists(file_path_overlaps): + closures = np.loadtxt(file_path_closures, dtype=int) + overlaps = np.loadtxt(file_path_overlaps) + assert len(closures) == len(overlaps) + print(f"Found closure ground truth at {file_path_closures}") + + else: + min_overlap = 0.1 + sampling_distance = 2.0 + segment_indices = get_segment_indices(gt_poses, sampling_distance) + closures = [] + overlaps = [] + tbar = tqdm(segment_indices, desc="Computing Ground Truth Closures, might take some time!") + for i, query_segment in enumerate(tbar): + query_pose = gt_poses[query_segment[0]] + query_points = get_global_points(dataset, query_segment, gt_poses[query_segment]) + n_skip_segments = int(2 * config.data.max_range / sampling_distance) + for _, ref_segment in enumerate( + segment_indices[i + n_skip_segments :], start=n_skip_segments + 1 + ): + ref_pose = gt_poses[ref_segment[0]] + dist = np.linalg.norm(query_pose[:3, -1] - ref_pose[:3, -1]) + if dist < config.data.max_range: + ref_points = get_global_points(dataset, ref_segment, gt_poses[ref_segment]) + overlap = compute_overlap(query_points, ref_points, voxel_size=0.5) + if overlap > min_overlap: + for qi in query_segment: + for ri in ref_segment: + closures.append({qi, ri}) + overlaps.append(overlap) + np.savetxt(file_path_closures, np.array(closures, dtype=int)) + np.savetxt(file_path_overlaps, np.array(overlaps)) + return closures, overlaps diff --git a/python/map_closures/tools/local_maps.py b/python/map_closures/tools/local_maps.py new file mode 100644 index 0000000..1d6f963 --- /dev/null +++ b/python/map_closures/tools/local_maps.py @@ -0,0 +1,37 @@ +# MIT License +# +# Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +# Ignacio Vizzo, 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. +import numpy as np + + +class LocalMap: + def __init__( + self, + pointcloud: np.ndarray, + density_map: np.ndarray, + scan_indices: np.ndarray, + scan_poses: np.ndarray, + ): + self.pointcloud = pointcloud + self.density_map = density_map + self.scan_indices = scan_indices + self.scan_poses = scan_poses diff --git a/python/pyproject.toml b/python/pyproject.toml new file mode 100644 index 0000000..1e8126a --- /dev/null +++ b/python/pyproject.toml @@ -0,0 +1,53 @@ +[build-system] +requires = ["scikit_build_core", "pybind11"] +build-backend = "scikit_build_core.build" + +[project] +name = "map_closures" +version = "0.1.0" +description = "Effectively Detecting Loop Closures using Point Cloud Density Maps" +readme = "README.md" +authors = [{ name = "Saurabh Gupta", email = "saurabh.gupta1002@gmail.com" }] +requires-python = ">=3.7" +keywords = ["Loop Closures", "Localization", "SLAM", "LiDAR"] +dependencies = [ + "numpy", + "pydantic>=2", + "pydantic-settings", + "tqdm", + "typer[all]>=0.6.0", + "rich", +] + +[project.optional-dependencies] +all = ["open3d>0.13", "PyYAML"] +visualizer = ["open3d>=0.13"] + +[project.scripts] +map_closure_pipeline = "map_closures.tools.cmd:run" + +[project.urls] +Homepage = "https://github.com/PRBonn/MapClosures" + +[tool.scikit-build] +build-dir = "build/{wheel_tag}" +cmake.verbose = false +cmake.minimum-version = "3.16" +editable.mode = "redirect" +editable.rebuild = true +editable.verbose = true +sdist.exclude = ["pybind/"] +wheel.install-dir = "map_closures/pybind/" + +[tool.black] +line-length = 100 + +[tool.isort] +profile = "black" + +[tool.pylint.format] +max-line-length = "100" + +[tool.cibuildwheel] +archs = ["auto64"] +skip = ["*-musllinux*", "pp*", "cp36-*"]