From db1e16c6e9000d308eb4aae575bc2d51f707f985 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 9 Jan 2025 17:41:10 +0100 Subject: [PATCH] Revert "Merge branch 'main' into tiziano/bonxai_minimal" This reverts commit da104ce5aa51cc7273c3eda4ff796c3d2d15c0bb, reversing changes made to 9b842863040d239f4480ef8333af35dcf040e559. --- Makefile | 5 ---- .../3rdparty/kiss_icp/kiss-icp.cmake | 2 +- cpp/kinematic_icp/CMakeLists.txt | 2 +- cpp/kinematic_icp/pipeline/KinematicICP.cpp | 21 ++++++++------ cpp/kinematic_icp/pipeline/KinematicICP.hpp | 7 ++--- .../registration/Registration.cpp | 29 +++++++++++++------ ros/CMakeLists.txt | 2 +- ros/package.xml | 2 +- 8 files changed, 39 insertions(+), 31 deletions(-) delete mode 100644 Makefile diff --git a/Makefile b/Makefile deleted file mode 100644 index 9aca83b..0000000 --- a/Makefile +++ /dev/null @@ -1,5 +0,0 @@ -.PHONY: cpp - -cpp: - @cmake -Bbuild cpp/kinematic_icp/ - @cmake --build build -j$(nproc --all) diff --git a/cpp/kinematic_icp/3rdparty/kiss_icp/kiss-icp.cmake b/cpp/kinematic_icp/3rdparty/kiss_icp/kiss-icp.cmake index c24a940..11bfe59 100644 --- a/cpp/kinematic_icp/3rdparty/kiss_icp/kiss-icp.cmake +++ b/cpp/kinematic_icp/3rdparty/kiss_icp/kiss-icp.cmake @@ -26,6 +26,6 @@ if(CMAKE_VERSION VERSION_GREATER 3.24) endif() include(FetchContent) -FetchContent_Declare(kiss_icp URL https://github.com/PRBonn/kiss-icp/archive/refs/tags/v1.2.0.tar.gz SOURCE_SUBDIR +FetchContent_Declare(kiss_icp URL https://github.com/PRBonn/kiss-icp/archive/refs/tags/v1.1.0.tar.gz SOURCE_SUBDIR cpp/kiss_icp) FetchContent_MakeAvailable(kiss_icp) diff --git a/cpp/kinematic_icp/CMakeLists.txt b/cpp/kinematic_icp/CMakeLists.txt index ed07e45..1d58cc6 100644 --- a/cpp/kinematic_icp/CMakeLists.txt +++ b/cpp/kinematic_icp/CMakeLists.txt @@ -21,7 +21,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. cmake_minimum_required(VERSION 3.16...3.26) -project(kinematic_icp_cpp VERSION 0.1.1 LANGUAGES CXX) +project(kinematic_icp_cpp VERSION 0.0.1 LANGUAGES CXX) # Setup build options for the underlying kiss dependency option(USE_CCACHE "Build using Ccache if found on the path" ON) diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.cpp b/cpp/kinematic_icp/pipeline/KinematicICP.cpp index f23d12f..9bfdabd 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.cpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.cpp @@ -23,6 +23,7 @@ #include "KinematicICP.hpp" #include +#include #include #include #include @@ -50,16 +51,18 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame( const std::vector ×tamps, const Sophus::SE3d &lidar_to_base, const Sophus::SE3d &relative_odometry) { - // Need to deskew in lidar frame - const Sophus::SE3d &relative_odometry_in_lidar = - lidar_to_base.inverse() * relative_odometry * lidar_to_base; - const auto &preprocessed_frame = - preprocessor_.Preprocess(frame, timestamps, relative_odometry_in_lidar); - // Give the frame in base frame - const auto &preprocessed_frame_in_base = transform_points(preprocessed_frame, lidar_to_base); + const auto &deskew_frame = [&]() -> std::vector { + if (!config_.deskew || timestamps.empty()) return frame; + return kiss_icp::DeSkewScan(frame, timestamps, + lidar_to_base.inverse() * relative_odometry * lidar_to_base); + }(); + const auto &deskew_frame_in_base = transform_points(deskew_frame, lidar_to_base); + // Preprocess the input cloud + const auto &cropped_frame = + kiss_icp::Preprocess(deskew_frame_in_base, config_.max_range, config_.min_range); + // Voxelize - const auto &[source, frame_downsample] = - Voxelize(preprocessed_frame_in_base, config_.voxel_size); + const auto &[source, frame_downsample] = Voxelize(cropped_frame, config_.voxel_size); // Get adaptive_threshold const double tau = correspondence_threshold_.ComputeThreshold(); diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.hpp b/cpp/kinematic_icp/pipeline/KinematicICP.hpp index e193388..d995a79 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.hpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.hpp @@ -89,8 +89,8 @@ class KinematicICP { std::vector LocalMap() const { return local_map_.Pointcloud(); }; - const kiss_icp::VoxelHashMap &VoxelMap() const { return local_map_; }; - kiss_icp::VoxelHashMap &VoxelMap() { return local_map_; }; + const SparseVoxelGrid &VoxelMap() const { return local_map_; }; + SparseVoxelGrid &VoxelMap() { return local_map_; }; const Sophus::SE3d &pose() const { return last_pose_; } Sophus::SE3d &pose() { return last_pose_; } @@ -101,8 +101,7 @@ class KinematicICP { KinematicRegistration registration_; CorrespondenceThreshold correspondence_threshold_; Config config_; - // KISS-ICP pipeline modules - kiss_icp::VoxelHashMap local_map_; + SparseVoxelGrid local_map_; }; } // namespace kinematic_icp::pipeline diff --git a/cpp/kinematic_icp/registration/Registration.cpp b/cpp/kinematic_icp/registration/Registration.cpp index 763ce10..06e712b 100644 --- a/cpp/kinematic_icp/registration/Registration.cpp +++ b/cpp/kinematic_icp/registration/Registration.cpp @@ -23,10 +23,8 @@ #include "Registration.hpp" #include -#include #include #include -#include #include #include @@ -39,7 +37,7 @@ #include using LinearSystem = std::pair; -using Correspondences = tbb::concurrent_vector>; +using Correspondences = std::vector>; namespace { constexpr double epsilon = std::numeric_limits::min(); @@ -63,20 +61,33 @@ Correspondences DataAssociation(const std::vector &points, const Sophus::SE3d &T, const double max_correspondance_distance) { using points_iterator = std::vector::const_iterator; - Correspondences correspondences; - correspondences.reserve(points.size()); - tbb::parallel_for( + Correspondences associations; + associations.reserve(points.size()); + associations = tbb::parallel_reduce( // Range tbb::blocked_range{points.cbegin(), points.cend()}, - [&](const tbb::blocked_range &r) { + // Identity + associations, + // 1st lambda: Parallel computation + [&](const tbb::blocked_range &r, Correspondences res) -> Correspondences { + res.reserve(r.size()); std::for_each(r.begin(), r.end(), [&](const auto &point) { const auto &[closest_neighbor, distance] = voxel_map.GetClosestNeighbor(T * point); if (distance < max_correspondance_distance) { - correspondences.emplace_back(point, closest_neighbor); + res.emplace_back(point, closest_neighbor); } }); + return res; + }, + // 2nd lambda: Parallel reduction + [](Correspondences a, const Correspondences &b) -> Correspondences { + a.insert(a.end(), // + std::make_move_iterator(b.cbegin()), // + std::make_move_iterator(b.cend())); + return a; }); - return correspondences; + + return associations; } Eigen::Vector2d ComputePerturbation(const Correspondences &correspondences, diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index ce12fad..58595f9 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -21,7 +21,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. cmake_minimum_required(VERSION 3.16...3.26) -project(kinematic_icp VERSION 0.1.1 LANGUAGES CXX) +project(kinematic_icp VERSION 0.0.1 LANGUAGES CXX) set(CMAKE_BUILD_TYPE Release) diff --git a/ros/package.xml b/ros/package.xml index 0138854..526514e 100644 --- a/ros/package.xml +++ b/ros/package.xml @@ -25,7 +25,7 @@ SOFTWARE. --> kinematic_icp - 0.1.1 + 0.0.1 ROS 2 Wrapper frevo137 MIT