diff --git a/cpp/kinematic_icp/kiss_icp/kiss-icp.cmake b/cpp/kinematic_icp/kiss_icp/kiss-icp.cmake
index 11bfe59..c24a940 100644
--- 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
index aece4c5..dc80f09 100644
--- a/cpp/kinematic_icp/pipeline/KinematicICP.cpp
+++ b/cpp/kinematic_icp/pipeline/KinematicICP.cpp
@@ -23,9 +23,7 @@
#include "KinematicICP.hpp"
#include <Eigen/Core>
-#include <kiss_icp/core/Deskew.hpp>
#include <kiss_icp/core/Preprocessing.hpp>
-#include <kiss_icp/core/Registration.hpp>
#include <kiss_icp/core/VoxelHashMap.hpp>
#include <vector>
@@ -52,18 +50,16 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame(
const std::vector<double> ×tamps,
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 +81,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
index c17a62e..c660699 100644
--- a/cpp/kinematic_icp/pipeline/KinematicICP.hpp
+++ b/cpp/kinematic_icp/pipeline/KinematicICP.hpp
@@ -24,9 +24,8 @@
#include <Eigen/Core>
#include <cmath>
-#include <kiss_icp/core/Threshold.hpp>
+#include <kiss_icp/core/Preprocessing.hpp>
#include <kiss_icp/core/VoxelHashMap.hpp>
-#include <kiss_icp/pipeline/KissICP.hpp>
#include <sophus/se3.hpp>
#include <tuple>
#include <vector>
@@ -76,6 +75,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 +104,7 @@ class KinematicICP {
CorrespondenceThreshold correspondence_threshold_;
Config config_;
// KISS-ICP pipeline modules
+ kiss_icp::Preprocessor preprocessor_;
kiss_icp::VoxelHashMap local_map_;
};
diff --git a/cpp/kinematic_icp/registration/Registration.cpp b/cpp/kinematic_icp/registration/Registration.cpp
index b95286a..6d4eab9 100644
--- a/cpp/kinematic_icp/registration/Registration.cpp
+++ b/cpp/kinematic_icp/registration/Registration.cpp
@@ -23,8 +23,10 @@
#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>
@@ -38,7 +40,7 @@
#include <tuple>
using LinearSystem = std::pair<Eigen::Matrix2d, Eigen::Vector2d>;
-using Correspondences = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
+using Correspondences = tbb::concurrent_vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
namespace {
constexpr double epsilon = std::numeric_limits<double>::min();
@@ -62,33 +64,20 @@ 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 associations;
- associations.reserve(points.size());
- associations = tbb::parallel_reduce(
+ Correspondences correspondences;
+ correspondences.reserve(points.size());
+ tbb::parallel_for(
// Range
tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
- // Identity
- associations,
- // 1st lambda: Parallel computation
- [&](const tbb::blocked_range<points_iterator> &r, Correspondences res) -> Correspondences {
- res.reserve(r.size());
+ [&](const tbb::blocked_range<points_iterator> &r) {
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) {
- res.emplace_back(point, closest_neighbor);
+ correspondences.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 associations;
+ return correspondences;
}
Eigen::Vector2d ComputePerturbation(const Correspondences &correspondences,