Don't access unverified optional in localizer
Change-Id: I3efc94d8a349470a4c49fc14f7fe62c1c78f4077
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index f352fd8..ba03398 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -310,10 +310,6 @@
const std::optional<State> state_at_capture =
ekf_.LastStateBeforeTime(capture_time);
- double theta_at_capture = state_at_capture.value()(StateIdx::kTheta);
- double camera_implied_theta = Z(Corrector::kTheta);
- constexpr double kDegToRad = M_PI / 180.0;
-
if (!state_at_capture.has_value()) {
VLOG(1) << "Rejecting image due to being too old.";
return RejectImage(camera_index, RejectionReason::IMAGE_TOO_OLD, &builder);
@@ -322,7 +318,13 @@
<< target.pose_error();
return RejectImage(camera_index, RejectionReason::HIGH_POSE_ERROR,
&builder);
- } else if (std::abs(camera_implied_theta - theta_at_capture) >
+ }
+
+ double theta_at_capture = state_at_capture.value()(StateIdx::kTheta);
+ double camera_implied_theta = Z(Corrector::kTheta);
+ constexpr double kDegToRad = M_PI / 180.0;
+
+ if (std::abs(camera_implied_theta - theta_at_capture) >
FLAGS_max_implied_yaw_error * kDegToRad) {
return RejectImage(camera_index, RejectionReason::HIGH_IMPLIED_YAW_ERROR,
&builder);