Improve clarity

wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java

@@ -140,40 +140,53 @@ public Pose2d getEstimatedPosition() {
    * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
    */
   public Optional<Pose2d> sampleAt(double timestampSeconds) {
+    // Step 0: If there are no odometry updates to sample, skip.
     if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
-      // No odometry updates, so nothing to sample
       return Optional.empty();
     }
-    // Make sure timestamp matches the sample from the odometry pose buffer
+
+    // Step 1: Make sure timestamp matches the sample from the odometry pose buffer. (When sampling,
+    // the buffer will always use a timestamp between the first and last timestamps)
     double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
     double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey();
     timestampSeconds =
         MathUtil.clamp(timestampSeconds, oldestOdometryTimestamp, newestOdometryTimestamp);
+
+    // Step 2: If there are no applicable vision updates, use the odometry-only information.
     if (m_visionUpdates.isEmpty() || timestampSeconds < m_visionUpdates.firstKey()) {
-      // No vision update from before the requested timestamp to apply
       return m_odometryPoseBuffer.getSample(timestampSeconds);
     }
+
+    // Step 3: Get the latest vision update from before or at the timestamp to sample at.
     double floorTimestamp = m_visionUpdates.floorKey(timestampSeconds);
     var visionUpdate = m_visionUpdates.get(floorTimestamp);
+
+    // Step 4: Get the pose measured by odometry at the time of the sample.
     var odometryEstimate = m_odometryPoseBuffer.getSample(timestampSeconds);
+
+    // Step 5: Apply the vision compensation to the odometry pose.
     return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose));
   }
 
   /** Removes stale vision updates that won't affect sampling. */
   private void cleanUpVisionUpdates() {
+    // Step 0: If there are no odometry samples, skip.
     if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
-      // No odometry updates, so no vision updates as well
       return;
     }
-    // Find the oldest timestamp that needs a vision update before or at it
+
+    // Step 1: Find the oldest timestamp that needs a vision update.
     double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
+
+    // Step 2: If there are no vision updates before that timestamp, skip.
     if (m_visionUpdates.isEmpty() || oldestOdometryTimestamp < m_visionUpdates.firstKey()) {
-      // No vision updates before the oldest odometry update, so no entries to clear
       return;
     }
-    // Find the newest vision update timestamp before or at the oldest timestamp
+
+    // Step 3: Find the newest vision update timestamp before or at the oldest timestamp.
     double newestNeededVisionUpdateTimestamp = m_visionUpdates.floorKey(oldestOdometryTimestamp);
-    // Remove all entries strictly before the newest timestamp we need
+
+    // Step 4: Remove all entries strictly before the newest timestamp we need.
     m_visionUpdates.headMap(newestNeededVisionUpdateTimestamp, false).clear();
   }
 

wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc

@@ -65,28 +65,39 @@ Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
 template <typename WheelSpeeds, typename WheelPositions>
 std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
     units::second_t timestamp) const {
+  // Step 0: If there are no odometry updates to sample, skip.
   if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
-    // No odometry updates, so nothing to sample
     return std::nullopt;
   }
-  // Make sure timestamp matches the sample from the odometry pose buffer
+
+  // Step 1: Make sure timestamp matches the sample from the odometry pose
+  // buffer. (When sampling, the buffer will always use a timestamp
+  // between the first and last timestamps)
   units::second_t oldestOdometryTimestamp =
       m_odometryPoseBuffer.GetInternalBuffer().front().first;
   units::second_t newestOdometryTimestamp =
       m_odometryPoseBuffer.GetInternalBuffer().back().first;
   timestamp =
       std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
+
+  // Step 2: If there are no applicable vision updates, use the odometry-only
+  // information.
   if (m_visionUpdates.empty() || timestamp < m_visionUpdates.begin()->first) {
-    // No vision update from before the requested timestamp to apply
     return m_odometryPoseBuffer.Sample(timestamp);
   }
-  // Find first one past timestamp, then go back one
-  // Note that upper_bound() won't return begin() because we check begin()
-  // earlier
+
+  // Step 3: Get the latest vision update from before or at the timestamp to
+  // sample at.
+  // First, find the iterator past the sample timestamp, then go back one. Note
+  // that upper_bound() won't return begin() because we check begin() earlier.
   auto floorIter = m_visionUpdates.upper_bound(timestamp);
   --floorIter;
   auto visionUpdate = floorIter->second;
+
+  // Step 4: Get the pose measured by odometry at the time of the sample.
   auto odometryEstimate = m_odometryPoseBuffer.Sample(timestamp);
+
+  // Step 5: Apply the vision compensation to the odometry pose.
   // TODO Replace with std::optional::transform() in C++23
   if (odometryEstimate) {
     return visionUpdate.Compensate(*odometryEstimate);
@@ -96,26 +107,31 @@ std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
 
 template <typename WheelSpeeds, typename WheelPositions>
 void PoseEstimator<WheelSpeeds, WheelPositions>::CleanUpVisionUpdates() {
+  // Step 0: If there are no odometry samples, skip.
   if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
-    // No odometry updates, so no vision updates as well
     return;
   }
-  // Find the oldest timestamp that needs a vision update before or at it
+
+  // Step 1: Find the oldest timestamp that needs a vision update.
   units::second_t oldestOdometryTimestamp =
       m_odometryPoseBuffer.GetInternalBuffer().front().first;
+
+  // Step 2: If there are no vision updates before that timestamp, skip.
   if (m_visionUpdates.empty() ||
       oldestOdometryTimestamp < m_visionUpdates.begin()->first) {
-    // No vision updates before the oldest odometry update, so no entries to
-    // clear
     return;
   }
-  // Find first one past oldest odometry timestamp, then go back one
-  // Note that upper_bound() won't return begin() because we check begin()
-  // earlier
+
+  // Step 3: Find the newest vision update timestamp before or at the oldest
+  // timestamp.
+  // First, find the iterator past the oldest odometry timestamp, then go
+  // back one. Note that upper_bound() won't return begin() because we check
+  // begin() earlier.
   auto newestNeededVisionUpdate =
       m_visionUpdates.upper_bound(oldestOdometryTimestamp);
   --newestNeededVisionUpdate;
-  // Remove all entries strictly before the newest timestamp we need
+
+  // Step 4: Remove all entries strictly before the newest timestamp we need.
   m_visionUpdates.erase(m_visionUpdates.begin(), newestNeededVisionUpdate);
 }