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.cc b/y2023/vision/vision_util.cc
index eed315a..f4937e5 100644
--- a/y2023/vision/vision_util.cc
+++ b/y2023/vision/vision_util.cc
@@ -3,6 +3,7 @@
#include "glog/logging.h"
namespace y2023::vision {
+
const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
const y2023::Constants &calibration_data, std::string_view node_name) {
CHECK(calibration_data.has_cameras());
@@ -16,4 +17,40 @@
}
LOG(FATAL) << ": Failed to find camera calibration for " << node_name;
}
+
+cv::Mat CameraExtrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration) {
+ CHECK(!camera_calibration->has_turret_extrinsics())
+ << "No turret on 2023 robot";
+
+ cv::Mat result(4, 4, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->fixed_extrinsics()->data()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(),
+ camera_calibration->fixed_extrinsics()->data()->size());
+
+ return result;
+}
+
+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;
+}
+
+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;
+}
+
} // namespace y2023::vision