From 917d52845cf005323a88924be3c532297fff1be9 Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Tue, 9 Jul 2024 09:42:03 +0000 Subject: [PATCH 01/12] Make voxel computation consistent across all source code (#354) * Make voxel computation consistent across all source code, also use std::floor to have explicit control over type casting * Use available function for conversion * Make it a one liner * remove numeric from includes * New proposal * remove numeric from include * shrink diff * Consistency as always --------- Co-authored-by: tizianoGuadagnino Co-authored-by: Ignacio Vizzo --- cpp/kiss_icp/core/Preprocessing.cpp | 9 ++++++++- cpp/kiss_icp/core/VoxelHashMap.cpp | 2 +- cpp/kiss_icp/core/VoxelHashMap.hpp | 6 +++--- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 221651bf..0a6a6e30 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -32,6 +32,7 @@ #include namespace { +// TODO(all): Maybe try to merge these voxel uitls with VoxelHashMap implementation using Voxel = Eigen::Vector3i; struct VoxelHash { size_t operator()(const Voxel &voxel) const { @@ -39,6 +40,12 @@ struct VoxelHash { return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); } }; + +Voxel PointToVoxel(const Eigen::Vector3d &point, 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 namespace kiss_icp { @@ -47,7 +54,7 @@ std::vector VoxelDownsample(const std::vector tsl::robin_map grid; grid.reserve(frame.size()); for (const auto &point : frame) { - const auto voxel = Voxel((point / voxel_size).cast()); + const auto voxel = PointToVoxel(point, voxel_size); if (grid.contains(voxel)) continue; grid.insert({voxel, point}); } diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 47a345b4..65790fba 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -75,7 +75,7 @@ void VoxelHashMap::Update(const std::vector &points, const Soph void VoxelHashMap::AddPoints(const std::vector &points) { std::for_each(points.cbegin(), points.cend(), [&](const auto &point) { - auto voxel = Voxel((point / voxel_size_).template cast()); + auto voxel = PointToVoxel(point); auto search = map_.find(voxel); if (search != map_.end()) { auto &voxel_block = search.value(); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 7eff4347..92a3d167 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -58,9 +58,9 @@ struct VoxelHashMap { inline void Clear() { map_.clear(); } inline bool Empty() const { return map_.empty(); } inline Voxel PointToVoxel(const Eigen::Vector3d &point) const { - return Voxel(static_cast(point.x() / voxel_size_), - static_cast(point.y() / voxel_size_), - static_cast(point.z() / 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_))); } void Update(const std::vector &points, const Eigen::Vector3d &origin); void Update(const std::vector &points, const Sophus::SE3d &pose); From e19823ab7275199caee037d75c6a3c7d6945a628 Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Tue, 9 Jul 2024 12:58:36 +0000 Subject: [PATCH 02/12] Remove redundant Modulus operation for Voxel Hash function (#358) * Make voxel computation consistent across all source code, also use std::floor to have explicit control over type casting * Use available function for conversion * Remove redundant Modulus operation for Vocel Hash function, set robin map params instead * Make it a one liner * remove numeric from includes * New proposal * remove numeric from include * shrink diff * Consistency as always * Split changes into two PRs * Revert "Merge remote-tracking branch 'origin' into gupta_fix_hash" This reverts commit 6e46c0d148213f7f80264909ed1ec89975891c9c, reversing changes made to 07634eb40541db676eb38a1383746329e5f795bd. --------- Co-authored-by: tizianoGuadagnino Co-authored-by: Ignacio Vizzo --- cpp/kiss_icp/core/VoxelHashMap.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 92a3d167..8ffe9ed9 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -46,7 +46,7 @@ struct VoxelHashMap { struct VoxelHash { size_t operator()(const Voxel &voxel) const { const uint32_t *vec = reinterpret_cast(voxel.data()); - return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); + return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); } }; From 8596de861751b6646115ea549b56eea982d80eb0 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Wed, 10 Jul 2024 09:12:08 +0200 Subject: [PATCH 03/12] Fix Ubuntu 20.04 dev build (#361) * Add a new job just to proof the build is now broken * force 22.04 to make sure it's only a 20.04 problem * Revert "force 22.04 to make sure it's only a 20.04 problem" This reverts commit e1b5e0276273f1176b3bb2ab2b9ccbc56fcccce0. * Add cache to ci build * Attempt to fix build in Ubuntu 20.04 * add this comment just to annoy ourselves --- .github/workflows/cpp.yml | 29 +++++++++++++++++++++++++++++ cpp/kiss_icp/core/Deskew.cpp | 1 + cpp/kiss_icp/core/Registration.cpp | 4 +++- 3 files changed, 33 insertions(+), 1 deletion(-) diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml index 131dda9e..bbb8ef8a 100644 --- a/.github/workflows/cpp.yml +++ b/.github/workflows/cpp.yml @@ -25,3 +25,32 @@ jobs: run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kiss_icp - name: Build run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + # As the previous job will always install the dependencies from cmake, and this is guaranteed to + # work, we also want to support dev sandboxes where the main dependencies are already + # pre-installed in the system. For now, we only support dev machines under a GNU/Linux + # environmnets. If you are reading this and need the same functionallity in Windows/macOS please + # open a ticket. + cpp_api_dev: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-22.04, ubuntu-20.04] + + steps: + - uses: actions/checkout@v3 + - name: Cache dependencies + uses: actions/cache@v2 + with: + path: ~/.apt/cache + key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }} + restore-keys: | + ${{ runner.os }}-apt- + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libeigen3-dev libtbb-dev + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kiss_icp + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} diff --git a/cpp/kiss_icp/core/Deskew.cpp b/cpp/kiss_icp/core/Deskew.cpp index 9b721fe2..fe20b77e 100644 --- a/cpp/kiss_icp/core/Deskew.cpp +++ b/cpp/kiss_icp/core/Deskew.cpp @@ -39,6 +39,7 @@ std::vector DeSkewScan(const std::vector &fram const Sophus::SE3d &delta) { const auto delta_pose = delta.log(); std::vector corrected_frame(frame.size()); + // TODO(All): This tbb execution is ignoring the max_n_threads config value tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) { const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose); corrected_frame[i] = motion * frame[i]; diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index d7adb4df..137fa50c 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -171,7 +172,8 @@ Registration::Registration(int max_num_iteration, double convergence_criterion, : max_num_iterations_(max_num_iteration), convergence_criterion_(convergence_criterion), // Only manipulate the number of threads if the user specifies something greater than 0 - max_num_threads_(max_num_threads > 0 ? max_num_threads : tbb::info::default_concurrency()) { + max_num_threads_(max_num_threads > 0 ? max_num_threads + : tbb::this_task_arena::max_concurrency()) { // This global variable requires static duration storage to be able to manipulate the max // concurrency from TBB across the entire class static const auto tbb_control_settings = tbb::global_control( From 5d420877a6e5f984ea86957da9e670ff6aed45e4 Mon Sep 17 00:00:00 2001 From: KARTHIK TARAKA SAI BOLLA <110156560+karthikbolla@users.noreply.github.com> Date: Thu, 11 Jul 2024 02:34:57 +0530 Subject: [PATCH 04/12] Handled Zero Division Error in pipeline.py (#344) * Handled Zero Division Error * Apply suggestions from code review Co-authored-by: Benedikt Mersch * Update pipeline.py updated pipeline.py based on @benemer suggestions where the results are only appended when avg fps is greater than zero * Fix formatting --------- Co-authored-by: Benedikt Mersch Co-authored-by: tizianoGuadagnino --- python/kiss_icp/pipeline.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py index 9994bfc3..f8dfd857 100644 --- a/python/kiss_icp/pipeline.py +++ b/python/kiss_icp/pipeline.py @@ -179,12 +179,14 @@ def _run_evaluation(self): # Run timing metrics evaluation, always def _get_fps(): total_time_s = sum(self.times) * 1e-9 - return float(len(self.times) / total_time_s) - - avg_fps = int(np.ceil(_get_fps())) - avg_ms = int(np.ceil(1e3 * (1 / _get_fps()))) - self.results.append(desc="Average Frequency", units="Hz", value=avg_fps, trunc=True) - self.results.append(desc="Average Runtime", units="ms", value=avg_ms, trunc=True) + return float(len(self.times) / total_time_s) if total_time_s > 0 else 0 + + fps = _get_fps() + avg_fps = int(np.floor(fps)) + avg_ms = int(np.ceil(1e3 / fps)) if fps > 0 else 0 + if avg_fps > 0: + self.results.append(desc="Average Frequency", units="Hz", value=avg_fps, trunc=True) + self.results.append(desc="Average Runtime", units="ms", value=avg_ms, trunc=True) def _write_log(self): if not self.results.empty(): From 70c3e8dd44edda31018bc6ea360c02b79cc0a40a Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Fri, 12 Jul 2024 09:19:19 +0000 Subject: [PATCH 05/12] Fix include headers (#365) * Make voxel computation consistent across all source code, also use std::floor to have explicit control over type casting * Use available function for conversion * Make it a one liner * remove numeric from includes * New proposal * remove numeric from include * shrink diff * Consistency as always * Fix include header, follow IWYU --------- Co-authored-by: tizianoGuadagnino Co-authored-by: Ignacio Vizzo --- cpp/kiss_icp/core/Preprocessing.cpp | 2 +- cpp/kiss_icp/core/Preprocessing.hpp | 2 -- cpp/kiss_icp/core/Threshold.cpp | 3 ++- cpp/kiss_icp/core/VoxelHashMap.cpp | 3 +-- cpp/kiss_icp/metrics/Metrics.cpp | 1 + 5 files changed, 5 insertions(+), 6 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 0a6a6e30..d9e1d805 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -26,9 +26,9 @@ #include #include +#include #include #include -#include #include namespace { diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index e919a422..1a8ae1ee 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -23,8 +23,6 @@ #pragma once #include -#include -#include #include namespace kiss_icp { diff --git a/cpp/kiss_icp/core/Threshold.cpp b/cpp/kiss_icp/core/Threshold.cpp index f9cb4131..1c51a511 100644 --- a/cpp/kiss_icp/core/Threshold.cpp +++ b/cpp/kiss_icp/core/Threshold.cpp @@ -22,8 +22,9 @@ // SOFTWARE. #include "Threshold.hpp" -#include +#include #include +#include namespace kiss_icp { AdaptiveThreshold::AdaptiveThreshold(double initial_threshold, diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 65790fba..6b3b061b 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -24,8 +24,7 @@ #include #include -#include -#include +#include #include namespace kiss_icp { diff --git a/cpp/kiss_icp/metrics/Metrics.cpp b/cpp/kiss_icp/metrics/Metrics.cpp index 26f025bf..78195586 100644 --- a/cpp/kiss_icp/metrics/Metrics.cpp +++ b/cpp/kiss_icp/metrics/Metrics.cpp @@ -23,6 +23,7 @@ #include "Metrics.hpp" #include +#include #include #include #include From a9f527daacd350511fdb315e338d5e11315644df Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Sat, 20 Jul 2024 10:23:18 +0200 Subject: [PATCH 06/12] Revise Voxel functionalities in VoxelHashMap and Preprocessing (#362) * Unify Voxel functionalities, now everything comes from VoxelHashMap * This looks like an even better unification * Modernize VoxelDownsample * VoxelUtils is born, now everthing is liteally in one place, and we have some side effect code reduction * Macos does not like my include * Local style include * With move * As of benchmark * For each for the win * Remove unnecessary include * Const correcteness * Refresh changes --------- Co-authored-by: Luca Lobefaro Co-authored-by: Ignacio Vizzo --- cpp/kiss_icp/core/Preprocessing.cpp | 17 +---------- cpp/kiss_icp/core/Registration.cpp | 5 ++-- cpp/kiss_icp/core/VoxelHashMap.cpp | 4 ++- cpp/kiss_icp/core/VoxelHashMap.hpp | 14 ++------- cpp/kiss_icp/core/VoxelUtils.hpp | 45 +++++++++++++++++++++++++++++ 5 files changed, 54 insertions(+), 31 deletions(-) create mode 100644 cpp/kiss_icp/core/VoxelUtils.hpp diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index d9e1d805..0e74910b 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -31,22 +31,7 @@ #include #include -namespace { -// TODO(all): Maybe try to merge these voxel uitls with VoxelHashMap implementation -using Voxel = Eigen::Vector3i; -struct VoxelHash { - size_t operator()(const Voxel &voxel) const { - const uint32_t *vec = reinterpret_cast(voxel.data()); - return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); - } -}; - -Voxel PointToVoxel(const Eigen::Vector3d &point, 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 +#include "VoxelUtils.hpp" namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 137fa50c..b8acea10 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -36,6 +36,7 @@ #include #include "VoxelHashMap.hpp" +#include "VoxelUtils.hpp" namespace Eigen { using Matrix6d = Eigen::Matrix; @@ -54,7 +55,7 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points [&](const auto &point) { return T * point; }); } -using Voxel = kiss_icp::VoxelHashMap::Voxel; +using Voxel = kiss_icp::Voxel; std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) { std::vector voxel_neighborhood; for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) { @@ -70,7 +71,7 @@ std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1 std::tuple GetClosestNeighbor(const Eigen::Vector3d &point, const kiss_icp::VoxelHashMap &voxel_map) { // Convert the point to voxel coordinates - const auto &voxel = voxel_map.PointToVoxel(point); + const auto &voxel = kiss_icp::PointToVoxel(point, voxel_map.voxel_size_); // Get nearby voxels on the map const auto &query_voxels = GetAdjacentVoxels(voxel); // Extract the points contained within the neighborhood voxels diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 6b3b061b..e6445ebd 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -27,6 +27,8 @@ #include #include +#include "VoxelUtils.hpp" + namespace kiss_icp { std::vector VoxelHashMap::GetPoints(const std::vector &query_voxels) const { @@ -74,7 +76,7 @@ void VoxelHashMap::Update(const std::vector &points, const Soph void VoxelHashMap::AddPoints(const std::vector &points) { std::for_each(points.cbegin(), points.cend(), [&](const auto &point) { - auto voxel = PointToVoxel(point); + auto voxel = PointToVoxel(point, voxel_size_); auto search = map_.find(voxel); if (search != map_.end()) { auto &voxel_block = search.value(); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 8ffe9ed9..a49b7096 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -32,9 +32,10 @@ #include #include +#include "VoxelUtils.hpp" + namespace kiss_icp { struct VoxelHashMap { - using Voxel = Eigen::Vector3i; struct VoxelBlock { // buffer of points with a max limit of n_points std::vector points; @@ -43,12 +44,6 @@ struct VoxelHashMap { if (points.size() < static_cast(num_points_)) points.push_back(point); } }; - struct VoxelHash { - 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); - } - }; explicit VoxelHashMap(double voxel_size, double max_distance, int max_points_per_voxel) : voxel_size_(voxel_size), @@ -57,11 +52,6 @@ struct VoxelHashMap { inline void Clear() { map_.clear(); } inline bool Empty() const { return map_.empty(); } - inline Voxel PointToVoxel(const Eigen::Vector3d &point) const { - 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_))); - } void Update(const std::vector &points, const Eigen::Vector3d &origin); void Update(const std::vector &points, const Sophus::SE3d &pose); void AddPoints(const std::vector &points); diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp new file mode 100644 index 00000000..331a2ce1 --- /dev/null +++ b/cpp/kiss_icp/core/VoxelUtils.hpp @@ -0,0 +1,45 @@ +// 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. +// +#pragma once + +#include +#include + +namespace kiss_icp { + +using Voxel = Eigen::Vector3i; + +inline 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))); +} + +struct VoxelHash { + 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); + } +}; +} // namespace kiss_icp From 1c28d1776eca0c91c561d31a605addb31176e3d3 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 22 Jul 2024 14:42:17 +0200 Subject: [PATCH 07/12] Style changes in voxelization and preprocessing utilities (#364) * Unify Voxel functionalities, now everything comes from VoxelHashMap * This looks like an even better unification * Modernize VoxelDownsample * VoxelUtils is born, now everthing is liteally in one place, and we have some side effect code reduction * Macos does not like my include * Local style include * With move * As of benchmark * For each for the win * Remove unnecessary include * Const correcteness * Refresh changes * Add style changes for a separate PR * Add missing headers * Remove more raw for loops * Move Voxeldownsample into VoxelUtils * Cmake needs to know * Miss cpp * Remove duplicate * cmake-format * Format * We never used noexcept anywhere, why adding it now? * We always use frame, if we want to move point_cloud, let's do it in a seprate PR * put back whitespaces * reduce diff --------- Co-authored-by: tizianoGuadagnino Co-authored-by: Luca Lobefaro Co-authored-by: Benedikt Mersch --- cpp/kiss_icp/core/CMakeLists.txt | 3 ++- cpp/kiss_icp/core/Preprocessing.cpp | 18 ------------------ cpp/kiss_icp/core/Preprocessing.hpp | 3 --- cpp/kiss_icp/core/VoxelHashMap.cpp | 12 ++++-------- cpp/kiss_icp/core/VoxelHashMap.hpp | 2 +- cpp/kiss_icp/core/VoxelUtils.cpp | 23 +++++++++++++++++++++++ cpp/kiss_icp/core/VoxelUtils.hpp | 14 ++++++++++---- 7 files changed, 40 insertions(+), 35 deletions(-) create mode 100644 cpp/kiss_icp/core/VoxelUtils.cpp diff --git a/cpp/kiss_icp/core/CMakeLists.txt b/cpp/kiss_icp/core/CMakeLists.txt index ce83625a..fc627bf4 100644 --- a/cpp/kiss_icp/core/CMakeLists.txt +++ b/cpp/kiss_icp/core/CMakeLists.txt @@ -21,6 +21,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. add_library(kiss_icp_core STATIC) -target_sources(kiss_icp_core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp Preprocessing.cpp Threshold.cpp) +target_sources(kiss_icp_core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp VoxelUtils.cpp Preprocessing.cpp + Threshold.cpp) target_link_libraries(kiss_icp_core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus) set_global_target_properties(kiss_icp_core) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 0e74910b..d7975497 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -34,24 +34,6 @@ #include "VoxelUtils.hpp" namespace kiss_icp { -std::vector VoxelDownsample(const std::vector &frame, - double voxel_size) { - tsl::robin_map grid; - grid.reserve(frame.size()); - for (const auto &point : frame) { - const auto voxel = PointToVoxel(point, voxel_size); - if (grid.contains(voxel)) continue; - grid.insert({voxel, point}); - } - std::vector frame_dowsampled; - frame_dowsampled.reserve(grid.size()); - for (const auto &[voxel, point] : grid) { - (void)voxel; - frame_dowsampled.emplace_back(point); - } - return frame_dowsampled; -} - std::vector Preprocess(const std::vector &frame, double max_range, double min_range) { diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index 1a8ae1ee..3cff777f 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -37,7 +37,4 @@ std::vector Preprocess(const std::vector &fram /// Originally introduced the calibration factor) std::vector CorrectKITTIScan(const std::vector &frame); -/// Voxelize point cloud keeping the original coordinates -std::vector VoxelDownsample(const std::vector &frame, - double voxel_size); } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index e6445ebd..c9b7313b 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -37,9 +37,8 @@ std::vector VoxelHashMap::GetPoints(const std::vector &q std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) { auto search = map_.find(query); if (search != map_.end()) { - for (const auto &point : search->second.points) { - points.emplace_back(point); - } + const auto &voxel_points = search->second.points; + points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend()); } }); points.shrink_to_fit(); @@ -50,11 +49,8 @@ std::vector VoxelHashMap::Pointcloud() const { std::vector points; points.reserve(map_.size() * static_cast(max_points_per_voxel_)); std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) { - const auto &[voxel, voxel_block] = map_element; - (void)voxel; - for (const auto &point : voxel_block.points) { - points.emplace_back(point); - } + const auto &voxel_points = map_element.second.points; + points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend()); }); points.shrink_to_fit(); return points; diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index a49b7096..1b3dbb59 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -62,6 +62,6 @@ struct VoxelHashMap { double voxel_size_; double max_distance_; int max_points_per_voxel_; - tsl::robin_map map_; + tsl::robin_map map_; }; } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/VoxelUtils.cpp b/cpp/kiss_icp/core/VoxelUtils.cpp new file mode 100644 index 00000000..f8f8ea8c --- /dev/null +++ b/cpp/kiss_icp/core/VoxelUtils.cpp @@ -0,0 +1,23 @@ +#include "VoxelUtils.hpp" + +#include + +namespace kiss_icp { + +std::vector VoxelDownsample(const std::vector &frame, + const double voxel_size) { + tsl::robin_map grid; + grid.reserve(frame.size()); + std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) { + const auto voxel = PointToVoxel(point, voxel_size); + if (!grid.contains(voxel)) grid.insert({voxel, point}); + }); + std::vector frame_dowsampled; + frame_dowsampled.reserve(grid.size()); + std::for_each(grid.cbegin(), grid.cend(), [&](const auto &voxel_and_point) { + frame_dowsampled.emplace_back(voxel_and_point.second); + }); + return frame_dowsampled; +} + +} // namespace kiss_icp diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp index 331a2ce1..580a2521 100644 --- a/cpp/kiss_icp/core/VoxelUtils.hpp +++ b/cpp/kiss_icp/core/VoxelUtils.hpp @@ -25,21 +25,27 @@ #include #include +#include namespace kiss_icp { using Voxel = Eigen::Vector3i; - inline 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))); } -struct VoxelHash { - size_t operator()(const Voxel &voxel) const { +/// Voxelize a point cloud keeping the original coordinates +std::vector VoxelDownsample(const std::vector &frame, + const double voxel_size); + +} // namespace kiss_icp + +template <> +struct std::hash { + std::size_t operator()(const kiss_icp::Voxel &voxel) const { const uint32_t *vec = reinterpret_cast(voxel.data()); return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); } }; -} // namespace kiss_icp From b43166f55f1fb032fc15fe97a2cb766a8ff732d6 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Wed, 24 Jul 2024 09:31:04 +0200 Subject: [PATCH 08/12] Nacho/remove kitti from core library (#369) * Remove kitti scan correction from core library Put it in python bindings, that is the only place where is actually used * Fix merge conflict --- cpp/kiss_icp/core/Preprocessing.cpp | 13 ------------- cpp/kiss_icp/core/Preprocessing.hpp | 6 ------ python/kiss_icp/pybind/kiss_icp_pybind.cpp | 18 +++++++++++++++++- 3 files changed, 17 insertions(+), 20 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index d7975497..482b8938 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -22,13 +22,11 @@ // SOFTWARE. #include "Preprocessing.hpp" -#include #include #include #include #include -#include #include #include "VoxelUtils.hpp" @@ -45,15 +43,4 @@ std::vector Preprocess(const std::vector &fram return inliers; } -std::vector CorrectKITTIScan(const std::vector &frame) { - constexpr double VERTICAL_ANGLE_OFFSET = (0.205 * M_PI) / 180.0; - std::vector corrected_frame(frame.size()); - tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) { - const auto &pt = frame[i]; - const Eigen::Vector3d rotationVector = pt.cross(Eigen::Vector3d(0., 0., 1.)); - corrected_frame[i] = - Eigen::AngleAxisd(VERTICAL_ANGLE_OFFSET, rotationVector.normalized()) * pt; - }); - return corrected_frame; -} } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index 3cff777f..d161177d 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -31,10 +31,4 @@ namespace kiss_icp { std::vector Preprocess(const std::vector &frame, double max_range, double min_range); - -/// This function only applies for the KITTI dataset, and should NOT be used by any other dataset, -/// the original idea and part of the implementation is taking from CT-ICP(Although IMLS-SLAM -/// Originally introduced the calibration factor) -std::vector CorrectKITTIScan(const std::vector &frame); - } // namespace kiss_icp diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index f3dc6d4d..2291c04f 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -27,6 +27,8 @@ #include #include +#include +#include #include #include @@ -118,7 +120,21 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { // prerpocessing modules m.def("_voxel_down_sample", &VoxelDownsample, "frame"_a, "voxel_size"_a); m.def("_preprocess", &Preprocess, "frame"_a, "max_range"_a, "min_range"_a); - m.def("_correct_kitti_scan", &CorrectKITTIScan, "frame"_a); + /// This function only applies for the KITTI dataset, and should NOT be used by any other + /// dataset, the original idea and part of the implementation is taking from CT-ICP(Although + /// IMLS-SLAM Originally introduced the calibration factor) + m.def( + "_correct_kitti_scan", + [](const std::vector &frame) { + constexpr double VERTICAL_ANGLE_OFFSET = (0.205 * M_PI) / 180.0; + std::vector frame_ = frame; + std::transform(frame_.cbegin(), frame_.cend(), frame_.begin(), [&](const auto pt) { + const Eigen::Vector3d rotationVector = pt.cross(Eigen::Vector3d(0., 0., 1.)); + return Eigen::AngleAxisd(VERTICAL_ANGLE_OFFSET, rotationVector.normalized()) * pt; + }); + return frame_; + }, + "frame"_a); // Metrics m.def("_kitti_seq_error", &metrics::SeqError, "gt_poses"_a, "results_poses"_a); From 0276c90636b9e18efd0cd612b0e3f1514b4df440 Mon Sep 17 00:00:00 2001 From: Pavlo Bashmakov <157482+bexcite@users.noreply.github.com> Date: Wed, 24 Jul 2024 02:27:00 -0700 Subject: [PATCH 09/12] Fix Ouster dataloader for 0.11 - 0.12 sdk updates (#372) * Ofix: fix datalodaer * Ofix: bump ouster-sdk version * Ofix: make Python style happy * Ofix: more style happiness * Ofix: 0.11 sdk also works --- python/kiss_icp/datasets/ouster.py | 64 ++++++------------------------ python/pyproject.toml | 2 +- 2 files changed, 14 insertions(+), 52 deletions(-) diff --git a/python/kiss_icp/datasets/ouster.py b/python/kiss_icp/datasets/ouster.py index 4ec3dd11..de242215 100644 --- a/python/kiss_icp/datasets/ouster.py +++ b/python/kiss_icp/datasets/ouster.py @@ -22,30 +22,12 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. -import glob import os from typing import Optional import numpy as np -def find_metadata_json(pcap_file: str) -> str: - """Attempts to resolve the metadata json file for a provided pcap file.""" - dir_path, filename = os.path.split(pcap_file) - if not filename: - return "" - if not dir_path: - dir_path = os.getcwd() - json_candidates = sorted(glob.glob(f"{dir_path}/*.json")) - if not json_candidates: - return "" - prefix_sizes = list( - map(lambda p: len(os.path.commonprefix((filename, os.path.basename(p)))), json_candidates) - ) - max_elem = max(range(len(prefix_sizes)), key=lambda i: prefix_sizes[i]) - return json_candidates[max_elem] - - class OusterDataloader: """Ouster pcap dataloader""" @@ -83,62 +65,42 @@ def __init__( """ try: - import ouster.pcap as pcap - from ouster import client + from ouster.sdk import client, open_source except ImportError: print(f'ouster-sdk is not installed on your system, run "pip install ouster-sdk"') exit(1) - # since we import ouster-sdk's client module locally, we keep it locally as well - self._client = client - assert os.path.isfile(data_dir), "Ouster pcap dataloader expects an existing PCAP file" # we expect `data_dir` param to be a path to the .pcap file, so rename for clarity pcap_file = data_dir - metadata_json = meta or find_metadata_json(pcap_file) - if not metadata_json: - print("Ouster pcap dataloader can't find metadata json file.") - exit(1) - print("Ouster pcap dataloader: using metadata json: ", metadata_json) + print("Indexing Ouster pcap to count the scans number ...") + source = open_source(str(pcap_file), meta=[meta] if meta else [], index=True) - self.data_dir = os.path.dirname(data_dir) + # since we import ouster-sdk's client module locally, we keep reference + # to it locally as well + self._client = client - with open(metadata_json) as json: - self._info_json = json.read() - self._info = client.SensorInfo(self._info_json) + self.data_dir = os.path.dirname(data_dir) # lookup table for 2D range image projection to a 3D point cloud - self._xyz_lut = client.XYZLut(self._info) + self._xyz_lut = client.XYZLut(source.metadata) self._pcap_file = str(data_dir) - # read pcap file for the first pass to count scans - print("Pre-reading Ouster pcap to count the scans number ...") - self._source = pcap.Pcap(self._pcap_file, self._info) - self._scans_num = sum((1 for _ in client.Scans(self._source))) + self._scans_num = len(source) print(f"Ouster pcap total scans number: {self._scans_num}") # frame timestamps array self._timestamps = np.linspace(0, self._scans_num, self._scans_num, endpoint=False) - # start Scans iterator for consumption in __getitem__ - self._source = pcap.Pcap(self._pcap_file, self._info) - self._scans_iter = iter(client.Scans(self._source)) - self._next_idx = 0 + self._source = source def __getitem__(self, idx): - # we assume that users always reads sequentially and do not - # pass idx as for a random access collection - assert self._next_idx == idx, ( - "Ouster pcap dataloader supports only sequential reads. " - f"Expected idx: {self._next_idx}, but got {idx}" - ) - scan = next(self._scans_iter) - self._next_idx += 1 - - self._timestamps[self._next_idx - 1] = 1e-9 * scan.timestamp[0] + scan = self._source[idx] + + self._timestamps[idx] = 1e-9 * scan.timestamp[0] timestamps = np.tile(np.linspace(0, 1.0, scan.w, endpoint=False), (scan.h, 1)) diff --git a/python/pyproject.toml b/python/pyproject.toml index 79fbf519..d90826eb 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -49,7 +49,7 @@ dependencies = [ [project.optional-dependencies] all = [ "open3d>=0.13", - "ouster-sdk>=0.7.1", + "ouster-sdk>=0.11", "pyntcloud", "PyYAML", "trimesh", From 775555ef661d2603b261657934dc27bb899bb729 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Wed, 24 Jul 2024 12:52:30 +0200 Subject: [PATCH 10/12] Add LiDAR odometry to tf tree (#371) * Add LiDAR odometry to tf tree * Transform the internal LiDAR odometry map to lidar frame I honestly do not know why this is not working, but we can see in the future. It's just to save so computation in the ROS node * Style --- ros/launch/odometry.launch.py | 17 +++--- ros/rviz/kiss_icp.rviz | 103 +++++++++++++++------------------- ros/src/OdometryServer.cpp | 19 +++++-- ros/src/OdometryServer.hpp | 3 +- 4 files changed, 72 insertions(+), 70 deletions(-) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index a8458139..d4b9c9bb 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -62,11 +62,6 @@ class config: def generate_launch_description(): use_sim_time = LaunchConfiguration("use_sim_time", default="true") - # tf tree configuration, these are the likely 3 parameters to change and nothing else - base_frame = LaunchConfiguration("base_frame", default="") - odom_frame = LaunchConfiguration("odom_frame", default="odom") - publish_odom_tf = LaunchConfiguration("publish_odom_tf", default=True) - # ROS configuration pointcloud_topic = LaunchConfiguration("topic") visualize = LaunchConfiguration("visualize", default="true") @@ -74,6 +69,12 @@ def generate_launch_description(): # Optional ros bag play bagfile = LaunchConfiguration("bagfile", default="") + # tf tree configuration, these are the likely parameters to change and nothing else + base_frame = LaunchConfiguration("base_frame", default="") # (base_link/base_footprint) + lidar_odom_frame = LaunchConfiguration("lidar_odom_frame", default="odom_lidar") + publish_odom_tf = LaunchConfiguration("publish_odom_tf", default=True) + invert_odom_tf = LaunchConfiguration("invert_odom_tf", default=True) + # KISS-ICP node kiss_icp_node = Node( package="kiss_icp", @@ -87,8 +88,9 @@ def generate_launch_description(): { # ROS node configuration "base_frame": base_frame, - "odom_frame": odom_frame, + "lidar_odom_frame": lidar_odom_frame, "publish_odom_tf": publish_odom_tf, + "invert_odom_tf": invert_odom_tf, # KISS-ICP configuration "max_range": config.max_range, "min_range": config.min_range, @@ -123,10 +125,11 @@ def generate_launch_description(): ) bagfile_play = ExecuteProcess( - cmd=["ros2", "bag", "play", bagfile], + cmd=["ros2", "bag", "play", "--rate", "1", bagfile, "--clock", "1000.0"], output="screen", condition=IfCondition(PythonExpression(["'", bagfile, "' != ''"])), ) + return LaunchDescription( [ kiss_icp_node, diff --git a/ros/rviz/kiss_icp.rviz b/ros/rviz/kiss_icp.rviz index 0bf30841..d47ec325 100644 --- a/ros/rviz/kiss_icp.rviz +++ b/ros/rviz/kiss_icp.rviz @@ -75,7 +75,7 @@ Visualization Manager: Color: 61; 229; 255 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -96,7 +96,7 @@ Visualization Manager: Value: /kiss/keypoints Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -133,60 +133,49 @@ Visualization Manager: Value: true Enabled: true Name: point_clouds - - Class: rviz_common/Group - Displays: - - Angle Tolerance: 0.10000000149011612 - Class: rviz_default_plugins/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 100 - Name: odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /kiss/odometry + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 Value: true - Enabled: false - Name: odometry - - Class: rviz_default_plugins/Axes + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false Enabled: true - Length: 1 - Name: odom_frame - Radius: 0.10000000149011612 - Reference Frame: odom + Keep: 100 + Name: LiDAR Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /kiss/odometry Value: true Enabled: true Global Options: Background Color: 0; 0; 0 - Fixed Frame: odom + Fixed Frame: odom_lidar Frame Rate: 30 Name: root Tools: @@ -229,25 +218,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 178.319580078125 + Distance: 51.88520812988281 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -68.01193237304688 - Y: 258.05126953125 - Z: -27.22064781188965 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.49020177125930786 - Target Frame: + Pitch: 0.7853981852531433 + Target Frame: odom_lidar Value: Orbit (rviz_default_plugins) - Yaw: 3.340390920639038 + Yaw: 0.7853981852531433 Saved: ~ Window Geometry: Displays: diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp index d3cb9023..687c3b58 100644 --- a/ros/src/OdometryServer.cpp +++ b/ros/src/OdometryServer.cpp @@ -74,8 +74,9 @@ using utils::PointCloud2ToEigen; OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) : rclcpp::Node("kiss_icp_node", options) { base_frame_ = declare_parameter("base_frame", base_frame_); - odom_frame_ = declare_parameter("odom_frame", odom_frame_); + lidar_odom_frame_ = declare_parameter("lidar_odom_frame", lidar_odom_frame_); publish_odom_tf_ = declare_parameter("publish_odom_tf", publish_odom_tf_); + invert_odom_tf_ = declare_parameter("invert_odom_tf", invert_odom_tf_); publish_debug_clouds_ = declare_parameter("publish_debug_clouds", publish_debug_clouds_); position_covariance_ = declare_parameter("position_covariance", 0.1); orientation_covariance_ = declare_parameter("orientation_covariance", 0.1); @@ -152,6 +153,7 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose, // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame const auto cloud_frame_id = header.frame_id; const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); + const auto moving_frame = egocentric_estimation ? cloud_frame_id : base_frame_; const auto pose = [&]() -> Sophus::SE3d { if (egocentric_estimation) return kiss_pose; const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_); @@ -162,16 +164,23 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose, if (publish_odom_tf_) { geometry_msgs::msg::TransformStamped transform_msg; transform_msg.header.stamp = header.stamp; - transform_msg.header.frame_id = odom_frame_; - transform_msg.child_frame_id = egocentric_estimation ? cloud_frame_id : base_frame_; - transform_msg.transform = tf2::sophusToTransform(pose); + if (invert_odom_tf_) { + transform_msg.header.frame_id = moving_frame; + transform_msg.child_frame_id = lidar_odom_frame_; + transform_msg.transform = tf2::sophusToTransform(pose.inverse()); + } else { + transform_msg.header.frame_id = lidar_odom_frame_; + transform_msg.child_frame_id = moving_frame; + transform_msg.transform = tf2::sophusToTransform(pose); + } tf_broadcaster_->sendTransform(transform_msg); } // publish odometry msg nav_msgs::msg::Odometry odom_msg; odom_msg.header.stamp = header.stamp; - odom_msg.header.frame_id = odom_frame_; + odom_msg.header.frame_id = lidar_odom_frame_; + odom_msg.child_frame_id = moving_frame; odom_msg.pose.pose = tf2::sophusToPose(pose); odom_msg.pose.covariance.fill(0.0); odom_msg.pose.covariance[0] = position_covariance_; diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp index f894ef9a..0f7c289f 100644 --- a/ros/src/OdometryServer.hpp +++ b/ros/src/OdometryServer.hpp @@ -61,6 +61,7 @@ class OdometryServer : public rclcpp::Node { std::unique_ptr tf_broadcaster_; std::unique_ptr tf2_buffer_; std::unique_ptr tf2_listener_; + bool invert_odom_tf_; bool publish_odom_tf_; bool publish_debug_clouds_; @@ -77,7 +78,7 @@ class OdometryServer : public rclcpp::Node { std::unique_ptr kiss_icp_; /// Global/map coordinate frame. - std::string odom_frame_{"odom"}; + std::string lidar_odom_frame_{"odom_lidar"}; std::string base_frame_{}; /// Covariance diagonal From e2364f2cf44cfe279236486e9622b6217eb7fdc5 Mon Sep 17 00:00:00 2001 From: Saurabh Gupta Date: Thu, 25 Jul 2024 11:37:51 +0000 Subject: [PATCH 11/12] Remove unncessary python list appends (#373) * Remove ugly unncessary python list appends * fix typo in name * Fix Visualizer * Satisfy style checker --- python/kiss_icp/datasets/paris_luco.py | 7 ++---- python/kiss_icp/metrics.py | 6 ++--- python/kiss_icp/pipeline.py | 33 ++++++++++++++------------ 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/python/kiss_icp/datasets/paris_luco.py b/python/kiss_icp/datasets/paris_luco.py index 816d0645..d243ccc2 100644 --- a/python/kiss_icp/datasets/paris_luco.py +++ b/python/kiss_icp/datasets/paris_luco.py @@ -63,9 +63,6 @@ def load_gt_poses(self, file_path): def apply_calibration(self, poses): """ParisLucoDataset only has a x, y, z trajectory, so we must will em all""" - new_poses = [] - for pose in poses: - T = pose.copy() - T[:3, :3] = np.eye(3) - new_poses.append(T) + new_poses = poses.copy() + new_poses[:, :3, :3] = np.eye(3) return new_poses diff --git a/python/kiss_icp/metrics.py b/python/kiss_icp/metrics.py index 002079ce..cd2d6031 100644 --- a/python/kiss_icp/metrics.py +++ b/python/kiss_icp/metrics.py @@ -20,20 +20,20 @@ # 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, Tuple +from typing import Tuple import numpy as np from kiss_icp.pybind import kiss_icp_pybind -def sequence_error(gt_poses: np.ndarray, results_poses: List[np.ndarray]) -> Tuple[float, float]: +def sequence_error(gt_poses: np.ndarray, results_poses: np.ndarray) -> Tuple[float, float]: """Sptis the sequence error for a given trajectory in camera coordinate frames.""" return kiss_icp_pybind._kitti_seq_error(gt_poses, results_poses) def absolute_trajectory_error( - gt_poses: np.ndarray, results_poses: List[np.ndarray] + gt_poses: np.ndarray, results_poses: np.ndarray ) -> Tuple[float, float]: """Sptis the sequence error for a given trajectory in camera coordinate frames.""" return kiss_icp_pybind._absolute_trajectory_error(gt_poses, results_poses) diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py index f8dfd857..c3cbb169 100644 --- a/python/kiss_icp/pipeline.py +++ b/python/kiss_icp/pipeline.py @@ -25,7 +25,7 @@ import os import time from pathlib import Path -from typing import List, Optional +from typing import Optional import numpy as np from pyquaternion import Quaternion @@ -64,8 +64,8 @@ def __init__( # Pipeline self.odometry = KissICP(config=self.config) self.results = PipelineResults() - self.times = [] - self.poses = [] + self.times = np.zeros(self._n_scans) + self.poses = np.zeros((self._n_scans, 4, 4)) self.has_gt = hasattr(self._dataset, "gt_poses") self.gt_poses = self._dataset.gt_poses[self._first : self._last] if self.has_gt else None self.dataset_name = self._dataset.__class__.__name__ @@ -97,9 +97,11 @@ def _run_pipeline(self): raw_frame, timestamps = self._next(idx) start_time = time.perf_counter_ns() source, keypoints = self.odometry.register_frame(raw_frame, timestamps) - self.poses.append(self.odometry.last_pose) - self.times.append(time.perf_counter_ns() - start_time) - self.visualizer.update(source, keypoints, self.odometry.local_map, self.poses[-1]) + self.poses[idx - self._first] = self.odometry.last_pose + self.times[idx - self._first] = time.perf_counter_ns() - start_time + self.visualizer.update( + source, keypoints, self.odometry.local_map, self.odometry.last_pose + ) def _next(self, idx): """TODO: re-arrange this logic""" @@ -112,22 +114,23 @@ def _next(self, idx): return frame, timestamps @staticmethod - def save_poses_kitti_format(filename: str, poses: List[np.ndarray]): + def save_poses_kitti_format(filename: str, poses: np.ndarray): def _to_kitti_format(poses: np.ndarray) -> np.ndarray: - return np.array([np.concatenate((pose[0], pose[1], pose[2])) for pose in poses]) + return poses[:, :3].reshape(-1, 12) np.savetxt(fname=f"{filename}_kitti.txt", X=_to_kitti_format(poses)) @staticmethod def save_poses_tum_format(filename, poses, timestamps): def _to_tum_format(poses, timestamps): - tum_data = [] + tum_data = np.zeros((len(poses), 8)) with contextlib.suppress(ValueError): for idx in range(len(poses)): - tx, ty, tz = poses[idx][:3, -1].flatten() + tx, ty, tz = poses[idx, :3, -1].flatten() qw, qx, qy, qz = Quaternion(matrix=poses[idx], atol=0.01).elements - tum_data.append([float(timestamps[idx]), tx, ty, tz, qx, qy, qz, qw]) - return np.array(tum_data).astype(np.float64) + tum_data[idx] = np.r_[float(timestamps[idx]), tx, ty, tz, qx, qy, qz, qw] + tum_data.flatten() + return tum_data.astype(np.float64) np.savetxt(fname=f"{filename}_tum.txt", X=_to_tum_format(poses, timestamps), fmt="%.4f") @@ -142,7 +145,7 @@ def _get_frames_timestamps(self): return ( self._dataset.get_frames_timestamps() if hasattr(self._dataset, "get_frames_timestamps") - else np.arange(0, len(self.poses), 1.0) + else np.arange(0, self._n_scans, 1.0) ) def _save_poses(self, filename: str, poses, timestamps): @@ -178,8 +181,8 @@ def _run_evaluation(self): # Run timing metrics evaluation, always def _get_fps(): - total_time_s = sum(self.times) * 1e-9 - return float(len(self.times) / total_time_s) if total_time_s > 0 else 0 + total_time_s = np.sum(self.times) * 1e-9 + return float(self._n_scans / total_time_s) if total_time_s > 0 else 0 fps = _get_fps() avg_fps = int(np.floor(fps)) From 345d0b42a7d2f815c80bd882d2324d7768ad5273 Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Thu, 25 Jul 2024 14:06:27 +0200 Subject: [PATCH 12/12] Bumb version (#375) * Bumb version * Finally 1.0.0 is here --- cpp/kiss_icp/CMakeLists.txt | 2 +- python/CMakeLists.txt | 2 +- python/kiss_icp/__init__.py | 2 +- python/pyproject.toml | 2 +- ros/CMakeLists.txt | 2 +- ros/package.xml | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/cpp/kiss_icp/CMakeLists.txt b/cpp/kiss_icp/CMakeLists.txt index abbb01a7..8220b520 100644 --- a/cpp/kiss_icp/CMakeLists.txt +++ b/cpp/kiss_icp/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(kiss_icp_cpp VERSION 0.4.0 LANGUAGES CXX) +project(kiss_icp_cpp VERSION 1.0.0 LANGUAGES CXX) # Setup build options option(USE_CCACHE "Build using Ccache if found on the path" ON) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index eaa2f9bd..8c6a682e 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(kiss_icp_pybind VERSION 0.4.0 LANGUAGES CXX) +project(kiss_icp_pybind VERSION 1.0.0 LANGUAGES CXX) # Set build type set(CMAKE_BUILD_TYPE Release) diff --git a/python/kiss_icp/__init__.py b/python/kiss_icp/__init__.py index 958055db..ff3e779e 100644 --- a/python/kiss_icp/__init__.py +++ b/python/kiss_icp/__init__.py @@ -20,4 +20,4 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. -__version__ = "0.4.0" +__version__ = "1.0.0" diff --git a/python/pyproject.toml b/python/pyproject.toml index d90826eb..613bd976 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build" [project] name = "kiss-icp" -version = "0.4.0" +version = "1.0.0" description = "Simple yet effective 3D LiDAR-Odometry registration pipeline" readme = "README.md" authors = [ diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index b36483b7..9a99896b 100644 --- a/ros/CMakeLists.txt +++ b/ros/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(kiss_icp VERSION 0.4.0 LANGUAGES CXX) +project(kiss_icp VERSION 1.0.0 LANGUAGES CXX) set(CMAKE_BUILD_TYPE Release) diff --git a/ros/package.xml b/ros/package.xml index cd3319de..19bb675d 100644 --- a/ros/package.xml +++ b/ros/package.xml @@ -25,7 +25,7 @@ --> kiss_icp - 0.4.0 + 1.0.0 KISS-ICP ROS 2 Wrapper ivizzo MIT