From a5aa03b776220c5270dbddec359f471af59799b9 Mon Sep 17 00:00:00 2001
From: Michael Grupp <grupp@magazino.eu>
Date: Tue, 23 Jun 2020 10:34:01 +0200
Subject: [PATCH] Implement fixed size transform interpolation buffer. (#1581)

---
 .../transform_interpolation_buffer.cc         | 23 ++++++++++++++++
 .../transform_interpolation_buffer.h          | 26 ++++++++++++++++---
 .../transform_interpolation_buffer_test.cc    | 16 ++++++++++++
 3 files changed, 62 insertions(+), 3 deletions(-)

diff --git a/cartographer/transform/transform_interpolation_buffer.cc b/cartographer/transform/transform_interpolation_buffer.cc
index 7d9c3d7..ae65721 100644
--- a/cartographer/transform/transform_interpolation_buffer.cc
+++ b/cartographer/transform/transform_interpolation_buffer.cc
@@ -40,8 +40,17 @@ void TransformInterpolationBuffer::Push(const common::Time time,
     CHECK_GE(time, latest_time()) << "New transform is older than latest.";
   }
   timestamped_transforms_.push_back(TimestampedTransform{time, transform});
+  RemoveOldTransformsIfNeeded();
 }
 
+void TransformInterpolationBuffer::SetSizeLimit(
+    const size_t buffer_size_limit) {
+  buffer_size_limit_ = buffer_size_limit;
+  RemoveOldTransformsIfNeeded();
+}
+
+void TransformInterpolationBuffer::Clear() { timestamped_transforms_.clear(); }
+
 bool TransformInterpolationBuffer::Has(const common::Time time) const {
   if (timestamped_transforms_.empty()) {
     return false;
@@ -65,6 +74,12 @@ transform::Rigid3d TransformInterpolationBuffer::Lookup(
   return Interpolate(*start, *end, time).transform;
 }
 
+void TransformInterpolationBuffer::RemoveOldTransformsIfNeeded() {
+  while (timestamped_transforms_.size() > buffer_size_limit_) {
+    timestamped_transforms_.pop_front();
+  }
+}
+
 common::Time TransformInterpolationBuffer::earliest_time() const {
   CHECK(!empty()) << "Empty buffer.";
   return timestamped_transforms_.front().time;
@@ -79,5 +94,13 @@ bool TransformInterpolationBuffer::empty() const {
   return timestamped_transforms_.empty();
 }
 
+size_t TransformInterpolationBuffer::size_limit() const {
+  return buffer_size_limit_;
+}
+
+size_t TransformInterpolationBuffer::size() const {
+  return timestamped_transforms_.size();
+}
+
 }  // namespace transform
 }  // namespace cartographer
diff --git a/cartographer/transform/transform_interpolation_buffer.h b/cartographer/transform/transform_interpolation_buffer.h
index 4dc51bb..84a7dcd 100644
--- a/cartographer/transform/transform_interpolation_buffer.h
+++ b/cartographer/transform/transform_interpolation_buffer.h
@@ -17,7 +17,8 @@
 #ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_
 #define CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_
 
-#include <vector>
+#include <deque>
+#include <limits>
 
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/proto/trajectory.pb.h"
@@ -27,18 +28,28 @@
 namespace cartographer {
 namespace transform {
 
+constexpr size_t kUnlimitedBufferSize = std::numeric_limits<size_t>::max();
+
 // A time-ordered buffer of transforms that supports interpolated lookups.
+// Unless explicitly set, the buffer size is unlimited.
 class TransformInterpolationBuffer {
  public:
   TransformInterpolationBuffer() = default;
   explicit TransformInterpolationBuffer(
       const mapping::proto::Trajectory& trajectory);
 
+  // Sets the transform buffer size limit and removes old transforms
+  // if it is exceeded.
+  void SetSizeLimit(size_t buffer_size_limit);
+
   // Adds a new transform to the buffer and removes the oldest transform if the
   // buffer size limit is exceeded.
   void Push(common::Time time, const transform::Rigid3d& transform);
 
-  // Returns true if an interpolated transfrom can be computed at 'time'.
+  // Clears the transform buffer.
+  void Clear();
+
+  // Returns true if an interpolated transform can be computed at 'time'.
   bool Has(common::Time time) const;
 
   // Returns an interpolated transform at 'time'. CHECK()s that a transform at
@@ -56,8 +67,17 @@ class TransformInterpolationBuffer {
   // Returns true if the buffer is empty.
   bool empty() const;
 
+  // Returns the maximum allowed size of the transform buffer.
+  size_t size_limit() const;
+
+  // Returns the current size of the transform buffer.
+  size_t size() const;
+
  private:
-  std::vector<TimestampedTransform> timestamped_transforms_;
+  void RemoveOldTransformsIfNeeded();
+
+  std::deque<TimestampedTransform> timestamped_transforms_;
+  size_t buffer_size_limit_ = kUnlimitedBufferSize;
 };
 
 }  // namespace transform
diff --git a/cartographer/transform/transform_interpolation_buffer_test.cc b/cartographer/transform/transform_interpolation_buffer_test.cc
index 193f95e..b78d569 100644
--- a/cartographer/transform/transform_interpolation_buffer_test.cc
+++ b/cartographer/transform/transform_interpolation_buffer_test.cc
@@ -70,6 +70,22 @@ TEST(TransformInterpolationBufferTest, testLookupSingleTransform) {
   EXPECT_THAT(interpolated, IsNearly(transform::Rigid3d::Identity(), 1e-6));
 }
 
+TEST(TransformInterpolationBufferTest, testSetSizeLimit) {
+  TransformInterpolationBuffer buffer;
+  EXPECT_EQ(buffer.size_limit(), kUnlimitedBufferSize);
+  buffer.Push(common::FromUniversal(0), transform::Rigid3d::Identity());
+  buffer.Push(common::FromUniversal(1), transform::Rigid3d::Identity());
+  buffer.Push(common::FromUniversal(2), transform::Rigid3d::Identity());
+  EXPECT_EQ(buffer.size(), 3);
+  buffer.SetSizeLimit(2);
+  EXPECT_EQ(buffer.size_limit(), 2);
+  EXPECT_EQ(buffer.size(), 2);
+  EXPECT_FALSE(buffer.Has(common::FromUniversal(0)));
+  buffer.Push(common::FromUniversal(3), transform::Rigid3d::Identity());
+  EXPECT_EQ(buffer.size(), 2);
+  EXPECT_FALSE(buffer.Has(common::FromUniversal(1)));
+}
+
 }  // namespace
 }  // namespace transform
 }  // namespace cartographer