Add pose error ratio to target poses
This lets us filter out estimates that had a secondary pose
estimate which was almost as good as the best one. We should
throw out such estimates, they have been causing one tag-based
implied robot position that is completely different from the others.
Adding this filter to target_mapping prevents jumping implied robot
positions, at least at distances from the charge station and closer.
Also makes it so that all our HP-based estimates are accurate.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I11e42a4886b8401eeb6f57cf8e547ce3a8ff90b7
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 66d82a1..01e3138 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -30,6 +30,7 @@
apriltag_pose_t pose;
double pose_error;
double distortion_factor;
+ double pose_error_ratio;
};
struct DetectionResult {
@@ -43,6 +44,10 @@
void SetWorkerpoolAffinities();
+ // Deletes the heap-allocated rotation and translation pointers in the given
+ // pose
+ void DestroyPose(apriltag_pose_t *pose) const;
+
// Undistorts the april tag corners using the camera calibration
void UndistortDetection(apriltag_detection_t *det) const;