Revert "Merge branch 'main' into tiziano/bonxai_minimal"

This reverts commit da104ce, reversing changes made to 9b84286.
PRBonn · Jan 9, 2025 · db1e16c · db1e16c
1 parent da104ce
commit db1e16c
Show file tree

Hide file tree

Showing 8 changed files with 39 additions and 31 deletions.
diff --git a/Makefile b/Makefile
diff --git 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
@@ -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
@@ -23,6 +23,7 @@
 #include "KinematicICP.hpp"
 
 #include <Eigen/Core>
+#include <kiss_icp/core/Deskew.hpp>
 #include <kiss_icp/core/Preprocessing.hpp>
 #include <kiss_icp/core/VoxelUtils.hpp>
 #include <vector>
@@ -50,16 +51,18 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame(
     const std::vector<double> &timestamps,
     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<Eigen::Vector3d> {
+        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
@@ -89,8 +89,8 @@ class KinematicICP {
 
     std::vector<Eigen::Vector3d> 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
@@ -23,10 +23,8 @@
 #include "Registration.hpp"
 
 #include <tbb/blocked_range.h>
-#include <tbb/concurrent_vector.h>
 #include <tbb/global_control.h>
 #include <tbb/info.h>
-#include <tbb/parallel_for.h>
 #include <tbb/parallel_reduce.h>
 #include <tbb/task_arena.h>
 
@@ -39,7 +37,7 @@
 #include <tuple>
 
 using LinearSystem = std::pair<Eigen::Matrix2d, Eigen::Vector2d>;
-using Correspondences = tbb::concurrent_vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
+using Correspondences = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
 
 namespace {
 constexpr double epsilon = std::numeric_limits<double>::min();
@@ -63,20 +61,33 @@ Correspondences DataAssociation(const std::vector<Eigen::Vector3d> &points,
                                 const Sophus::SE3d &T,
                                 const double max_correspondance_distance) {
     using points_iterator = std::vector<Eigen::Vector3d>::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_iterator>{points.cbegin(), points.cend()},
-        [&](const tbb::blocked_range<points_iterator> &r) {
+        // Identity
+        associations,
+        // 1st lambda: Parallel computation
+        [&](const tbb::blocked_range<points_iterator> &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
@@ -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
@@ -25,7 +25,7 @@ SOFTWARE.
 -->
 <package format="3">
   <name>kinematic_icp</name>
-  <version>0.1.1</version>
+  <version>0.0.1</version>
   <description>ROS 2 Wrapper</description>
   <maintainer email="[email protected]">frevo137</maintainer>
   <license>MIT</license>