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.cc b/y2023/localizer/localizer.cc
index ffc1df3..d7ca4f9 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -12,6 +12,9 @@
DEFINE_double(distortion_noise_scalar, 1.0,
"Scale the target pose distortion factor by this when computing "
"the noise.");
+DEFINE_double(max_implied_yaw_error, 30.0,
+ "Reject target poses that imply a robot yaw of this many degrees "
+ "off from our estimate.");
namespace y2023::localizer {
namespace {
@@ -307,6 +310,15 @@
<< target.pose_error();
return RejectImage(camera_index, RejectionReason::HIGH_POSE_ERROR,
&builder);
+ } else {
+ 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);
+ }
}
const Input U = ekf_.MostRecentInput();
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
diff --git a/y2023/localizer/status.fbs b/y2023/localizer/status.fbs
index 1b5c6f6..7f97fc7 100644
--- a/y2023/localizer/status.fbs
+++ b/y2023/localizer/status.fbs
@@ -17,6 +17,8 @@
NO_SUCH_TARGET = 3,
// Pose estimation error was higher than any normal detection.
HIGH_POSE_ERROR = 4,
+ // Pose estimate implied a robot yaw far off from our estimate.
+ HIGH_IMPLIED_YAW_ERROR = 5,
}
table RejectionCount {