Scale detection noise by distortion factor
That way, we treat detections that were affected by distortion more with
less value, and hopefully don't have as many conflicting pose estimates.
NOTE: localizer will probably have to be retuned since the noises will
be scaled differently.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I7cc7d1188797ed2d3c7cf096fe1ce56abc58b3c4
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index 42dc4d8..ace3663 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(distortion_noise_scalar, 1.0,
+ "Scale the target pose distortion factor by this when computing "
+ "the noise.");
namespace y2023::localizer {
namespace {
@@ -281,6 +284,8 @@
// TODO(james): Tune this. Also, gain schedule for auto mode?
Eigen::Matrix<double, 3, 1> noises(1.0, 1.0, 0.5);
+ // Scale noise by the distortion factor for this detection
+ noises *= (1.0 + FLAGS_distortion_noise_scalar * target.distortion_factor());
Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
R.diagonal() = noises.cwiseAbs2();