Reject low-confidence targets in 2022 localizer
Change-Id: Iab8de619386d57a64c3a398333494a90069537df
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 2b3c72c..e7b6421 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -16,6 +16,9 @@
constexpr double kVisionTargetX = 0.0;
constexpr double kVisionTargetY = 0.0;
+// Minimum confidence to require to use a target match.
+constexpr double kMinTargetEstimateConfidence = 0.2;
+
template <int N>
Eigen::Matrix<double, N, 1> MakeState(std::vector<double> values) {
CHECK_EQ(static_cast<size_t>(N), values.size());
@@ -585,6 +588,12 @@
const y2022::vision::TargetEstimate *target, int camera_index) {
std::optional<RejectionReason> rejection_reason;
+ if (target->confidence() < kMinTargetEstimateConfidence) {
+ rejection_reason = RejectionReason::LOW_CONFIDENCE;
+ TallyRejection(rejection_reason.value());
+ return;
+ }
+
const OldPosition &state = GetStateForTime(sample_time);
Eigen::Matrix<double, 4, 4> H_field_camera_measured;
const std::optional<Eigen::Vector2d> measured_robot_position =