From e5044f87774fe148cf8aaa92d8d008139a48c499 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 14:15:10 +0100 Subject: [PATCH 01/35] First draft on core library --- cpp/kiss_icp/core/Registration.cpp | 95 +++++++++++++++++++++++++++++- cpp/kiss_icp/core/VoxelHashMap.cpp | 94 ----------------------------- cpp/kiss_icp/core/VoxelHashMap.hpp | 3 - 3 files changed, 94 insertions(+), 98 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 1d22bef0..e34ddb64 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -31,6 +31,8 @@ #include #include +#include "VoxelHashMap.hpp" + namespace Eigen { using Matrix6d = Eigen::Matrix; using Matrix3_6d = Eigen::Matrix; @@ -57,6 +59,15 @@ struct ResultTuple { Eigen::Vector6d JTr; }; +struct ResultTuple2 { + ResultTuple2(std::size_t n) { + source.reserve(n); + target.reserve(n); + } + std::vector source; + std::vector target; +}; + void TransformPoints(const Sophus::SE3d &T, std::vector &points) { std::transform(points.cbegin(), points.cend(), points.begin(), [&](const auto &point) { return T * point; }); @@ -101,6 +112,88 @@ std::tuple BuildLinearSystem( return std::make_tuple(JTJ, JTr); } + +using Vector3dVector = std::vector; +using Vector3dVectorTuple = std::tuple; +Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { + // Lambda Function to obtain the KNN of one point, maybe refactor + auto GetClosestNeighboor = [&](const Eigen::Vector3d &point) { + auto kx = static_cast(point[0] / voxel_map.voxel_size_); + auto ky = static_cast(point[1] / voxel_map.voxel_size_); + auto kz = static_cast(point[2] / voxel_map.voxel_size_); + std::vector voxels; + voxels.reserve(27); + for (int i = kx - 1; i < kx + 1 + 1; ++i) { + for (int j = ky - 1; j < ky + 1 + 1; ++j) { + for (int k = kz - 1; k < kz + 1 + 1; ++k) { + voxels.emplace_back(i, j, k); + } + } + } + + Vector3dVector neighboors; + neighboors.reserve(27 * voxel_map.max_points_per_voxel_); + std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) { + auto search = voxel_map.map_.find(voxel); + if (search != voxel_map.map_.end()) { + const auto &points = search->second.points; + if (!points.empty()) { + for (const auto &point : points) { + neighboors.emplace_back(point); + } + } + } + }); + + Eigen::Vector3d closest_neighbor; + double closest_distance2 = std::numeric_limits::max(); + std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor) { + double distance = (neighbor - point).squaredNorm(); + if (distance < closest_distance2) { + closest_neighbor = neighbor; + closest_distance2 = distance; + } + }); + + return closest_neighbor; + }; + + using points_iterator = std::vector::const_iterator; + const auto [source, target] = tbb::parallel_reduce( + // Range + tbb::blocked_range{points.cbegin(), points.cend()}, + // Identity + ResultTuple2(points.size()), + // 1st lambda: Parallel computation + [max_correspondance_distance, &GetClosestNeighboor]( + const tbb::blocked_range &r, ResultTuple2 res) -> ResultTuple2 { + auto &[src, tgt] = res; + src.reserve(r.size()); + tgt.reserve(r.size()); + for (const auto &point : r) { + Eigen::Vector3d closest_neighboors = GetClosestNeighboor(point); + if ((closest_neighboors - point).norm() < max_correspondance_distance) { + src.emplace_back(point); + tgt.emplace_back(closest_neighboors); + } + } + return res; + }, + // 2nd lambda: Parallel reduction + [](ResultTuple2 a, const ResultTuple2 &b) -> ResultTuple2 { + auto &[src, tgt] = a; + const auto &[srcp, tgtp] = b; + src.insert(src.end(), // + std::make_move_iterator(srcp.begin()), std::make_move_iterator(srcp.end())); + tgt.insert(tgt.end(), // + std::make_move_iterator(tgtp.begin()), std::make_move_iterator(tgtp.end())); + return a; + }); + + return std::make_tuple(source, target); +} } // namespace namespace kiss_icp { @@ -120,7 +213,7 @@ Sophus::SE3d RegisterFrame(const std::vector &frame, Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < MAX_NUM_ITERATIONS_; ++j) { // Equation (10) - const auto &[src, tgt] = voxel_map.GetCorrespondences(source, max_correspondence_distance); + const auto &[src, tgt] = GetCorrespondences(source, voxel_map, max_correspondence_distance); // Equation (11) const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 2212dd67..3497f2d4 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -22,9 +22,6 @@ // SOFTWARE. #include "VoxelHashMap.hpp" -#include -#include - #include #include #include @@ -32,99 +29,8 @@ #include #include -// This parameters are not intended to be changed, therefore we do not expose it -namespace { -struct ResultTuple { - ResultTuple(std::size_t n) { - source.reserve(n); - target.reserve(n); - } - std::vector source; - std::vector target; -}; -} // namespace - namespace kiss_icp { -VoxelHashMap::Vector3dVectorTuple VoxelHashMap::GetCorrespondences( - const Vector3dVector &points, double max_correspondance_distance) const { - // Lambda Function to obtain the KNN of one point, maybe refactor - auto GetClosestNeighboor = [&](const Eigen::Vector3d &point) { - auto kx = static_cast(point[0] / voxel_size_); - auto ky = static_cast(point[1] / voxel_size_); - auto kz = static_cast(point[2] / voxel_size_); - std::vector voxels; - voxels.reserve(27); - for (int i = kx - 1; i < kx + 1 + 1; ++i) { - for (int j = ky - 1; j < ky + 1 + 1; ++j) { - for (int k = kz - 1; k < kz + 1 + 1; ++k) { - voxels.emplace_back(i, j, k); - } - } - } - - using Vector3dVector = std::vector; - Vector3dVector neighboors; - neighboors.reserve(27 * max_points_per_voxel_); - std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) { - auto search = map_.find(voxel); - if (search != map_.end()) { - const auto &points = search->second.points; - if (!points.empty()) { - for (const auto &point : points) { - neighboors.emplace_back(point); - } - } - } - }); - - Eigen::Vector3d closest_neighbor; - double closest_distance2 = std::numeric_limits::max(); - std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor) { - double distance = (neighbor - point).squaredNorm(); - if (distance < closest_distance2) { - closest_neighbor = neighbor; - closest_distance2 = distance; - } - }); - - return closest_neighbor; - }; - using points_iterator = std::vector::const_iterator; - const auto [source, target] = tbb::parallel_reduce( - // Range - tbb::blocked_range{points.cbegin(), points.cend()}, - // Identity - ResultTuple(points.size()), - // 1st lambda: Parallel computation - [max_correspondance_distance, &GetClosestNeighboor]( - const tbb::blocked_range &r, ResultTuple res) -> ResultTuple { - auto &[src, tgt] = res; - src.reserve(r.size()); - tgt.reserve(r.size()); - for (const auto &point : r) { - Eigen::Vector3d closest_neighboors = GetClosestNeighboor(point); - if ((closest_neighboors - point).norm() < max_correspondance_distance) { - src.emplace_back(point); - tgt.emplace_back(closest_neighboors); - } - } - return res; - }, - // 2nd lambda: Parallel reduction - [](ResultTuple a, const ResultTuple &b) -> ResultTuple { - auto &[src, tgt] = a; - const auto &[srcp, tgtp] = b; - src.insert(src.end(), // - std::make_move_iterator(srcp.begin()), std::make_move_iterator(srcp.end())); - tgt.insert(tgt.end(), // - std::make_move_iterator(tgtp.begin()), std::make_move_iterator(tgtp.end())); - return a; - }); - - return std::make_tuple(source, target); -} - std::vector VoxelHashMap::Pointcloud() const { std::vector points; points.reserve(max_points_per_voxel_ * map_.size()); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 075a676a..782cf8ab 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -35,7 +35,6 @@ namespace kiss_icp { struct VoxelHashMap { using Vector3dVector = std::vector; - using Vector3dVectorTuple = std::tuple; using Voxel = Eigen::Vector3i; struct VoxelBlock { // buffer of points with a max limit of n_points @@ -57,8 +56,6 @@ struct VoxelHashMap { max_distance_(max_distance), max_points_per_voxel_(max_points_per_voxel) {} - Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, - double max_correspondance_distance) const; inline void Clear() { map_.clear(); } inline bool Empty() const { return map_.empty(); } void Update(const std::vector &points, const Eigen::Vector3d &origin); From 6c7e5051087551204d6d5452074d57db19e92b55 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 14:15:34 +0100 Subject: [PATCH 02/35] Update python API --- python/kiss_icp/mapping.py | 12 ------------ python/kiss_icp/pybind/kiss_icp_pybind.cpp | 4 +--- 2 files changed, 1 insertion(+), 15 deletions(-) diff --git a/python/kiss_icp/mapping.py b/python/kiss_icp/mapping.py index b0f7f67e..652f706e 100644 --- a/python/kiss_icp/mapping.py +++ b/python/kiss_icp/mapping.py @@ -68,15 +68,3 @@ def remove_far_away_points(self, origin): def point_cloud(self) -> np.ndarray: """Return the internal representaion as a np.array (pointcloud).""" return np.asarray(self._internal_map._point_cloud()) - - def get_correspondences( - self, - points: np.ndarray, - max_correspondance_distance: float, - ) -> Tuple[np.ndarray, np.ndarray]: - """Get the pair of {source, target} pointcloud of the same size.""" - _points = kiss_icp_pybind._Vector3dVector(points) - source, target = self._internal_map._get_correspondences( - _points, max_correspondance_distance - ) - return np.asarray(source), np.asarray(target) diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index 32cb1702..ca9a5bb3 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -70,9 +70,7 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { "points"_a, "pose"_a) .def("_add_points", &VoxelHashMap::AddPoints, "points"_a) .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a) - .def("_point_cloud", &VoxelHashMap::Pointcloud) - .def("_get_correspondences", &VoxelHashMap::GetCorrespondences, "points"_a, - "max_correspondance_distance"_a); + .def("_point_cloud", &VoxelHashMap::Pointcloud); // Point Cloud registration m.def( From 80248bb394fb8454cd78a5b23f3255ce0674165e Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 14:23:04 +0100 Subject: [PATCH 03/35] Rearange --- cpp/kiss_icp/core/Registration.cpp | 67 ++++++++++++++++-------------- 1 file changed, 35 insertions(+), 32 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index e34ddb64..0d41fb54 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -41,45 +41,41 @@ using Vector6d = Eigen::Matrix; namespace { -inline double square(double x) { return x * x; } - -struct ResultTuple { - ResultTuple() { - JTJ.setZero(); - JTr.setZero(); - } - - ResultTuple operator+(const ResultTuple &other) { - this->JTJ += other.JTJ; - this->JTr += other.JTr; - return *this; - } +// constants +constexpr int MAX_NUM_ITERATIONS_ = 500; +constexpr double ESTIMATION_THRESHOLD_ = 0.0001; - Eigen::Matrix6d JTJ; - Eigen::Vector6d JTr; -}; +// aliases +using Vector3dVector = std::vector; +using Vector3dVectorTuple = std::tuple; -struct ResultTuple2 { - ResultTuple2(std::size_t n) { - source.reserve(n); - target.reserve(n); - } - std::vector source; - std::vector target; -}; +inline double square(double x) { return x * x; } void TransformPoints(const Sophus::SE3d &T, std::vector &points) { std::transform(points.cbegin(), points.cend(), points.begin(), [&](const auto &point) { return T * point; }); } -constexpr int MAX_NUM_ITERATIONS_ = 500; -constexpr double ESTIMATION_THRESHOLD_ = 0.0001; - std::tuple BuildLinearSystem( const std::vector &source, const std::vector &target, double kernel) { + struct ResultTuple { + ResultTuple() { + JTJ.setZero(); + JTr.setZero(); + } + + ResultTuple operator+(const ResultTuple &other) { + this->JTJ += other.JTJ; + this->JTr += other.JTr; + return *this; + } + + Eigen::Matrix6d JTJ; + Eigen::Vector6d JTr; + }; + auto compute_jacobian_and_residual = [&](auto i) { const Eigen::Vector3d residual = source[i] - target[i]; Eigen::Matrix3_6d J_r; @@ -113,11 +109,18 @@ std::tuple BuildLinearSystem( return std::make_tuple(JTJ, JTr); } -using Vector3dVector = std::vector; -using Vector3dVectorTuple = std::tuple; Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, const kiss_icp::VoxelHashMap &voxel_map, double max_correspondance_distance) { + struct ResultTuple { + ResultTuple(std::size_t n) { + source.reserve(n); + target.reserve(n); + } + std::vector source; + std::vector target; + }; + // Lambda Function to obtain the KNN of one point, maybe refactor auto GetClosestNeighboor = [&](const Eigen::Vector3d &point) { auto kx = static_cast(point[0] / voxel_map.voxel_size_); @@ -165,10 +168,10 @@ Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, // Range tbb::blocked_range{points.cbegin(), points.cend()}, // Identity - ResultTuple2(points.size()), + ResultTuple(points.size()), // 1st lambda: Parallel computation [max_correspondance_distance, &GetClosestNeighboor]( - const tbb::blocked_range &r, ResultTuple2 res) -> ResultTuple2 { + const tbb::blocked_range &r, ResultTuple res) -> ResultTuple { auto &[src, tgt] = res; src.reserve(r.size()); tgt.reserve(r.size()); @@ -182,7 +185,7 @@ Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, return res; }, // 2nd lambda: Parallel reduction - [](ResultTuple2 a, const ResultTuple2 &b) -> ResultTuple2 { + [](ResultTuple a, const ResultTuple &b) -> ResultTuple { auto &[src, tgt] = a; const auto &[srcp, tgtp] = b; src.insert(src.end(), // From 505ab616120dbc83944abc67234e5e941ea88c1a Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 14:28:50 +0100 Subject: [PATCH 04/35] Remove type alias --- cpp/kiss_icp/core/VoxelHashMap.cpp | 7 ++++--- cpp/kiss_icp/core/VoxelHashMap.hpp | 1 - 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 3497f2d4..96b04bc4 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -43,13 +43,14 @@ std::vector VoxelHashMap::Pointcloud() const { return points; } -void VoxelHashMap::Update(const Vector3dVector &points, const Eigen::Vector3d &origin) { +void VoxelHashMap::Update(const std::vector &points, + const Eigen::Vector3d &origin) { AddPoints(points); RemovePointsFarFromLocation(origin); } -void VoxelHashMap::Update(const Vector3dVector &points, const Sophus::SE3d &pose) { - Vector3dVector points_transformed(points.size()); +void VoxelHashMap::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(); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 782cf8ab..d307bf5d 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -34,7 +34,6 @@ namespace kiss_icp { struct VoxelHashMap { - using Vector3dVector = std::vector; using Voxel = Eigen::Vector3i; struct VoxelBlock { // buffer of points with a max limit of n_points From 1dc13a795b2dee72bdc8e095444e4e3e4d5cfa13 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 14:35:21 +0100 Subject: [PATCH 05/35] Fix build --- python/kiss_icp/pybind/kiss_icp_pybind.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index ca9a5bb3..6fc4fb61 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -57,12 +57,12 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { .def("_clear", &VoxelHashMap::Clear) .def("_empty", &VoxelHashMap::Empty) .def("_update", - py::overload_cast( + py::overload_cast &, const Eigen::Vector3d &>( &VoxelHashMap::Update), "points"_a, "origin"_a) .def( "_update", - [](VoxelHashMap &self, const VoxelHashMap::Vector3dVector &points, + [](VoxelHashMap &self, const std::vector &points, const Eigen::Matrix4d &T) { Sophus::SE3d pose(T); self.Update(points, pose); From 5053877a977071e0c0da4aa8bef461f4905391b1 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 16:53:15 +0100 Subject: [PATCH 06/35] Wrap constants into a configuration struct --- cpp/kiss_icp/core/Registration.cpp | 17 ++++++----------- cpp/kiss_icp/core/Registration.hpp | 16 ++++++++++++++-- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 0d41fb54..8168e721 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -40,11 +40,6 @@ using Vector6d = Eigen::Matrix; } // namespace Eigen namespace { - -// constants -constexpr int MAX_NUM_ITERATIONS_ = 500; -constexpr double ESTIMATION_THRESHOLD_ = 0.0001; - // aliases using Vector3dVector = std::vector; using Vector3dVectorTuple = std::tuple; @@ -204,8 +199,7 @@ namespace kiss_icp { Sophus::SE3d RegisterFrame(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, - double max_correspondence_distance, - double kernel) { + const RegistrationConfig &config) { if (voxel_map.Empty()) return initial_guess; // Equation (9) @@ -214,11 +208,12 @@ Sophus::SE3d RegisterFrame(const std::vector &frame, // ICP-loop Sophus::SE3d T_icp = Sophus::SE3d(); - for (int j = 0; j < MAX_NUM_ITERATIONS_; ++j) { + for (int j = 0; j < config.max_num_iterations; ++j) { // Equation (10) - const auto &[src, tgt] = GetCorrespondences(source, voxel_map, max_correspondence_distance); + const auto &[src, tgt] = + GetCorrespondences(source, voxel_map, config.max_correspondence_distance); // Equation (11) - const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, kernel); + const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, config.kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); const Sophus::SE3d estimation = Sophus::SE3d::exp(dx); // Equation (12) @@ -226,7 +221,7 @@ Sophus::SE3d RegisterFrame(const std::vector &frame, // Update iterations T_icp = estimation * T_icp; // Termination criteria - if (dx.norm() < ESTIMATION_THRESHOLD_) break; + if (dx.norm() < config.estimation_threshold) break; } // Spit the final transformation return T_icp * initial_guess; diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index eb82d37b..736666ac 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -30,9 +30,21 @@ namespace kiss_icp { +struct RegistrationConfig { + // constans + int max_num_iterations = 500; + double estimation_threshold = 0.0001; + + // variables + double max_correspondence_distance = 0.0; + double kernel = 0.0; +}; + +// Register a point cloud to the given internal map represetnation. The config input parameter +// contains all the neccesary parametrization for the ICP loop Sophus::SE3d RegisterFrame(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, - double max_correspondence_distance, - double kernel); + const RegistrationConfig &config); + } // namespace kiss_icp From 0e015d92b227dbf54d3906f0a9a45648d0a5bfe6 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 16:53:28 +0100 Subject: [PATCH 07/35] Split the watters --- cpp/kiss_icp/core/Registration.cpp | 56 ++++-------------------------- cpp/kiss_icp/core/Registration.hpp | 17 ++++----- cpp/kiss_icp/core/VoxelHashMap.cpp | 42 ++++++++++++++++++++++ cpp/kiss_icp/core/VoxelHashMap.hpp | 1 + 4 files changed, 57 insertions(+), 59 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 8168e721..d38bca1a 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -116,48 +116,6 @@ Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, std::vector target; }; - // Lambda Function to obtain the KNN of one point, maybe refactor - auto GetClosestNeighboor = [&](const Eigen::Vector3d &point) { - auto kx = static_cast(point[0] / voxel_map.voxel_size_); - auto ky = static_cast(point[1] / voxel_map.voxel_size_); - auto kz = static_cast(point[2] / voxel_map.voxel_size_); - std::vector voxels; - voxels.reserve(27); - for (int i = kx - 1; i < kx + 1 + 1; ++i) { - for (int j = ky - 1; j < ky + 1 + 1; ++j) { - for (int k = kz - 1; k < kz + 1 + 1; ++k) { - voxels.emplace_back(i, j, k); - } - } - } - - Vector3dVector neighboors; - neighboors.reserve(27 * voxel_map.max_points_per_voxel_); - std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) { - auto search = voxel_map.map_.find(voxel); - if (search != voxel_map.map_.end()) { - const auto &points = search->second.points; - if (!points.empty()) { - for (const auto &point : points) { - neighboors.emplace_back(point); - } - } - } - }); - - Eigen::Vector3d closest_neighbor; - double closest_distance2 = std::numeric_limits::max(); - std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor) { - double distance = (neighbor - point).squaredNorm(); - if (distance < closest_distance2) { - closest_neighbor = neighbor; - closest_distance2 = distance; - } - }); - - return closest_neighbor; - }; - using points_iterator = std::vector::const_iterator; const auto [source, target] = tbb::parallel_reduce( // Range @@ -165,13 +123,12 @@ Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, // Identity ResultTuple(points.size()), // 1st lambda: Parallel computation - [max_correspondance_distance, &GetClosestNeighboor]( - const tbb::blocked_range &r, ResultTuple res) -> ResultTuple { + [&](const tbb::blocked_range &r, ResultTuple res) -> ResultTuple { auto &[src, tgt] = res; src.reserve(r.size()); tgt.reserve(r.size()); for (const auto &point : r) { - Eigen::Vector3d closest_neighboors = GetClosestNeighboor(point); + Eigen::Vector3d closest_neighboors = voxel_map.GetClosestNeighboor(point); if ((closest_neighboors - point).norm() < max_correspondance_distance) { src.emplace_back(point); tgt.emplace_back(closest_neighboors); @@ -199,7 +156,9 @@ namespace kiss_icp { Sophus::SE3d RegisterFrame(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, - const RegistrationConfig &config) { + const RegistrationConfig &config, + double max_distance = 0.0, + double kernel = 0.0) { if (voxel_map.Empty()) return initial_guess; // Equation (9) @@ -210,10 +169,9 @@ Sophus::SE3d RegisterFrame(const std::vector &frame, Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < config.max_num_iterations; ++j) { // Equation (10) - const auto &[src, tgt] = - GetCorrespondences(source, voxel_map, config.max_correspondence_distance); + const auto &[src, tgt] = GetCorrespondences(source, voxel_map, max_distance); // Equation (11) - const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, config.kernel); + const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); const Sophus::SE3d estimation = Sophus::SE3d::exp(dx); // Equation (12) diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index 736666ac..f842d589 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -31,20 +31,17 @@ namespace kiss_icp { struct RegistrationConfig { - // constans - int max_num_iterations = 500; - double estimation_threshold = 0.0001; - - // variables - double max_correspondence_distance = 0.0; - double kernel = 0.0; + const int max_num_iterations = 500; + const double estimation_threshold = 0.0001; }; -// Register a point cloud to the given internal map represetnation. The config input parameter -// contains all the neccesary parametrization for the ICP loop +// Register a point cloud to the given internal map representation. The config input parameter +// contains all the necessary parametrization for the ICP loop Sophus::SE3d RegisterFrame(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, - const RegistrationConfig &config); + const RegistrationConfig &config, + double max_distance, + double kernel); } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 96b04bc4..e3a6b2e7 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -31,6 +31,48 @@ namespace kiss_icp { +// Function to obtain the KNN of one point +Eigen::Vector3d VoxelHashMap::GetClosestNeighboor(const Eigen::Vector3d &point) const { + auto kx = static_cast(point[0] / voxel_size_); + auto ky = static_cast(point[1] / voxel_size_); + auto kz = static_cast(point[2] / voxel_size_); + std::vector voxels; + voxels.reserve(27); + for (int i = kx - 1; i < kx + 1 + 1; ++i) { + for (int j = ky - 1; j < ky + 1 + 1; ++j) { + for (int k = kz - 1; k < kz + 1 + 1; ++k) { + voxels.emplace_back(i, j, k); + } + } + } + + std::vector neighboors; + neighboors.reserve(static_cast(27 * max_points_per_voxel_)); + std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) { + auto search = map_.find(voxel); + if (search != map_.end()) { + const auto &points = search->second.points; + if (!points.empty()) { + for (const auto &point : points) { + neighboors.emplace_back(point); + } + } + } + }); + + Eigen::Vector3d closest_neighbor; + double closest_distance2 = std::numeric_limits::max(); + std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor) { + double distance = (neighbor - point).squaredNorm(); + if (distance < closest_distance2) { + closest_neighbor = neighbor; + closest_distance2 = distance; + } + }); + + return closest_neighbor; +}; + std::vector VoxelHashMap::Pointcloud() const { std::vector points; points.reserve(max_points_per_voxel_ * map_.size()); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index d307bf5d..d97aa5c5 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -62,6 +62,7 @@ struct VoxelHashMap { void AddPoints(const std::vector &points); void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); std::vector Pointcloud() const; + Eigen::Vector3d GetClosestNeighboor(const Eigen::Vector3d &point) const; double voxel_size_; double max_distance_; From d4378b4f7a68b1cf7a0583a08749dc5de69d4d99 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 17:03:08 +0100 Subject: [PATCH 08/35] It's all about drafts --- cpp/kiss_icp/core/Registration.cpp | 15 +++++++-------- cpp/kiss_icp/core/Registration.hpp | 25 +++++++++++++------------ cpp/kiss_icp/pipeline/KissICP.cpp | 10 +++++----- cpp/kiss_icp/pipeline/KissICP.hpp | 7 +++++++ 4 files changed, 32 insertions(+), 25 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index d38bca1a..1725a3cf 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -153,12 +153,11 @@ Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, namespace kiss_icp { -Sophus::SE3d RegisterFrame(const std::vector &frame, - const VoxelHashMap &voxel_map, - const Sophus::SE3d &initial_guess, - const RegistrationConfig &config, - double max_distance = 0.0, - double kernel = 0.0) { +Sophus::SE3d Registration::RegisterFrame(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_distance, + double kernel) { if (voxel_map.Empty()) return initial_guess; // Equation (9) @@ -167,7 +166,7 @@ Sophus::SE3d RegisterFrame(const std::vector &frame, // ICP-loop Sophus::SE3d T_icp = Sophus::SE3d(); - for (int j = 0; j < config.max_num_iterations; ++j) { + for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) const auto &[src, tgt] = GetCorrespondences(source, voxel_map, max_distance); // Equation (11) @@ -179,7 +178,7 @@ Sophus::SE3d RegisterFrame(const std::vector &frame, // Update iterations T_icp = estimation * T_icp; // Termination criteria - if (dx.norm() < config.estimation_threshold) break; + if (dx.norm() < estimation_threshold_) break; } // Spit the final transformation return T_icp * initial_guess; diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index f842d589..7005af45 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -30,18 +30,19 @@ namespace kiss_icp { -struct RegistrationConfig { - const int max_num_iterations = 500; - const double estimation_threshold = 0.0001; -}; +struct Registration { + explicit Registration(int max_num_iteration, double estimation_threshold) + : max_num_iterations_(max_num_iteration), estimation_threshold_(estimation_threshold) {} -// Register a point cloud to the given internal map representation. The config input parameter -// contains all the necessary parametrization for the ICP loop -Sophus::SE3d RegisterFrame(const std::vector &frame, - const VoxelHashMap &voxel_map, - const Sophus::SE3d &initial_guess, - const RegistrationConfig &config, - double max_distance, - double kernel); + // Register a point cloud to the given internal map representation. The config input parameter + // contains all the necessary parametrization for the ICP loop + Sophus::SE3d RegisterFrame(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_distance, + double kernel); + int max_num_iterations_; + double estimation_threshold_; +}; } // namespace kiss_icp diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp index 3e06f7f0..befbece3 100644 --- a/cpp/kiss_icp/pipeline/KissICP.cpp +++ b/cpp/kiss_icp/pipeline/KissICP.cpp @@ -68,11 +68,11 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector #include "kiss_icp/core/Deskew.hpp" +#include "kiss_icp/core/Registration.hpp" #include "kiss_icp/core/Threshold.hpp" #include "kiss_icp/core/VoxelHashMap.hpp" @@ -42,6 +43,10 @@ struct KISSConfig { double min_motion_th = 0.1; double initial_threshold = 2.0; + // registration params + int max_num_iterations = 500; + double estimation_threshold = 0.0001; + // Motion compensation bool deskew = false; }; @@ -54,6 +59,7 @@ class KissICP { public: explicit KissICP(const KISSConfig &config) : config_(config), + registration_(config.max_num_iterations, config.estimation_threshold), local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel), adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {} @@ -77,6 +83,7 @@ class KissICP { // KISS-ICP pipeline modules std::vector poses_; KISSConfig config_; + Registration registration_; VoxelHashMap local_map_; AdaptiveThreshold adaptive_threshold_; }; From ebdfab56f7ac9b1684d37e0a99557d67b33d137b Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 17:18:07 +0100 Subject: [PATCH 09/35] Update python API I'm alredy regreting about this --- python/kiss_icp/config/config.py | 5 +++ python/kiss_icp/config/parser.py | 8 ++++- python/kiss_icp/kiss_icp.py | 5 +-- python/kiss_icp/mapping.py | 2 -- python/kiss_icp/pybind/kiss_icp_pybind.cpp | 27 ++++++++------ python/kiss_icp/registration.py | 42 +++++++++++++++------- 6 files changed, 61 insertions(+), 28 deletions(-) diff --git a/python/kiss_icp/config/config.py b/python/kiss_icp/config/config.py index 4c9a29e9..e402e7de 100644 --- a/python/kiss_icp/config/config.py +++ b/python/kiss_icp/config/config.py @@ -36,6 +36,11 @@ class MappingConfig(BaseModel): max_points_per_voxel: int = 20 +class RegistrationConfig(BaseModel): + max_num_iterations: Optional[int] = 500 + estimation_threshold: Optional[float] = 0.0001 + + class AdaptiveThresholdConfig(BaseModel): fixed_threshold: Optional[float] = None initial_threshold: float = 2.0 diff --git a/python/kiss_icp/config/parser.py b/python/kiss_icp/config/parser.py index 831f3039..558a01eb 100644 --- a/python/kiss_icp/config/parser.py +++ b/python/kiss_icp/config/parser.py @@ -30,13 +30,19 @@ from pydantic_settings import BaseSettings, SettingsConfigDict -from kiss_icp.config.config import AdaptiveThresholdConfig, DataConfig, MappingConfig +from kiss_icp.config.config import ( + AdaptiveThresholdConfig, + DataConfig, + MappingConfig, + RegistrationConfig, +) class KISSConfig(BaseSettings): model_config = SettingsConfigDict(env_prefix="kiss_icp_") out_dir: str = "results" data: DataConfig = DataConfig() + registration: RegistrationConfig = RegistrationConfig() mapping: MappingConfig = MappingConfig() adaptive_threshold: AdaptiveThresholdConfig = AdaptiveThresholdConfig() diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py index 6bcdae0b..d35ecc4a 100644 --- a/python/kiss_icp/kiss_icp.py +++ b/python/kiss_icp/kiss_icp.py @@ -26,7 +26,7 @@ from kiss_icp.deskew import get_motion_compensator from kiss_icp.mapping import get_voxel_hash_map from kiss_icp.preprocess import get_preprocessor -from kiss_icp.registration import register_frame +from kiss_icp.registration import get_registration from kiss_icp.threshold import get_threshold_estimator from kiss_icp.voxelization import voxel_down_sample @@ -37,6 +37,7 @@ def __init__(self, config: KISSConfig): self.config = config self.compensator = get_motion_compensator(config) self.adaptive_threshold = get_threshold_estimator(self.config) + self.registration = get_registration(self.config) self.local_map = get_voxel_hash_map(self.config) self.preprocess = get_preprocessor(self.config) @@ -59,7 +60,7 @@ def register_frame(self, frame, timestamps): initial_guess = last_pose @ prediction # Run ICP - new_pose = register_frame( + new_pose = self.registration.register_frame( points=source, voxel_map=self.local_map, initial_guess=initial_guess, diff --git a/python/kiss_icp/mapping.py b/python/kiss_icp/mapping.py index 652f706e..ba1dfb73 100644 --- a/python/kiss_icp/mapping.py +++ b/python/kiss_icp/mapping.py @@ -20,8 +20,6 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. -from typing import Tuple - import numpy as np from kiss_icp.config import KISSConfig diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index 6fc4fb61..6712c28f 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -73,16 +73,23 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { .def("_point_cloud", &VoxelHashMap::Pointcloud); // Point Cloud registration - m.def( - "_register_point_cloud", - [](const std::vector &points, const VoxelHashMap &voxel_map, - const Eigen::Matrix4d &T_guess, double max_correspondence_distance, double kernel) { - Sophus::SE3d initial_guess(T_guess); - return RegisterFrame(points, voxel_map, initial_guess, max_correspondence_distance, - kernel) - .matrix(); - }, - "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a, "kernel"_a); + py::class_ internal_registration(m, "_Registration", "Don't use this"); + internal_registration + .def(py::init(), "max_num_iterations"_a, "estimation_threshold"_a) + .def( + "_register_point_cloud", + [](Registration &self, const std::vector &points, + const VoxelHashMap &voxel_map, const Eigen::Matrix4d &T_guess, + double max_correspondence_distance, double kernel) { + Sophus::SE3d initial_guess(T_guess); + return self + .RegisterFrame(points, voxel_map, initial_guess, max_correspondence_distance, + kernel) + .matrix(); + }, + "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a, + "kernel"_a); + // AdaptiveThreshold bindings py::class_ adaptive_threshold(m, "_AdaptiveThreshold", "Don't use this"); adaptive_threshold diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py index ebba5706..86505d0b 100644 --- a/python/kiss_icp/registration.py +++ b/python/kiss_icp/registration.py @@ -22,21 +22,37 @@ # SOFTWARE. import numpy as np +from kiss_icp.config.parser import KISSConfig from kiss_icp.mapping import VoxelHashMap from kiss_icp.pybind import kiss_icp_pybind -def register_frame( - points: np.ndarray, - voxel_map: VoxelHashMap, - initial_guess: np.ndarray, - max_correspondance_distance: float, - kernel: float, -) -> np.ndarray: - return kiss_icp_pybind._register_point_cloud( - points=kiss_icp_pybind._Vector3dVector(points), - voxel_map=voxel_map._internal_map, - initial_guess=initial_guess, - max_correspondance_distance=max_correspondance_distance, - kernel=kernel, +def get_registration(config: KISSConfig): + return Registration( + max_num_iterations=config.registration.max_num_iterations, + estimation_threshold=config.registration.estimation_threshold, ) + + +class Registration: + def __init__(self, max_num_iterations: int, estimation_threshold: float): + self._registration = kiss_icp_pybind._Registration( + max_num_iterations=max_num_iterations, + estimation_threshold=estimation_threshold, + ) + + def register_frame( + self, + points: np.ndarray, + voxel_map: VoxelHashMap, + initial_guess: np.ndarray, + max_correspondance_distance: float, + kernel: float, + ) -> np.ndarray: + return self._registration._register_point_cloud( + points=kiss_icp_pybind._Vector3dVector(points), + voxel_map=voxel_map._internal_map, + initial_guess=initial_guess, + max_correspondance_distance=max_correspondance_distance, + kernel=kernel, + ) From 598e1a64ff34d06f7a7ca982d1fefe3899e706ff Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 17:50:52 +0100 Subject: [PATCH 10/35] Some renaming just because --- cpp/kiss_icp/core/Registration.cpp | 32 +++++++++++++++--------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 1725a3cf..dd097b73 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -41,8 +41,7 @@ using Vector6d = Eigen::Matrix; namespace { // aliases -using Vector3dVector = std::vector; -using Vector3dVectorTuple = std::tuple; +using Vector3dVectorTuple = std::tuple, std::vector>; inline double square(double x) { return x * x; } @@ -55,13 +54,13 @@ std::tuple BuildLinearSystem( const std::vector &source, const std::vector &target, double kernel) { - struct ResultTuple { - ResultTuple() { + struct LinearSystemRes { + LinearSystemRes() { JTJ.setZero(); JTr.setZero(); } - ResultTuple operator+(const ResultTuple &other) { + LinearSystemRes operator+(const LinearSystemRes &other) { this->JTJ += other.JTJ; this->JTr += other.JTr; return *this; @@ -83,9 +82,9 @@ std::tuple BuildLinearSystem( // Range tbb::blocked_range{0, source.size()}, // Identity - ResultTuple(), + LinearSystemRes(), // 1st Lambda: Parallel computation - [&](const tbb::blocked_range &r, ResultTuple J) -> ResultTuple { + [&](const tbb::blocked_range &r, LinearSystemRes J) -> LinearSystemRes { auto Weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); }; @@ -99,16 +98,16 @@ std::tuple BuildLinearSystem( return J; }, // 2nd Lambda: Parallel reduction of the private Jacboians - [&](ResultTuple a, const ResultTuple &b) -> ResultTuple { return a + b; }); + [&](LinearSystemRes a, const LinearSystemRes &b) -> LinearSystemRes { return a + b; }); - return std::make_tuple(JTJ, JTr); + return {JTJ, JTr}; } -Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, +Vector3dVectorTuple GetCorrespondences(const std::vector &points, const kiss_icp::VoxelHashMap &voxel_map, double max_correspondance_distance) { - struct ResultTuple { - ResultTuple(std::size_t n) { + struct CorrespondenceSet { + CorrespondenceSet(std::size_t n) { source.reserve(n); target.reserve(n); } @@ -121,9 +120,10 @@ Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, // Range tbb::blocked_range{points.cbegin(), points.cend()}, // Identity - ResultTuple(points.size()), + CorrespondenceSet(points.size()), // 1st lambda: Parallel computation - [&](const tbb::blocked_range &r, ResultTuple res) -> ResultTuple { + [&](const tbb::blocked_range &r, + CorrespondenceSet res) -> CorrespondenceSet { auto &[src, tgt] = res; src.reserve(r.size()); tgt.reserve(r.size()); @@ -137,7 +137,7 @@ Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, return res; }, // 2nd lambda: Parallel reduction - [](ResultTuple a, const ResultTuple &b) -> ResultTuple { + [](CorrespondenceSet a, const CorrespondenceSet &b) -> CorrespondenceSet { auto &[src, tgt] = a; const auto &[srcp, tgtp] = b; src.insert(src.end(), // @@ -147,7 +147,7 @@ Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, return a; }); - return std::make_tuple(source, target); + return {source, target}; } } // namespace From 18465fd730d8ce0ec9337689d952783d1d138203 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 17:55:24 +0100 Subject: [PATCH 11/35] Changing names trying to auto-convince myself... --- cpp/kiss_icp/core/Registration.cpp | 10 +++++----- cpp/kiss_icp/core/Registration.hpp | 10 +++++----- cpp/kiss_icp/pipeline/KissICP.cpp | 10 +++++----- python/kiss_icp/kiss_icp.py | 2 +- python/kiss_icp/pybind/kiss_icp_pybind.cpp | 6 +++--- python/kiss_icp/registration.py | 4 ++-- 6 files changed, 21 insertions(+), 21 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index dd097b73..7b6a9b36 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -153,11 +153,11 @@ Vector3dVectorTuple GetCorrespondences(const std::vector &point namespace kiss_icp { -Sophus::SE3d Registration::RegisterFrame(const std::vector &frame, - const VoxelHashMap &voxel_map, - const Sophus::SE3d &initial_guess, - double max_distance, - double kernel) { +Sophus::SE3d Registration::AlignCloudToMap(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_distance, + double kernel) { if (voxel_map.Empty()) return initial_guess; // Equation (9) diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index 7005af45..9e178629 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -36,11 +36,11 @@ struct Registration { // Register a point cloud to the given internal map representation. The config input parameter // contains all the necessary parametrization for the ICP loop - Sophus::SE3d RegisterFrame(const std::vector &frame, - const VoxelHashMap &voxel_map, - const Sophus::SE3d &initial_guess, - double max_distance, - double kernel); + Sophus::SE3d AlignCloudToMap(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_distance, + double kernel); int max_num_iterations_; double estimation_threshold_; diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp index befbece3..6936ec0f 100644 --- a/cpp/kiss_icp/pipeline/KissICP.cpp +++ b/cpp/kiss_icp/pipeline/KissICP.cpp @@ -68,11 +68,11 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector(), "max_num_iterations"_a, "estimation_threshold"_a) .def( - "_register_point_cloud", + "_align_cloud_to_map", [](Registration &self, const std::vector &points, const VoxelHashMap &voxel_map, const Eigen::Matrix4d &T_guess, double max_correspondence_distance, double kernel) { Sophus::SE3d initial_guess(T_guess); return self - .RegisterFrame(points, voxel_map, initial_guess, max_correspondence_distance, - kernel) + .AlignCloudToMap(points, voxel_map, initial_guess, max_correspondence_distance, + kernel) .matrix(); }, "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a, diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py index 86505d0b..056f5685 100644 --- a/python/kiss_icp/registration.py +++ b/python/kiss_icp/registration.py @@ -41,7 +41,7 @@ def __init__(self, max_num_iterations: int, estimation_threshold: float): estimation_threshold=estimation_threshold, ) - def register_frame( + def align_cloud_to_map( self, points: np.ndarray, voxel_map: VoxelHashMap, @@ -49,7 +49,7 @@ def register_frame( max_correspondance_distance: float, kernel: float, ) -> np.ndarray: - return self._registration._register_point_cloud( + return self._registration._align_cloud_to_map( points=kiss_icp_pybind._Vector3dVector(points), voxel_map=voxel_map._internal_map, initial_guess=initial_guess, From 3606d0cd5270507b7da8501a5143deed0a0cbc35 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 1 Mar 2024 17:59:55 +0100 Subject: [PATCH 12/35] Fix c++ build --- cpp/kiss_icp/core/VoxelHashMap.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index e3a6b2e7..9dec20f2 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -71,7 +71,7 @@ Eigen::Vector3d VoxelHashMap::GetClosestNeighboor(const Eigen::Vector3d &point) }); return closest_neighbor; -}; +} std::vector VoxelHashMap::Pointcloud() const { std::vector points; From 9fa9f2db909ee006fdb42eed72af453b4b44156d Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Mon, 4 Mar 2024 10:13:46 +0100 Subject: [PATCH 13/35] rename function --- cpp/kiss_icp/core/Registration.cpp | 2 +- cpp/kiss_icp/core/VoxelHashMap.cpp | 2 +- cpp/kiss_icp/core/VoxelHashMap.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 7b6a9b36..6aae8327 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -128,7 +128,7 @@ Vector3dVectorTuple GetCorrespondences(const std::vector &point src.reserve(r.size()); tgt.reserve(r.size()); for (const auto &point : r) { - Eigen::Vector3d closest_neighboors = voxel_map.GetClosestNeighboor(point); + Eigen::Vector3d closest_neighboors = voxel_map.GetClosestNeighbor(point); if ((closest_neighboors - point).norm() < max_correspondance_distance) { src.emplace_back(point); tgt.emplace_back(closest_neighboors); diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 9dec20f2..1e9ad396 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -32,7 +32,7 @@ namespace kiss_icp { // Function to obtain the KNN of one point -Eigen::Vector3d VoxelHashMap::GetClosestNeighboor(const Eigen::Vector3d &point) const { +Eigen::Vector3d VoxelHashMap::GetClosestNeighbor(const Eigen::Vector3d &point) const { auto kx = static_cast(point[0] / voxel_size_); auto ky = static_cast(point[1] / voxel_size_); auto kz = static_cast(point[2] / voxel_size_); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index d97aa5c5..7d880015 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -62,7 +62,7 @@ struct VoxelHashMap { void AddPoints(const std::vector &points); void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); std::vector Pointcloud() const; - Eigen::Vector3d GetClosestNeighboor(const Eigen::Vector3d &point) const; + Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point) const; double voxel_size_; double max_distance_; From ea7b3134f5bc34789ab6f77439a61364e01dd426 Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Mon, 4 Mar 2024 10:19:36 +0100 Subject: [PATCH 14/35] renaming variables --- cpp/kiss_icp/core/Registration.cpp | 6 +++--- cpp/kiss_icp/core/VoxelHashMap.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 6aae8327..4b05c81a 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -128,10 +128,10 @@ Vector3dVectorTuple GetCorrespondences(const std::vector &point src.reserve(r.size()); tgt.reserve(r.size()); for (const auto &point : r) { - Eigen::Vector3d closest_neighboors = voxel_map.GetClosestNeighbor(point); - if ((closest_neighboors - point).norm() < max_correspondance_distance) { + Eigen::Vector3d closest_neighbors = voxel_map.GetClosestNeighbor(point); + if ((closest_neighbors - point).norm() < max_correspondance_distance) { src.emplace_back(point); - tgt.emplace_back(closest_neighboors); + tgt.emplace_back(closest_neighbors); } } return res; diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 1e9ad396..c4995f14 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -46,15 +46,15 @@ Eigen::Vector3d VoxelHashMap::GetClosestNeighbor(const Eigen::Vector3d &point) c } } - std::vector neighboors; - neighboors.reserve(static_cast(27 * max_points_per_voxel_)); + std::vector neighbors; + neighbors.reserve(static_cast(27 * max_points_per_voxel_)); std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) { auto search = map_.find(voxel); if (search != map_.end()) { const auto &points = search->second.points; if (!points.empty()) { for (const auto &point : points) { - neighboors.emplace_back(point); + neighbors.emplace_back(point); } } } @@ -62,7 +62,7 @@ Eigen::Vector3d VoxelHashMap::GetClosestNeighbor(const Eigen::Vector3d &point) c Eigen::Vector3d closest_neighbor; double closest_distance2 = std::numeric_limits::max(); - std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor) { + std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { double distance = (neighbor - point).squaredNorm(); if (distance < closest_distance2) { closest_neighbor = neighbor; From 5c8fc258d81236dede74639b3aedd4447a3b17da Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Mon, 4 Mar 2024 10:22:39 +0100 Subject: [PATCH 15/35] renaming, should be one neighbor only --- cpp/kiss_icp/core/Registration.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 4b05c81a..2cea8a48 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -128,10 +128,10 @@ Vector3dVectorTuple GetCorrespondences(const std::vector &point src.reserve(r.size()); tgt.reserve(r.size()); for (const auto &point : r) { - Eigen::Vector3d closest_neighbors = voxel_map.GetClosestNeighbor(point); - if ((closest_neighbors - point).norm() < max_correspondance_distance) { + Eigen::Vector3d closest_neighbor = voxel_map.GetClosestNeighbor(point); + if ((closest_neighbor - point).norm() < max_correspondance_distance) { src.emplace_back(point); - tgt.emplace_back(closest_neighbors); + tgt.emplace_back(closest_neighbor); } } return res; From 01b11c570adea964b76b171e641b9caebf260702 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 4 Mar 2024 16:37:58 +0100 Subject: [PATCH 16/35] Tizianified a little bit --- cpp/kiss_icp/core/Registration.cpp | 89 +++++++++++------------------- 1 file changed, 31 insertions(+), 58 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 2cea8a48..3188b834 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -39,10 +39,11 @@ using Matrix3_6d = Eigen::Matrix; using Vector6d = Eigen::Matrix; } // namespace Eigen -namespace { // aliases -using Vector3dVectorTuple = std::tuple, std::vector>; +using CorrespondenceVector = std::vector>; +using LinearSystem = std::pair; +namespace { inline double square(double x) { return x * x; } void TransformPoints(const Sophus::SE3d &T, std::vector &points) { @@ -50,41 +51,23 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points [&](const auto &point) { return T * point; }); } -std::tuple BuildLinearSystem( - const std::vector &source, - const std::vector &target, - double kernel) { - struct LinearSystemRes { - LinearSystemRes() { - JTJ.setZero(); - JTr.setZero(); - } - - LinearSystemRes operator+(const LinearSystemRes &other) { - this->JTJ += other.JTJ; - this->JTr += other.JTr; - return *this; - } - - Eigen::Matrix6d JTJ; - Eigen::Vector6d JTr; - }; - +LinearSystem BuildLinearSystem(const CorrespondenceVector &associations, double kernel) { auto compute_jacobian_and_residual = [&](auto i) { - const Eigen::Vector3d residual = source[i] - target[i]; + const auto &[p_source, p_target] = associations[i]; + const Eigen::Vector3d residual = p_source - p_target; Eigen::Matrix3_6d J_r; J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source[i]); + J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(p_source); return std::make_tuple(J_r, residual); }; const auto &[JTJ, JTr] = tbb::parallel_reduce( // Range - tbb::blocked_range{0, source.size()}, + tbb::blocked_range{0, associations.size()}, // Identity - LinearSystemRes(), + LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()), // 1st Lambda: Parallel computation - [&](const tbb::blocked_range &r, LinearSystemRes J) -> LinearSystemRes { + [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { auto Weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); }; @@ -98,56 +81,46 @@ std::tuple BuildLinearSystem( return J; }, // 2nd Lambda: Parallel reduction of the private Jacboians - [&](LinearSystemRes a, const LinearSystemRes &b) -> LinearSystemRes { return a + b; }); + [&](LinearSystem a, const LinearSystem &b) -> LinearSystem { + a.first += b.first; + a.second += b.second; + return a; + }); return {JTJ, JTr}; } -Vector3dVectorTuple GetCorrespondences(const std::vector &points, - const kiss_icp::VoxelHashMap &voxel_map, - double max_correspondance_distance) { - struct CorrespondenceSet { - CorrespondenceSet(std::size_t n) { - source.reserve(n); - target.reserve(n); - } - std::vector source; - std::vector target; - }; - +CorrespondenceVector GetCorrespondences(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { using points_iterator = std::vector::const_iterator; - const auto [source, target] = tbb::parallel_reduce( + CorrespondenceVector points_associated; + points_associated.reserve(points.size()); + points_associated = tbb::parallel_reduce( // Range tbb::blocked_range{points.cbegin(), points.cend()}, // Identity - CorrespondenceSet(points.size()), + points_associated, // 1st lambda: Parallel computation [&](const tbb::blocked_range &r, - CorrespondenceSet res) -> CorrespondenceSet { - auto &[src, tgt] = res; - src.reserve(r.size()); - tgt.reserve(r.size()); + CorrespondenceVector res) -> CorrespondenceVector { + res.reserve(r.size()); for (const auto &point : r) { Eigen::Vector3d closest_neighbor = voxel_map.GetClosestNeighbor(point); if ((closest_neighbor - point).norm() < max_correspondance_distance) { - src.emplace_back(point); - tgt.emplace_back(closest_neighbor); + res.emplace_back(point, closest_neighbor); } } return res; }, // 2nd lambda: Parallel reduction - [](CorrespondenceSet a, const CorrespondenceSet &b) -> CorrespondenceSet { - auto &[src, tgt] = a; - const auto &[srcp, tgtp] = b; - src.insert(src.end(), // - std::make_move_iterator(srcp.begin()), std::make_move_iterator(srcp.end())); - tgt.insert(tgt.end(), // - std::make_move_iterator(tgtp.begin()), std::make_move_iterator(tgtp.end())); + [](CorrespondenceVector a, const CorrespondenceVector &b) -> CorrespondenceVector { + a.insert(a.end(), // + std::make_move_iterator(b.cbegin()), std::make_move_iterator(b.cend())); return a; }); - return {source, target}; + return points_associated; } } // namespace @@ -168,9 +141,9 @@ Sophus::SE3d Registration::AlignCloudToMap(const std::vector &f Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) - const auto &[src, tgt] = GetCorrespondences(source, voxel_map, max_distance); + const auto associations = GetCorrespondences(source, voxel_map, max_distance); // Equation (11) - const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, kernel); + const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); const Sophus::SE3d estimation = Sophus::SE3d::exp(dx); // Equation (12) From f4a246524b6448f4ae8b9278f3875738be4b45fa Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 4 Mar 2024 21:58:16 +0100 Subject: [PATCH 17/35] Draft on voxelhashmap --- cpp/kiss_icp/core/Registration.cpp | 20 +++++++++++++- cpp/kiss_icp/core/VoxelHashMap.cpp | 42 +++++++++++------------------- cpp/kiss_icp/core/VoxelHashMap.hpp | 4 ++- 3 files changed, 37 insertions(+), 29 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 3188b834..41ec9044 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -51,6 +51,24 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points [&](const auto &point) { return T * point; }); } +// Function to obtain the KNN of one point +Eigen::Vector3d GetClosestNeighborInMap(const Eigen::Vector3d &point, + const kiss_icp::VoxelHashMap &voxel_map) { + const auto &voxels = voxel_map.GetVoxelNeighborhoodAroundPoint(point); + const auto &neighbors = voxel_map.PointCloud(voxels); + Eigen::Vector3d closest_neighbor; + double closest_distance2 = std::numeric_limits::max(); + std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { + double distance = (neighbor - point).squaredNorm(); + if (distance < closest_distance2) { + closest_neighbor = neighbor; + closest_distance2 = distance; + } + }); + + return closest_neighbor; +} + LinearSystem BuildLinearSystem(const CorrespondenceVector &associations, double kernel) { auto compute_jacobian_and_residual = [&](auto i) { const auto &[p_source, p_target] = associations[i]; @@ -106,7 +124,7 @@ CorrespondenceVector GetCorrespondences(const std::vector &poin CorrespondenceVector res) -> CorrespondenceVector { res.reserve(r.size()); for (const auto &point : r) { - Eigen::Vector3d closest_neighbor = voxel_map.GetClosestNeighbor(point); + Eigen::Vector3d closest_neighbor = GetClosestNeighborInMap(point, voxel_map); if ((closest_neighbor - point).norm() < max_correspondance_distance) { res.emplace_back(point, closest_neighbor); } diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index c4995f14..c214a8d0 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -31,46 +31,34 @@ namespace kiss_icp { -// Function to obtain the KNN of one point -Eigen::Vector3d VoxelHashMap::GetClosestNeighbor(const Eigen::Vector3d &point) const { +std::vector VoxelHashMap::GetVoxelNeighborhoodAroundPoint( + const Eigen::Vector3d &point, int adjacent_voxels) const { auto kx = static_cast(point[0] / voxel_size_); auto ky = static_cast(point[1] / voxel_size_); auto kz = static_cast(point[2] / voxel_size_); - std::vector voxels; - voxels.reserve(27); - for (int i = kx - 1; i < kx + 1 + 1; ++i) { - for (int j = ky - 1; j < ky + 1 + 1; ++j) { - for (int k = kz - 1; k < kz + 1 + 1; ++k) { - voxels.emplace_back(i, j, k); + std::vector voxel_neighborhood; + for (int i = kx - adjacent_voxels; i < kx + adjacent_voxels + 1; ++i) { + for (int j = ky - adjacent_voxels; j < ky + adjacent_voxels + 1; ++j) { + for (int k = kz - adjacent_voxels; k < kz + adjacent_voxels + 1; ++k) { + voxel_neighborhood.emplace_back(i, j, k); } } } + return voxel_neighborhood; +} - std::vector neighbors; - neighbors.reserve(static_cast(27 * max_points_per_voxel_)); +std::vector VoxelHashMap::PointCloud(const std::vector &voxels) const { + std::vector points; + points.reserve(voxels.size() * static_cast(max_points_per_voxel_)); std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) { auto search = map_.find(voxel); if (search != map_.end()) { - const auto &points = search->second.points; - if (!points.empty()) { - for (const auto &point : points) { - neighbors.emplace_back(point); - } + for (const auto &point : search->second.points) { + points.emplace_back(point); } } }); - - Eigen::Vector3d closest_neighbor; - double closest_distance2 = std::numeric_limits::max(); - std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { - double distance = (neighbor - point).squaredNorm(); - if (distance < closest_distance2) { - closest_neighbor = neighbor; - closest_distance2 = distance; - } - }); - - return closest_neighbor; + return points; } std::vector VoxelHashMap::Pointcloud() const { diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 7d880015..afafec6b 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -62,7 +62,9 @@ struct VoxelHashMap { void AddPoints(const std::vector &points); void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); std::vector Pointcloud() const; - Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point) const; + std::vector GetPointsInVoxels(const std::vector &voxels) const; + std::vector GetVoxelNeighborhoodAroundPoint(const Eigen::Vector3d &point, + int adjacent_voxels = 1) const; double voxel_size_; double max_distance_; From 0cb69a7b0c146f74ec50e82d7ead3ec68957642e Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 4 Mar 2024 22:09:36 +0100 Subject: [PATCH 18/35] Rename --- cpp/kiss_icp/core/Registration.cpp | 4 ++-- cpp/kiss_icp/core/VoxelHashMap.cpp | 12 ++++++------ cpp/kiss_icp/core/VoxelHashMap.hpp | 6 +++--- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 41ec9044..09b14951 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -54,8 +54,8 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points // Function to obtain the KNN of one point Eigen::Vector3d GetClosestNeighborInMap(const Eigen::Vector3d &point, const kiss_icp::VoxelHashMap &voxel_map) { - const auto &voxels = voxel_map.GetVoxelNeighborhoodAroundPoint(point); - const auto &neighbors = voxel_map.PointCloud(voxels); + const auto &query_voxels = voxel_map.GetAdjacentVoxels(point); + const auto &neighbors = voxel_map.GetPoints(query_voxels); Eigen::Vector3d closest_neighbor; double closest_distance2 = std::numeric_limits::max(); std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index c214a8d0..39a0e703 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -31,8 +31,8 @@ namespace kiss_icp { -std::vector VoxelHashMap::GetVoxelNeighborhoodAroundPoint( - const Eigen::Vector3d &point, int adjacent_voxels) const { +std::vector VoxelHashMap::GetAdjacentVoxels(const Eigen::Vector3d &point, + int adjacent_voxels) const { auto kx = static_cast(point[0] / voxel_size_); auto ky = static_cast(point[1] / voxel_size_); auto kz = static_cast(point[2] / voxel_size_); @@ -47,11 +47,11 @@ std::vector VoxelHashMap::GetVoxelNeighborhoodAroundPoint( return voxel_neighborhood; } -std::vector VoxelHashMap::PointCloud(const std::vector &voxels) const { +std::vector VoxelHashMap::GetPoints(const std::vector &query_voxels) const { std::vector points; - points.reserve(voxels.size() * static_cast(max_points_per_voxel_)); - std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) { - auto search = map_.find(voxel); + points.reserve(query_voxels.size() * static_cast(max_points_per_voxel_)); + 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); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index afafec6b..65a1f94f 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -62,9 +62,9 @@ struct VoxelHashMap { void AddPoints(const std::vector &points); void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); std::vector Pointcloud() const; - std::vector GetPointsInVoxels(const std::vector &voxels) const; - std::vector GetVoxelNeighborhoodAroundPoint(const Eigen::Vector3d &point, - int adjacent_voxels = 1) const; + std::vector GetPoints(const std::vector &query_voxels) const; + std::vector GetAdjacentVoxels(const Eigen::Vector3d &point, + int adjacent_voxels = 1) const; double voxel_size_; double max_distance_; From 39e4cbf953173cc958cd52b8cc096aa5cbbbe0a7 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 4 Mar 2024 22:44:24 +0100 Subject: [PATCH 19/35] Rename Correspondences -> Associations Otherwise too much mix a match on my opinion --- cpp/kiss_icp/core/Registration.cpp | 32 +++++++++++++++--------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 09b14951..71c42205 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -40,7 +40,7 @@ using Vector6d = Eigen::Matrix; } // namespace Eigen // aliases -using CorrespondenceVector = std::vector>; +using Associations = std::vector>; using LinearSystem = std::pair; namespace { @@ -69,7 +69,7 @@ Eigen::Vector3d GetClosestNeighborInMap(const Eigen::Vector3d &point, return closest_neighbor; } -LinearSystem BuildLinearSystem(const CorrespondenceVector &associations, double kernel) { +LinearSystem BuildLinearSystem(const Associations &associations, double kernel) { auto compute_jacobian_and_residual = [&](auto i) { const auto &[p_source, p_target] = associations[i]; const Eigen::Vector3d residual = p_source - p_target; @@ -108,20 +108,19 @@ LinearSystem BuildLinearSystem(const CorrespondenceVector &associations, double return {JTJ, JTr}; } -CorrespondenceVector GetCorrespondences(const std::vector &points, - const kiss_icp::VoxelHashMap &voxel_map, - double max_correspondance_distance) { +Associations GetDataAssociations(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { using points_iterator = std::vector::const_iterator; - CorrespondenceVector points_associated; - points_associated.reserve(points.size()); - points_associated = tbb::parallel_reduce( + Associations associations; + associations.reserve(points.size()); + associations = tbb::parallel_reduce( // Range tbb::blocked_range{points.cbegin(), points.cend()}, // Identity - points_associated, + associations, // 1st lambda: Parallel computation - [&](const tbb::blocked_range &r, - CorrespondenceVector res) -> CorrespondenceVector { + [&](const tbb::blocked_range &r, Associations res) -> Associations { res.reserve(r.size()); for (const auto &point : r) { Eigen::Vector3d closest_neighbor = GetClosestNeighborInMap(point, voxel_map); @@ -132,13 +131,14 @@ CorrespondenceVector GetCorrespondences(const std::vector &poin return res; }, // 2nd lambda: Parallel reduction - [](CorrespondenceVector a, const CorrespondenceVector &b) -> CorrespondenceVector { - a.insert(a.end(), // - std::make_move_iterator(b.cbegin()), std::make_move_iterator(b.cend())); + [](Associations a, const Associations &b) -> Associations { + a.insert(a.end(), // + std::make_move_iterator(b.cbegin()), // + std::make_move_iterator(b.cend())); return a; }); - return points_associated; + return associations; } } // namespace @@ -159,7 +159,7 @@ Sophus::SE3d Registration::AlignCloudToMap(const std::vector &f Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) - const auto associations = GetCorrespondences(source, voxel_map, max_distance); + const auto associations = GetDataAssociations(source, voxel_map, max_distance); // Equation (11) const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); From b434e045f4e9bdd5476332d520691e1cb9fa845c Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 4 Mar 2024 22:48:13 +0100 Subject: [PATCH 20/35] Move stuff around only --- cpp/kiss_icp/core/Registration.cpp | 67 +++++++++++++++--------------- 1 file changed, 33 insertions(+), 34 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 71c42205..a4ef43d9 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -51,7 +51,6 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points [&](const auto &point) { return T * point; }); } -// Function to obtain the KNN of one point Eigen::Vector3d GetClosestNeighborInMap(const Eigen::Vector3d &point, const kiss_icp::VoxelHashMap &voxel_map) { const auto &query_voxels = voxel_map.GetAdjacentVoxels(point); @@ -69,6 +68,39 @@ Eigen::Vector3d GetClosestNeighborInMap(const Eigen::Vector3d &point, return closest_neighbor; } +Associations GetDataAssociations(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { + using points_iterator = std::vector::const_iterator; + Associations associations; + associations.reserve(points.size()); + associations = tbb::parallel_reduce( + // Range + tbb::blocked_range{points.cbegin(), points.cend()}, + // Identity + associations, + // 1st lambda: Parallel computation + [&](const tbb::blocked_range &r, Associations res) -> Associations { + res.reserve(r.size()); + for (const auto &point : r) { + Eigen::Vector3d closest_neighbor = GetClosestNeighborInMap(point, voxel_map); + if ((closest_neighbor - point).norm() < max_correspondance_distance) { + res.emplace_back(point, closest_neighbor); + } + } + return res; + }, + // 2nd lambda: Parallel reduction + [](Associations a, const Associations &b) -> Associations { + a.insert(a.end(), // + std::make_move_iterator(b.cbegin()), // + std::make_move_iterator(b.cend())); + return a; + }); + + return associations; +} + LinearSystem BuildLinearSystem(const Associations &associations, double kernel) { auto compute_jacobian_and_residual = [&](auto i) { const auto &[p_source, p_target] = associations[i]; @@ -107,39 +139,6 @@ LinearSystem BuildLinearSystem(const Associations &associations, double kernel) return {JTJ, JTr}; } - -Associations GetDataAssociations(const std::vector &points, - const kiss_icp::VoxelHashMap &voxel_map, - double max_correspondance_distance) { - using points_iterator = std::vector::const_iterator; - Associations associations; - associations.reserve(points.size()); - associations = tbb::parallel_reduce( - // Range - tbb::blocked_range{points.cbegin(), points.cend()}, - // Identity - associations, - // 1st lambda: Parallel computation - [&](const tbb::blocked_range &r, Associations res) -> Associations { - res.reserve(r.size()); - for (const auto &point : r) { - Eigen::Vector3d closest_neighbor = GetClosestNeighborInMap(point, voxel_map); - if ((closest_neighbor - point).norm() < max_correspondance_distance) { - res.emplace_back(point, closest_neighbor); - } - } - return res; - }, - // 2nd lambda: Parallel reduction - [](Associations a, const Associations &b) -> Associations { - a.insert(a.end(), // - std::make_move_iterator(b.cbegin()), // - std::make_move_iterator(b.cend())); - return a; - }); - - return associations; -} } // namespace namespace kiss_icp { From 70a64575108be83c133fc18a2bdeface7bbb7abb Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 4 Mar 2024 22:53:08 +0100 Subject: [PATCH 21/35] Remove redunant name --- cpp/kiss_icp/core/Registration.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index a4ef43d9..0fe36d7a 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -51,8 +51,8 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points [&](const auto &point) { return T * point; }); } -Eigen::Vector3d GetClosestNeighborInMap(const Eigen::Vector3d &point, - const kiss_icp::VoxelHashMap &voxel_map) { +Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point, + const kiss_icp::VoxelHashMap &voxel_map) { const auto &query_voxels = voxel_map.GetAdjacentVoxels(point); const auto &neighbors = voxel_map.GetPoints(query_voxels); Eigen::Vector3d closest_neighbor; @@ -83,7 +83,7 @@ Associations GetDataAssociations(const std::vector &points, [&](const tbb::blocked_range &r, Associations res) -> Associations { res.reserve(r.size()); for (const auto &point : r) { - Eigen::Vector3d closest_neighbor = GetClosestNeighborInMap(point, voxel_map); + Eigen::Vector3d closest_neighbor = GetClosestNeighbor(point, voxel_map); if ((closest_neighbor - point).norm() < max_correspondance_distance) { res.emplace_back(point, closest_neighbor); } From 876fedca06f337ddc6b1faee11800b339cdc857b Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 4 Mar 2024 22:54:17 +0100 Subject: [PATCH 22/35] They are not there, we need to find them! --- cpp/kiss_icp/core/Registration.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 0fe36d7a..37d1fcc3 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -68,9 +68,9 @@ Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point, return closest_neighbor; } -Associations GetDataAssociations(const std::vector &points, - const kiss_icp::VoxelHashMap &voxel_map, - double max_correspondance_distance) { +Associations FindDataAssociations(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { using points_iterator = std::vector::const_iterator; Associations associations; associations.reserve(points.size()); @@ -158,7 +158,7 @@ Sophus::SE3d Registration::AlignCloudToMap(const std::vector &f Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) - const auto associations = GetDataAssociations(source, voxel_map, max_distance); + const auto associations = FindDataAssociations(source, voxel_map, max_distance); // Equation (11) const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); From c338afb09f707d4681611572d51aad8f728b8ce3 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 4 Mar 2024 22:55:57 +0100 Subject: [PATCH 23/35] Shrink --- cpp/kiss_icp/core/Registration.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 37d1fcc3..02180088 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -38,8 +38,6 @@ using Matrix6d = Eigen::Matrix; using Matrix3_6d = Eigen::Matrix; using Vector6d = Eigen::Matrix; } // namespace Eigen - -// aliases using Associations = std::vector>; using LinearSystem = std::pair; From 2b7937002c19f720fbfdfc6d391ab33e226a0b05 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 4 Mar 2024 23:39:05 +0100 Subject: [PATCH 24/35] Tiziano shows to guys -> with for_each --- cpp/kiss_icp/core/Registration.cpp | 41 ++++++++++++++++-------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 3188b834..e9170a08 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -52,40 +53,42 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points } LinearSystem BuildLinearSystem(const CorrespondenceVector &associations, double kernel) { - auto compute_jacobian_and_residual = [&](auto i) { - const auto &[p_source, p_target] = associations[i]; - const Eigen::Vector3d residual = p_source - p_target; + auto compute_jacobian_and_residual = [](auto source_and_target) { + const auto &[source, target] = source_and_target; + const Eigen::Vector3d residual = source - target; Eigen::Matrix3_6d J_r; J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(p_source); + J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source); return std::make_tuple(J_r, residual); }; + auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) { + a.first += b.first; + a.second += b.second; + return a; + }; + + auto GM_weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); }; + + using associations_iterator = CorrespondenceVector::const_iterator; const auto &[JTJ, JTr] = tbb::parallel_reduce( // Range - tbb::blocked_range{0, associations.size()}, + tbb::blocked_range{associations.cbegin(), associations.cend()}, // Identity LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()), // 1st Lambda: Parallel computation - [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { - auto Weight = [&](double residual2) { - return square(kernel) / square(kernel + residual2); - }; - auto &[JTJ_private, JTr_private] = J; - for (auto i = r.begin(); i < r.end(); ++i) { - const auto &[J_r, residual] = compute_jacobian_and_residual(i); - const double w = Weight(residual.squaredNorm()); + [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { + std::for_each(r.begin(), r.end(), [&](const auto &association) { + auto &[JTJ_private, JTr_private] = J; + const auto &[J_r, residual] = compute_jacobian_and_residual(association); + const double w = GM_weight(residual.squaredNorm()); JTJ_private.noalias() += J_r.transpose() * w * J_r; JTr_private.noalias() += J_r.transpose() * w * residual; - } + }); return J; }, // 2nd Lambda: Parallel reduction of the private Jacboians - [&](LinearSystem a, const LinearSystem &b) -> LinearSystem { - a.first += b.first; - a.second += b.second; - return a; - }); + sum_linear_systems); return {JTJ, JTr}; } From d83417056396efb1096d68fd28a5eccd927237f5 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Mon, 4 Mar 2024 23:41:58 +0100 Subject: [PATCH 25/35] Tiziano shows to guys -> with transform_reduce....sexy --- cpp/kiss_icp/core/Registration.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index e9170a08..0713d3e7 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -78,14 +78,12 @@ LinearSystem BuildLinearSystem(const CorrespondenceVector &associations, double LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()), // 1st Lambda: Parallel computation [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { - std::for_each(r.begin(), r.end(), [&](const auto &association) { - auto &[JTJ_private, JTr_private] = J; - const auto &[J_r, residual] = compute_jacobian_and_residual(association); - const double w = GM_weight(residual.squaredNorm()); - JTJ_private.noalias() += J_r.transpose() * w * J_r; - JTr_private.noalias() += J_r.transpose() * w * residual; - }); - return J; + return std::transform_reduce( + r.begin(), r.end(), J, sum_linear_systems, [&](const auto &association) { + const auto &[J_r, residual] = compute_jacobian_and_residual(association); + const double w = GM_weight(residual.squaredNorm()); + return LinearSystem(J_r.transpose() * w * J_r, J_r.transpose() * w * residual); + }); }, // 2nd Lambda: Parallel reduction of the private Jacboians sum_linear_systems); From f88310f3c78be2bc0edf456c6bf0fb86be07f1d8 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 5 Mar 2024 10:19:12 +0100 Subject: [PATCH 26/35] Consistent naming --- cpp/kiss_icp/core/Registration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 4f4ac713..0a0ba132 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -100,8 +100,8 @@ Associations FindDataAssociations(const std::vector &points, } LinearSystem BuildLinearSystem(const Associations &associations, double kernel) { - auto compute_jacobian_and_residual = [](auto source_and_target) { - const auto &[source, target] = source_and_target; + auto compute_jacobian_and_residual = [](auto association) { + const auto &[source, target] = association; const Eigen::Vector3d residual = source - target; Eigen::Matrix3_6d J_r; J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); From 8fb80db75bf710710a08b6c5a9711d00f63751ab Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 5 Mar 2024 10:24:59 +0100 Subject: [PATCH 27/35] Bring comments for readbilty --- cpp/kiss_icp/core/Registration.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 0a0ba132..ba01155f 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -129,7 +129,8 @@ LinearSystem BuildLinearSystem(const Associations &associations, double kernel) r.begin(), r.end(), J, sum_linear_systems, [&](const auto &association) { const auto &[J_r, residual] = compute_jacobian_and_residual(association); const double w = GM_weight(residual.squaredNorm()); - return LinearSystem(J_r.transpose() * w * J_r, J_r.transpose() * w * residual); + return LinearSystem(J_r.transpose() * w * J_r, // JTJ + J_r.transpose() * w * residual); // JTr }); }, // 2nd Lambda: Parallel reduction of the private Jacboians From f5c152425252f449ea72a1fb7428f3b6dc2dc7ae Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 5 Mar 2024 10:37:56 +0100 Subject: [PATCH 28/35] rename variable --- cpp/kiss_icp/core/Registration.cpp | 5 +++-- cpp/kiss_icp/core/Registration.hpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index ba01155f..db6f093e 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -145,7 +145,7 @@ namespace kiss_icp { Sophus::SE3d Registration::AlignCloudToMap(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, - double max_distance, + double max_correspondence_distance, double kernel) { if (voxel_map.Empty()) return initial_guess; @@ -157,7 +157,8 @@ Sophus::SE3d Registration::AlignCloudToMap(const std::vector &f Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) - const auto associations = FindDataAssociations(source, voxel_map, max_distance); + const auto associations = + FindDataAssociations(source, voxel_map, max_correspondence_distance); // Equation (11) const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index 9e178629..1bfc7b35 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -39,7 +39,7 @@ struct Registration { Sophus::SE3d AlignCloudToMap(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, - double max_distance, + double max_correspondence_distance, double kernel); int max_num_iterations_; From 318cd863ad93363fc3aae2db5e8c2a213d0cbadf Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 5 Mar 2024 10:38:06 +0100 Subject: [PATCH 29/35] Sacrifice name for one-liner --- cpp/kiss_icp/core/Registration.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index db6f093e..561e9144 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -66,9 +66,9 @@ Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point, return closest_neighbor; } -Associations FindDataAssociations(const std::vector &points, - const kiss_icp::VoxelHashMap &voxel_map, - double max_correspondance_distance) { +Associations FindAssociations(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { using points_iterator = std::vector::const_iterator; Associations associations; associations.reserve(points.size()); @@ -157,8 +157,7 @@ Sophus::SE3d Registration::AlignCloudToMap(const std::vector &f Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) - const auto associations = - FindDataAssociations(source, voxel_map, max_correspondence_distance); + const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance); // Equation (11) const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); From 0c823ae5c65eebf5b177c7a5689af65c9775d1c7 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 5 Mar 2024 10:39:37 +0100 Subject: [PATCH 30/35] AlignCloudToMap -> AlignPointsToMap --- cpp/kiss_icp/core/Registration.cpp | 10 +++++----- cpp/kiss_icp/core/Registration.hpp | 10 +++++----- cpp/kiss_icp/pipeline/KissICP.cpp | 10 +++++----- python/kiss_icp/kiss_icp.py | 2 +- python/kiss_icp/pybind/kiss_icp_pybind.cpp | 6 +++--- python/kiss_icp/registration.py | 4 ++-- 6 files changed, 21 insertions(+), 21 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 561e9144..b2b54d4e 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -142,11 +142,11 @@ LinearSystem BuildLinearSystem(const Associations &associations, double kernel) namespace kiss_icp { -Sophus::SE3d Registration::AlignCloudToMap(const std::vector &frame, - const VoxelHashMap &voxel_map, - const Sophus::SE3d &initial_guess, - double max_correspondence_distance, - double kernel) { +Sophus::SE3d Registration::AlignPointsToMap(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_correspondence_distance, + double kernel) { if (voxel_map.Empty()) return initial_guess; // Equation (9) diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index 1bfc7b35..23e737e0 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -36,11 +36,11 @@ struct Registration { // Register a point cloud to the given internal map representation. The config input parameter // contains all the necessary parametrization for the ICP loop - Sophus::SE3d AlignCloudToMap(const std::vector &frame, - const VoxelHashMap &voxel_map, - const Sophus::SE3d &initial_guess, - double max_correspondence_distance, - double kernel); + Sophus::SE3d AlignPointsToMap(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_correspondence_distance, + double kernel); int max_num_iterations_; double estimation_threshold_; diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp index 6936ec0f..e1aaf1d8 100644 --- a/cpp/kiss_icp/pipeline/KissICP.cpp +++ b/cpp/kiss_icp/pipeline/KissICP.cpp @@ -68,11 +68,11 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector(), "max_num_iterations"_a, "estimation_threshold"_a) .def( - "_align_cloud_to_map", + "_align_points_to_map", [](Registration &self, const std::vector &points, const VoxelHashMap &voxel_map, const Eigen::Matrix4d &T_guess, double max_correspondence_distance, double kernel) { Sophus::SE3d initial_guess(T_guess); return self - .AlignCloudToMap(points, voxel_map, initial_guess, max_correspondence_distance, - kernel) + .AlignPointsToMap(points, voxel_map, initial_guess, max_correspondence_distance, + kernel) .matrix(); }, "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a, diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py index 056f5685..b3260702 100644 --- a/python/kiss_icp/registration.py +++ b/python/kiss_icp/registration.py @@ -41,7 +41,7 @@ def __init__(self, max_num_iterations: int, estimation_threshold: float): estimation_threshold=estimation_threshold, ) - def align_cloud_to_map( + def align_points_to_map( self, points: np.ndarray, voxel_map: VoxelHashMap, @@ -49,7 +49,7 @@ def align_cloud_to_map( max_correspondance_distance: float, kernel: float, ) -> np.ndarray: - return self._registration._align_cloud_to_map( + return self._registration._align_points_to_map( points=kiss_icp_pybind._Vector3dVector(points), voxel_map=voxel_map._internal_map, initial_guess=initial_guess, From 7a45c9df675b66cf40583090a46b13e306c551d8 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 5 Mar 2024 10:44:49 +0100 Subject: [PATCH 31/35] Make rename like a book on ProbRob --- cpp/kiss_icp/core/Registration.cpp | 46 ++++++++++++++++-------------- 1 file changed, 24 insertions(+), 22 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index b2b54d4e..574be387 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -39,7 +39,7 @@ using Matrix6d = Eigen::Matrix; using Matrix3_6d = Eigen::Matrix; using Vector6d = Eigen::Matrix; } // namespace Eigen -using Associations = std::vector>; +using CorrespondenceVector = std::vector>; using LinearSystem = std::pair; namespace { @@ -66,42 +66,43 @@ Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point, return closest_neighbor; } -Associations FindAssociations(const std::vector &points, - const kiss_icp::VoxelHashMap &voxel_map, - double max_correspondance_distance) { +CorrespondenceVector DataAssociation(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { using points_iterator = std::vector::const_iterator; - Associations associations; - associations.reserve(points.size()); - associations = tbb::parallel_reduce( + CorrespondenceVector correspondences; + correspondences.reserve(points.size()); + correspondences = tbb::parallel_reduce( // Range tbb::blocked_range{points.cbegin(), points.cend()}, // Identity - associations, + correspondences, // 1st lambda: Parallel computation - [&](const tbb::blocked_range &r, Associations res) -> Associations { + [&](const tbb::blocked_range &r, + CorrespondenceVector res) -> CorrespondenceVector { res.reserve(r.size()); - for (const auto &point : r) { + std::for_each(r.begin(), r.end(), [&](const Eigen::Vector3d &point) { Eigen::Vector3d closest_neighbor = GetClosestNeighbor(point, voxel_map); if ((closest_neighbor - point).norm() < max_correspondance_distance) { res.emplace_back(point, closest_neighbor); } - } + }); return res; }, // 2nd lambda: Parallel reduction - [](Associations a, const Associations &b) -> Associations { + [](CorrespondenceVector a, const CorrespondenceVector &b) -> CorrespondenceVector { a.insert(a.end(), // std::make_move_iterator(b.cbegin()), // std::make_move_iterator(b.cend())); return a; }); - return associations; + return correspondences; } -LinearSystem BuildLinearSystem(const Associations &associations, double kernel) { - auto compute_jacobian_and_residual = [](auto association) { - const auto &[source, target] = association; +LinearSystem BuildLinearSystem(const CorrespondenceVector &correspondences, double kernel) { + auto compute_jacobian_and_residual = [](const auto &source_and_target) { + const auto &[source, target] = source_and_target; const Eigen::Vector3d residual = source - target; Eigen::Matrix3_6d J_r; J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); @@ -117,14 +118,15 @@ LinearSystem BuildLinearSystem(const Associations &associations, double kernel) auto GM_weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); }; - using associations_iterator = Associations::const_iterator; + using correspondence_iterator = CorrespondenceVector::const_iterator; const auto &[JTJ, JTr] = tbb::parallel_reduce( // Range - tbb::blocked_range{associations.cbegin(), associations.cend()}, + tbb::blocked_range{correspondences.cbegin(), + correspondences.cend()}, // Identity LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()), // 1st Lambda: Parallel computation - [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { + [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { return std::transform_reduce( r.begin(), r.end(), J, sum_linear_systems, [&](const auto &association) { const auto &[J_r, residual] = compute_jacobian_and_residual(association); @@ -145,7 +147,7 @@ namespace kiss_icp { Sophus::SE3d Registration::AlignPointsToMap(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, - double max_correspondence_distance, + double max_distance, double kernel) { if (voxel_map.Empty()) return initial_guess; @@ -157,9 +159,9 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector & Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) - const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance); + const auto correspondences = DataAssociation(source, voxel_map, max_distance); // Equation (11) - const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel); + const auto &[JTJ, JTr] = BuildLinearSystem(correspondences, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); const Sophus::SE3d estimation = Sophus::SE3d::exp(dx); // Equation (12) From 26dd7b0bf596a083e8311257b45a0d9b55f72fa8 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 5 Mar 2024 11:13:04 +0100 Subject: [PATCH 32/35] Revert "Make rename like a book on ProbRob" This reverts commit 7a45c9df675b66cf40583090a46b13e306c551d8. --- cpp/kiss_icp/core/Registration.cpp | 46 ++++++++++++++---------------- 1 file changed, 22 insertions(+), 24 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 574be387..b2b54d4e 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -39,7 +39,7 @@ using Matrix6d = Eigen::Matrix; using Matrix3_6d = Eigen::Matrix; using Vector6d = Eigen::Matrix; } // namespace Eigen -using CorrespondenceVector = std::vector>; +using Associations = std::vector>; using LinearSystem = std::pair; namespace { @@ -66,43 +66,42 @@ Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point, return closest_neighbor; } -CorrespondenceVector DataAssociation(const std::vector &points, - const kiss_icp::VoxelHashMap &voxel_map, - double max_correspondance_distance) { +Associations FindAssociations(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { using points_iterator = std::vector::const_iterator; - CorrespondenceVector correspondences; - correspondences.reserve(points.size()); - correspondences = tbb::parallel_reduce( + Associations associations; + associations.reserve(points.size()); + associations = tbb::parallel_reduce( // Range tbb::blocked_range{points.cbegin(), points.cend()}, // Identity - correspondences, + associations, // 1st lambda: Parallel computation - [&](const tbb::blocked_range &r, - CorrespondenceVector res) -> CorrespondenceVector { + [&](const tbb::blocked_range &r, Associations res) -> Associations { res.reserve(r.size()); - std::for_each(r.begin(), r.end(), [&](const Eigen::Vector3d &point) { + for (const auto &point : r) { Eigen::Vector3d closest_neighbor = GetClosestNeighbor(point, voxel_map); if ((closest_neighbor - point).norm() < max_correspondance_distance) { res.emplace_back(point, closest_neighbor); } - }); + } return res; }, // 2nd lambda: Parallel reduction - [](CorrespondenceVector a, const CorrespondenceVector &b) -> CorrespondenceVector { + [](Associations a, const Associations &b) -> Associations { a.insert(a.end(), // std::make_move_iterator(b.cbegin()), // std::make_move_iterator(b.cend())); return a; }); - return correspondences; + return associations; } -LinearSystem BuildLinearSystem(const CorrespondenceVector &correspondences, double kernel) { - auto compute_jacobian_and_residual = [](const auto &source_and_target) { - const auto &[source, target] = source_and_target; +LinearSystem BuildLinearSystem(const Associations &associations, double kernel) { + auto compute_jacobian_and_residual = [](auto association) { + const auto &[source, target] = association; const Eigen::Vector3d residual = source - target; Eigen::Matrix3_6d J_r; J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); @@ -118,15 +117,14 @@ LinearSystem BuildLinearSystem(const CorrespondenceVector &correspondences, doub auto GM_weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); }; - using correspondence_iterator = CorrespondenceVector::const_iterator; + using associations_iterator = Associations::const_iterator; const auto &[JTJ, JTr] = tbb::parallel_reduce( // Range - tbb::blocked_range{correspondences.cbegin(), - correspondences.cend()}, + tbb::blocked_range{associations.cbegin(), associations.cend()}, // Identity LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()), // 1st Lambda: Parallel computation - [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { + [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { return std::transform_reduce( r.begin(), r.end(), J, sum_linear_systems, [&](const auto &association) { const auto &[J_r, residual] = compute_jacobian_and_residual(association); @@ -147,7 +145,7 @@ namespace kiss_icp { Sophus::SE3d Registration::AlignPointsToMap(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, - double max_distance, + double max_correspondence_distance, double kernel) { if (voxel_map.Empty()) return initial_guess; @@ -159,9 +157,9 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector & Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) - const auto correspondences = DataAssociation(source, voxel_map, max_distance); + const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance); // Equation (11) - const auto &[JTJ, JTr] = BuildLinearSystem(correspondences, kernel); + const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); const Sophus::SE3d estimation = Sophus::SE3d::exp(dx); // Equation (12) From d5a38f454e01f19289a149a96e7f053cd04785b7 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 5 Mar 2024 11:28:42 +0100 Subject: [PATCH 33/35] estimation_threshold -> convergence_criterion Requested by benedikt --- config/advanced.yaml | 4 ++++ cpp/kiss_icp/core/Registration.cpp | 2 +- cpp/kiss_icp/core/Registration.hpp | 6 +++--- cpp/kiss_icp/pipeline/KissICP.hpp | 4 ++-- python/kiss_icp/config/config.py | 2 +- python/kiss_icp/pybind/kiss_icp_pybind.cpp | 2 +- python/kiss_icp/registration.py | 6 +++--- 7 files changed, 15 insertions(+), 11 deletions(-) diff --git a/config/advanced.yaml b/config/advanced.yaml index a9124ae2..5c28b8f6 100644 --- a/config/advanced.yaml +++ b/config/advanced.yaml @@ -16,3 +16,7 @@ adaptive_threshold: fixed_threshold: 0.3 # <- optional, disables adaptive threshold # initial_threshold: 2.0 # min_motion_th: 0.1 + +registration: + max_num_iterations: 500 # <- optional + estimation_threshold: 0.0001 # <- optional diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index b2b54d4e..90a6473e 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -167,7 +167,7 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector & // Update iterations T_icp = estimation * T_icp; // Termination criteria - if (dx.norm() < estimation_threshold_) break; + if (dx.norm() < convergence_criterion_) break; } // Spit the final transformation return T_icp * initial_guess; diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index 23e737e0..f5a27d7a 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -31,8 +31,8 @@ namespace kiss_icp { struct Registration { - explicit Registration(int max_num_iteration, double estimation_threshold) - : max_num_iterations_(max_num_iteration), estimation_threshold_(estimation_threshold) {} + explicit Registration(int max_num_iteration, double convergence_criterion) + : max_num_iterations_(max_num_iteration), convergence_criterion_(convergence_criterion) {} // Register a point cloud to the given internal map representation. The config input parameter // contains all the necessary parametrization for the ICP loop @@ -43,6 +43,6 @@ struct Registration { double kernel); int max_num_iterations_; - double estimation_threshold_; + double convergence_criterion_; }; } // namespace kiss_icp diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp index 62d01ff5..8605b44a 100644 --- a/cpp/kiss_icp/pipeline/KissICP.hpp +++ b/cpp/kiss_icp/pipeline/KissICP.hpp @@ -45,7 +45,7 @@ struct KISSConfig { // registration params int max_num_iterations = 500; - double estimation_threshold = 0.0001; + double convergence_criterion = 0.0001; // Motion compensation bool deskew = false; @@ -59,7 +59,7 @@ class KissICP { public: explicit KissICP(const KISSConfig &config) : config_(config), - registration_(config.max_num_iterations, config.estimation_threshold), + registration_(config.max_num_iterations, config.convergence_criterion), local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel), adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {} diff --git a/python/kiss_icp/config/config.py b/python/kiss_icp/config/config.py index e402e7de..92dc8d98 100644 --- a/python/kiss_icp/config/config.py +++ b/python/kiss_icp/config/config.py @@ -38,7 +38,7 @@ class MappingConfig(BaseModel): class RegistrationConfig(BaseModel): max_num_iterations: Optional[int] = 500 - estimation_threshold: Optional[float] = 0.0001 + convergence_criterion: Optional[float] = 0.0001 class AdaptiveThresholdConfig(BaseModel): diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index 5a04bc91..c2ff3eb2 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -75,7 +75,7 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { // Point Cloud registration py::class_ internal_registration(m, "_Registration", "Don't use this"); internal_registration - .def(py::init(), "max_num_iterations"_a, "estimation_threshold"_a) + .def(py::init(), "max_num_iterations"_a, "convergence_criterion"_a) .def( "_align_points_to_map", [](Registration &self, const std::vector &points, diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py index b3260702..48a05d32 100644 --- a/python/kiss_icp/registration.py +++ b/python/kiss_icp/registration.py @@ -30,15 +30,15 @@ def get_registration(config: KISSConfig): return Registration( max_num_iterations=config.registration.max_num_iterations, - estimation_threshold=config.registration.estimation_threshold, + convergence_criterion=config.registration.convergence_criterion, ) class Registration: - def __init__(self, max_num_iterations: int, estimation_threshold: float): + def __init__(self, max_num_iterations: int, convergence_criterion: float): self._registration = kiss_icp_pybind._Registration( max_num_iterations=max_num_iterations, - estimation_threshold=estimation_threshold, + convergence_criterion=convergence_criterion, ) def align_points_to_map( From f2e5d60f9416b6f3af766da8b7073978689ec616 Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Tue, 5 Mar 2024 12:59:15 +0100 Subject: [PATCH 34/35] Rename threshold also here --- config/advanced.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/advanced.yaml b/config/advanced.yaml index 5c28b8f6..a1650c10 100644 --- a/config/advanced.yaml +++ b/config/advanced.yaml @@ -19,4 +19,4 @@ adaptive_threshold: registration: max_num_iterations: 500 # <- optional - estimation_threshold: 0.0001 # <- optional + convergence_criterion: 0.0001 # <- optional From bd063e9344ca58795de9eddb9462a3d4ac5a2e59 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 5 Mar 2024 13:43:54 +0100 Subject: [PATCH 35/35] Remove comment --- cpp/kiss_icp/core/Registration.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index f5a27d7a..25d3abf5 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -34,8 +34,6 @@ struct Registration { explicit Registration(int max_num_iteration, double convergence_criterion) : max_num_iterations_(max_num_iteration), convergence_criterion_(convergence_criterion) {} - // Register a point cloud to the given internal map representation. The config input parameter - // contains all the necessary parametrization for the ICP loop Sophus::SE3d AlignPointsToMap(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess,