commit 258e4eea45fbac0f839c3e653b57e9e2904eb005
author James Kuszmaul <jabukuszmaul@gmail.com> Thu Feb 23 14:22:30 2023 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Thu Feb 23 14:28:38 2023 -0800
tree 8a2b4911aba51f0614c624f2946f4f2cfee8f6e4
parent f645c7dcfc28cff2e898f0e9404558aa7309a0eb

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>

y2023/vision/aprilrobotics.h

diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 4477856..66b0aac 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -38,7 +38,7 @@
   std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
       cv::Mat image);
 
-  const cv::Mat extrinsics() const { return extrinsics_; }
+  const std::optional<cv::Mat> extrinsics() const { return extrinsics_; }
   const cv::Mat intrinsics() const { return intrinsics_; }
   const cv::Mat dist_coeffs() const { return dist_coeffs_; }
 
@@ -57,7 +57,7 @@
   const frc971::vision::calibration::CameraCalibration *calibration_;
   cv::Mat intrinsics_;
   cv::Mat projection_matrix_;
-  cv::Mat extrinsics_;
+  std::optional<cv::Mat> extrinsics_;
   cv::Mat dist_coeffs_;
 
   aos::Ftrace ftrace_;

y2023/vision/target_mapping.cc

diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 921c172..419bd38 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -86,7 +86,7 @@
   auto detector_ptr = std::make_unique<AprilRoboticsDetector>(
       detection_event_loop, kImageChannel);
   // Get the camera extrinsics
-  cv::Mat extrinsics_cv = detector_ptr->extrinsics();
+  cv::Mat extrinsics_cv = detector_ptr->extrinsics().value();
   Eigen::Matrix4d extrinsics_matrix;
   cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
   const auto extrinsics = Eigen::Affine3d(extrinsics_matrix);

y2023/vision/vision_util.cc

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())));

y2023/vision/vision_util.h

diff --git a/y2023/vision/vision_util.h b/y2023/vision/vision_util.h
index ce1a69d..79f5c92 100644
--- a/y2023/vision/vision_util.h
+++ b/y2023/vision/vision_util.h
@@ -10,7 +10,7 @@
 const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
     const y2023::Constants &calibration_data, std::string_view node_name);
 
-cv::Mat CameraExtrinsics(
+std::optional<cv::Mat> CameraExtrinsics(
     const frc971::vision::calibration::CameraCalibration *camera_calibration);
 
 cv::Mat CameraIntrinsics(