Use correct extrinsics in viewer
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I91717d094b92a1b06bf86917e45fba8385072651
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index f847dbd..f99cf97 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -203,10 +203,13 @@
cv::Mat intrinsics;
intrinsics_float.convertTo(intrinsics, CV_64F);
- const auto extrinsics_float =
- cv::Mat(4, 4, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- calibration->fixed_extrinsics()->data()->data())));
+ const frc971::vision::calibration::TransformationMatrix *transform =
+ calibration->has_turret_extrinsics() ? calibration->turret_extrinsics()
+ : calibration->fixed_extrinsics();
+
+ const auto extrinsics_float = cv::Mat(
+ 4, 4, CV_32F,
+ const_cast<void *>(static_cast<const void *>(transform->data()->data())));
cv::Mat extrinsics;
extrinsics_float.convertTo(extrinsics, CV_64F);