Reject target poses with high pose errors
We should rarely see estimates with pose errors above 1e-6.
Do this in localizer so we can log more poses.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I91f535f236a8d5f6d5430392cadcd449ea96047e
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index 4378f61..42dc4d8 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -3,9 +3,13 @@
#include "aos/containers/sized_array.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/pose.h"
+#include "gflags/gflags.h"
#include "y2023/constants.h"
#include "y2023/localizer/utils.h"
+DEFINE_double(max_pose_error, 1e-6,
+ "Throw out target poses with a higher pose error than this");
+
namespace y2023::localizer {
namespace {
constexpr std::array<std::string_view, Localizer::kNumCameras> kPisToUse{
@@ -37,7 +41,6 @@
}
} // namespace
-
std::array<Localizer::CameraState, Localizer::kNumCameras>
Localizer::MakeCameras(const Constants &constants, aos::EventLoop *event_loop) {
CHECK(constants.has_cameras());
@@ -293,6 +296,11 @@
if (!state_at_capture.has_value()) {
VLOG(1) << "Rejecting image due to being too old.";
return RejectImage(camera_index, RejectionReason::IMAGE_TOO_OLD, &builder);
+ } else if (target.pose_error() > FLAGS_max_pose_error) {
+ VLOG(1) << "Rejecting target due to high pose error "
+ << target.pose_error();
+ return RejectImage(camera_index, RejectionReason::HIGH_POSE_ERROR,
+ &builder);
}
const Input U = ekf_.MostRecentInput();
diff --git a/y2023/localizer/localizer_test.cc b/y2023/localizer/localizer_test.cc
index 554ab5d..947771f 100644
--- a/y2023/localizer/localizer_test.cc
+++ b/y2023/localizer/localizer_test.cc
@@ -152,6 +152,7 @@
target_builder.add_id(send_target_id_);
target_builder.add_position(position_offset);
target_builder.add_orientation(quat_offset);
+ target_builder.add_pose_error(pose_error_);
auto target_offset = target_builder.Finish();
auto targets_offset = builder.fbb()->CreateVector({target_offset});
@@ -273,6 +274,7 @@
std::unique_ptr<aos::logger::Logger> logger_;
uint64_t send_target_id_ = kTargetId;
+ double pose_error_ = 1e-7;
gflags::FlagSaver flag_saver_;
};
@@ -457,4 +459,26 @@
status_fetcher_->statistics()->Get(0)->total_candidates());
}
+// Tests that we correctly reject a detection with a high pose error.
+TEST_F(LocalizerTest, HighPoseError) {
+ output_voltages_ << 0.0, 0.0;
+ send_targets_ = true;
+ // Send the minimum pose error to be rejected
+ constexpr double kEps = 1e-9;
+ pose_error_ = 1e-6 + kEps;
+
+ 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_POSE_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 4d0a9c1..1b5c6f6 100644
--- a/y2023/localizer/status.fbs
+++ b/y2023/localizer/status.fbs
@@ -15,6 +15,8 @@
MESSAGE_BRIDGE_DISCONNECTED = 2,
// The target ID does not exist.
NO_SUCH_TARGET = 3,
+ // Pose estimation error was higher than any normal detection.
+ HIGH_POSE_ERROR = 4,
}
table RejectionCount {