Allow aprilrobotics to start without camera extrinsics
This makes it so that it doesn't crash-loop prior to having extrinsics.
Change-Id: Ib7f494d5b2ab1504a6bffa0ed7606097707dc47c
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/vision/vision_util.cc b/y2023/vision/vision_util.cc
index f4937e5..ca5ad89 100644
--- a/y2023/vision/vision_util.cc
+++ b/y2023/vision/vision_util.cc
@@ -18,11 +18,15 @@
LOG(FATAL) << ": Failed to find camera calibration for " << node_name;
}
-cv::Mat CameraExtrinsics(
+std::optional<cv::Mat> CameraExtrinsics(
const frc971::vision::calibration::CameraCalibration *camera_calibration) {
CHECK(!camera_calibration->has_turret_extrinsics())
<< "No turret on 2023 robot";
+ if (!camera_calibration->has_fixed_extrinsics()) {
+ return std::nullopt;
+ }
+ CHECK(camera_calibration->fixed_extrinsics()->has_data());
cv::Mat result(4, 4, CV_32F,
const_cast<void *>(static_cast<const void *>(
camera_calibration->fixed_extrinsics()->data()->data())));