commit 81420a2f05775a4bf72c0afec8eee20cb199eabb
author Milind Upadhyay <milind.upadhyay@gmail.com> Wed Dec 14 21:13:06 2022 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sat Dec 17 12:10:29 2022 -0700
tree a208911e6f9b0e2b3007a699651234cb2a37b08d
parent cf757776de9d711f44b3c50b33227caa9155911b

Skip target detections before first robot pose

Previously, we would segfault if any detections came before the first
robot pose.

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Ic5b4b7d64f7de2a89e677ccf19f214a8799f31e2

frc971/vision/target_mapper.cc

diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 5a7b400..b4b1a87 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -105,6 +105,12 @@
   auto robot_pose_it = timestamped_robot_poses.begin();
   for (const auto &timestamped_detection : timestamped_target_detections) {
     aos::distributed_clock::time_point target_time = timestamped_detection.time;
+
+    // Skip this target detection if we have no robot poses before it
+    if (robot_pose_it->time > target_time) {
+      continue;
+    }
+
     // Find the robot point right before this localization
     while (robot_pose_it->time > target_time ||
            (robot_pose_it + 1)->time <= target_time) {
@@ -119,6 +125,11 @@
                                InterpolatePose(*start, *end, target_time));
   }
 
+  // In the case that all target detections were before the first robot
+  // detection, we would have no interpolated poses at this point
+  CHECK_GT(interpolated_poses.size(), 0ul)
+      << "Need a robot pose before and after every target detection";
+
   // Match consecutive detections
   std::vector<ceres::examples::Constraint2d> target_constraints;
   std::vector<ceres::examples::Pose2d> robot_delta_poses;