[wpimath] Fix pose estimator performance (#4111)

Fixes #4087.

wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h

@@ -7,6 +7,7 @@
 #include <array>
 #include <functional>
 #include <map>
+#include <optional>
 #include <utility>
 #include <vector>
 
@@ -81,12 +82,16 @@ class TimeInterpolatableBuffer {
   void Clear() { m_pastSnapshots.clear(); }
 
   /**
-   * Sample the buffer at the given time. If there are no elements in the
-   * buffer, calling this function results in undefined behavior.
+   * Sample the buffer at the given time. If the buffer is empty, an empty
+   * optional is returned.
    *
    * @param time The time at which to sample the buffer.
    */
-  T Sample(units::second_t time) {
+  std::optional<T> Sample(units::second_t time) {
+    if (m_pastSnapshots.empty()) {
+      return {};
+    }
+
     // We will perform a binary search to find the index of the element in the
     // vector that has a timestamp that is equal to or greater than the vision
     // measurement timestamp.