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
diff --git a/y2023/vision/vision_util.h b/y2023/vision/vision_util.h
index 58f9a7f..ce1a69d 100644
--- a/y2023/vision/vision_util.h
+++ b/y2023/vision/vision_util.h
@@ -2,10 +2,23 @@
#define Y2023_VISION_VISION_UTIL_H_
#include <string_view>
+#include "opencv2/imgproc.hpp"
#include "y2023/constants/constants_generated.h"
+
namespace y2023::vision {
const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
const y2023::Constants &calibration_data, std::string_view node_name);
-}
+
+cv::Mat CameraExtrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration);
+
+cv::Mat CameraIntrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration);
+
+cv::Mat CameraDistCoeffs(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration);
+
+} // namespace y2023::vision
+
#endif // Y2023_VISION_VISION_UTIL_H_