From d700d78611ca2f2a56128ee71dd157961a1f4fb4 Mon Sep 17 00:00:00 2001
From: Eric Cousineau <eric.cousineau@tri.global>
Date: Wed, 22 May 2024 17:04:24 -0400
Subject: [PATCH] [TimeCache] Improve performance for insertData() and
 pruneList() (#680)

Signed-off-by: Eric Cousineau <eric.cousineau@tri.global>
Co-authored-by: Chris Lalancette <clalancette@gmail.com>
---
 tf2/src/cache.cpp | 46 ++++++++++++++++++++++++++++++++++------------
 1 file changed, 34 insertions(+), 12 deletions(-)

diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp
index cbbc71ef3..607fc1523 100644
--- a/tf2/src/cache.cpp
+++ b/tf2/src/cache.cpp
@@ -248,25 +248,48 @@ CompactFrameID TimeCache::getParent(
 
 bool TimeCache::insertData(const TransformStorage & new_data)
 {
+  // In order to minimize the number of times we iterate over this data, we:
+  // (1) Prune all old data first, regardless if new_data is added,
+  // (2) We use find_if to scan from newest to oldest, and stop at the first
+  //     point where the timestamp is equal or older to new_data's.
+  // (3) From this point, we scan with more expensive full equality checks to
+  //     ensure we do not reinsert the same exact data.
+  // (4) If we the data is not duplicated, then we simply insert new_data at
+  //     the point found in (2).
   const TimePoint latest_time = getLatestTimestamp();
 
+  // (1) Always prune data.
+  pruneList();
+
   // Avoid inserting data in the past that already exceeds the max_storage_time_
   if (!storage_.empty() && new_data.stamp_ < latest_time - max_storage_time_) {
     return false;
   }
 
-  // Find the oldest element in the list before the incoming stamp.
-  auto last_transform_pos = std::find_if(
-    storage_.begin(), storage_.end(), [&](const auto & transfrom) {
-      return transfrom.stamp_ <= new_data.stamp_;
+  // (2) Find the oldest element in the list before the incoming stamp.
+  auto insertion_pos = std::find_if(
+    storage_.begin(), storage_.end(), [&](const auto & transform) {
+      return transform.stamp_ <= new_data.stamp_;
     });
 
-  // Insert elements only if not already present
-  if (std::find(storage_.begin(), storage_.end(), new_data) == storage_.end()) {
-    storage_.insert(last_transform_pos, new_data);
+  bool should_insert = true;
+
+  // (3) Search along all data with same timestamp (sorted), and see if we have
+  // an exact duplicate.
+  auto maybe_same_pos = insertion_pos;
+  while (maybe_same_pos != storage_.end() && maybe_same_pos->stamp_ == new_data.stamp_) {
+    if (*maybe_same_pos == new_data) {
+      should_insert = false;
+      break;
+    }
+    maybe_same_pos++;
+  }
+
+  // (4) Insert elements only if not already present
+  if (should_insert) {
+    storage_.insert(insertion_pos, new_data);
   }
 
-  pruneList();
   return true;
 }
 
@@ -317,9 +340,8 @@ void TimeCache::pruneList()
 {
   const TimePoint latest_time = getLatestTimestamp();
 
-  storage_.remove_if(
-    [&](const auto & transform) {
-      return transform.stamp_ < latest_time - max_storage_time_;
-    });
+  while (!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time) {
+    storage_.pop_back();
+  }
 }
 }  // namespace tf2