Get Preprocessor from KISS

PRBonn · Jan 9, 2025 · 38205bb · 38205bb
1 parent 26b1abe
commit 38205bb
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 14 deletions.
diff --git a/cpp/kinematic_icp/kiss_icp/kiss-icp.cmake b/cpp/kinematic_icp/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.1.0.tar.gz SOURCE_SUBDIR
+FetchContent_Declare(kiss_icp URL https://github.com/PRBonn/kiss-icp/archive/refs/tags/v1.2.0.tar.gz SOURCE_SUBDIR
                                   cpp/kiss_icp)
 FetchContent_MakeAvailable(kiss_icp)
diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.cpp b/cpp/kinematic_icp/pipeline/KinematicICP.cpp
@@ -52,18 +52,16 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame(
     const std::vector<double> &timestamps,
     const Sophus::SE3d &lidar_to_base,
     const Sophus::SE3d &relative_odometry) {
-    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);
-
+    // 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);
     // Voxelize
-    const auto &[source, frame_downsample] = Voxelize(cropped_frame, config_.voxel_size);
+    const auto &[source, frame_downsample] =
+        Voxelize(preprocessed_frame_in_base, config_.voxel_size);
 
     // Get adaptive_threshold
     const double tau = correspondence_threshold_.ComputeThreshold();
@@ -85,6 +83,6 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame(
 
     // Return the (deskew) input raw scan (frame) and the points used for
     // registration (source)
-    return {deskew_frame_in_base, source};
+    return {preprocessed_frame_in_base, source};
 }
 }  // namespace kinematic_icp::pipeline
diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.hpp b/cpp/kinematic_icp/pipeline/KinematicICP.hpp
@@ -24,9 +24,9 @@
 
 #include <Eigen/Core>
 #include <cmath>
+#include <kiss_icp/core/Preprocessing.hpp>
 #include <kiss_icp/core/Threshold.hpp>
 #include <kiss_icp/core/VoxelHashMap.hpp>
-#include <kiss_icp/pipeline/KissICP.hpp>
 #include <sophus/se3.hpp>
 #include <tuple>
 #include <vector>
@@ -76,6 +76,7 @@ class KinematicICP {
                                     config.use_adaptive_threshold,
                                     config.fixed_threshold),
           config_(config),
+          preprocessor_(config.max_range, config.min_range, config.deskew, config.max_num_threads),
           local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel) {}
 
     Vector3dVectorTuple RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
@@ -104,6 +105,7 @@ class KinematicICP {
     CorrespondenceThreshold correspondence_threshold_;
     Config config_;
     // KISS-ICP pipeline modules
+    kiss_icp::Preprocessor preprocessor_;
     kiss_icp::VoxelHashMap local_map_;
 };