This is disgusting

PRBonn · Nov 12, 2024 · ee2ab55 · ee2ab55
1 parent 3fafa2f
commit ee2ab55
Show file tree

Hide file tree

Showing 10 changed files with 46 additions and 52 deletions.
diff --git a/cpp/kinematic_icp/pipeline/CMakeLists.txt b/cpp/kinematic_icp/pipeline/CMakeLists.txt
@@ -23,5 +23,5 @@
 add_library(kinematic_icp_pipeline STATIC)
 target_sources(kinematic_icp_pipeline PRIVATE KinematicICP.cpp)
 target_link_libraries(kinematic_icp_pipeline PUBLIC kinematic_icp_registration kinematic_icp_threshold
-                                                    kiss_icp_pipeline)
+  kiss_icp_pipeline Eigen3::Eigen Sophus::Sophus)
 set_global_target_properties(kinematic_icp_pipeline)
diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.cpp b/cpp/kinematic_icp/pipeline/KinematicICP.cpp
@@ -29,47 +29,28 @@
 #include <kiss_icp/core/VoxelHashMap.hpp>
 #include <vector>
 
-namespace {
-auto transform_points(const std::vector<Eigen::Vector3d> &points, const Sophus::SE3d &pose) {
-    std::vector<Eigen::Vector3d> points_transformed(points.size());
-    std::transform(points.cbegin(), points.cend(), points_transformed.begin(),
-                   [&](const auto &point) { return pose * point; });
-    return points_transformed;
-}
+#include "kinematic_icp/preprocessing/Preprocessing.hpp"
+#include "kinematic_icp/preprocessing/StampedPointCloud.hpp"
 
-auto Voxelize(const std::vector<Eigen::Vector3d> &frame, const double voxel_size) {
-    const std::vector<Eigen::Vector3d> &frame_downsample =
-        kiss_icp::VoxelDownsample(frame, voxel_size * 0.5);
-    const std::vector<Eigen::Vector3d> &source =
-        kiss_icp::VoxelDownsample(frame_downsample, voxel_size * 1.5);
-    return std::make_tuple(source, frame_downsample);
-}
-}  // namespace
 namespace kinematic_icp::pipeline {
 
-KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame(
-    const std::vector<Eigen::Vector3d> &frame,
-    const std::vector<double> &timestamps,
+KinematicICP::StampedPointCloudTuple KinematicICP::RegisterFrame(
+    const StampedPointCloud &stamped_frame,
     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);
-
-    // Voxelize
-    const auto &[source, frame_downsample] = Voxelize(cropped_frame, config_.voxel_size);
+        ClipMinMaxRange(stamped_frame, config_.max_range, config_.min_range);
 
+    const auto &mapping_frame = VoxelDownsample(cropped_frame, config_.voxel_size * 0.5);
+    const auto &registration_frame = VoxelDownsample(mapping_frame, config_.voxel_size * 1.5);
+    const auto &deskewed_registration_frame =
+        config_.deskew ? DeSkew(registration_frame, relative_odometry) : registration_frame;
     // Get adaptive_threshold
     const double &tau = correspondence_threshold_.ComputeThreshold();
 
     // Run ICP
-    const auto new_pose = registration_.ComputeRobotMotion(source,             // frame
+    const auto new_pose = registration_.ComputeRobotMotion(deskewed_registration_frame,  // frame
                                                            local_map_,         // voxel_map
                                                            last_pose_,         // last_pose
                                                            relative_odometry,  // robot_motion
@@ -80,11 +61,14 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame(
 
     // Update step: threshold, local map and the last pose
     correspondence_threshold_.UpdateModelError(model_deviation);
-    local_map_.Update(frame_downsample, new_pose);
+    const auto estimated_relative_motion = last_pose_.inverse() * new_pose;
+    const auto &deskewed_mapping_frame =
+        config_.deskew ? DeSkew(mapping_frame, estimated_relative_motion) : mapping_frame;
+    local_map_.Update(deskewed_mapping_frame, new_pose);
     last_pose_ = new_pose;
 
     // Return the (deskew) input raw scan (frame) and the points used for
     // registration (source)
-    return {deskew_frame_in_base, source};
+    return {deskewed_mapping_frame, deskewed_registration_frame};
 }
 }  // namespace kinematic_icp::pipeline
diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.hpp b/cpp/kinematic_icp/pipeline/KinematicICP.hpp
@@ -32,6 +32,7 @@
 #include <vector>
 
 #include "kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp"
+#include "kinematic_icp/preprocessing/StampedPointCloud.hpp"
 #include "kinematic_icp/registration/Registration.hpp"
 
 namespace kinematic_icp::pipeline {
@@ -62,8 +63,7 @@ struct Config {
 
 class KinematicICP {
 public:
-    using Vector3dVector = std::vector<Eigen::Vector3d>;
-    using Vector3dVectorTuple = std::tuple<Vector3dVector, Vector3dVector>;
+    using StampedPointCloudTuple = std::tuple<StampedPointCloud, StampedPointCloud>;
 
     explicit KinematicICP(const Config &config)
         : registration_(config.max_num_iterations,
@@ -78,10 +78,9 @@ class KinematicICP {
           config_(config),
           local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel) {}
 
-    Vector3dVectorTuple RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
-                                      const std::vector<double> &timestamps,
-                                      const Sophus::SE3d &lidar_to_base,
-                                      const Sophus::SE3d &relative_odometry);
+    StampedPointCloudTuple RegisterFrame(const StampedPointCloud &stamped_frame,
+                                         const Sophus::SE3d &lidar_to_base,
+                                         const Sophus::SE3d &relative_odometry);
 
     inline void SetPose(const Sophus::SE3d &pose) {
         last_pose_ = pose;

diff --git a/cpp/kinematic_icp/preprocessing/CMakeLists.txt b/cpp/kinematic_icp/preprocessing/CMakeLists.txt
@@ -21,6 +21,6 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 add_library(kinematic_icp_preprocessing STATIC)
-target_sources(kinematic_icp_preprocessing PRIVATE Preprocessing.cpp)
+target_sources(kinematic_icp_preprocessing PRIVATE StampedPointCloud.cpp Preprocessing.cpp)
 target_link_libraries(kinematic_icp_preprocessing PUBLIC Sophus::Sophus Eigen3::Eigen)
 set_global_target_properties(kinematic_icp_preprocessing)
diff --git a/cpp/kinematic_icp/preprocessing/Preprocessing.cpp b/cpp/kinematic_icp/preprocessing/Preprocessing.cpp
@@ -2,7 +2,7 @@
 
 #include <unordered_map>
 
-#include "kinematic_icp/preprocessing/PointCloud.hpp"
+#include "StampedPointCloud.hpp"
 
 namespace {
 Eigen::Vector3i PointToVoxel(const Eigen::Vector3d &point, const double voxel_size) {

diff --git a/cpp/kinematic_icp/preprocessing/Preprocessing.hpp b/cpp/kinematic_icp/preprocessing/Preprocessing.hpp
@@ -1,5 +1,5 @@
 #pragma once
-#include "PointCloud.hpp"
+#include "StampedPointCloud.hpp"
 
 namespace kinematic_icp {
 

diff --git a/cpp/kinematic_icp/preprocessing/StampedPointCloud.cpp b/cpp/kinematic_icp/preprocessing/StampedPointCloud.cpp
@@ -0,0 +1,7 @@
+#include "StampedPointCloud.hpp"
+
+namespace kinematic_icp {
+StampedPoint::StampedPoint(const Eigen::Vector3d &point, const double stamp)
+    : coordinates(point), timestamp(stamp){};
+
+}
diff --git a/...inematic_icp/preprocessing/PointCloud.hpp → ...c_icp/preprocessing/StampedPointCloud.hpp b/...inematic_icp/preprocessing/PointCloud.hpp → ...c_icp/preprocessing/StampedPointCloud.hpp
@@ -7,8 +7,7 @@ namespace kinematic_icp {
 
 struct StampedPoint {
     StampedPoint() = default;
-    StampedPoint(const Eigen::Vector3d &point, const double stamp)
-        : coordinates(point), timestamp(stamp){};
+    StampedPoint(const Eigen::Vector3d &point, const double stamp);
 
     Eigen::Vector3d coordinates = Eigen::Vector3d::Zero();
     double timestamp = 0.0;

diff --git a/cpp/kinematic_icp/registration/Registration.cpp b/cpp/kinematic_icp/registration/Registration.cpp
@@ -37,6 +37,8 @@
 #include <sophus/so3.hpp>
 #include <tuple>
 
+#include "kinematic_icp/preprocessing/StampedPointCloud.hpp"
+
 using LinearSystem = std::pair<Eigen::Matrix2d, Eigen::Vector2d>;
 using Correspondences = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
 
@@ -57,7 +59,7 @@ double ComputeOdometryRegularization(const Correspondences &associations,
     return beta;
 }
 
-Correspondences DataAssociation(const std::vector<Eigen::Vector3d> &points,
+Correspondences DataAssociation(const kinematic_icp::StampedPointCloud &points,
                                 const kiss_icp::VoxelHashMap &voxel_map,
                                 const Sophus::SE3d &T,
                                 const double max_correspondance_distance) {
@@ -73,9 +75,10 @@ Correspondences DataAssociation(const std::vector<Eigen::Vector3d> &points,
         [&](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);
+                const auto &[closest_neighbor, distance] =
+                    voxel_map.GetClosestNeighbor(T * point.coordinates);
                 if (distance < max_correspondance_distance) {
-                    res.emplace_back(point, closest_neighbor);
+                    res.emplace_back(point.coordinates, closest_neighbor);
                 }
             });
             return res;
@@ -159,11 +162,12 @@ KinematicRegistration::KinematicRegistration(const int max_num_iteration,
         tbb::global_control::max_allowed_parallelism, static_cast<size_t>(max_num_threads_));
 }
 
-Sophus::SE3d KinematicRegistration::ComputeRobotMotion(const std::vector<Eigen::Vector3d> &frame,
-                                                       const kiss_icp::VoxelHashMap &voxel_map,
-                                                       const Sophus::SE3d &last_robot_pose,
-                                                       const Sophus::SE3d &relative_wheel_odometry,
-                                                       const double max_correspondence_distance) {
+Sophus::SE3d KinematicRegistration::ComputeRobotMotion(
+    const kinematic_icp::StampedPointCloud &frame,
+    const kiss_icp::VoxelHashMap &voxel_map,
+    const Sophus::SE3d &last_robot_pose,
+    const Sophus::SE3d &relative_wheel_odometry,
+    const double max_correspondence_distance) {
     Sophus::SE3d current_estimate = last_robot_pose * relative_wheel_odometry;
     if (voxel_map.Empty()) return current_estimate;
 

diff --git a/cpp/kinematic_icp/registration/Registration.hpp b/cpp/kinematic_icp/registration/Registration.hpp
@@ -23,6 +23,7 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <kinematic_icp/preprocessing/StampedPointCloud.hpp>
 #include <kiss_icp/core/VoxelHashMap.hpp>
 #include <sophus/se3.hpp>
 #include <vector>
@@ -36,7 +37,7 @@ struct KinematicRegistration {
                                    const bool use_adaptive_odometry_regularization,
                                    const double fixed_regularization);
 
-    Sophus::SE3d ComputeRobotMotion(const std::vector<Eigen::Vector3d> &frame,
+    Sophus::SE3d ComputeRobotMotion(const StampedPointCloud &frame,
                                     const kiss_icp::VoxelHashMap &voxel_map,
                                     const Sophus::SE3d &last_robot_pose,
                                     const Sophus::SE3d &relative_wheel_odometry,