Reject TargetPoses that imply a high yaw error
We trust the localizer's yaw estimate and any vision estimates
suggesting a very high yaw error are likely outliers.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I69bbd64f217b6feab963cd8280ee7378ed7f4e7a
diff --git a/y2023/localizer/localizer_test.cc b/y2023/localizer/localizer_test.cc
index 57b9dd0..c8c7b6b 100644
--- a/y2023/localizer/localizer_test.cc
+++ b/y2023/localizer/localizer_test.cc
@@ -123,7 +123,7 @@
const frc971::control_loops::Pose robot_pose(
{drivetrain_plant_.GetPosition().x(),
drivetrain_plant_.GetPosition().y(), 0.0},
- drivetrain_plant_.state()(2, 0));
+ drivetrain_plant_.state()(2, 0) + implied_yaw_error_);
const Eigen::Matrix<double, 4, 4> H_field_camera =
robot_pose.AsTransformationMatrix() * H_robot_camera;
@@ -275,6 +275,7 @@
uint64_t send_target_id_ = kTargetId;
double pose_error_ = 1e-7;
+ double implied_yaw_error_ = 0.0;
gflags::FlagSaver flag_saver_;
};
@@ -483,4 +484,25 @@
status_fetcher_->statistics()->Get(0)->total_candidates());
}
+// Tests that we correctly reject a detection with a high implied yaw error.
+TEST_F(LocalizerTest, HighImpliedYawError) {
+ output_voltages_ << 0.0, 0.0;
+ send_targets_ = true;
+ implied_yaw_error_ = 31.0 * M_PI / 180.0;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+ ASSERT_EQ(
+ status_fetcher_->statistics()
+ ->Get(0)
+ ->rejection_reasons()
+ ->Get(static_cast<size_t>(RejectionReason::HIGH_IMPLIED_YAW_ERROR))
+ ->count(),
+ status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
} // namespace y2023::localizer::testing