diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp
index 58d0e731e..977493796 100644
--- a/tf2/src/buffer_core.cpp
+++ b/tf2/src/buffer_core.cpp
@@ -479,14 +479,14 @@ tf2::TF2Error BufferCore::walkToTopParent(
     }
     // Erase all duplicate items from frame_chain
     if (n > 0u) {
-      frame_chain->erase(frame_chain->begin() + (n - 1u), frame_chain->end());
+      frame_chain->erase(frame_chain->begin() + (n + 1u), frame_chain->end());
     }
 
     if (m < reverse_frame_chain.size()) {
-      size_t i = m + 1uL;
-      while (i > 0u) {
-        --i;
+      int i = m + 1;
+      while (i >= 0) {
         frame_chain->push_back(reverse_frame_chain[i]);
+        --i;
       }
     }
   }
@@ -1583,25 +1583,24 @@ void BufferCore::_chainAsVector(
     }
   }
 
-  if (source_time != target_time) {
-    std::vector<CompactFrameID> target_frame_chain;
-    retval = walkToTopParent(
-      accum, target_time, target_id, fixed_id, &error_string,
-      &target_frame_chain);
+  std::vector<CompactFrameID> target_frame_chain;
+  retval = walkToTopParent(
+    accum, target_time, fixed_id, target_id, &error_string,
+    &target_frame_chain);
 
-    if (retval != tf2::TF2Error::TF2_NO_ERROR) {
-      switch (retval) {
-        case tf2::TF2Error::TF2_CONNECTIVITY_ERROR:
-          throw ConnectivityException(error_string);
-        case tf2::TF2Error::TF2_EXTRAPOLATION_ERROR:
-          throw ExtrapolationException(error_string);
-        case tf2::TF2Error::TF2_LOOKUP_ERROR:
-          throw LookupException(error_string);
-        default:
-          CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
-          assert(0);
-      }
+  if (retval != tf2::TF2Error::TF2_NO_ERROR) {
+    switch (retval) {
+      case tf2::TF2Error::TF2_CONNECTIVITY_ERROR:
+        throw ConnectivityException(error_string);
+      case tf2::TF2Error::TF2_EXTRAPOLATION_ERROR:
+        throw ExtrapolationException(error_string);
+      case tf2::TF2Error::TF2_LOOKUP_ERROR:
+        throw LookupException(error_string);
+      default:
+        CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
+        assert(0);
     }
+
     size_t m = target_frame_chain.size();
     size_t n = source_frame_chain.size();
     while (m > 0u && n > 0u) {
@@ -1613,13 +1612,13 @@ void BufferCore::_chainAsVector(
     }
     // Erase all duplicate items from frame_chain
     if (n > 0u) {
-      source_frame_chain.erase(source_frame_chain.begin() + (n - 1u), source_frame_chain.end());
+      source_frame_chain.erase(source_frame_chain.begin() + (n + 1u), source_frame_chain.end());
     }
 
-    if (m < target_frame_chain.size()) {
-      for (size_t i = 0u; i <= m; ++i) {
-        source_frame_chain.push_back(target_frame_chain[i]);
-      }
+    int i = m + 1;
+    while (i >= 0) {
+      source_frame_chain.push_back(target_frame_chain[i]);
+      --i;
     }
   }