Take distortion into account for mapping noise
Again, following the localizers logic here, but with thresholds we can
tune separately.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I2b9f6f7cace2bf1de3468e3a627383452db863f2
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index e399470..9e52c20 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -76,6 +76,7 @@
ceres::examples::Pose3d target_pose_camera =
PoseUtils::Affine3dToPose3d(H_camera_target);
double distance_from_camera = target_pose_camera.p.norm();
+ double distortion_factor = target_pose_fbs->distortion_factor();
CHECK(map.has_monotonic_timestamp_ns())
<< "Need detection timestamps for mapping";
@@ -85,6 +86,7 @@
.time = pi_distributed_time,
.H_robot_target = H_robot_target,
.distance_from_camera = distance_from_camera,
+ .distortion_factor = distortion_factor,
.id = static_cast<TargetMapper::TargetId>(target_pose.id)});
}
}