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 =
diff --git a/y2022/localizer/localizer_status.fbs b/y2022/localizer/localizer_status.fbs
index 8be770e..ae96b63 100644
--- a/y2022/localizer/localizer_status.fbs
+++ b/y2022/localizer/localizer_status.fbs
@@ -7,6 +7,7 @@
NO_CALIBRATION = 1,
TURRET_TOO_FAST = 2,
MESSAGE_BRIDGE_DISCONNECTED = 3,
+ LOW_CONFIDENCE = 4,
}
table CumulativeStatistics {
diff --git a/y2022/localizer/localizer_test.cc b/y2022/localizer/localizer_test.cc
index 3c11dff..791adf3 100644
--- a/y2022/localizer/localizer_test.cc
+++ b/y2022/localizer/localizer_test.cc
@@ -468,6 +468,8 @@
estimate->camera_calibration->turret_extrinsics->data =
MatrixToVector(CameraTurretTransformation());
+ estimate->confidence = 1.0;
+
auto builder = target_sender_.MakeBuilder();
builder.CheckOk(
builder.Send(TargetEstimate::Pack(*builder.fbb(), estimate.get())));