diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml index 3fdd244..8af2cbe 100644 --- a/.github/workflows/cpp.yml +++ b/.github/workflows/cpp.yml @@ -49,7 +49,7 @@ jobs: - name: Install dependencies run: | sudo apt-get update - sudo apt-get install -y build-essential cmake git libeigen3-dev libtbb-dev libopencv-dev + sudo apt-get install -y build-essential cmake git libeigen3-dev libopencv-dev - name: Configure CMake run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp - name: Build diff --git a/.github/workflows/pypi.yml b/.github/workflows/pypi.yml index 0041dbf..3962e81 100644 --- a/.github/workflows/pypi.yml +++ b/.github/workflows/pypi.yml @@ -59,7 +59,7 @@ jobs: steps: - uses: actions/download-artifact@v4 with: - pattern: artifact-* + pattern: artifact* path: dist merge-multiple: true diff --git a/README.md b/README.md index 87a045b..7d9b7bd 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@   •   Install   •   - Paper + ICRA24 Paper   •   Contact Us
@@ -30,7 +30,6 @@ Effectively Detecting Loop Closures using Point Cloud Density Maps. 1. Include the following snippet in your project's `CMakeLists.txt`: ```cmake set(USE_SYSTEM_EIGEN3 ON CACHE BOOL "use system eigen3") -set(USE_SYSTEM_TBB ON CACHE BOOL "use system tbb") set(USE_SYSTEM_OPENCV ON CACHE BOOL "use system opencv") include(FetchContent) @@ -102,11 +101,6 @@ git checkout ICRA2024 ``` Our development aims to push the performances of **MapClosures** above the original results of the paper. -**Note**: You can download the ground-truth loop closure candidates for the datasets used in the paper from [here](https://www.ipb.uni-bonn.de/html/projects/gupta2024icra/MapClosuresGroundtruth.zip). When run with `-e` flag, our pipeline will search for groundtruth data under the folder at path `/loop_closure/`. If not found, it will first generate the groundtruth closures which might consume some time. You can also generate the groundtruth closures following the approach mentioned in our paper using the following command: -```sh -gt_closure_pipeline -``` - ## Acknowledgement diff --git a/config/basic_config.yaml b/config/basic_config.yaml index 26fac82..8e2e424 100644 --- a/config/basic_config.yaml +++ b/config/basic_config.yaml @@ -2,4 +2,3 @@ density_map_resolution: 0.5 density_threshold: 0.05 hamming_distance_threshold: 50 inliers_threshold: 5 -local_map_factor: 1.0 # Local Map size as a multiple of the maximum range of the LiDAR diff --git a/cpp/3rdparty/find_dependencies.cmake b/cpp/3rdparty/find_dependencies.cmake index 29c8c62..991e663 100644 --- a/cpp/3rdparty/find_dependencies.cmake +++ b/cpp/3rdparty/find_dependencies.cmake @@ -41,10 +41,8 @@ macro(find_external_dependency PACKAGE_NAME TARGET_NAME INCLUDED_CMAKE_PATH) endmacro() find_external_dependency("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake") -find_external_dependency("TBB" "TBB::tbb" "${CMAKE_CURRENT_LIST_DIR}/tbb/tbb.cmake") find_external_dependency("OpenCV" "opencv_features2d" "${CMAKE_CURRENT_LIST_DIR}/opencv/opencv.cmake") -find_external_dependency("tsl-robin-map" "tsl::robin_map" - "${CMAKE_CURRENT_LIST_DIR}/tsl_robin/tsl_robin.cmake") +find_external_dependency("Sophus" "Sophus::Sophus" "${CMAKE_CURRENT_LIST_DIR}/sophus/sophus.cmake") include(${CMAKE_CURRENT_LIST_DIR}/hbst/hbst.cmake) diff --git a/cpp/3rdparty/tsl_robin/LICENSE b/cpp/3rdparty/sophus/LICENSE similarity index 85% rename from cpp/3rdparty/tsl_robin/LICENSE rename to cpp/3rdparty/sophus/LICENSE index e9c5ae9..9f9cc4d 100644 --- a/cpp/3rdparty/tsl_robin/LICENSE +++ b/cpp/3rdparty/sophus/LICENSE @@ -1,6 +1,4 @@ -MIT License - -Copyright (c) 2017 Thibaut Goetghebuer-Planchon +Copyright (c) 2008-2015 Jesse Beder. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -9,13 +7,13 @@ 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 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. +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWAR diff --git a/cpp/3rdparty/tsl_robin/tsl_robin.cmake b/cpp/3rdparty/sophus/sophus.cmake similarity index 66% rename from cpp/3rdparty/tsl_robin/tsl_robin.cmake rename to cpp/3rdparty/sophus/sophus.cmake index 9eb019b..bc893f9 100644 --- a/cpp/3rdparty/tsl_robin/tsl_robin.cmake +++ b/cpp/3rdparty/sophus/sophus.cmake @@ -1,7 +1,6 @@ # MIT License # -# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill -# Stachniss. +# # Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Cyrill Stachniss, University of Bonn # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal @@ -21,6 +20,13 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. include(FetchContent) -FetchContent_Declare(tessil SYSTEM - URL https://github.com/Tessil/robin-map/archive/refs/tags/v1.2.1.tar.gz) -FetchContent_MakeAvailable(tessil) + +set(SOPHUS_USE_BASIC_LOGGING ON CACHE BOOL "Don't use fmt for Sophus libraru") +set(BUILD_SOPHUS_TESTS OFF CACHE BOOL "Don't build Sophus tests") +set(BUILD_SOPHUS_EXAMPLES OFF CACHE BOOL "Don't build Sophus Examples") + +FetchContent_Declare( + sophus SYSTEM URL https://github.com/strasdat/Sophus/archive/refs/tags/1.22.10.tar.gz + PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/sophus.patch UPDATE_DISCONNECTED 1) + +FetchContent_MakeAvailable(sophus) diff --git a/cpp/3rdparty/sophus/sophus.patch b/cpp/3rdparty/sophus/sophus.patch new file mode 100644 index 0000000..abf7591 --- /dev/null +++ b/cpp/3rdparty/sophus/sophus.patch @@ -0,0 +1,12 @@ +diff --git a/sophus/common.hpp b/sophus/common.hpp +index 5634d35b..56a1b590 100644 +--- a/sophus/common.hpp ++++ b/sophus/common.hpp +@@ -18,7 +18,7 @@ + #define SOPHUS_FMT_CSTR(description, ...) description + #define SOPHUS_FMT_STR(description, ...) std::string(description) + #define SOPHUS_FMT_PRINT(description, ...) std::printf("%s\n", description) +-#define SOPHUS_FMT_ARG(arg) ++#define SOPHUS_FMT_ARG(arg) arg + + #else // !SOPHUS_USE_BASIC_LOGGING diff --git a/cpp/3rdparty/tbb/LICENSE b/cpp/3rdparty/tbb/LICENSE deleted file mode 100644 index 261eeb9..0000000 --- a/cpp/3rdparty/tbb/LICENSE +++ /dev/null @@ -1,201 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/cpp/3rdparty/tbb/tbb.cmake b/cpp/3rdparty/tbb/tbb.cmake deleted file mode 100644 index 3939688..0000000 --- a/cpp/3rdparty/tbb/tbb.cmake +++ /dev/null @@ -1,45 +0,0 @@ -# MIT License -# -# Copyright (c) 2024 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. - -option(BUILD_SHARED_LIBS OFF) -option(TBBMALLOC_BUILD OFF) -option(TBB_EXAMPLES OFF) -option(TBB_STRICT OFF) -option(TBB_TEST OFF) - -include(FetchContent) -FetchContent_Declare(tbb - URL https://github.com/oneapi-src/oneTBB/archive/refs/tags/v2021.8.0.tar.gz) -FetchContent_GetProperties(tbb) -if(NOT tbb_POPULATED) - FetchContent_Populate(tbb) - if(${CMAKE_VERSION} GREATER_EQUAL 3.25) - add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) - else() - # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will - # consider this 3rdparty headers as source code and fail due the -Werror flag. - add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} EXCLUDE_FROM_ALL) - get_target_property(tbb_include_dirs tbb INTERFACE_INCLUDE_DIRECTORIES) - set_target_properties(tbb PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${tbb_include_dirs}") - endif() -endif() diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index 16fc213..69be9a3 100644 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -22,10 +22,9 @@ # SOFTWARE. cmake_minimum_required(VERSION 3.16...3.26) -project(map_closures_cpp VERSION 1.0.1 LANGUAGES CXX) +project(map_closures_cpp VERSION 2.0.0 LANGUAGES CXX) option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) -option(USE_SYSTEM_TBB "Use system pre-installed TBB" ON) option(USE_SYSTEM_OPENCV "Use system pre-installed OpenCV" ON) set(CMAKE_BUILD_TYPE Release) @@ -36,4 +35,3 @@ include(3rdparty/find_dependencies.cmake) include(cmake/CompilerOptions.cmake) add_subdirectory(map_closures) -add_subdirectory(gt_closures) diff --git a/cpp/gt_closures/CMakeLists.txt b/cpp/gt_closures/CMakeLists.txt deleted file mode 100644 index c7719bd..0000000 --- a/cpp/gt_closures/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -# 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(gt_closures STATIC) -target_sources(gt_closures PRIVATE VoxelHashSet.cpp GTClosures.cpp) -target_include_directories(gt_closures PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/..) -target_link_libraries(gt_closures PUBLIC Eigen3::Eigen TBB::tbb tsl::robin_map) -set_global_target_properties(gt_closures) diff --git a/cpp/gt_closures/GTClosures.cpp b/cpp/gt_closures/GTClosures.cpp deleted file mode 100644 index 07cc147..0000000 --- a/cpp/gt_closures/GTClosures.cpp +++ /dev/null @@ -1,143 +0,0 @@ -// 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 "GTClosures.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "VoxelHashSet.hpp" - -namespace gt_closures { - -GTClosures::GTClosures(const int dataset_size, - const double sampling_distance, - const double overlap_threshold, - const double voxel_size, - const double max_range) { - sampling_distance_ = sampling_distance; - overlap_threshold_ = overlap_threshold; - voxel_size_ = voxel_size; - max_range_ = max_range; - n_skip_segments_ = static_cast(2 * max_range_ / sampling_distance_); - - poses_.reserve(dataset_size); - scan_occupancies_.reserve(dataset_size); - dataset_indices_.resize(dataset_size); - std::iota(dataset_indices_.begin(), dataset_indices_.end(), 0); -} - -void GTClosures::AddPointCloud(const int scan_idx, - const std::vector &pointcloud, - const Eigen::Matrix4d &pose) { - poses_.insert({scan_idx, pose}); - auto scan_occupancy = VoxelHashSet(voxel_size_); - scan_occupancy.AddPoints(pointcloud, pose.block<3, 3>(0, 0), pose.block<3, 1>(0, 3)); - scan_occupancies_.insert({scan_idx, scan_occupancy}); -} - -int GTClosures::GetSegments() { - double traveled_distance = 0.0; - Eigen::Matrix4d last_pose = poses_.at(0); - - int segment_idx = 0; - std::vector segment_indices; - VoxelHashSet segment_occupancy(voxel_size_); - - std::for_each(dataset_indices_.begin(), dataset_indices_.end(), [&](const int idx) { - traveled_distance += - (last_pose.block<3, 1>(0, 3) - poses_.at(idx).block<3, 1>(0, 3)).norm(); - if (traveled_distance > sampling_distance_) { - segment_indices.shrink_to_fit(); - segments_.insert({segment_idx, {segment_indices, segment_occupancy}}); - segments_indices_.emplace_back(segment_idx); - traveled_distance = 0.0; - segment_indices.clear(); - segment_occupancy.clear(); - segment_idx++; - } - segment_indices.emplace_back(idx); - segment_occupancy.AddVoxels(scan_occupancies_.at(idx)); - last_pose = poses_.at(idx); - scan_occupancies_.erase(idx); - }); - return static_cast(segments_.size()) - n_skip_segments_; -} - -std::vector GTClosures::ComputeClosuresForQuerySegment( - const int query_segment_idx) { - auto &[query_segment, query_segment_occupancy] = segments_.at(query_segment_idx); - auto query_seqment_pose = poses_.at(query_segment[0]); - - Closures closures; - closures.reserve(query_segment.size() * poses_.size()); - closures = tbb::parallel_reduce( - tbb::blocked_range::const_iterator>{ - segments_indices_.cbegin() + query_segment_idx + n_skip_segments_, - segments_indices_.cend()}, - closures, - // Transform - [&](const tbb::blocked_range::const_iterator> &r, - Closures closure) -> Closures { - std::for_each(r.begin(), r.end(), [&](const int ref_segment_idx) { - auto &[ref_segment, ref_segment_occupancy] = segments_.at(ref_segment_idx); - auto ref_segment_pose = poses_.at(ref_segment[0]); - auto distance = - (query_seqment_pose.block<3, 1>(0, 3) - ref_segment_pose.block<3, 1>(0, 3)) - .norm(); - if (distance < max_range_) { - auto overlap_3d = query_segment_occupancy.ComputeOverlap(ref_segment_occupancy); - if (overlap_3d > overlap_threshold_) { - std::for_each( - ref_segment.cbegin(), ref_segment.cend(), [&](const int ref_id) { - std::for_each( - query_segment.cbegin(), query_segment.cend(), - [&](const int query_id) { - closure.emplace_back(Eigen::Vector2i(query_id, ref_id)); - }); - }); - } - } - }); - return closure; - }, - // Reduce - [](Closures lhs, const Closures &rhs) -> Closures { - if (!rhs.empty()) { - lhs.insert(lhs.end(), std::make_move_iterator(rhs.cbegin()), - std::make_move_iterator(rhs.cend())); - } - return lhs; - }); - closures.shrink_to_fit(); - segments_.erase(query_segment_idx); - return closures; -} -} // namespace gt_closures diff --git a/cpp/gt_closures/GTClosures.hpp b/cpp/gt_closures/GTClosures.hpp deleted file mode 100644 index 6fb1fa7..0000000 --- a/cpp/gt_closures/GTClosures.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// 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 "VoxelHashSet.hpp" - -using Closures = std::vector; - -namespace gt_closures { - -struct Segment { - std::vector indices; - VoxelHashSet occupancy; -}; - -class GTClosures { -public: - GTClosures(const int dataset_size, - const double sampling_distance, - const double overlap_threshold, - double overlap_voxel_size, - const double max_range); - - void AddPointCloud(const int idx, - const std::vector &pointcloud, - const Eigen::Matrix4d &pose); - int GetSegments(); - std::vector ComputeClosuresForQuerySegment(const int query_segment_idx); - -private: - std::vector dataset_indices_; - std::unordered_map poses_; - std::unordered_map scan_occupancies_; - - std::vector segments_indices_; - std::unordered_map segments_; - - double sampling_distance_ = 2.0; - double overlap_threshold_ = 0.5; - double voxel_size_ = 0.5; - double max_range_ = 100.0; - int n_skip_segments_ = 0; -}; -} // namespace gt_closures diff --git a/cpp/gt_closures/VoxelHashSet.cpp b/cpp/gt_closures/VoxelHashSet.cpp deleted file mode 100644 index 647e652..0000000 --- a/cpp/gt_closures/VoxelHashSet.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// 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 "VoxelHashSet.hpp" - -#include -#include -#include - -namespace { -Voxel PointToVoxel(const Eigen::Vector3d &point, const double voxel_size) { - return Voxel(static_cast(std::floor(point.x() / voxel_size)), - static_cast(std::floor(point.y() / voxel_size)), - static_cast(std::floor(point.z() / voxel_size))); -} -} // namespace - -void VoxelHashSet::AddPoints(const std::vector &points, - const Eigen::Matrix3d &R, - const Eigen::Vector3d &t) { - std::for_each(points.cbegin(), points.cend(), [&](const auto &point) { - const auto voxel = PointToVoxel(R * point + t, voxel_size_); - if (set_.find(voxel) == set_.end()) { - set_.insert(voxel); - } - }); -} - -void VoxelHashSet::AddVoxels(const VoxelHashSet &other_set) { - std::for_each(other_set.set_.cbegin(), other_set.set_.cend(), [&](const auto &voxel) { - if (set_.find(voxel) == set_.end()) { - set_.insert(voxel); - } - }); -} - -double VoxelHashSet::ComputeOverlap(const VoxelHashSet &other_set) { - int overlapping_voxels = 0; - std::for_each(other_set.set_.cbegin(), other_set.set_.cend(), [&](const auto &voxel) { - if (set_.find(voxel) != set_.end()) { - overlapping_voxels++; - } - }); - double overlap_score = static_cast(overlapping_voxels) / - static_cast(std::min(this->size(), other_set.size())); - return overlap_score; -} diff --git a/cpp/map_closures/AlignRansac2D.cpp b/cpp/map_closures/AlignRansac2D.cpp index 7e87457..729bd92 100644 --- a/cpp/map_closures/AlignRansac2D.cpp +++ b/cpp/map_closures/AlignRansac2D.cpp @@ -76,7 +76,8 @@ namespace map_closures { PointPair::PointPair(const Eigen::Vector2d &r, const Eigen::Vector2d &q) : ref(r), query(q) {} -std::pair RansacAlignment2D(const std::vector &keypoint_pairs) { +std::pair RansacAlignment2D( + const std::vector &keypoint_pairs) { const size_t max_inliers = keypoint_pairs.size(); std::vector sample_keypoint_pairs(2); diff --git a/cpp/map_closures/AlignRansac2D.hpp b/cpp/map_closures/AlignRansac2D.hpp index e9ab80d..b104d73 100644 --- a/cpp/map_closures/AlignRansac2D.hpp +++ b/cpp/map_closures/AlignRansac2D.hpp @@ -37,5 +37,6 @@ struct PointPair { Eigen::Vector2d query = Eigen::Vector2d::Zero(); }; -std::pair RansacAlignment2D(const std::vector &keypoint_pairs); +std::pair RansacAlignment2D( + const std::vector &keypoint_pairs); } // namespace map_closures diff --git a/cpp/map_closures/CMakeLists.txt b/cpp/map_closures/CMakeLists.txt index e43c781..8032645 100644 --- a/cpp/map_closures/CMakeLists.txt +++ b/cpp/map_closures/CMakeLists.txt @@ -22,7 +22,8 @@ # SOFTWARE. add_library(map_closures STATIC) -target_sources(map_closures PRIVATE DensityMap.cpp AlignRansac2D.cpp MapClosures.cpp) +target_sources(map_closures PRIVATE DensityMap.cpp AlignRansac2D.cpp GroundAlign.cpp + MapClosures.cpp) target_include_directories(map_closures PUBLIC ${hbst_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/..) -target_link_libraries(map_closures PUBLIC Eigen3::Eigen TBB::tbb ${OpenCV_LIBS}) +target_link_libraries(map_closures PUBLIC Eigen3::Eigen ${OpenCV_LIBS} Sophus::Sophus) set_global_target_properties(map_closures) diff --git a/cpp/map_closures/DensityMap.cpp b/cpp/map_closures/DensityMap.cpp index 0540c89..6477ac8 100644 --- a/cpp/map_closures/DensityMap.cpp +++ b/cpp/map_closures/DensityMap.cpp @@ -35,51 +35,58 @@ struct ComparePixels { return lhs.x() < rhs.x() || (lhs.x() == rhs.x() && lhs.y() < rhs.y()); } }; -using PointCounterType = std::map; constexpr int max_int = std::numeric_limits::max(); constexpr int min_int = std::numeric_limits::min(); } // namespace namespace map_closures { -DensityMap::DensityMap(const int num_rows, const int num_cols, const double resolution_) - : lower_bound(0, 0), resolution(resolution_), grid(num_rows, num_cols, CV_8UC1, 0.0) {} +DensityMap::DensityMap(const int num_rows, + const int num_cols, + const double resolution, + const Eigen::Vector2i &lower_bound) + : lower_bound(lower_bound), resolution(resolution), grid(num_rows, num_cols, CV_8UC1, 0.0) {} DensityMap GenerateDensityMap(const std::vector &pcd, + const Eigen::Matrix4d &T_ground, const float density_map_resolution, const float density_threshold) { - PointCounterType point_counter; - double max_points = std::numeric_limits::min(); double min_points = std::numeric_limits::max(); Eigen::Array2i lower_bound_coordinates = Eigen::Array2i::Constant(max_int); Eigen::Array2i upper_bound_coordinates = Eigen::Array2i::Constant(min_int); - auto Discretize2D = [&density_map_resolution](const Eigen::Vector3d &p) -> Eigen::Array2i { - return (p.head<2>() / density_map_resolution).array().floor().cast(); + auto Discretize2D = [&](const Eigen::Vector3d &p) -> Eigen::Array2i { + return ((T_ground.block<3, 3>(0, 0) * p + T_ground.block<3, 1>(0, 3)).head<2>() / + density_map_resolution) + .array() + .floor() + .cast(); }; - std::for_each(pcd.cbegin(), pcd.cend(), [&](const Eigen::Vector3d &point) { + std::vector pixels(pcd.size()); + std::transform(pcd.cbegin(), pcd.cend(), pixels.begin(), [&](const Eigen::Vector3d &point) { const auto pixel = Discretize2D(point); - point_counter[pixel] += 1.0; - auto &num_points = point_counter[pixel]; - max_points = std::max(max_points, num_points); - min_points = std::min(min_points, num_points); lower_bound_coordinates = lower_bound_coordinates.min(pixel); upper_bound_coordinates = upper_bound_coordinates.max(pixel); + return pixel; }); const auto rows_and_columns = upper_bound_coordinates - lower_bound_coordinates; const auto n_rows = rows_and_columns.x() + 1; const auto n_cols = rows_and_columns.y() + 1; - const double min_max_normalizer = max_points - min_points; - DensityMap density_map(n_rows, n_cols, density_map_resolution); - density_map.lower_bound = lower_bound_coordinates; - std::for_each(point_counter.cbegin(), point_counter.cend(), [&](const auto &entry) { - const auto &[pixel, point_count] = entry; - auto density = (point_count - min_points) / min_max_normalizer; - density = density > density_threshold ? density : 0.0; + cv::Mat counting_grid(n_rows, n_cols, CV_64FC1, 0.0); + std::for_each(pixels.cbegin(), pixels.cend(), [&](const auto &pixel) { const auto px = pixel - lower_bound_coordinates; - density_map(px.x(), px.y()) = static_cast(255 * density); + counting_grid.at(px.x(), px.y()) += 1; + max_points = std::max(max_points, counting_grid.at(px.x(), px.y())); + min_points = std::min(min_points, counting_grid.at(px.x(), px.y())); + }); + + DensityMap density_map(n_rows, n_cols, density_map_resolution, lower_bound_coordinates); + counting_grid.forEach([&](const double count, const int pos[]) { + auto density = (count - min_points) / (max_points - min_points); + density = density > density_threshold ? density : 0.0; + density_map(pos[0], pos[1]) = static_cast(255 * density); }); return density_map; diff --git a/cpp/map_closures/DensityMap.hpp b/cpp/map_closures/DensityMap.hpp index ba9f8e4..95df5e3 100644 --- a/cpp/map_closures/DensityMap.hpp +++ b/cpp/map_closures/DensityMap.hpp @@ -24,14 +24,16 @@ #pragma once #include -#include #include #include namespace map_closures { struct DensityMap { - DensityMap(const int num_rows, const int num_cols, const double resolution); + DensityMap(const int num_rows, + const int num_cols, + const double resolution, + const Eigen::Vector2i &lower_bound); DensityMap(const DensityMap &other) = delete; DensityMap(DensityMap &&other) = default; DensityMap &operator=(DensityMap &&other) = default; @@ -43,6 +45,7 @@ struct DensityMap { }; DensityMap GenerateDensityMap(const std::vector &pointcloud_map, + const Eigen::Matrix4d &T_ground, const float density_map_resolution, const float density_threshold); } // namespace map_closures diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp new file mode 100644 index 0000000..b9bc198 --- /dev/null +++ b/cpp/map_closures/GroundAlign.cpp @@ -0,0 +1,123 @@ +// MIT License +// +// Copyright (c) 2025 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Niklas Trekel, Meher Malladi, and 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 "GroundAlign.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace { +struct PixelHash { + size_t operator()(const Eigen::Vector2i &pixel) const { + const uint32_t *vec = reinterpret_cast(pixel.data()); + return (vec[0] * 73856093 ^ vec[1] * 19349669); + } +}; + +void TransformPoints(const Sophus::SE3d &T, std::vector &pointcloud) { + std::transform(pointcloud.cbegin(), pointcloud.cend(), pointcloud.begin(), + [&](const auto &point) { return T * point; }); +} + +using LinearSystem = std::pair; +LinearSystem BuildLinearSystem(const std::vector &points, + const double resolution) { + auto compute_jacobian_and_residual = [](const auto &point) { + const double residual = point.z(); + Eigen::Matrix J; + J(0, 0) = 1.0; + J(0, 1) = point.y(); + J(0, 2) = -point.x(); + return std::make_pair(J, residual); + }; + + auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) { + a.first += b.first; + a.second += b.second; + return a; + }; + + const auto &[H, b] = + std::transform_reduce(points.cbegin(), points.cend(), + LinearSystem(Eigen::Matrix3d::Zero(), Eigen::Vector3d::Zero()), + sum_linear_systems, [&](const auto &point) { + const auto &[J, residual] = compute_jacobian_and_residual(point); + const double w = std::abs(residual) <= resolution ? 1.0 : 0.0; + return LinearSystem(J.transpose() * w * J, // JTJ + J.transpose() * w * residual); // JTr + }); + return {H, b}; +} + +static constexpr double convergence_threshold = 1e-3; +static constexpr int max_iterations = 20; + +std::vector ComputeLowestPoints(const std::vector &pointcloud, + const double resolution) { + std::unordered_map lowest_point_hash_map; + auto PointToPixel = [&resolution](const Eigen::Vector3d &pt) -> Eigen::Vector2i { + return Eigen::Vector2i(static_cast(std::floor(pt.x() / resolution)), + static_cast(std::floor(pt.y() / resolution))); + }; + + std::for_each(pointcloud.cbegin(), pointcloud.cend(), [&](const Eigen::Vector3d &point) { + auto pixel = PointToPixel(point); + if (lowest_point_hash_map.find(pixel) == lowest_point_hash_map.cend()) { + lowest_point_hash_map.insert({pixel, point}); + } else if (point.z() < lowest_point_hash_map[pixel].z()) { + lowest_point_hash_map[pixel] = point; + } + }); + + std::vector low_lying_points; + low_lying_points.reserve(lowest_point_hash_map.size()); + std::for_each(lowest_point_hash_map.cbegin(), lowest_point_hash_map.cend(), + [&](const auto &entry) { low_lying_points.emplace_back(entry.second); }); + return low_lying_points; +} +} // namespace + +namespace map_closures { +Eigen::Matrix4d AlignToLocalGround(const std::vector &pointcloud, + const double resolution) { + Sophus::SE3d T = Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero()); + auto low_lying_points = ComputeLowestPoints(pointcloud, resolution); + + for (int iters = 0; iters < max_iterations; iters++) { + const auto &[H, b] = BuildLinearSystem(low_lying_points, resolution); + const Eigen::Vector3d dx = H.ldlt().solve(-b); + Eigen::Matrix se3 = Eigen::Matrix::Zero(); + se3.block<3, 1>(2, 0) = dx; + Sophus::SE3d estimation(Sophus::SE3d::exp(se3)); + TransformPoints(estimation, low_lying_points); + T = estimation * T; + if (dx.norm() < convergence_threshold) break; + } + return T.matrix(); +} +} // namespace map_closures diff --git a/cpp/gt_closures/VoxelHashSet.hpp b/cpp/map_closures/GroundAlign.hpp similarity index 55% rename from cpp/gt_closures/VoxelHashSet.hpp rename to cpp/map_closures/GroundAlign.hpp index 655613e..04067db 100644 --- a/cpp/gt_closures/VoxelHashSet.hpp +++ b/cpp/map_closures/GroundAlign.hpp @@ -1,7 +1,7 @@ // MIT License // -// Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, -// Ignacio Vizzo, Cyrill Stachniss. +// Copyright (c) 2025 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, +// Niklas Trekel, Meher Malladi, and 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 @@ -23,32 +23,10 @@ #pragma once -#include - #include #include -using Voxel = Eigen::Vector3i; - -template <> -struct std::hash { - std::size_t operator()(const Voxel &voxel) const { - const uint32_t *vec = reinterpret_cast(voxel.data()); - return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); - } -}; - -struct VoxelHashSet { - VoxelHashSet(const double voxel_size) : voxel_size_(voxel_size) {} - - void AddPoints(const std::vector &points, - const Eigen::Matrix3d &R, - const Eigen::Vector3d &t); - void AddVoxels(const VoxelHashSet &other_set); - double ComputeOverlap(const VoxelHashSet &other_set); - std::size_t size() const { return set_.size(); } - void clear() { set_.clear(); } - - double voxel_size_; - tsl::robin_set set_; -}; +namespace map_closures { +Eigen::Matrix4d AlignToLocalGround(const std::vector &pointcloud, + const double resolution); +} // namespace map_closures diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 2d917f2..7d098c5 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -23,9 +23,6 @@ #include "MapClosures.hpp" -#include -#include - #include #include #include @@ -36,11 +33,17 @@ #include "AlignRansac2D.hpp" #include "DensityMap.hpp" +#include "GroundAlign.hpp" #include "srrg_hbst/types/binary_tree.hpp" namespace { +static constexpr int min_no_of_matches = 2; +static constexpr int no_of_local_maps_to_skip = 3; +static constexpr double ground_alignment_resolution = 5.0; +static constexpr double self_similarity_threshold = 35.0; + // fixed parameters for OpenCV ORB Features -static constexpr float scale_factor = 1.00; +static constexpr float scale_factor = 1.00f; static constexpr int n_levels = 1; static constexpr int first_level = 0; static constexpr int WTA_K = 2; @@ -64,40 +67,42 @@ MapClosures::MapClosures(const Config &config) : config_(config) { cv::ORB::ScoreType(score_type), patch_size, fast_threshold); } -ClosureCandidate MapClosures::MatchAndAdd(const int id, - const std::vector &local_map) { - DensityMap density_map = - GenerateDensityMap(local_map, config_.density_map_resolution, config_.density_threshold); +void MapClosures::MatchAndAddToDatabase(const int id, + const std::vector &local_map) { + Eigen::Matrix4d T_ground = AlignToLocalGround(local_map, ground_alignment_resolution); + DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, + config_.density_threshold); cv::Mat orb_descriptors; std::vector orb_keypoints; orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, orb_descriptors); - auto hbst_matchable = Tree::getMatchables(orb_descriptors, orb_keypoints, id); + auto matcher = cv::BFMatcher(cv::NORM_HAMMING); + std::vector> bf_matches; + matcher.knnMatch(orb_descriptors, orb_descriptors, bf_matches, 2); + + std::for_each(orb_keypoints.begin(), orb_keypoints.end(), [&](cv::KeyPoint &keypoint) { + keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); + keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); + }); + density_maps_.emplace(id, std::move(density_map)); + ground_alignments_.emplace(id, T_ground); + + std::vector hbst_matchable; + hbst_matchable.reserve(orb_descriptors.rows); + std::for_each(bf_matches.cbegin(), bf_matches.cend(), [&](const auto &bf_match) { + if (bf_match[1].distance > self_similarity_threshold) { + auto index_descriptor = bf_match[0].queryIdx; + auto keypoint = orb_keypoints[index_descriptor]; + hbst_matchable.emplace_back( + new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); + } + }); + hbst_matchable.shrink_to_fit(); + hbst_binary_tree_->matchAndAdd(hbst_matchable, descriptor_matches_, config_.hamming_distance_threshold, srrg_hbst::SplittingStrategy::SplitEven); - density_maps_.emplace(id, std::move(density_map)); - std::vector indices(descriptor_matches_.size()); - std::transform(descriptor_matches_.cbegin(), descriptor_matches_.cend(), indices.begin(), - [](const auto &descriptor_match) { return descriptor_match.first; }); - auto compare_closure_candidates = [](ClosureCandidate a, - const ClosureCandidate &b) -> ClosureCandidate { - return a.number_of_inliers > b.number_of_inliers ? a : b; - }; - using iterator_type = std::vector::const_iterator; - const auto &closure = tbb::parallel_reduce( - tbb::blocked_range{indices.cbegin(), indices.cend()}, ClosureCandidate(), - [&](const tbb::blocked_range &r, - ClosureCandidate candidate) -> ClosureCandidate { - return std::transform_reduce( - r.begin(), r.end(), candidate, compare_closure_candidates, [&](const auto &ref_id) { - const bool is_far_enough = std::abs(static_cast(ref_id) - id) > 3; - return is_far_enough ? ValidateClosure(ref_id, id) : ClosureCandidate(); - }); - }, - compare_closure_candidates); - return closure; } ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int query_id) const { @@ -105,29 +110,79 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int const size_t num_matches = matches.size(); ClosureCandidate closure; - if (num_matches > 2) { - const auto &ref_map_lower_bound = density_maps_.at(reference_id).lower_bound; - const auto &qry_map_lower_bound = density_maps_.at(query_id).lower_bound; - auto to_world_point = [](const auto &p, const auto &offset) { - return Eigen::Vector2d(p.y + static_cast(offset.x()), - p.x + static_cast(offset.y())); - }; + if (num_matches > min_no_of_matches) { std::vector keypoint_pairs(num_matches); - std::transform( - matches.cbegin(), matches.cend(), keypoint_pairs.begin(), - [&](const Tree::Match &match) { - auto ref_point = to_world_point(match.object_references[0].pt, ref_map_lower_bound); - auto query_point = to_world_point(match.object_query.pt, qry_map_lower_bound); - return PointPair(ref_point, query_point); - }); + std::transform(matches.cbegin(), matches.cend(), keypoint_pairs.begin(), + [&](const Tree::Match &match) { + auto query_point = + Eigen::Vector2d(match.object_query.pt.y, match.object_query.pt.x); + auto ref_point = Eigen::Vector2d(match.object_references[0].pt.y, + match.object_references[0].pt.x); + return PointPair(ref_point, query_point); + }); const auto &[pose2d, number_of_inliers] = RansacAlignment2D(keypoint_pairs); closure.source_id = reference_id; closure.target_id = query_id; closure.pose.block<2, 2>(0, 0) = pose2d.linear(); closure.pose.block<2, 1>(0, 3) = pose2d.translation() * config_.density_map_resolution; + closure.pose = ground_alignments_.at(query_id).inverse() * closure.pose * + ground_alignments_.at(reference_id); closure.number_of_inliers = number_of_inliers; } return closure; } + +ClosureCandidate MapClosures::GetBestClosure(const int query_id, + const std::vector &local_map) { + MatchAndAddToDatabase(query_id, local_map); + auto compare_closure_candidates = [](ClosureCandidate a, + const ClosureCandidate &b) -> ClosureCandidate { + return a.number_of_inliers > b.number_of_inliers ? a : b; + }; + auto is_far_enough = [](const int ref_id, const int query_id) { + return std::abs(query_id - ref_id) > no_of_local_maps_to_skip; + }; + const auto &closure = std::transform_reduce( + descriptor_matches_.cbegin(), descriptor_matches_.cend(), ClosureCandidate(), + compare_closure_candidates, [&](const auto &descriptor_match) { + const auto ref_id = static_cast(descriptor_match.first); + return is_far_enough(ref_id, query_id) ? ValidateClosure(ref_id, query_id) + : ClosureCandidate(); + }); + return closure; +} + +std::vector MapClosures::GetTopKClosures( + const int query_id, const std::vector &local_map, const int k) { + MatchAndAddToDatabase(query_id, local_map); + auto compare_closure_candidates = [](const ClosureCandidate &a, const ClosureCandidate &b) { + return a.number_of_inliers >= b.number_of_inliers; + }; + auto is_far_enough = [](const int ref_id, const int query_id) { + return std::abs(query_id - ref_id) > no_of_local_maps_to_skip; + }; + + std::vector closures; + closures.reserve(query_id); + std::for_each(descriptor_matches_.cbegin(), descriptor_matches_.cend(), + [&](const auto &descriptor_match) { + const auto ref_id = static_cast(descriptor_match.first); + if (is_far_enough(ref_id, query_id)) { + ClosureCandidate closure = + ValidateClosure(descriptor_match.first, query_id); + if (closure.number_of_inliers > min_no_of_matches) { + closures.emplace_back(closure); + } + } + }); + if (k == -1) return closures; + + std::vector top_k_closures; + top_k_closures.reserve(std::min(k, static_cast(closures.size()))); + std::sort(closures.begin(), closures.end(), compare_closure_candidates); + std::copy_n(closures.cbegin(), std::min(k, static_cast(closures.size())), + std::back_inserter(top_k_closures)); + return top_k_closures; +} } // namespace map_closures diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp index 7fc4c31..b34f830 100644 --- a/cpp/map_closures/MapClosures.hpp +++ b/cpp/map_closures/MapClosures.hpp @@ -50,7 +50,7 @@ struct ClosureCandidate { int source_id = -1; int target_id = -1; Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); - size_t number_of_inliers = 0; + std::size_t number_of_inliers = 0; }; class MapClosures { @@ -59,17 +59,27 @@ class MapClosures { explicit MapClosures(const Config &config); ~MapClosures() = default; - ClosureCandidate MatchAndAdd(const int id, const std::vector &local_map); - ClosureCandidate ValidateClosure(const int reference_id, const int query_id) const; - + ClosureCandidate GetBestClosure(const int query_id, + const std::vector &local_map); + std::vector GetTopKClosures(const int query_id, + const std::vector &local_map, + const int k); + std::vector GetClosures(const int query_id, + const std::vector &local_map) { + return GetTopKClosures(query_id, local_map, -1); + } const DensityMap &getDensityMapFromId(const int &map_id) const { return density_maps_.at(map_id); } -private: +protected: + void MatchAndAddToDatabase(const int id, const std::vector &local_map); + ClosureCandidate ValidateClosure(const int reference_id, const int query_id) const; + Config config_; Tree::MatchVectorMap descriptor_matches_; std::unordered_map density_maps_; + std::unordered_map ground_alignments_; std::unique_ptr hbst_binary_tree_ = std::make_unique(); cv::Ptr orb_extractor_; }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 6942e29..17add9b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -21,7 +21,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. cmake_minimum_required(VERSION 3.16...3.26) -project(map_closure_pybind VERSION 1.0.1 LANGUAGES CXX) +project(map_closure_pybind VERSION 2.0.0 LANGUAGES CXX) # Set build type set(CMAKE_BUILD_TYPE Release) diff --git a/python/README.md b/python/README.md index e69de29..6473635 100644 --- a/python/README.md +++ b/python/README.md @@ -0,0 +1,78 @@ +
+

