From 7a1927477f7eeee729687f98df7e732aa9cee991 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 22 Nov 2024 11:10:35 +0100 Subject: [PATCH 01/16] Add Bonxai and move kiss icp cmake --- cpp/kinematic_icp/3rdparty/bonxai/LICENSE | 373 ++++++++++++++++++ .../3rdparty/bonxai/bonxai.cmake | 30 ++ .../3rdparty/include_dependencies.cmake | 2 + .../{ => 3rdparty}/kiss_icp/LICENSE | 0 .../{ => 3rdparty}/kiss_icp/kiss-icp.cmake | 0 cpp/kinematic_icp/CMakeLists.txt | 4 +- 6 files changed, 408 insertions(+), 1 deletion(-) create mode 100644 cpp/kinematic_icp/3rdparty/bonxai/LICENSE create mode 100644 cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake create mode 100644 cpp/kinematic_icp/3rdparty/include_dependencies.cmake rename cpp/kinematic_icp/{ => 3rdparty}/kiss_icp/LICENSE (100%) rename cpp/kinematic_icp/{ => 3rdparty}/kiss_icp/kiss-icp.cmake (100%) diff --git a/cpp/kinematic_icp/3rdparty/bonxai/LICENSE b/cpp/kinematic_icp/3rdparty/bonxai/LICENSE new file mode 100644 index 0000000..a612ad9 --- /dev/null +++ b/cpp/kinematic_icp/3rdparty/bonxai/LICENSE @@ -0,0 +1,373 @@ +Mozilla Public License Version 2.0 +================================== + +1. Definitions +-------------- + +1.1. "Contributor" + means each individual or legal entity that creates, contributes to + the creation of, or owns Covered Software. + +1.2. "Contributor Version" + means the combination of the Contributions of others (if any) used + by a Contributor and that particular Contributor's Contribution. + +1.3. "Contribution" + means Covered Software of a particular Contributor. + +1.4. "Covered Software" + means Source Code Form to which the initial Contributor has attached + the notice in Exhibit A, the Executable Form of such Source Code + Form, and Modifications of such Source Code Form, in each case + including portions thereof. + +1.5. "Incompatible With Secondary Licenses" + means + + (a) that the initial Contributor has attached the notice described + in Exhibit B to the Covered Software; or + + (b) that the Covered Software was made available under the terms of + version 1.1 or earlier of the License, but not also under the + terms of a Secondary License. + +1.6. "Executable Form" + means any form of the work other than Source Code Form. + +1.7. "Larger Work" + means a work that combines Covered Software with other material, in + a separate file or files, that is not Covered Software. + +1.8. "License" + means this document. + +1.9. "Licensable" + means having the right to grant, to the maximum extent possible, + whether at the time of the initial grant or subsequently, any and + all of the rights conveyed by this License. + +1.10. "Modifications" + means any of the following: + + (a) any file in Source Code Form that results from an addition to, + deletion from, or modification of the contents of Covered + Software; or + + (b) any new file in Source Code Form that contains any Covered + Software. + +1.11. "Patent Claims" of a Contributor + means any patent claim(s), including without limitation, method, + process, and apparatus claims, in any patent Licensable by such + Contributor that would be infringed, but for the grant of the + License, by the making, using, selling, offering for sale, having + made, import, or transfer of either its Contributions or its + Contributor Version. + +1.12. "Secondary License" + means either the GNU General Public License, Version 2.0, the GNU + Lesser General Public License, Version 2.1, the GNU Affero General + Public License, Version 3.0, or any later versions of those + licenses. + +1.13. "Source Code Form" + means the form of the work preferred for making modifications. + +1.14. "You" (or "Your") + means an individual or a legal entity exercising rights under this + License. For legal entities, "You" includes any entity that + controls, is controlled by, or is under common control with You. For + purposes of this definition, "control" means (a) the power, direct + or indirect, to cause the direction or management of such entity, + whether by contract or otherwise, or (b) ownership of more than + fifty percent (50%) of the outstanding shares or beneficial + ownership of such entity. + +2. License Grants and Conditions +-------------------------------- + +2.1. Grants + +Each Contributor hereby grants You a world-wide, royalty-free, +non-exclusive license: + +(a) under intellectual property rights (other than patent or trademark) + Licensable by such Contributor to use, reproduce, make available, + modify, display, perform, distribute, and otherwise exploit its + Contributions, either on an unmodified basis, with Modifications, or + as part of a Larger Work; and + +(b) under Patent Claims of such Contributor to make, use, sell, offer + for sale, have made, import, and otherwise transfer either its + Contributions or its Contributor Version. + +2.2. Effective Date + +The licenses granted in Section 2.1 with respect to any Contribution +become effective for each Contribution on the date the Contributor first +distributes such Contribution. + +2.3. Limitations on Grant Scope + +The licenses granted in this Section 2 are the only rights granted under +this License. No additional rights or licenses will be implied from the +distribution or licensing of Covered Software under this License. +Notwithstanding Section 2.1(b) above, no patent license is granted by a +Contributor: + +(a) for any code that a Contributor has removed from Covered Software; + or + +(b) for infringements caused by: (i) Your and any other third party's + modifications of Covered Software, or (ii) the combination of its + Contributions with other software (except as part of its Contributor + Version); or + +(c) under Patent Claims infringed by Covered Software in the absence of + its Contributions. + +This License does not grant any rights in the trademarks, service marks, +or logos of any Contributor (except as may be necessary to comply with +the notice requirements in Section 3.4). + +2.4. Subsequent Licenses + +No Contributor makes additional grants as a result of Your choice to +distribute the Covered Software under a subsequent version of this +License (see Section 10.2) or under the terms of a Secondary License (if +permitted under the terms of Section 3.3). + +2.5. Representation + +Each Contributor represents that the Contributor believes its +Contributions are its original creation(s) or it has sufficient rights +to grant the rights to its Contributions conveyed by this License. + +2.6. Fair Use + +This License is not intended to limit any rights You have under +applicable copyright doctrines of fair use, fair dealing, or other +equivalents. + +2.7. Conditions + +Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted +in Section 2.1. + +3. Responsibilities +------------------- + +3.1. Distribution of Source Form + +All distribution of Covered Software in Source Code Form, including any +Modifications that You create or to which You contribute, must be under +the terms of this License. You must inform recipients that the Source +Code Form of the Covered Software is governed by the terms of this +License, and how they can obtain a copy of this License. You may not +attempt to alter or restrict the recipients' rights in the Source Code +Form. + +3.2. Distribution of Executable Form + +If You distribute Covered Software in Executable Form then: + +(a) such Covered Software must also be made available in Source Code + Form, as described in Section 3.1, and You must inform recipients of + the Executable Form how they can obtain a copy of such Source Code + Form by reasonable means in a timely manner, at a charge no more + than the cost of distribution to the recipient; and + +(b) You may distribute such Executable Form under the terms of this + License, or sublicense it under different terms, provided that the + license for the Executable Form does not attempt to limit or alter + the recipients' rights in the Source Code Form under this License. + +3.3. Distribution of a Larger Work + +You may create and distribute a Larger Work under terms of Your choice, +provided that You also comply with the requirements of this License for +the Covered Software. If the Larger Work is a combination of Covered +Software with a work governed by one or more Secondary Licenses, and the +Covered Software is not Incompatible With Secondary Licenses, this +License permits You to additionally distribute such Covered Software +under the terms of such Secondary License(s), so that the recipient of +the Larger Work may, at their option, further distribute the Covered +Software under the terms of either this License or such Secondary +License(s). + +3.4. Notices + +You may not remove or alter the substance of any license notices +(including copyright notices, patent notices, disclaimers of warranty, +or limitations of liability) contained within the Source Code Form of +the Covered Software, except that You may alter any license notices to +the extent required to remedy known factual inaccuracies. + +3.5. Application of Additional Terms + +You may choose to offer, and to charge a fee for, warranty, support, +indemnity or liability obligations to one or more recipients of Covered +Software. However, You may do so only on Your own behalf, and not on +behalf of any Contributor. You must make it absolutely clear that any +such warranty, support, indemnity, or liability obligation is offered by +You alone, and You hereby agree to indemnify every Contributor for any +liability incurred by such Contributor as a result of warranty, support, +indemnity or liability terms You offer. You may include additional +disclaimers of warranty and limitations of liability specific to any +jurisdiction. + +4. Inability to Comply Due to Statute or Regulation +--------------------------------------------------- + +If it is impossible for You to comply with any of the terms of this +License with respect to some or all of the Covered Software due to +statute, judicial order, or regulation then You must: (a) comply with +the terms of this License to the maximum extent possible; and (b) +describe the limitations and the code they affect. Such description must +be placed in a text file included with all distributions of the Covered +Software under this License. Except to the extent prohibited by statute +or regulation, such description must be sufficiently detailed for a +recipient of ordinary skill to be able to understand it. + +5. Termination +-------------- + +5.1. The rights granted under this License will terminate automatically +if You fail to comply with any of its terms. However, if You become +compliant, then the rights granted under this License from a particular +Contributor are reinstated (a) provisionally, unless and until such +Contributor explicitly and finally terminates Your grants, and (b) on an +ongoing basis, if such Contributor fails to notify You of the +non-compliance by some reasonable means prior to 60 days after You have +come back into compliance. Moreover, Your grants from a particular +Contributor are reinstated on an ongoing basis if such Contributor +notifies You of the non-compliance by some reasonable means, this is the +first time You have received notice of non-compliance with this License +from such Contributor, and You become compliant prior to 30 days after +Your receipt of the notice. + +5.2. If You initiate litigation against any entity by asserting a patent +infringement claim (excluding declaratory judgment actions, +counter-claims, and cross-claims) alleging that a Contributor Version +directly or indirectly infringes any patent, then the rights granted to +You by any and all Contributors for the Covered Software under Section +2.1 of this License shall terminate. + +5.3. In the event of termination under Sections 5.1 or 5.2 above, all +end user license agreements (excluding distributors and resellers) which +have been validly granted by You or Your distributors under this License +prior to termination shall survive termination. + +************************************************************************ +* * +* 6. Disclaimer of Warranty * +* ------------------------- * +* * +* Covered Software is provided under this License on an "as is" * +* basis, without warranty of any kind, either expressed, implied, or * +* statutory, including, without limitation, warranties that the * +* Covered Software is free of defects, merchantable, fit for a * +* particular purpose or non-infringing. The entire risk as to the * +* quality and performance of the Covered Software is with You. * +* Should any Covered Software prove defective in any respect, You * +* (not any Contributor) assume the cost of any necessary servicing, * +* repair, or correction. This disclaimer of warranty constitutes an * +* essential part of this License. No use of any Covered Software is * +* authorized under this License except under this disclaimer. * +* * +************************************************************************ + +************************************************************************ +* * +* 7. Limitation of Liability * +* -------------------------- * +* * +* Under no circumstances and under no legal theory, whether tort * +* (including negligence), contract, or otherwise, shall any * +* Contributor, or anyone who distributes Covered Software as * +* permitted above, be liable to You for any direct, indirect, * +* special, incidental, or consequential damages of any character * +* including, without limitation, damages for lost profits, loss of * +* goodwill, work stoppage, computer failure or malfunction, or any * +* and all other commercial damages or losses, even if such party * +* shall have been informed of the possibility of such damages. This * +* limitation of liability shall not apply to liability for death or * +* personal injury resulting from such party's negligence to the * +* extent applicable law prohibits such limitation. Some * +* jurisdictions do not allow the exclusion or limitation of * +* incidental or consequential damages, so this exclusion and * +* limitation may not apply to You. * +* * +************************************************************************ + +8. Litigation +------------- + +Any litigation relating to this License may be brought only in the +courts of a jurisdiction where the defendant maintains its principal +place of business and such litigation shall be governed by laws of that +jurisdiction, without reference to its conflict-of-law provisions. +Nothing in this Section shall prevent a party's ability to bring +cross-claims or counter-claims. + +9. Miscellaneous +---------------- + +This License represents the complete agreement concerning the subject +matter hereof. If any provision of this License is held to be +unenforceable, such provision shall be reformed only to the extent +necessary to make it enforceable. Any law or regulation which provides +that the language of a contract shall be construed against the drafter +shall not be used to construe this License against a Contributor. + +10. Versions of the License +--------------------------- + +10.1. New Versions + +Mozilla Foundation is the license steward. Except as provided in Section +10.3, no one other than the license steward has the right to modify or +publish new versions of this License. Each version will be given a +distinguishing version number. + +10.2. Effect of New Versions + +You may distribute the Covered Software under the terms of the version +of the License under which You originally received the Covered Software, +or under the terms of any subsequent version published by the license +steward. + +10.3. Modified Versions + +If you create software not governed by this License, and you want to +create a new license for such software, you may create and use a +modified version of this License if you rename the license and remove +any references to the name of the license steward (except to note that +such modified license differs from this License). + +10.4. Distributing Source Code Form that is Incompatible With Secondary +Licenses + +If You choose to distribute Source Code Form that is Incompatible With +Secondary Licenses under the terms of this version of the License, the +notice described in Exhibit B of this License must be attached. + +Exhibit A - Source Code Form License Notice +------------------------------------------- + + This Source Code Form is subject to the terms of the Mozilla Public + License, v. 2.0. If a copy of the MPL was not distributed with this + file, You can obtain one at http://mozilla.org/MPL/2.0/. + +If it is not possible or desirable to put the notice in a particular +file, then You may include the notice in a location (such as a LICENSE +file in a relevant directory) where a recipient would be likely to look +for such a notice. + +You may add additional accurate notices of copyright ownership. + +Exhibit B - "Incompatible With Secondary Licenses" Notice +--------------------------------------------------------- + + This Source Code Form is "Incompatible With Secondary Licenses", as + defined by the Mozilla Public License, v. 2.0. diff --git a/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake new file mode 100644 index 0000000..47fcfc0 --- /dev/null +++ b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake @@ -0,0 +1,30 @@ +# MIT License +# +# Copyright (c) 2024 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. +# Silence timestamp warning +if(CMAKE_VERSION VERSION_GREATER 3.24) + cmake_policy(SET CMP0135 OLD) +endif() + +include(FetchContent) +FetchContent_Declare(bonxai URL https://github.com/facontidavide/Bonxai/archive/refs/tags/v0.6.0.tar.gz) +FetchContent_MakeAvailable(bonxai) diff --git a/cpp/kinematic_icp/3rdparty/include_dependencies.cmake b/cpp/kinematic_icp/3rdparty/include_dependencies.cmake new file mode 100644 index 0000000..ac45514 --- /dev/null +++ b/cpp/kinematic_icp/3rdparty/include_dependencies.cmake @@ -0,0 +1,2 @@ +include(${CMAKE_CURRENT_LIST_DIR}/kiss_icp/kiss-icp.cmake) +include(${CMAKE_CURRENT_LIST_DIR}/bonxai/bonxai.cmake) diff --git a/cpp/kinematic_icp/kiss_icp/LICENSE b/cpp/kinematic_icp/3rdparty/kiss_icp/LICENSE similarity index 100% rename from cpp/kinematic_icp/kiss_icp/LICENSE rename to cpp/kinematic_icp/3rdparty/kiss_icp/LICENSE diff --git a/cpp/kinematic_icp/kiss_icp/kiss-icp.cmake b/cpp/kinematic_icp/3rdparty/kiss_icp/kiss-icp.cmake similarity index 100% rename from cpp/kinematic_icp/kiss_icp/kiss-icp.cmake rename to cpp/kinematic_icp/3rdparty/kiss_icp/kiss-icp.cmake diff --git a/cpp/kinematic_icp/CMakeLists.txt b/cpp/kinematic_icp/CMakeLists.txt index 93889b4..1d58cc6 100644 --- a/cpp/kinematic_icp/CMakeLists.txt +++ b/cpp/kinematic_icp/CMakeLists.txt @@ -29,7 +29,8 @@ option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON) option(USE_SYSTEM_TSL-ROBIN-MAP "Use system pre-installed tsl_robin" ON) option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) -include(kiss_icp/kiss-icp.cmake) + +include(3rdparty/include_dependencies.cmake) # KISS dependecies that are also used in Kinematic. # If the internal FetchContent from kiss is in use (through the above options) @@ -54,6 +55,7 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) include(cmake/CompilerOptions.cmake) +add_subdirectory(local_map) add_subdirectory(correspondence_threshold) add_subdirectory(registration) add_subdirectory(pipeline) From 00845545aeb27b29ec70264c43719673bc65c646 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 22 Nov 2024 11:10:58 +0100 Subject: [PATCH 02/16] Start to work on the local map representation --- cpp/kinematic_icp/local_map/CMakeLists.txt | 26 ++++++++ .../local_map/SparseVoxelGrid.cpp | 38 ++++++++++++ .../local_map/SparseVoxelGrid.hpp | 60 +++++++++++++++++++ 3 files changed, 124 insertions(+) create mode 100644 cpp/kinematic_icp/local_map/CMakeLists.txt create mode 100644 cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp create mode 100644 cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp diff --git a/cpp/kinematic_icp/local_map/CMakeLists.txt b/cpp/kinematic_icp/local_map/CMakeLists.txt new file mode 100644 index 0000000..9515d8c --- /dev/null +++ b/cpp/kinematic_icp/local_map/CMakeLists.txt @@ -0,0 +1,26 @@ +# MIT License +# +# Copyright (c) 2024 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(kinematic_icp_local_map STATIC) +target_sources(kinematic_icp_local_map PRIVATE SparseVoxelGrid.cpp) +target_link_libraries(kinematic_icp_local_map PUBLIC Eigen3::Eigen Sophus::Sophus bonxai_core) +set_global_target_properties(kinematic_icp_local_map) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp new file mode 100644 index 0000000..11a0ad7 --- /dev/null +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -0,0 +1,38 @@ +// MIT License + +// Copyright (c) 2024 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 "SparseVoxelGrid.hpp" + +#include +#include +#include + +namespace kinematic_icp { +void VoxelBlock::addPoint(const Eigen::Vector3d &p) { + points_[size_++] = p; + if (size_ > 20) { + throw std::runtime_error("VoxelBlock| size is too big, want to fix somehow"); + } + { statements } +} +} // namespace kinematic_icp diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp new file mode 100644 index 0000000..29c608b --- /dev/null +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp @@ -0,0 +1,60 @@ +// MIT License + +// Copyright (c) 2024 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 + +namespace kinematic_icp { +// Following the Cult of Faconti +struct VoxelBlock { + using IteratorType = std::array::iterator; + using ConstIteratorType = std::array::const_iterator; + VoxelBlock() = default; + VoxelBlock(const VoxelBlock &other) = delete; + VoxelBlock(VoxelBlock &&other) = default; + void addPoint(const Eigen::Vector3d &p); + + const Eigen::Vector3d &front() const { return points_.front(); } + inline IteratorType begin() { return points_.begin(); } + inline ConstIteratorType cbegin() const { return points_.cbegin(); } + + inline IteratorType end() { return points_.end(); } + inline ConstIteratorType cend() const { return points_.cend(); } + +private: + std::array points_; + std::size_t size_ = 0; +}; +struct SparseVoxelGrid { + explicit SparseVoxelGrid(const double voxel_size, + const double clipping_distance, + const unsigned int max_points_per_voxel); + + double voxel_size_; + double max_distance_; + unsigned int max_points_per_voxel_; + Bonxai::VoxelGrid map_; +}; +} // namespace kinematic_icp From 4adc4e021d7080e9093f4956960bdb81a20788d6 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 22 Nov 2024 12:14:32 +0100 Subject: [PATCH 03/16] Add old API --- .../local_map/SparseVoxelGrid.cpp | 55 ++++++++++++++++++- .../local_map/SparseVoxelGrid.hpp | 38 ++++++++++--- 2 files changed, 84 insertions(+), 9 deletions(-) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp index 11a0ad7..b7d126e 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -30,9 +30,60 @@ namespace kinematic_icp { void VoxelBlock::addPoint(const Eigen::Vector3d &p) { points_[size_++] = p; - if (size_ > 20) { + if (size_ > VoxelBlock::MAX_SIZE) { throw std::runtime_error("VoxelBlock| size is too big, want to fix somehow"); } - { statements } } + +SparseVoxelGrid::SparseVoxelGrid(const double voxel_size, + const double clipping_distance, + const unsigned int max_points_per_voxel) + : voxel_size_(voxel_size), + clipping_distance_(clipping_distance), + max_points_per_voxel_(max_points_per_voxel), + map_(voxel_size), + accessor_(map_.createAccessor()) {} + +void SparseVoxelGrid::AddPoints(const std::vector &points) { + const double map_resolution = std::sqrt(voxel_size_ * voxel_size_ / max_points_per_voxel_); + std::for_each(points.cbegin(), points.cend(), [this](const Eigen::Vector3d &p) { + const auto voxel_coordinates = map_.posToCoord(p.x(), p.y(), p.z()); + VoxelBlock *voxel_points = accessor_.value(voxel_coordinates, true); + if (voxel_points->size() == max_points_per_voxel_ || + std::any_of(voxel_points->cbegin(), voxel_points->cend(), [&](const auto &voxel_point) { + return (voxel_point - p).norm() < map_resolution; + })) { + return; + } + voxel_points->addPoint(p); + }); +} + +void SparseVoxelGrid::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { + map_.forEachCell([&origin, this](VoxelBlock &block, const auto &coordinate) { + if ((block.front() - origin).norm() >= clipping_distance_) { + accessor_.setCellOff(coordinate); + } + }); + map_.releaseUnusedMemory(); +} + +void SparseVoxelGrid::Update(const std::vector &points, const Sophus::SE3d &pose) { + std::vector points_transformed(points.size()); + std::transform(points.cbegin(), points.cend(), points_transformed.begin(), + [&](const auto &point) { return pose * point; }); + const Eigen::Vector3d &origin = pose.translation(); + AddPoints(points_transformed); + RemovePointsFarFromLocation(origin); +} + +std::vector SparseVoxelGrid::Pointcloud() const { + std::vector point_cloud; + point_cloud.reserve(map_.activeCellsCount() * max_points_per_voxel_); + map_.forEachCell([&point_cloud, this](VoxelBlock &block, const auto &) { + point_cloud.insert(point_cloud.end(), block.cbegin(), block.cend()); + }); + return point_cloud; +} + } // namespace kinematic_icp diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp index 29c608b..6563aeb 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp @@ -29,32 +29,56 @@ namespace kinematic_icp { // Following the Cult of Faconti struct VoxelBlock { - using IteratorType = std::array::iterator; - using ConstIteratorType = std::array::const_iterator; + static constexpr size_t MAX_SIZE = 20; + using PointsContainerType = std::array; + using IteratorType = PointsContainerType::iterator; + using ConstIteratorType = PointsContainerType::const_iterator; + VoxelBlock() = default; + VoxelBlock(const VoxelBlock &other) = delete; + VoxelBlock &operator=(const VoxelBlock &) = delete; + VoxelBlock(VoxelBlock &&other) = default; + VoxelBlock &operator=(VoxelBlock &&other) = default; + void addPoint(const Eigen::Vector3d &p); + inline std::size_t size() const { return std::distance(cbegin(), cend()); } + const Eigen::Vector3d &front() const { return points_.front(); } inline IteratorType begin() { return points_.begin(); } inline ConstIteratorType cbegin() const { return points_.cbegin(); } - inline IteratorType end() { return points_.end(); } - inline ConstIteratorType cend() const { return points_.cend(); } + inline IteratorType end() { return std::next(begin(), size_); } + inline ConstIteratorType cend() const { return std::next(cbegin(), size_); } private: - std::array points_; - std::size_t size_ = 0; + std::array points_; + uint16_t size_ = 0; }; + struct SparseVoxelGrid { explicit SparseVoxelGrid(const double voxel_size, const double clipping_distance, const unsigned int max_points_per_voxel); + inline void Clear() { map_.clear(Bonxai::ClearOption::CLEAR_MEMORY); } + inline bool Empty() { return map_.activeCellsCount() == 0; } + void Update(const std::vector &points, const Sophus::SE3d &pose); + void AddPoints(const std::vector &points); + void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); + std::vector Pointcloud() const; + std::tuple GetClosestNeighbor(const Eigen::Vector3d &query) const; + double voxel_size_; - double max_distance_; + double clipping_distance_; unsigned int max_points_per_voxel_; Bonxai::VoxelGrid map_; + +private: + using AccessorType = typename Bonxai::VoxelGrid::Accessor; + AccessorType accessor_; }; + } // namespace kinematic_icp From 01188b4ff8c1ff48e73f7649919a056fd0a827da Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 22 Nov 2024 17:18:25 +0100 Subject: [PATCH 04/16] At least replicate the functionalities of kiss map, need to test and find a way to remove unused voxels --- .../local_map/SparseVoxelGrid.cpp | 50 +++++++++++++++++-- .../local_map/SparseVoxelGrid.hpp | 4 +- cpp/kinematic_icp/pipeline/CMakeLists.txt | 2 +- cpp/kinematic_icp/pipeline/KinematicICP.hpp | 11 ++-- cpp/kinematic_icp/registration/CMakeLists.txt | 2 +- .../registration/Registration.cpp | 5 +- .../registration/Registration.hpp | 5 +- 7 files changed, 59 insertions(+), 20 deletions(-) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp index b7d126e..df6cae0 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -27,6 +27,23 @@ #include #include +#include "bonxai/grid_coord.hpp" + +namespace { +static std::array shifts{ + Bonxai::CoordT{-1, -1, -1}, Bonxai::CoordT{-1, -1, 0}, Bonxai::CoordT{-1, -1, 1}, + Bonxai::CoordT{-1, 0, -1}, Bonxai::CoordT{-1, 0, 0}, Bonxai::CoordT{-1, 0, 1}, + Bonxai::CoordT{-1, 1, -1}, Bonxai::CoordT{-1, 1, 0}, Bonxai::CoordT{-1, 1, 1}, + + Bonxai::CoordT{0, -1, -1}, Bonxai::CoordT{0, -1, 0}, Bonxai::CoordT{0, -1, 1}, + Bonxai::CoordT{0, 0, -1}, Bonxai::CoordT{0, 0, 0}, Bonxai::CoordT{0, 0, 1}, + Bonxai::CoordT{0, 1, -1}, Bonxai::CoordT{0, 1, 0}, Bonxai::CoordT{0, 1, 1}, + + Bonxai::CoordT{1, -1, -1}, Bonxai::CoordT{1, -1, 0}, Bonxai::CoordT{1, -1, 1}, + Bonxai::CoordT{1, 0, -1}, Bonxai::CoordT{1, 0, 0}, Bonxai::CoordT{1, 0, 1}, + Bonxai::CoordT{1, 1, -1}, Bonxai::CoordT{1, 1, 0}, Bonxai::CoordT{1, 1, 1}}; +} + namespace kinematic_icp { void VoxelBlock::addPoint(const Eigen::Vector3d &p) { points_[size_++] = p; @@ -44,9 +61,33 @@ SparseVoxelGrid::SparseVoxelGrid(const double voxel_size, map_(voxel_size), accessor_(map_.createAccessor()) {} +std::tuple SparseVoxelGrid::GetClosestNeighbor( + const Eigen::Vector3d &query) const { + Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); + double closest_distance = std::numeric_limits::max(); + const auto const_accessor = map_.createConstAccessor(); + const Bonxai::CoordT query_voxel = map_.posToCoord(query); + std::for_each(shifts.cbegin(), shifts.cend(), [&](const Bonxai::CoordT &voxel_coordinates) { + const VoxelBlock *voxel_points = const_accessor.value(query_voxel + voxel_coordinates); + if (voxel_points != nullptr) { + const Eigen::Vector3d &neighbor = + *std::min_element(voxel_points->cbegin(), voxel_points->cend(), + [&](const auto &lhs, const auto &rhs) { + return (lhs - query).norm() < (rhs - query).norm(); + }); + double distance = (neighbor - query).norm(); + if (distance < closest_distance) { + closest_neighbor = neighbor; + closest_distance = distance; + } + } + }); + return std::make_tuple(closest_neighbor, closest_distance); +} + void SparseVoxelGrid::AddPoints(const std::vector &points) { const double map_resolution = std::sqrt(voxel_size_ * voxel_size_ / max_points_per_voxel_); - std::for_each(points.cbegin(), points.cend(), [this](const Eigen::Vector3d &p) { + std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &p) { const auto voxel_coordinates = map_.posToCoord(p.x(), p.y(), p.z()); VoxelBlock *voxel_points = accessor_.value(voxel_coordinates, true); if (voxel_points->size() == max_points_per_voxel_ || @@ -60,12 +101,13 @@ void SparseVoxelGrid::AddPoints(const std::vector &points) { } void SparseVoxelGrid::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { - map_.forEachCell([&origin, this](VoxelBlock &block, const auto &coordinate) { + auto remove_voxel = [this, &origin](VoxelBlock &block, const Bonxai::CoordT &coordinate) { if ((block.front() - origin).norm() >= clipping_distance_) { accessor_.setCellOff(coordinate); } - }); - map_.releaseUnusedMemory(); + }; + map_.forEachCell(remove_voxel); + // map_.releaseUnusedMemory(); } void SparseVoxelGrid::Update(const std::vector &points, const Sophus::SE3d &pose) { diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp index 6563aeb..e83e499 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp @@ -20,7 +20,7 @@ // 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 @@ -64,7 +64,7 @@ struct SparseVoxelGrid { const unsigned int max_points_per_voxel); inline void Clear() { map_.clear(Bonxai::ClearOption::CLEAR_MEMORY); } - inline bool Empty() { return map_.activeCellsCount() == 0; } + inline bool Empty() const { return map_.activeCellsCount() == 0; } void Update(const std::vector &points, const Sophus::SE3d &pose); void AddPoints(const std::vector &points); void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); diff --git a/cpp/kinematic_icp/pipeline/CMakeLists.txt b/cpp/kinematic_icp/pipeline/CMakeLists.txt index 3a95025..95262ac 100644 --- a/cpp/kinematic_icp/pipeline/CMakeLists.txt +++ b/cpp/kinematic_icp/pipeline/CMakeLists.txt @@ -23,5 +23,5 @@ add_library(kinematic_icp_pipeline STATIC) target_sources(kinematic_icp_pipeline PRIVATE KinematicICP.cpp) target_link_libraries(kinematic_icp_pipeline PUBLIC kinematic_icp_registration kinematic_icp_threshold - kiss_icp_pipeline) + kinematic_icp_local_map kiss_icp_core) set_global_target_properties(kinematic_icp_pipeline) diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.hpp b/cpp/kinematic_icp/pipeline/KinematicICP.hpp index c17a62e..d995a79 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.hpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.hpp @@ -24,14 +24,12 @@ #include #include -#include -#include -#include #include #include #include #include "kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp" +#include "kinematic_icp/local_map/SparseVoxelGrid.hpp" #include "kinematic_icp/registration/Registration.hpp" namespace kinematic_icp::pipeline { @@ -91,8 +89,8 @@ class KinematicICP { std::vector LocalMap() const { return local_map_.Pointcloud(); }; - const kiss_icp::VoxelHashMap &VoxelMap() const { return local_map_; }; - kiss_icp::VoxelHashMap &VoxelMap() { return local_map_; }; + const SparseVoxelGrid &VoxelMap() const { return local_map_; }; + SparseVoxelGrid &VoxelMap() { return local_map_; }; const Sophus::SE3d &pose() const { return last_pose_; } Sophus::SE3d &pose() { return last_pose_; } @@ -103,8 +101,7 @@ class KinematicICP { KinematicRegistration registration_; CorrespondenceThreshold correspondence_threshold_; Config config_; - // KISS-ICP pipeline modules - kiss_icp::VoxelHashMap local_map_; + SparseVoxelGrid local_map_; }; } // namespace kinematic_icp::pipeline diff --git a/cpp/kinematic_icp/registration/CMakeLists.txt b/cpp/kinematic_icp/registration/CMakeLists.txt index 3beaf8f..4dea243 100644 --- a/cpp/kinematic_icp/registration/CMakeLists.txt +++ b/cpp/kinematic_icp/registration/CMakeLists.txt @@ -22,5 +22,5 @@ # SOFTWARE. add_library(kinematic_icp_registration STATIC) target_sources(kinematic_icp_registration PRIVATE Registration.cpp) -target_link_libraries(kinematic_icp_registration PUBLIC kiss_icp_core Eigen3::Eigen TBB::tbb Sophus::Sophus) +target_link_libraries(kinematic_icp_registration PUBLIC kinematic_icp_local_map Eigen3::Eigen TBB::tbb Sophus::Sophus) set_global_target_properties(kinematic_icp_registration) diff --git a/cpp/kinematic_icp/registration/Registration.cpp b/cpp/kinematic_icp/registration/Registration.cpp index b95286a..06e712b 100644 --- a/cpp/kinematic_icp/registration/Registration.cpp +++ b/cpp/kinematic_icp/registration/Registration.cpp @@ -30,7 +30,6 @@ #include #include -#include #include #include #include @@ -58,7 +57,7 @@ double ComputeOdometryRegularization(const Correspondences &associations, } Correspondences DataAssociation(const std::vector &points, - const kiss_icp::VoxelHashMap &voxel_map, + const kinematic_icp::SparseVoxelGrid &voxel_map, const Sophus::SE3d &T, const double max_correspondance_distance) { using points_iterator = std::vector::const_iterator; @@ -160,7 +159,7 @@ KinematicRegistration::KinematicRegistration(const int max_num_iteration, } Sophus::SE3d KinematicRegistration::ComputeRobotMotion(const std::vector &frame, - const kiss_icp::VoxelHashMap &voxel_map, + const SparseVoxelGrid &voxel_map, const Sophus::SE3d &last_robot_pose, const Sophus::SE3d &relative_wheel_odometry, const double max_correspondence_distance) { diff --git a/cpp/kinematic_icp/registration/Registration.hpp b/cpp/kinematic_icp/registration/Registration.hpp index 4eec162..d17cbe8 100644 --- a/cpp/kinematic_icp/registration/Registration.hpp +++ b/cpp/kinematic_icp/registration/Registration.hpp @@ -23,10 +23,11 @@ #pragma once #include -#include #include #include +#include "kinematic_icp/local_map/SparseVoxelGrid.hpp" + namespace kinematic_icp { struct KinematicRegistration { @@ -37,7 +38,7 @@ struct KinematicRegistration { const double fixed_regularization); Sophus::SE3d ComputeRobotMotion(const std::vector &frame, - const kiss_icp::VoxelHashMap &voxel_map, + const SparseVoxelGrid &voxel_map, const Sophus::SE3d &last_robot_pose, const Sophus::SE3d &relative_wheel_odometry, const double max_correspondence_distance); From be18fae806c298529afdb8e53a986eacdb069996 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 25 Nov 2024 09:34:32 +0100 Subject: [PATCH 05/16] Just build bonxai_core --- cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake index 47fcfc0..3ce085f 100644 --- a/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake +++ b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake @@ -26,5 +26,5 @@ if(CMAKE_VERSION VERSION_GREATER 3.24) endif() include(FetchContent) -FetchContent_Declare(bonxai URL https://github.com/facontidavide/Bonxai/archive/refs/tags/v0.6.0.tar.gz) +FetchContent_Declare(bonxai URL https://github.com/facontidavide/Bonxai/archive/refs/tags/v0.6.0.tar.gz SOURCE_SUBDIR bonxai_core) FetchContent_MakeAvailable(bonxai) From 11b9c5aa68c97599eb2d75f746c4d051a0addab3 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 25 Nov 2024 15:41:56 +0100 Subject: [PATCH 06/16] Add patch --- cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake index 3ce085f..edf2613 100644 --- a/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake +++ b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake @@ -26,5 +26,7 @@ if(CMAKE_VERSION VERSION_GREATER 3.24) endif() include(FetchContent) -FetchContent_Declare(bonxai URL https://github.com/facontidavide/Bonxai/archive/refs/tags/v0.6.0.tar.gz SOURCE_SUBDIR bonxai_core) +FetchContent_Declare( + bonxai URL https://github.com/facontidavide/Bonxai/archive/refs/tags/v0.6.0.tar.gz SOURCE_SUBDIR bonxai_core + PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/bonxai.patch UPDATE_DISCONNECTED 1) FetchContent_MakeAvailable(bonxai) From a7ddd20fd6816b51ee4d2c425dda26fbed0bc111 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 25 Nov 2024 15:42:09 +0100 Subject: [PATCH 07/16] To fix const correctness --- cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch diff --git a/cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch new file mode 100644 index 0000000..0ec94d8 --- /dev/null +++ b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch @@ -0,0 +1,13 @@ +diff --git a/bonxai_core/include/bonxai/bonxai.hpp b/bonxai_core/include/bonxai/bonxai.hpp +index 7d360cf..8359ba8 100644 +--- a/bonxai_core/include/bonxai/bonxai.hpp ++++ b/bonxai_core/include/bonxai/bonxai.hpp +@@ -367,7 +367,7 @@ inline size_t Grid::memUsage() const { + template + inline void VoxelGrid::releaseUnusedMemory() { + std::vector keys_to_delete; +- for (const auto& [key, inner_grid] : root_map) { ++ for (auto& [key, inner_grid] : root_map) { + for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) { + const int32_t inner_index = *inner_it; + auto& leaf_grid = inner_grid.cell(inner_index); From 7e9726f3b47f66b51e421d3283cfbf3be58a6913 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 26 Nov 2024 09:38:04 +0100 Subject: [PATCH 08/16] Remove small vector --- .../local_map/SparseVoxelGrid.cpp | 11 ++----- .../local_map/SparseVoxelGrid.hpp | 30 +------------------ 2 files changed, 4 insertions(+), 37 deletions(-) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp index df6cae0..7f4022e 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -45,12 +45,6 @@ static std::array shifts{ } namespace kinematic_icp { -void VoxelBlock::addPoint(const Eigen::Vector3d &p) { - points_[size_++] = p; - if (size_ > VoxelBlock::MAX_SIZE) { - throw std::runtime_error("VoxelBlock| size is too big, want to fix somehow"); - } -} SparseVoxelGrid::SparseVoxelGrid(const double voxel_size, const double clipping_distance, @@ -96,7 +90,8 @@ void SparseVoxelGrid::AddPoints(const std::vector &points) { })) { return; } - voxel_points->addPoint(p); + voxel_points->reserve(max_points_per_voxel_); + voxel_points->emplace_back(p); }); } @@ -107,7 +102,7 @@ void SparseVoxelGrid::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) } }; map_.forEachCell(remove_voxel); - // map_.releaseUnusedMemory(); + map_.releaseUnusedMemory(); } void SparseVoxelGrid::Update(const std::vector &points, const Sophus::SE3d &pose) { diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp index e83e499..92a2e03 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp @@ -27,36 +27,8 @@ #include namespace kinematic_icp { -// Following the Cult of Faconti -struct VoxelBlock { - static constexpr size_t MAX_SIZE = 20; - using PointsContainerType = std::array; - using IteratorType = PointsContainerType::iterator; - using ConstIteratorType = PointsContainerType::const_iterator; - VoxelBlock() = default; - - VoxelBlock(const VoxelBlock &other) = delete; - VoxelBlock &operator=(const VoxelBlock &) = delete; - - VoxelBlock(VoxelBlock &&other) = default; - VoxelBlock &operator=(VoxelBlock &&other) = default; - - void addPoint(const Eigen::Vector3d &p); - - inline std::size_t size() const { return std::distance(cbegin(), cend()); } - - const Eigen::Vector3d &front() const { return points_.front(); } - inline IteratorType begin() { return points_.begin(); } - inline ConstIteratorType cbegin() const { return points_.cbegin(); } - - inline IteratorType end() { return std::next(begin(), size_); } - inline ConstIteratorType cend() const { return std::next(cbegin(), size_); } - -private: - std::array points_; - uint16_t size_ = 0; -}; +using VoxelBlock = std::vector; struct SparseVoxelGrid { explicit SparseVoxelGrid(const double voxel_size, From c4c1d901fe113ee45e781a10eecef082ab400863 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 29 Nov 2024 14:12:53 +0100 Subject: [PATCH 09/16] Some renaming and cleaning --- cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp index 7f4022e..52af542 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -30,7 +30,7 @@ #include "bonxai/grid_coord.hpp" namespace { -static std::array shifts{ +static constexpr std::array shifts{ Bonxai::CoordT{-1, -1, -1}, Bonxai::CoordT{-1, -1, 0}, Bonxai::CoordT{-1, -1, 1}, Bonxai::CoordT{-1, 0, -1}, Bonxai::CoordT{-1, 0, 0}, Bonxai::CoordT{-1, 0, 1}, Bonxai::CoordT{-1, 1, -1}, Bonxai::CoordT{-1, 1, 0}, Bonxai::CoordT{-1, 1, 1}, @@ -60,9 +60,10 @@ std::tuple SparseVoxelGrid::GetClosestNeighbor( Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); double closest_distance = std::numeric_limits::max(); const auto const_accessor = map_.createConstAccessor(); - const Bonxai::CoordT query_voxel = map_.posToCoord(query); - std::for_each(shifts.cbegin(), shifts.cend(), [&](const Bonxai::CoordT &voxel_coordinates) { - const VoxelBlock *voxel_points = const_accessor.value(query_voxel + voxel_coordinates); + const Bonxai::CoordT voxel = map_.posToCoord(query); + std::for_each(shifts.cbegin(), shifts.cend(), [&](const Bonxai::CoordT &voxel_shift) { + const Bonxai::CoordT query_voxel = voxel + voxel_shift; + const VoxelBlock *voxel_points = const_accessor.value(query_voxel); if (voxel_points != nullptr) { const Eigen::Vector3d &neighbor = *std::min_element(voxel_points->cbegin(), voxel_points->cend(), From 2af1115103587ecc41d5fe10c3404f3da0ce0182 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 29 Nov 2024 14:17:50 +0100 Subject: [PATCH 10/16] For more clarity --- cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp index 52af542..a3e71c1 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -83,8 +83,8 @@ std::tuple SparseVoxelGrid::GetClosestNeighbor( void SparseVoxelGrid::AddPoints(const std::vector &points) { const double map_resolution = std::sqrt(voxel_size_ * voxel_size_ / max_points_per_voxel_); std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &p) { - const auto voxel_coordinates = map_.posToCoord(p.x(), p.y(), p.z()); - VoxelBlock *voxel_points = accessor_.value(voxel_coordinates, true); + const auto voxel_coordinates = map_.posToCoord(p); + VoxelBlock *voxel_points = accessor_.value(voxel_coordinates, /*create_if_missing=*/true); if (voxel_points->size() == max_points_per_voxel_ || std::any_of(voxel_points->cbegin(), voxel_points->cend(), [&](const auto &voxel_point) { return (voxel_point - p).norm() < map_resolution; From b76abda6ce6130f605d030f813d8e64a558735cb Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 29 Nov 2024 16:06:15 +0100 Subject: [PATCH 11/16] Modify bonxai patch --- .../3rdparty/bonxai/bonxai.patch | 41 ++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch index 0ec94d8..6ea4da3 100644 --- a/cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch +++ b/cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch @@ -1,5 +1,5 @@ diff --git a/bonxai_core/include/bonxai/bonxai.hpp b/bonxai_core/include/bonxai/bonxai.hpp -index 7d360cf..8359ba8 100644 +index 7d360cf..e43771e 100644 --- a/bonxai_core/include/bonxai/bonxai.hpp +++ b/bonxai_core/include/bonxai/bonxai.hpp @@ -367,7 +367,7 @@ inline size_t Grid::memUsage() const { @@ -11,3 +11,42 @@ index 7d360cf..8359ba8 100644 for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) { const int32_t inner_index = *inner_it; auto& leaf_grid = inner_grid.cell(inner_index); +@@ -404,9 +404,9 @@ inline VoxelGrid::VoxelGrid(double voxel_size, uint8_t inner_bits, uint8_ + template + inline CoordT VoxelGrid::posToCoord(double x, double y, double z) const { + return { +- static_cast(std::nearbyint(x * inv_resolution)), +- static_cast(std::nearbyint(y * inv_resolution)), +- static_cast(std::nearbyint(z * inv_resolution))}; ++ static_cast(std::floor(x * inv_resolution)), ++ static_cast(std::floor(y * inv_resolution)), ++ static_cast(std::floor(z * inv_resolution))}; + } + + template +diff --git a/bonxai_core/include/bonxai/grid_coord.hpp b/bonxai_core/include/bonxai/grid_coord.hpp +index da8fb7d..1f9727b 100644 +--- a/bonxai_core/include/bonxai/grid_coord.hpp ++++ b/bonxai_core/include/bonxai/grid_coord.hpp +@@ -79,9 +79,9 @@ struct CoordT { + + [[nodiscard]] inline CoordT PosToCoord(const Point3D& point, double inv_resolution) { + return { +- static_cast(std::nearbyint(point.x * inv_resolution)), +- static_cast(std::nearbyint(point.y * inv_resolution)), +- static_cast(std::nearbyint(point.z * inv_resolution))}; ++ static_cast(std::floor(point.x * inv_resolution)), ++ static_cast(std::floor(point.y * inv_resolution)), ++ static_cast(std::floor(point.z * inv_resolution))}; + } + + [[nodiscard]] inline Point3D CoordToPos(const CoordT& coord, double resolution) { +@@ -212,7 +212,7 @@ struct hash { + std::size_t operator()(const Bonxai::CoordT& p) const { + // same as OpenVDB + return ((1 << 20) - 1) & (static_cast(p.x) * 73856093 ^ // +- static_cast(p.y) * 19349663 ^ // ++ static_cast(p.y) * 19349669 ^ // + static_cast(p.z) * 83492791); + } + }; From f456e765efa4730ac5d14861dee694524d500b9d Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Sat, 30 Nov 2024 19:13:06 +0100 Subject: [PATCH 12/16] IWYU --- cpp/kinematic_icp/pipeline/KinematicICP.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.cpp b/cpp/kinematic_icp/pipeline/KinematicICP.cpp index aece4c5..9bfdabd 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.cpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.cpp @@ -25,8 +25,7 @@ #include #include #include -#include -#include +#include #include namespace { From bbb3eccb02db6c4173b5d57df376653377e856dc Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Sun, 1 Dec 2024 19:49:05 +0100 Subject: [PATCH 13/16] Just reset the block --- cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp index a3e71c1..dba54f6 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -100,10 +100,10 @@ void SparseVoxelGrid::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) auto remove_voxel = [this, &origin](VoxelBlock &block, const Bonxai::CoordT &coordinate) { if ((block.front() - origin).norm() >= clipping_distance_) { accessor_.setCellOff(coordinate); + block = VoxelBlock(); } }; map_.forEachCell(remove_voxel); - map_.releaseUnusedMemory(); } void SparseVoxelGrid::Update(const std::vector &points, const Sophus::SE3d &pose) { From 57977f15af895f57746da00eb34f087bcd09d3e1 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 2 Dec 2024 14:33:52 +0100 Subject: [PATCH 14/16] Actually remove the keys when leafs are removed, a bit slower --- .../local_map/SparseVoxelGrid.cpp | 29 +++++++++++++++---- 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp index dba54f6..dec023d 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -24,6 +24,7 @@ #include "SparseVoxelGrid.hpp" #include +#include #include #include @@ -97,13 +98,29 @@ void SparseVoxelGrid::AddPoints(const std::vector &points) { } void SparseVoxelGrid::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { - auto remove_voxel = [this, &origin](VoxelBlock &block, const Bonxai::CoordT &coordinate) { - if ((block.front() - origin).norm() >= clipping_distance_) { - accessor_.setCellOff(coordinate); - block = VoxelBlock(); - } + auto is_too_far_away = [&](const VoxelBlock &block) { + return (block.front() - origin).norm() > clipping_distance_; }; - map_.forEachCell(remove_voxel); + + std::vector keys_to_delete; + auto &root_map = map_.rootMap(); + for (auto &[key, inner_grid] : root_map) { + for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) { + const int32_t inner_index = *inner_it; + auto &leaf_grid = inner_grid.cell(inner_index); + const auto &voxel_block = leaf_grid->cell(leaf_grid->mask().findFirstOn()); + if (is_too_far_away(voxel_block)) { + inner_grid.mask().setOff(inner_index); + leaf_grid.reset(); + } + } + if (inner_grid.mask().isOff()) { + keys_to_delete.push_back(key); + } + } + for (const auto &key : keys_to_delete) { + root_map.erase(key); + } } void SparseVoxelGrid::Update(const std::vector &points, const Sophus::SE3d &pose) { From 45c1b2b25ae837cffe745578dd1adfe188f89371 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 3 Dec 2024 11:34:51 +0100 Subject: [PATCH 15/16] Control leaf and grid size --- cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp index dec023d..6f2bde1 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include "bonxai/grid_coord.hpp" @@ -43,7 +44,10 @@ static constexpr std::array shifts{ Bonxai::CoordT{1, -1, -1}, Bonxai::CoordT{1, -1, 0}, Bonxai::CoordT{1, -1, 1}, Bonxai::CoordT{1, 0, -1}, Bonxai::CoordT{1, 0, 0}, Bonxai::CoordT{1, 0, 1}, Bonxai::CoordT{1, 1, -1}, Bonxai::CoordT{1, 1, 0}, Bonxai::CoordT{1, 1, 1}}; -} + +static constexpr uint8_t inner_grid_log2_size = 2; +static constexpr uint8_t leaf_grid_log2_size = 2; +} // namespace namespace kinematic_icp { @@ -53,7 +57,7 @@ SparseVoxelGrid::SparseVoxelGrid(const double voxel_size, : voxel_size_(voxel_size), clipping_distance_(clipping_distance), max_points_per_voxel_(max_points_per_voxel), - map_(voxel_size), + map_(voxel_size, inner_grid_log2_size, leaf_grid_log2_size), accessor_(map_.createAccessor()) {} std::tuple SparseVoxelGrid::GetClosestNeighbor( From 9859082a5bc534743a83830fbc749b01d14ae774 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 3 Dec 2024 16:17:20 +0100 Subject: [PATCH 16/16] Forget to push the default config --- cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp index 6f2bde1..db627c7 100644 --- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp +++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp @@ -45,8 +45,8 @@ static constexpr std::array shifts{ Bonxai::CoordT{1, 0, -1}, Bonxai::CoordT{1, 0, 0}, Bonxai::CoordT{1, 0, 1}, Bonxai::CoordT{1, 1, -1}, Bonxai::CoordT{1, 1, 0}, Bonxai::CoordT{1, 1, 1}}; -static constexpr uint8_t inner_grid_log2_size = 2; -static constexpr uint8_t leaf_grid_log2_size = 2; +static constexpr uint8_t inner_grid_log2_dim = 2; +static constexpr uint8_t leaf_grid_log2_dim = 3; } // namespace namespace kinematic_icp { @@ -57,7 +57,7 @@ SparseVoxelGrid::SparseVoxelGrid(const double voxel_size, : voxel_size_(voxel_size), clipping_distance_(clipping_distance), max_points_per_voxel_(max_points_per_voxel), - map_(voxel_size, inner_grid_log2_size, leaf_grid_log2_size), + map_(voxel_size, inner_grid_log2_dim, leaf_grid_log2_dim), accessor_(map_.createAccessor()) {} std::tuple SparseVoxelGrid::GetClosestNeighbor(