Only keep first and last two poses

kin-dergarten · Oct 17, 2023 · 0ed940b · 0ed940b
1 parent 3c17e06
commit 0ed940b
Show file tree

Hide file tree

Showing 4 changed files with 51 additions and 24 deletions.
diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -40,14 +40,13 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
         if (!config_.deskew) return frame;
         // TODO(Nacho) Add some asserts here to sanitize the timestamps
 
+        const auto &start_pose = poses_.next_to_last();
+        const auto &finish_pose = poses_.last();
         //  If not enough poses for the estimation, do not de-skew
-        const size_t N = poses().size();
-        if (N <= 2) return frame;
+        if (!start_pose.has_value() || !finish_pose.has_value()) return frame;
 
         // Estimate linear and angular velocities
-        const auto &start_pose = poses_[N - 2];
-        const auto &finish_pose = poses_[N - 1];
-        return DeSkewScan(frame, timestamps, start_pose, finish_pose);
+        return DeSkewScan(frame, timestamps, start_pose.value(), finish_pose.value());
     }();
     return RegisterFrame(deskew_frame);
 }
@@ -64,19 +63,19 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
 
     // Compute initial_guess for ICP
     const auto prediction = GetPredictionModel();
-    const auto last_pose = !poses_.empty() ? poses_.back() : Sophus::SE3d();
+    const auto last_pose = poses_.last().has_value() ? poses_.last().value() : Sophus::SE3d();
     const auto initial_guess = last_pose * prediction;
 
     // Run icp
-    const Sophus::SE3d new_pose = kiss_icp::RegisterFrame(source,         //
-                                                          local_map_,     //
-                                                          initial_guess,  //
-                                                          3.0 * sigma,    //
-                                                          sigma / 3.0);
+    Sophus::SE3d new_pose = kiss_icp::RegisterFrame(source,         //
+                                                    local_map_,     //
+                                                    initial_guess,  //
+                                                    3.0 * sigma,    //
+                                                    sigma / 3.0);
     const auto model_deviation = initial_guess.inverse() * new_pose;
     adaptive_threshold_.UpdateModelDeviation(model_deviation);
     local_map_.Update(frame_downsample, new_pose);
-    poses_.push_back(new_pose);
+    poses_.push(std::move(new_pose));
     return {frame, source};
 }
 
@@ -95,16 +94,21 @@ double KissICP::GetAdaptiveThreshold() {
 }
 
 Sophus::SE3d KissICP::GetPredictionModel() const {
-    Sophus::SE3d pred = Sophus::SE3d();
-    const size_t N = poses_.size();
-    if (N < 2) return pred;
-    return poses_[N - 2].inverse() * poses_[N - 1];
+    const auto &last = poses_.last();
+    const auto &next_to_last = poses_.next_to_last();
+    if (last.has_value() && next_to_last.has_value()) {
+        return next_to_last.value().inverse() * last.value();
+    }
+    return {};
 }
 
 bool KissICP::HasMoved() {
-    if (poses_.empty()) return false;
-    const double motion = (poses_.front().inverse() * poses_.back()).translation().norm();
-    return motion > 5.0 * config_.min_motion_th;
+    if (poses_.first().has_value() && poses_.last().has_value()) {
+        const double motion =
+            (poses_.first().value().inverse() * poses_.last().value()).translation().norm();
+        return motion > 5.0 * config_.min_motion_th;
+    }
+    return false;
 }
 
 }  // namespace kiss_icp::pipeline
diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp
@@ -22,6 +22,8 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <array>
+#include <optional>
 #include <tuple>
 #include <vector>
 
@@ -70,12 +72,33 @@ class KissICP {
 
 public:
     // Extra C++ API to facilitate ROS debugging
-    std::vector<Eigen::Vector3d> LocalMap() const { return local_map_.Pointcloud(); };
-    std::vector<Sophus::SE3d> poses() const { return poses_; };
+    std::vector<Eigen::Vector3d> LocalMap() const { return local_map_.Pointcloud(); }
+    Sophus::SE3d last_pose() const {
+        return poses_.last().has_value() ? poses_.last().value() : Sophus::SE3d{};
+    }
 
 private:
     // KISS-ICP pipeline modules
-    std::vector<Sophus::SE3d> poses_;
+    class Poses {
+    public:
+        void push(Sophus::SE3d pose) {
+            if (!first_pose_.has_value()) {
+                first_pose_ = pose;
+            }
+            last_pose_index_ = (last_pose_index_ + 1) % 2;
+            poses_[last_pose_index_] = std::move(pose);
+        }
+        const std::optional<Sophus::SE3d> &first() const { return first_pose_; }
+        const std::optional<Sophus::SE3d> &last() const { return poses_[last_pose_index_ % 2]; }
+        const std::optional<Sophus::SE3d> &next_to_last() const {
+            return poses_[(last_pose_index_ - 1 + 2) % 2];
+        }
+
+    private:
+        std::optional<Sophus::SE3d> first_pose_{};
+        std::array<std::optional<Sophus::SE3d>, 2> poses_{};
+        std::int32_t last_pose_index_{};
+    } poses_;
     KISSConfig config_;
     VoxelHashMap local_map_;
     AdaptiveThreshold adaptive_threshold_;

diff --git a/ros/ros1/OdometryServer.cpp b/ros/ros1/OdometryServer.cpp
@@ -110,7 +110,7 @@ void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2 &msg) {
     const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps);
 
     // PublishPose
-    const auto pose = odometry_.poses().back();
+    const auto pose = odometry_.last_pose();
 
     // Convert from Eigen to ROS types
     const Eigen::Vector3d t_current = pose.translation();

diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp
@@ -125,7 +125,7 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::SharedPt
     const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps);
 
     // PublishPose
-    const auto pose = odometry_.poses().back();
+    const auto pose = odometry_.last_pose();
 
     // Convert from Eigen to ROS types
     const Eigen::Vector3d t_current = pose.translation();