MapClosures

+ + + + + +
+
+   •   + Install +   •   + ICRA24 Paper +   •   + Contact Us +
+
+ +Effectively Detecting Loop Closures using Point Cloud Density Maps. + +

+ +![image](https://github.com/PRBonn/MapClosures/assets/28734882/18d5ee54-61a9-4d9f-87f2-8aba16de0f75) +

+
+
+ +## Install the Python API and CLI +`pip install map-closures` + +### Usage +
+ +The following command will provide details about how to use our pipeline: + +```sh +map_closure_pipeline --help +``` + + +![CLI_usage](https://github.com/PRBonn/MapClosures/assets/28734882/6dfbd767-ca63-4671-9582-3129752d0244) +
+ +
+ +Providing the -v flag will initialize the visualizer: + +```sh +map_closure_pipeline -v +``` + + +![Visualizer](https://github.com/user-attachments/assets/34aa2b2f-c0ce-4dfb-a0e0-cbcc04487a5a) +
+ +## Citation + +If you use this library for any academic work, please cite our original [paper](https://www.ipb.uni-bonn.de/pdfs/gupta2024icra.pdf). + +```bibtex +@inproceedings{gupta2024icra, + author = {S. Gupta and T. Guadagnino and B. Mersch and I. Vizzo and C. Stachniss}, + title = {{Effectively Detecting Loop Closures using Point Cloud Density Maps}}, + booktitle = {IEEE International Conference on Robotics and Automation (ICRA)}, + year = {2024}, + codeurl = {https://github.com/PRBonn/MapClosures}, +} +``` + +## Acknowledgement + +This repository is heavily inspired by, and also depends on [KISS-ICP](https://github.com/PRBonn/kiss-icp) + +## Contributors + + + + diff --git a/python/map_closures/__init__.py b/python/map_closures/__init__.py index 6eae826..bb76253 100644 --- a/python/map_closures/__init__.py +++ b/python/map_closures/__init__.py @@ -21,4 +21,4 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. -__version__ = "1.0.1" +__version__ = "2.0.0" diff --git a/python/map_closures/config/config.py b/python/map_closures/config/config.py index c1120c9..e0ca7dd 100644 --- a/python/map_closures/config/config.py +++ b/python/map_closures/config/config.py @@ -34,8 +34,7 @@ class MapClosuresConfig(BaseModel): density_map_resolution: float = 0.5 density_threshold: float = 0.05 hamming_distance_threshold: int = 50 - inliers_threshold: int = 10 - local_map_factor: float = 1.0 + inliers_threshold: int = 5 def load_config(config_file: Optional[Path]) -> MapClosuresConfig: diff --git a/python/map_closures/map_closures.py b/python/map_closures/map_closures.py index ff98682..de68399 100644 --- a/python/map_closures/map_closures.py +++ b/python/map_closures/map_closures.py @@ -20,6 +20,8 @@ # 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 List + import numpy as np from typing_extensions import TypeAlias @@ -34,12 +36,22 @@ def __init__(self, config: MapClosuresConfig = MapClosuresConfig()): self._config = config self._pipeline = map_closures_pybind._MapClosures(self._config.model_dump()) - def match_and_add(self, map_idx: int, local_map: np.ndarray) -> ClosureCandidate: + def get_best_closure(self, query_idx: int, local_map: np.ndarray) -> ClosureCandidate: + pcd = map_closures_pybind._Vector3dVector(local_map) + closure = self._pipeline._GetBestClosure(query_idx, pcd) + return closure + + def get_top_k_closures( + self, query_idx: int, local_map: np.ndarray, k: int + ) -> List[ClosureCandidate]: pcd = map_closures_pybind._Vector3dVector(local_map) - return self._pipeline._MatchAndAdd(map_idx, pcd) + top_k_closures = self._pipeline._GetTopKClosures(query_idx, pcd, k) + return top_k_closures + + def get_closures(self, query_idx: int, local_map: np.ndarray) -> List[ClosureCandidate]: + pcd = map_closures_pybind._Vector3dVector(local_map) + closures = self._pipeline._GetClosures(query_idx, pcd) + return closures def get_density_map_from_id(self, map_id: int) -> np.ndarray: return self._pipeline._getDensityMapFromId(map_id) - - def validate_closure(self, ref_idx: int, query_idx: int) -> ClosureCandidate: - return self._pipeline._ValidateClosure(ref_idx, query_idx) diff --git a/python/map_closures/pipeline.py b/python/map_closures/pipeline.py index f805d32..3fb1ff9 100644 --- a/python/map_closures/pipeline.py +++ b/python/map_closures/pipeline.py @@ -28,14 +28,12 @@ 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 kiss_icp.mapping import VoxelHashMap from tqdm.auto import trange from map_closures.config import load_config, write_config from map_closures.map_closures import MapClosures from map_closures.tools.evaluation import EvaluationPipeline, LocalMap, StubEvaluation -from map_closures.tools.gt_closures import generate_gt_closures from map_closures.visualizer.visualizer import StubVisualizer, Visualizer @@ -65,28 +63,32 @@ def __init__( self._eval = eval self._vis = vis + self.closure_config = load_config(config_path) + self.map_closures = MapClosures(self.closure_config) + self.kiss_config = KISSConfig() self.kiss_config.mapping.voxel_size = 1.0 + self.kiss_config.data.max_range = 100.0 self.odometry = KissICP(self.kiss_config) - self.voxel_local_map = get_voxel_hash_map(self.kiss_config) - self.closure_config = load_config(config_path) - self._map_range = self.closure_config.local_map_factor * self.kiss_config.data.max_range - self.map_closures = MapClosures(self.closure_config) + self.voxel_local_map = VoxelHashMap( + voxel_size=self.closure_config.density_map_resolution, + max_distance=self.kiss_config.data.max_range, + max_points_per_voxel=20, + ) + self._map_range = self.kiss_config.data.max_range self.closures = [] self.local_maps = [] self.density_maps = [] self.odom_poses = np.zeros((self._n_scans, 4, 4)) - self.closure_overlap_threshold = 0.5 + gt_closures_path = os.path.join( + self._dataset.sequence_dir, "loop_closure", "gt_closures.txt" + ) self.gt_closures = ( - generate_gt_closures( - self._dataset, - self.kiss_config.data.max_range, - self.closure_overlap_threshold, - ) - if (self._eval and hasattr(self._dataset, "gt_poses")) + np.loadtxt(gt_closures_path, dtype=int) + if (self._eval and os.path.exists(gt_closures_path)) else None ) @@ -97,7 +99,7 @@ def __init__( self._dataset_name, self.closure_distance_threshold, ) - if self._eval + if self._eval and self.gt_closures is not None else StubEvaluation() ) @@ -132,13 +134,12 @@ def _run_pipeline(self): frame = self._dataset[scan_idx] timestamps = np.zeros(len(frame)) - source, keypoints = self.odometry.register_frame(frame, timestamps) + frame, _ = self.odometry.register_frame(frame, timestamps) self.odom_poses[scan_idx] = self.odometry.last_pose current_frame_pose = self.odometry.last_pose - 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)) + self.voxel_local_map.add_points(transform_points(frame, frame_to_map_pose)) self.visualizer.update_registration( frame, self.odometry.local_map.point_cloud(), @@ -149,7 +150,7 @@ def _run_pipeline(self): scan_idx == self._n_scans - 1 ): local_map_pointcloud = self.voxel_local_map.point_cloud() - closure = self.map_closures.match_and_add(map_idx, local_map_pointcloud) + closures = self.map_closures.get_closures(map_idx, local_map_pointcloud) scan_indices_in_local_map.append(scan_idx) poses_in_local_map.append(current_frame_pose) @@ -167,35 +168,36 @@ def _run_pipeline(self): self.local_maps[-1].density_map, current_map_pose, ) - if closure.number_of_inliers > self.closure_config.inliers_threshold: - reference_local_map = self.local_maps[closure.source_id] - query_local_map = self.local_maps[closure.target_id] - self.closures.append( - np.r_[ - closure.source_id, - closure.target_id, - reference_local_map.scan_indices[0], - query_local_map.scan_indices[0], - closure.pose.flatten(), - ] - ) - - if self._eval: - self.results.append( - reference_local_map, - query_local_map, - closure.pose, - self.closure_distance_threshold, - closure.number_of_inliers, + for closure in closures: + if closure.number_of_inliers > self.closure_config.inliers_threshold: + reference_local_map = self.local_maps[closure.source_id] + query_local_map = self.local_maps[closure.target_id] + self.closures.append( + np.r_[ + closure.source_id, + closure.target_id, + reference_local_map.scan_indices[0], + query_local_map.scan_indices[0], + closure.pose.flatten(), + ] ) - self.visualizer.update_closures( - np.asarray(closure.pose), [closure.source_id, closure.target_id] - ) + if self._eval: + self.results.append( + reference_local_map, + query_local_map, + closure.pose, + self.closure_distance_threshold, + closure.number_of_inliers, + ) + + self.visualizer.update_closures( + np.asarray(closure.pose), [closure.source_id, closure.target_id] + ) 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.clear() self.voxel_local_map.add_points( transform_points( pts_to_keep, np.linalg.inv(current_frame_pose) @ current_map_pose diff --git a/python/map_closures/pybind/CMakeLists.txt b/python/map_closures/pybind/CMakeLists.txt index 8241c60..75cb584 100644 --- a/python/map_closures/pybind/CMakeLists.txt +++ b/python/map_closures/pybind/CMakeLists.txt @@ -23,7 +23,3 @@ pybind11_add_module(map_closures_pybind MODULE map_closures_pybind.cpp) target_link_libraries(map_closures_pybind PUBLIC map_closures) install(TARGETS map_closures_pybind DESTINATION .) - -pybind11_add_module(gt_closures_pybind MODULE gt_closures_pybind.cpp) -target_link_libraries(gt_closures_pybind PUBLIC gt_closures) -install(TARGETS gt_closures_pybind DESTINATION .) diff --git a/python/map_closures/pybind/gt_closures_pybind.cpp b/python/map_closures/pybind/gt_closures_pybind.cpp deleted file mode 100644 index 7da74d5..0000000 --- a/python/map_closures/pybind/gt_closures_pybind.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// 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 "gt_closures/GTClosures.hpp" -#include "stl_vector_eigen.h" - -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); - -namespace py = pybind11; -using namespace py::literals; - -namespace gt_closures { - -PYBIND11_MODULE(gt_closures_pybind, m) { - auto vector3dvector = pybind_eigen_vector_of_vector( - m, "_Vector3dVector", "std::vector", - py::py_array_to_vectors_double); - auto vector2ivector = pybind_eigen_vector_of_vector( - m, "_Vector2iVector", "std::vector", - py::py_array_to_vectors_int); - - py::class_> gt_closures(m, "_GTClosures", ""); - gt_closures - .def(py::init(), "dataset_size"_a, - "sampling_distance"_a, "overlap_threshold"_a, "voxel_size"_a, "max_range"_a) - .def("_AddPointCloud", >Closures::AddPointCloud, "idx"_a, "pointcloud"_a, "pose"_a) - .def("_GetSegments", >Closures::GetSegments) - .def("_ComputeClosuresForQuerySegment", >Closures::ComputeClosuresForQuerySegment, - "query_segment_idx"_a); -} -} // namespace gt_closures diff --git a/python/map_closures/pybind/map_closures_pybind.cpp b/python/map_closures/pybind/map_closures_pybind.cpp index 718a667..92a1354 100644 --- a/python/map_closures/pybind/map_closures_pybind.cpp +++ b/python/map_closures/pybind/map_closures_pybind.cpp @@ -77,7 +77,8 @@ PYBIND11_MODULE(map_closures_pybind, m) { cv::cv2eigen(density_map.grid, density_map_eigen); return density_map_eigen; }) - .def("_MatchAndAdd", &MapClosures::MatchAndAdd, "map_id"_a, "local_map"_a) - .def("_ValidateClosure", &MapClosures::ValidateClosure, "reference_id"_a, "query_id"_a); + .def("_GetBestClosure", &MapClosures::GetBestClosure, "query_id"_a, "local_map"_a) + .def("_GetTopKClosures", &MapClosures::GetTopKClosures, "query_id"_a, "local_map"_a, "k"_a) + .def("_GetClosures", &MapClosures::GetClosures, "query_id"_a, "local_map"_a); } } // namespace map_closures diff --git a/python/map_closures/tools/gt_closures.py b/python/map_closures/tools/gt_closures.py deleted file mode 100644 index da55b8b..0000000 --- a/python/map_closures/tools/gt_closures.py +++ /dev/null @@ -1,167 +0,0 @@ -# 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 pathlib import Path -from typing import Optional - -import numpy as np -import typer -from tqdm import tqdm - -from map_closures.datasets import eval_dataloaders, sequence_dataloaders -from map_closures.pybind import gt_closures_pybind - - -def name_callback(value: str): - if not value: - return value - dl = eval_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 = eval_dataloaders() - -docstring = f""" -Generate Ground Truth Loop Closure Indices\n -\b -Examples:\n -# Use a specific dataloader with groundtruth poses: apollo, kitti, mulran, ncd, helipr -$ gt_closure_pipeline mulran -""" - - -@app.command(help=docstring) -def gt_closure_pipeline( - dataloader: str = typer.Argument( - None, - show_default=False, - case_sensitive=False, - autocompletion=eval_dataloaders, - callback=name_callback, - help="Use a specific dataloader from those supported by MapClosures", - ), - data: Path = typer.Argument( - ..., - help="The data directory used by the specified dataloader", - show_default=False, - ), - # Aditional Options --------------------------------------------------------------------------- - sequence: Optional[str] = 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", - ), - max_range: Optional[float] = typer.Option( - 100.0, - "--max_range", - "-r", - show_default=True, - help="[Optional] Maximum range of sensor / Maximum distance between closures", - rich_help_panel="Additional Options", - ), - overlap_threshold: Optional[float] = typer.Option( - 0.5, - "--overlap", - "-o", - show_default=True, - help="[Optional] Overlap Threshold between scans at closures", - rich_help_panel="Additional Options", - ), - voxel_size: Optional[float] = typer.Option( - 0.5, - "--voxel_size", - "-v", - show_default=True, - help="[Optional] Voxel size for downsamlping scans", - rich_help_panel="Additional Options", - ), -): - if dataloader in sequence_dataloaders() and sequence is None: - print('[ERROR] You must specify a sequence "--sequence"') - raise typer.Exit(code=1) - - from map_closures.datasets import dataset_factory - - dataset = dataset_factory( - dataloader=dataloader, - data_dir=data, - # Additional options - sequence=sequence, - ) - if hasattr(dataset, "gt_poses"): - generate_gt_closures(dataset, max_range, overlap_threshold, voxel_size) - else: - print("[ERROR] Groundtruth poses not found") - raise typer.Exit(code=1) - - -def run(): - app() - - -def generate_gt_closures( - dataset, max_range: float, overlap_threshold: float = 0.5, voxel_size: float = 0.5 -): - base_dir = dataset.sequence_dir if hasattr(dataset, "sequence_dir") else "" - os.makedirs(os.path.join(base_dir, "loop_closure"), exist_ok=True) - file_path_closures = os.path.join(base_dir, "loop_closure", "gt_closures.txt") - if os.path.exists(file_path_closures): - closures = np.loadtxt(file_path_closures, dtype=int) - print(f"[INFO] Found closure ground truth at {file_path_closures}") - - else: - print("[INFO] Computing Ground Truth Closures!") - sampling_distance = 2.0 - gt_closures_pipeline = gt_closures_pybind._GTClosures( - len(dataset), sampling_distance, overlap_threshold, voxel_size, max_range - ) - for idx in tqdm(range(len(dataset)), "Loading Data ..."): - try: - points, _ = dataset[idx] - except ValueError: - points = dataset[idx] - points = points[np.where(np.linalg.norm(points, axis=1) < max_range)[0]] - gt_closures_pipeline._AddPointCloud( - idx, gt_closures_pybind._Vector3dVector(points), dataset.gt_poses[idx] - ) - num_query_segments = gt_closures_pipeline._GetSegments() - closures = [] - for query_segment_idx in tqdm( - range(num_query_segments), "Computing Groundtruth Closures ..." - ): - closures.append( - np.asarray( - gt_closures_pipeline._ComputeClosuresForQuerySegment(query_segment_idx), int - ) - ) - closures = np.vstack(closures) - np.savetxt(file_path_closures, closures) - - return closures diff --git a/python/pyproject.toml b/python/pyproject.toml index d3bd914..748ea75 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build" [project] name = "map_closures" -version = "1.0.1" +version = "2.0.0" description = "Effectively Detecting Loop Closures using Point Cloud Density Maps" readme = "README.md" authors = [{ name = "Saurabh Gupta" }, { name = "Tiziano Guadagnino" }]