commit cafdd5d0bce6d91a6fb5f48edf73a15dad80033b
author milind-u <milind.upadhyay@gmail.com> Tue Mar 01 19:58:57 2022 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 06 14:30:35 2022 -0800
tree 6311be6f6293084351e676e5eaece3f85b8d3484
parent 8f38ad8622c6db16ad80b0d33c7ffc79bb8adc03

Use extrinsics to seed vision solver

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I674da5535fa3894468e66f11cbbc3e9d32d8cedf

y2022/vision/camera_reader.h

diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index 3b1eca6..edc3a1a 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -25,8 +25,6 @@
 using namespace frc971::vision;
 
 // TODO<jim>: Probably need to break out LED control to separate process
-// TODO<jim>: Need to add sync with camera to strobe lights
-
 class CameraReader {
  public:
   CameraReader(aos::ShmEventLoop *event_loop,
@@ -66,18 +64,20 @@
   void ReadImage();
 
   cv::Mat CameraIntrinsics() const {
-    const cv::Mat result(3, 3, CV_32F,
-                         const_cast<void *>(static_cast<const void *>(
-                             camera_calibration_->intrinsics()->data())));
+    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 CameraExtrinsics() const {
-    const cv::Mat result(
+    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;