Add pose error ratio filtering to localizer
Using same logic as target_mapping in the parent commit.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Iecdf3f672583e7b06f87fa1703cb701bebe91e39
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index 4bb07e2..efffca3 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -9,6 +9,9 @@
DEFINE_double(max_pose_error, 1e-6,
"Throw out target poses with a higher pose error than this");
+DEFINE_double(
+ max_pose_error_ratio, 0.4,
+ "Throw out target poses with a higher pose error ratio than this");
DEFINE_double(distortion_noise_scalar, 1.0,
"Scale the target pose distortion factor by this when computing "
"the noise.");
@@ -337,6 +340,11 @@
<< target.pose_error();
return RejectImage(camera_index, RejectionReason::HIGH_POSE_ERROR,
&builder);
+ } else if (target.pose_error_ratio() > FLAGS_max_pose_error_ratio) {
+ VLOG(1) << "Rejecting target due to high pose error ratio "
+ << target.pose_error_ratio();
+ return RejectImage(camera_index, RejectionReason::HIGH_POSE_ERROR_RATIO,
+ &builder);
}
double theta_at_capture = state_at_capture.value()(StateIdx::kTheta);
diff --git a/y2023/localizer/localizer_test.cc b/y2023/localizer/localizer_test.cc
index 7248b33..e8b7d56 100644
--- a/y2023/localizer/localizer_test.cc
+++ b/y2023/localizer/localizer_test.cc
@@ -155,6 +155,7 @@
target_builder.add_position(position_offset);
target_builder.add_orientation(quat_offset);
target_builder.add_pose_error(pose_error_);
+ target_builder.add_pose_error_ratio(pose_error_ratio_);
auto target_offset = target_builder.Finish();
auto targets_offset = builder.fbb()->CreateVector({target_offset});
@@ -277,6 +278,7 @@
uint64_t send_target_id_ = kTargetId;
double pose_error_ = 1e-7;
+ double pose_error_ratio_ = 0.1;
double implied_yaw_error_ = 0.0;
gflags::FlagSaver flag_saver_;
@@ -507,4 +509,27 @@
status_fetcher_->statistics()->Get(0)->total_candidates());
}
+// Tests that we correctly reject a detection with a high pose error ratio.
+TEST_F(LocalizerTest, HighPoseErrorRatio) {
+ output_voltages_ << 0.0, 0.0;
+ send_targets_ = true;
+ // Send the minimum pose error to be rejected
+ constexpr double kEps = 1e-9;
+ pose_error_ratio_ = 0.4 + 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_RATIO))
+ ->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 0ea779f..854603e 100644
--- a/y2023/localizer/status.fbs
+++ b/y2023/localizer/status.fbs
@@ -24,6 +24,8 @@
HIGH_DISTANCE_TO_TARGET = 6,
// The robot was travelling too fast; we don't trust the target.
ROBOT_TOO_FAST = 7,
+ // Pose estimation error ratio was higher than any normal detection.
+ HIGH_POSE_ERROR_RATIO = 8,
}
table RejectionCount {