From 47e8e64abdaf4a6b135a6c739613e6df76776b9c Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Wed, 12 Jun 2024 14:58:13 +0200 Subject: [PATCH 1/8] Improvement for downsample one cycle is enough --- cpp/kiss_icp/core/Preprocessing.cpp | 31 ++++++++++++++++------------- cpp/kiss_icp/core/Preprocessing.hpp | 2 +- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 5256275f..4d870767 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -27,8 +27,8 @@ #include #include -#include #include +#include #include namespace { @@ -43,20 +43,23 @@ struct VoxelHash { 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 = Voxel((point / voxel_size).cast()); - if (grid.contains(voxel)) continue; - grid.insert({voxel, point}); - } + const double voxel_size) { + std::unordered_set voxels_set; std::vector frame_dowsampled; - frame_dowsampled.reserve(grid.size()); - for (const auto &[voxel, point] : grid) { - (void)voxel; - frame_dowsampled.emplace_back(point); - } + + voxels_set.reserve(frame.size()); + frame_dowsampled.reserve(frame.size()); + + std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) { + const auto voxel = Voxel((point / voxel_size).template cast()); + if (voxels_set.find(voxel) == voxels_set.end()) { + voxels_set.insert(voxel); + frame_dowsampled.emplace_back(point); + } + }); + + frame_dowsampled.shrink_to_fit(); + return frame_dowsampled; } diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index e919a422..69cc9935 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -41,5 +41,5 @@ std::vector CorrectKITTIScan(const std::vector /// Voxelize point cloud keeping the original coordinates std::vector VoxelDownsample(const std::vector &frame, - double voxel_size); + const double voxel_size); } // namespace kiss_icp From 527f9d4fc029038552a8ab6dfa65e2e89257c944 Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Thu, 13 Jun 2024 10:57:03 +0200 Subject: [PATCH 2/8] Remove template case --- cpp/kiss_icp/core/Preprocessing.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 4d870767..ef76a648 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -50,8 +50,8 @@ std::vector VoxelDownsample(const std::vector voxels_set.reserve(frame.size()); frame_dowsampled.reserve(frame.size()); - std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) { - const auto voxel = Voxel((point / voxel_size).template cast()); + std::for_each(frame.cbegin(), frame.cend(), [&](const Eigen::Vector3d &point) { + const auto voxel = Voxel((point / voxel_size).cast()); if (voxels_set.find(voxel) == voxels_set.end()) { voxels_set.insert(voxel); frame_dowsampled.emplace_back(point); From 23bb337a6186a2ef66b38a45bf533021a197ac74 Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Thu, 13 Jun 2024 12:40:56 +0200 Subject: [PATCH 3/8] Robin Set instead of std set --- cpp/kiss_icp/core/Preprocessing.cpp | 11 +++++------ tests.sh | 13 +++++++++++++ 2 files changed, 18 insertions(+), 6 deletions(-) create mode 100644 tests.sh diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index ef76a648..8dbd67b3 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -23,12 +23,11 @@ #include "Preprocessing.hpp" #include -#include +#include #include #include #include -#include #include namespace { @@ -44,16 +43,16 @@ struct VoxelHash { namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, const double voxel_size) { - std::unordered_set voxels_set; + tsl::robin_set voxels; std::vector frame_dowsampled; - voxels_set.reserve(frame.size()); + voxels.reserve(frame.size()); frame_dowsampled.reserve(frame.size()); std::for_each(frame.cbegin(), frame.cend(), [&](const Eigen::Vector3d &point) { const auto voxel = Voxel((point / voxel_size).cast()); - if (voxels_set.find(voxel) == voxels_set.end()) { - voxels_set.insert(voxel); + if (!voxels.contains(voxel)) { + voxels.insert(voxel); frame_dowsampled.emplace_back(point); } }); diff --git a/tests.sh b/tests.sh new file mode 100644 index 00000000..834af590 --- /dev/null +++ b/tests.sh @@ -0,0 +1,13 @@ +echo("MulRan-Riverside02") +kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/riverside/Riverside02 +echo("MulRan-KAIST01") +kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/kaist/KAIST01 +echo("MulRan-DCC03") +kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/dcc/DCC03 +echo("MulRan-Sejong02") +kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/sejong/Sejong02 +echo("Apollo-ColumbiaPark") +kiss_icp_pipeline --dataloader apollo /media/luca/T7/apollo_dataset/disk7/luweixin/Apollo-SourthBay/TestData/ColumbiaPark/2018-10-11/ +echo("NCLT") +kiss_icp_pipeline --dataloader nclt /home/luca/HDD8T/datasets/NCLT/2012-01-08/ + From 3239530711a26af96050d94e499b4892edec0801 Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Thu, 13 Jun 2024 12:41:40 +0200 Subject: [PATCH 4/8] Remove test scripts sorry --- tests.sh | 13 ------------- 1 file changed, 13 deletions(-) delete mode 100644 tests.sh diff --git a/tests.sh b/tests.sh deleted file mode 100644 index 834af590..00000000 --- a/tests.sh +++ /dev/null @@ -1,13 +0,0 @@ -echo("MulRan-Riverside02") -kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/riverside/Riverside02 -echo("MulRan-KAIST01") -kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/kaist/KAIST01 -echo("MulRan-DCC03") -kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/dcc/DCC03 -echo("MulRan-Sejong02") -kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/sejong/Sejong02 -echo("Apollo-ColumbiaPark") -kiss_icp_pipeline --dataloader apollo /media/luca/T7/apollo_dataset/disk7/luweixin/Apollo-SourthBay/TestData/ColumbiaPark/2018-10-11/ -echo("NCLT") -kiss_icp_pipeline --dataloader nclt /home/luca/HDD8T/datasets/NCLT/2012-01-08/ - From 7498fe73afc8a42e637875bcd93ac28fc3a434d9 Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Thu, 13 Jun 2024 12:53:16 +0200 Subject: [PATCH 5/8] Revert "Remove test scripts sorry" This reverts commit 3239530711a26af96050d94e499b4892edec0801. --- tests.sh | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 tests.sh diff --git a/tests.sh b/tests.sh new file mode 100644 index 00000000..834af590 --- /dev/null +++ b/tests.sh @@ -0,0 +1,13 @@ +echo("MulRan-Riverside02") +kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/riverside/Riverside02 +echo("MulRan-KAIST01") +kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/kaist/KAIST01 +echo("MulRan-DCC03") +kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/dcc/DCC03 +echo("MulRan-Sejong02") +kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/sejong/Sejong02 +echo("Apollo-ColumbiaPark") +kiss_icp_pipeline --dataloader apollo /media/luca/T7/apollo_dataset/disk7/luweixin/Apollo-SourthBay/TestData/ColumbiaPark/2018-10-11/ +echo("NCLT") +kiss_icp_pipeline --dataloader nclt /home/luca/HDD8T/datasets/NCLT/2012-01-08/ + From 9bf1fc17f1d26073f72f000694f9115b827afcc3 Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Thu, 13 Jun 2024 12:55:05 +0200 Subject: [PATCH 6/8] Revert "Robin Set instead of std set" This reverts commit 23bb337a6186a2ef66b38a45bf533021a197ac74. --- cpp/kiss_icp/core/Preprocessing.cpp | 11 ++++++----- tests.sh | 13 ------------- 2 files changed, 6 insertions(+), 18 deletions(-) delete mode 100644 tests.sh diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 8dbd67b3..ef76a648 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -23,11 +23,12 @@ #include "Preprocessing.hpp" #include -#include +#include #include #include #include +#include #include namespace { @@ -43,16 +44,16 @@ struct VoxelHash { namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, const double voxel_size) { - tsl::robin_set voxels; + std::unordered_set voxels_set; std::vector frame_dowsampled; - voxels.reserve(frame.size()); + voxels_set.reserve(frame.size()); frame_dowsampled.reserve(frame.size()); std::for_each(frame.cbegin(), frame.cend(), [&](const Eigen::Vector3d &point) { const auto voxel = Voxel((point / voxel_size).cast()); - if (!voxels.contains(voxel)) { - voxels.insert(voxel); + if (voxels_set.find(voxel) == voxels_set.end()) { + voxels_set.insert(voxel); frame_dowsampled.emplace_back(point); } }); diff --git a/tests.sh b/tests.sh deleted file mode 100644 index 834af590..00000000 --- a/tests.sh +++ /dev/null @@ -1,13 +0,0 @@ -echo("MulRan-Riverside02") -kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/riverside/Riverside02 -echo("MulRan-KAIST01") -kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/kaist/KAIST01 -echo("MulRan-DCC03") -kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/dcc/DCC03 -echo("MulRan-Sejong02") -kiss_icp_pipeline --dataloader mulran /home/luca/HDD8T/datasets/MulRan/sejong/Sejong02 -echo("Apollo-ColumbiaPark") -kiss_icp_pipeline --dataloader apollo /media/luca/T7/apollo_dataset/disk7/luweixin/Apollo-SourthBay/TestData/ColumbiaPark/2018-10-11/ -echo("NCLT") -kiss_icp_pipeline --dataloader nclt /home/luca/HDD8T/datasets/NCLT/2012-01-08/ - From 5fc8473cee81d8dc0fed963eca89d83722842172 Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Thu, 13 Jun 2024 12:57:25 +0200 Subject: [PATCH 7/8] Robin Set instead of std set --- cpp/kiss_icp/core/Preprocessing.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index ef76a648..8dbd67b3 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -23,12 +23,11 @@ #include "Preprocessing.hpp" #include -#include +#include #include #include #include -#include #include namespace { @@ -44,16 +43,16 @@ struct VoxelHash { namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, const double voxel_size) { - std::unordered_set voxels_set; + tsl::robin_set voxels; std::vector frame_dowsampled; - voxels_set.reserve(frame.size()); + voxels.reserve(frame.size()); frame_dowsampled.reserve(frame.size()); std::for_each(frame.cbegin(), frame.cend(), [&](const Eigen::Vector3d &point) { const auto voxel = Voxel((point / voxel_size).cast()); - if (voxels_set.find(voxel) == voxels_set.end()) { - voxels_set.insert(voxel); + if (!voxels.contains(voxel)) { + voxels.insert(voxel); frame_dowsampled.emplace_back(point); } }); From 06529e5553701264c2a66ec6ca6ea16e1532ad9b Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Thu, 27 Jun 2024 11:30:05 +0200 Subject: [PATCH 8/8] Single downsample --- python/kiss_icp/kiss_icp.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py index e7087dc8..8cff5848 100644 --- a/python/kiss_icp/kiss_icp.py +++ b/python/kiss_icp/kiss_icp.py @@ -50,7 +50,7 @@ def register_frame(self, frame, timestamps): frame = self.preprocess(frame) # Voxelize - source, frame_downsample = self.voxelize(frame) + source = voxel_down_sample(frame, self.config.mapping.voxel_size * 1.5) # Get adaptive_threshold sigma = self.adaptive_threshold.get_threshold() @@ -72,14 +72,9 @@ def register_frame(self, frame, timestamps): # Update step: threshold, local map, delta, and the last pose self.adaptive_threshold.update_model_deviation(model_deviation) - self.local_map.update(frame_downsample, new_pose) + self.local_map.update(source, new_pose) self.last_delta = np.linalg.inv(self.last_pose) @ new_pose self.last_pose = new_pose # Return the (deskew) input raw scan (frame) and the points used for registration (source) return frame, source - - def voxelize(self, iframe): - frame_downsample = voxel_down_sample(iframe, self.config.mapping.voxel_size * 0.5) - source = voxel_down_sample(frame_downsample, self.config.mapping.voxel_size * 1.5) - return source, frame_downsample