Better

PRBonn · Dec 5, 2024 · bbe87aa · bbe87aa
1 parent 54fa150
commit bbe87aa
Showing 1 changed file with 17 additions and 21 deletions.
diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp
@@ -39,28 +39,23 @@
 namespace {
 static constexpr double end_pose_stamp{1.0};
 struct StubDeskewer {
-    StubDeskewer(const std::vector<Eigen::Vector3d> &frame,
-                 const std::vector<double> &timestamps,
-                 const Sophus::SE3d &relative_motion)
-        : points_(&frame), stamps_(&timestamps), motion_(relative_motion) {}
+    StubDeskewer(const std::vector<double> &timestamps, const Sophus::SE3d &relative_motion)
+        : stamps_(timestamps), motion_(relative_motion) {}
 
-    Eigen::Vector3d operator()(const size_t idx) { return points_->at(idx); }
+    Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &) { return point; }
 
-    const std::vector<Eigen::Vector3d> *points_;
-    const std::vector<double> *stamps_;
+    const std::vector<double> &stamps_;
     const Sophus::SE3d motion_;
 };
 
 struct MotionDeskewer : public StubDeskewer {
-    MotionDeskewer(const std::vector<Eigen::Vector3d> &frame,
-                   const std::vector<double> &timestamps,
-                   const Sophus::SE3d &relative_motion)
-        : StubDeskewer(frame, timestamps, relative_motion) {}
+    MotionDeskewer(const std::vector<double> &timestamps, const Sophus::SE3d &relative_motion)
+        : StubDeskewer(timestamps, relative_motion) {}
 
-    Eigen::Vector3d operator()(const size_t idx) {
+    Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &idx) {
         const auto delta_pose = motion_.log();
-        const auto motion = Sophus::SE3d::exp((stamps_->at(idx) - end_pose_stamp) * delta_pose);
-        return motion * points_->at(idx);
+        const auto motion = Sophus::SE3d::exp((stamps_.at(idx) - end_pose_stamp) * delta_pose);
+        return motion * point;
     }
 };
 }  // namespace
@@ -85,10 +80,10 @@ Preprocessor::Preprocessor(const double max_range,
 std::vector<Eigen::Vector3d> Preprocessor::Preprocess(const std::vector<Eigen::Vector3d> &frame,
                                                       const std::vector<double> &timestamps,
                                                       const Sophus::SE3d &relative_motion) const {
-    using DeskewerType = std::function<Eigen::Vector3d(const size_t)>;
-    DeskewerType compute_deskewed_point = (deskew_ && !timestamps.empty())
-                                              ? MotionDeskewer(frame, timestamps, relative_motion)
-                                              : StubDeskewer(frame, timestamps, relative_motion);
+    using DeskewerType = std::function<Eigen::Vector3d(const Eigen::Vector3d &, const size_t &)>;
+    DeskewerType deskewer = (deskew_ && !timestamps.empty())
+                                ? MotionDeskewer(timestamps, relative_motion)
+                                : StubDeskewer(timestamps, relative_motion);
     tbb::concurrent_vector<Eigen::Vector3d> preprocessed_frame;
     preprocessed_frame.reserve(frame.size());
     tbb::parallel_for(
@@ -97,10 +92,11 @@ std::vector<Eigen::Vector3d> Preprocessor::Preprocess(const std::vector<Eigen::V
         // Parallel Compute
         [&](const tbb::blocked_range<size_t> &r) {
             for (size_t idx = r.begin(); idx < r.end(); ++idx) {
-                const double point_range = frame.at(idx).norm();
+                const auto &point = frame.at(idx);
+                const double point_range = point.norm();
                 if (point_range < max_range_ && point_range > min_range_) {
-                    const auto &point = compute_deskewed_point(idx);
-                    preprocessed_frame.emplace_back(point);
+                    const auto &deskewed_point = deskewer(point, idx);
+                    preprocessed_frame.emplace_back(deskewed_point);
                 }
             };
         });