commit f2a4e32739d75564ba17a19da8a928bd4a4f81dc
author milind-u <milind.upadhyay@gmail.com> Wed Feb 01 19:33:10 2023 -0800
committer milind-u <milind.upadhyay@gmail.com> Sat Feb 11 15:47:34 2023 -0800
tree 20ad3d5ac2a5a6e7dc75612ca176678ef2a75650
parent 9cf2c3ea4c751809acc43b0779e070f2e040047f

Undistort april detection corners

To have more accurate pose estimates (especially when the
april tag is in the corners of the image), we need to account for
distortion. Undistorting just the detected corners is the most efficient
way to do this.

Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ic3872dee1e6fef060b49260e6abc22498f76a353

y2023/vision/aprilrobotics.h

diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 8c2c0ad..4477856 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -5,6 +5,7 @@
 #include "aos/events/shm_event_loop.h"
 #include "aos/network/team_number.h"
 #include "aos/realtime.h"
+#include "frc971/constants/constants_sender_lib.h"
 #include "frc971/vision/calibration_generated.h"
 #include "frc971/vision/charuco_lib.h"
 #include "frc971/vision/target_map_generated.h"
@@ -15,9 +16,8 @@
 #include "third_party/apriltag/apriltag.h"
 #include "third_party/apriltag/apriltag_pose.h"
 #include "third_party/apriltag/tag16h5.h"
-#include "y2023/vision/april_debug_generated.h"
 #include "y2023/constants/constants_generated.h"
-#include "frc971/constants/constants_sender_lib.h"
+#include "y2023/vision/april_debug_generated.h"
 
 DECLARE_int32(team_number);
 
@@ -28,14 +28,20 @@
  public:
   AprilRoboticsDetector(aos::EventLoop *event_loop,
                         std::string_view channel_name);
-
   ~AprilRoboticsDetector();
 
   void SetWorkerpoolAffinities();
 
+  // 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);
 
+  const cv::Mat extrinsics() const { return extrinsics_; }
+  const cv::Mat intrinsics() const { return intrinsics_; }
+  const cv::Mat dist_coeffs() const { return dist_coeffs_; }
+
  private:
   void HandleImage(cv::Mat image, aos::monotonic_clock::time_point eof);
 
@@ -44,36 +50,15 @@
       frc971::vision::TargetMapper::TargetId target_id,
       flatbuffers::FlatBufferBuilder *fbb);
 
-
-  static cv::Mat CameraIntrinsics(
-      const frc971::vision::calibration::CameraCalibration
-          *camera_calibration) {
-    cv::Mat result(3, 3, CV_32F,
-                   const_cast<void *>(static_cast<const void *>(
-                       camera_calibration->intrinsics()->data())));
-    result.convertTo(result, CV_64F);
-    CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
-
-    return result;
-  }
-
-  static cv::Mat CameraDistCoeffs(
-      const frc971::vision::calibration::CameraCalibration
-          *camera_calibration) {
-    const cv::Mat result(5, 1, CV_32F,
-                         const_cast<void *>(static_cast<const void *>(
-                             camera_calibration->dist_coeffs()->data())));
-    CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
-    return result;
-  }
-
   apriltag_family_t *tag_family_;
   apriltag_detector_t *tag_detector_;
 
   const frc971::constants::ConstantsFetcher<Constants> calibration_data_;
   const frc971::vision::calibration::CameraCalibration *calibration_;
   cv::Mat intrinsics_;
-  cv::Mat camera_distortion_coeffs_;
+  cv::Mat projection_matrix_;
+  cv::Mat extrinsics_;
+  cv::Mat dist_coeffs_;
 
   aos::Ftrace ftrace_;