Add pose estimation error to TargetMap
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I86104646bedd32124b85f6717b5445ff7e1d6136
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 1e5b2e8..a16986e 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -23,6 +23,13 @@
class AprilRoboticsDetector {
public:
+ // Aprilrobotics representation of a tag detection
+ struct Detection {
+ apriltag_detection_t det;
+ apriltag_pose_t pose;
+ double pose_error;
+ };
+
AprilRoboticsDetector(aos::EventLoop *event_loop,
std::string_view channel_name);
~AprilRoboticsDetector();
@@ -32,8 +39,8 @@
// Undistorts the april tag corners using the camera calibration
void UndistortDetection(apriltag_detection_t *det) const;
- std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
- cv::Mat image, aos::monotonic_clock::time_point eof);
+ std::vector<Detection> DetectTags(cv::Mat image,
+ aos::monotonic_clock::time_point eof);
const std::optional<cv::Mat> extrinsics() const { return extrinsics_; }
const cv::Mat intrinsics() const { return intrinsics_; }
@@ -43,8 +50,7 @@
void HandleImage(cv::Mat image, aos::monotonic_clock::time_point eof);
flatbuffers::Offset<frc971::vision::TargetPoseFbs> BuildTargetPose(
- const apriltag_pose_t &pose, const apriltag_detection_t &det,
- flatbuffers::FlatBufferBuilder *fbb);
+ const Detection &detection, flatbuffers::FlatBufferBuilder *fbb);
apriltag_family_t *tag_family_;
apriltag_detector_t *tag_detector_;