Allow opencv8 distortion model in PhotonCamera (#1317)

We previously assumed only OpenCV5 but mrcal uses opencv8
2026-07-04 03:11:40 +00:00 · 2024-05-29 17:28:35 -04:00
parent fcca858a37
commit 19b4802094
17 changed files with 172 additions and 94 deletions
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -121,7 +121,7 @@ jobs:
          path: build/html
  build-photonlib-host:
    env:
-      MACOSX_DEPLOYMENT_TARGET: 12
+      MACOSX_DEPLOYMENT_TARGET: 13
    strategy:
      fail-fast: false
      matrix:
@@ -129,9 +129,9 @@ jobs:
          - os: windows-2022
            artifact-name: Win64
            architecture: x64
-          - os: macos-12
+          - os: macos-14
            artifact-name: macOS
-            architecture: x64
+            architecture: aarch64
          - os: ubuntu-22.04
            artifact-name: Linux

@@ -146,6 +146,7 @@ jobs:
        with:
          java-version: 17
          distribution: temurin
+          architecture: ${{ matrix.architecture }}
      - run: git fetch --tags --force
      - run: |
          chmod +x gradlew
--- a/build.gradle
+++ b/build.gradle
@@ -109,3 +109,17 @@ wrapper {
 ext.getCurrentArch = {
    return NativePlatforms.desktop
 }
+
+subprojects {
+    tasks.withType(JavaCompile) {
+        options.compilerArgs.add '-XDstringConcat=inline'
+        options.encoding = 'UTF-8'
+    }
+
+    // Enables UTF-8 support in Javadoc
+    tasks.withType(Javadoc) {
+        options.addStringOption("charset", "utf-8")
+        options.addStringOption("docencoding", "utf-8")
+        options.addStringOption("encoding", "utf-8")
+    }
+}
--- a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java
+++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java
@@ -52,8 +52,7 @@ public class TargetCalculations {

        if (cameraCal != null) {
            // undistort
-            MatOfPoint2f temp = new MatOfPoint2f();
-            temp.fromArray(new Point(targetCenterX, targetCenterY));
+            MatOfPoint2f temp = new MatOfPoint2f(new Point(targetCenterX, targetCenterY));
            // Tighten up termination criteria
            var termCriteria = new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, 1e-6);
            Calib3d.undistortImagePoints(
@@ -68,7 +67,7 @@ public class TargetCalculations {

            // if outside of the imager, convergence fails, or really really bad user camera cal,
            // undistort will fail by giving us nans. at some point we should log this failure
-            // if we can't undistort, don't change the cnter location
+            // if we can't undistort, don't change the center location
            if (Float.isFinite(buff[0]) && Float.isFinite(buff[1])) {
                targetCenterX = buff[0];
                targetCenterY = buff[1];
--- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
@@ -46,6 +46,7 @@ import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Timer;
+import java.util.Arrays;
 import java.util.List;
 import java.util.Optional;
 import java.util.stream.Collectors;
@@ -313,10 +314,19 @@ public class PhotonCamera implements AutoCloseable {
        } else return Optional.empty();
    }

-    public Optional<Matrix<N5, N1>> getDistCoeffs() {
+    /**
+     * The camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms are set
+     * to 0
+     */
+    public Optional<Matrix<N8, N1>> getDistCoeffs() {
        var distCoeffs = cameraDistortionSubscriber.get();
-        if (distCoeffs != null && distCoeffs.length == 5) {
-            return Optional.of(MatBuilder.fill(Nat.N5(), Nat.N1(), distCoeffs));
+        if (distCoeffs != null && distCoeffs.length <= 8) {
+            // Copy into array of length 8, and explicitly null higher order terms out
+            double[] data = new double[8];
+            Arrays.fill(data, 0);
+            System.arraycopy(distCoeffs, 0, data, 0, distCoeffs.length);
+
+            return Optional.of(MatBuilder.fill(Nat.N8(), Nat.N1(), data));
        } else return Optional.empty();
    }

--- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -36,7 +36,7 @@ import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.numbers.N8;
 import edu.wpi.first.wpilibj.DriverStation;
 import java.util.ArrayList;
 import java.util.HashSet;
@@ -348,7 +348,7 @@ public class PhotonPoseEstimator {
    public Optional<EstimatedRobotPose> update(
            PhotonPipelineResult cameraResult,
            Optional<Matrix<N3, N3>> cameraMatrix,
-            Optional<Matrix<N5, N1>> distCoeffs) {
+            Optional<Matrix<N8, N1>> distCoeffs) {
        // Time in the past -- give up, since the following if expects times > 0
        if (cameraResult.getTimestampSeconds() < 0) {
            return Optional.empty();
@@ -376,7 +376,7 @@ public class PhotonPoseEstimator {
    private Optional<EstimatedRobotPose> update(
            PhotonPipelineResult cameraResult,
            Optional<Matrix<N3, N3>> cameraMatrix,
-            Optional<Matrix<N5, N1>> distCoeffs,
+            Optional<Matrix<N8, N1>> distCoeffs,
            PoseStrategy strat) {
        Optional<EstimatedRobotPose> estimatedPose;
        switch (strat) {
@@ -418,7 +418,7 @@ public class PhotonPoseEstimator {
    private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
            PhotonPipelineResult result,
            Optional<Matrix<N3, N3>> cameraMatrixOpt,
-            Optional<Matrix<N5, N1>> distCoeffsOpt) {
+            Optional<Matrix<N8, N1>> distCoeffsOpt) {
        if (result.getMultiTagResult().estimatedPose.isPresent) {
            var best_tf = result.getMultiTagResult().estimatedPose.best;
            var best =
@@ -440,7 +440,7 @@ public class PhotonPoseEstimator {
    private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
            PhotonPipelineResult result,
            Optional<Matrix<N3, N3>> cameraMatrixOpt,
-            Optional<Matrix<N5, N1>> distCoeffsOpt) {
+            Optional<Matrix<N8, N1>> distCoeffsOpt) {
        boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
        // cannot run multitagPNP, use fallback strategy
        if (!hasCalibData || result.getTargets().size() < 2) {
--- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java
+++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java
@@ -41,6 +41,7 @@ import edu.wpi.first.wpilibj.DriverStation;
 import java.io.IOException;
 import java.nio.file.Path;
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 import java.util.Random;
 import org.ejml.data.DMatrix3;
@@ -71,7 +72,7 @@ public class SimCameraProperties {
    private int resWidth;
    private int resHeight;
    private Matrix<N3, N3> camIntrinsics;
-    private Matrix<N5, N1> distCoeffs;
+    private Matrix<N8, N1> distCoeffs;
    private double avgErrorPx;
    private double errorStdDevPx;
    // performance
@@ -132,7 +133,8 @@ public class SimCameraProperties {
                    jsonIntrinsics[j] = jsonIntrinsicsNode.get(j).asDouble();
                }
                var jsonDistortNode = calib.get("distCoeffs").get("data");
-                double[] jsonDistortion = new double[jsonDistortNode.size()];
+                double[] jsonDistortion = new double[8];
+                Arrays.fill(jsonDistortion, 0);
                for (int j = 0; j < jsonDistortNode.size(); j++) {
                    jsonDistortion[j] = jsonDistortNode.get(j).asDouble();
                }
@@ -148,7 +150,7 @@ public class SimCameraProperties {
                        jsonWidth,
                        jsonHeight,
                        MatBuilder.fill(Nat.N3(), Nat.N3(), jsonIntrinsics),
-                        MatBuilder.fill(Nat.N5(), Nat.N1(), jsonDistortion));
+                        MatBuilder.fill(Nat.N8(), Nat.N1(), jsonDistortion));
                setCalibError(jsonAvgError, jsonErrorStdDev);
                success = true;
            }
@@ -174,7 +176,7 @@ public class SimCameraProperties {
        var fovHeight = new Rotation2d(Math.atan(diagRatio * (resHeight / resDiag)) * 2);

        // assume no distortion
-        var distCoeff = VecBuilder.fill(0, 0, 0, 0, 0);
+        var distCoeff = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);

        // assume centered principal point (pixels)
        double cx = resWidth / 2.0 - 0.5;
@@ -190,7 +192,7 @@ public class SimCameraProperties {
    }

    public void setCalibration(
-            int resWidth, int resHeight, Matrix<N3, N3> camIntrinsics, Matrix<N5, N1> distCoeffs) {
+            int resWidth, int resHeight, Matrix<N3, N3> camIntrinsics, Matrix<N8, N1> distCoeffs) {
        this.resWidth = resWidth;
        this.resHeight = resHeight;
        this.camIntrinsics = camIntrinsics;
@@ -280,7 +282,7 @@ public class SimCameraProperties {
        return camIntrinsics.copy();
    }

-    public Vector<N5> getDistCoeffs() {
+    public Vector<N8> getDistCoeffs() {
        return new Vector<>(distCoeffs);
    }

@@ -616,7 +618,10 @@ public class SimCameraProperties {
                        -0.9166265114485799,
                        0.0019519890627236526,
                        -0.0036071725380870333,
-                        1.5627234622420942));
+                        1.5627234622420942,
+                        0,
+                        0,
+                        0));
        prop.setCalibError(0.21, 0.0124);
        prop.setFPS(30);
        prop.setAvgLatencyMs(30);
@@ -647,7 +652,10 @@ public class SimCameraProperties {
                        -1.2350335805796528,
                        0.0024990767286192732,
                        -0.0026958287600230705,
-                        2.2951386729115537));
+                        2.2951386729115537,
+                        0,
+                        0,
+                        0));
        prop.setCalibError(0.26, 0.046);
        prop.setFPS(15);
        prop.setAvgLatencyMs(65);
@@ -677,7 +685,10 @@ public class SimCameraProperties {
                        -0.5142936883324216,
                        0.012461562046896614,
                        0.0014084973492408186,
-                        0.35160648971214437));
+                        0.35160648971214437,
+                        0,
+                        0,
+                        0));
        prop.setCalibError(0.25, 0.05);
        prop.setFPS(15);
        prop.setAvgLatencyMs(35);
@@ -708,7 +719,10 @@ public class SimCameraProperties {
                        -0.49903003669627627,
                        0.007468423590519429,
                        0.002496885298683693,
-                        0.3443122090208624));
+                        0.3443122090208624,
+                        0,
+                        0,
+                        0));
        prop.setCalibError(0.35, 0.10);
        prop.setFPS(10);
        prop.setAvgLatencyMs(50);
@@ -739,7 +753,10 @@ public class SimCameraProperties {
                        -0.2904345656989261,
                        8.32475714507539E-4,
                        -3.694397782014239E-4,
-                        0.09487962227027584));
+                        0.09487962227027584,
+                        0,
+                        0,
+                        0));
        prop.setCalibError(0.37, 0.06);
        prop.setFPS(7);
        prop.setAvgLatencyMs(60);
--- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
+++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
@@ -159,20 +159,6 @@ LEDMode PhotonCamera::GetLEDMode() const {
  return static_cast<LEDMode>(static_cast<int>(ledModeSub.Get()));
 }

-std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
-  auto camCoeffs = cameraIntrinsicsSubscriber.Get();
-  if (camCoeffs.size() == 9) {
-    cv::Mat retVal(3, 3, CV_64FC1);
-    for (int i = 0; i < 3; i++) {
-      for (int j = 0; j < 3; j++) {
-        retVal.at<double>(i, j) = camCoeffs[(j * 3) + i];
-      }
-    }
-    return retVal;
-  }
-  return std::nullopt;
-}
-
 void PhotonCamera::SetLEDMode(LEDMode mode) {
  ledModePub.Set(static_cast<int>(mode));
 }
@@ -181,13 +167,26 @@ const std::string_view PhotonCamera::GetCameraName() const {
  return cameraName;
 }

-std::optional<cv::Mat> PhotonCamera::GetDistCoeffs() {
+std::optional<PhotonCamera::CameraMatrix> PhotonCamera::GetCameraMatrix() {
+  auto camCoeffs = cameraIntrinsicsSubscriber.Get();
+  if (camCoeffs.size() == 9) {
+    PhotonCamera::CameraMatrix retVal =
+        Eigen::Map<const PhotonCamera::CameraMatrix>(camCoeffs.data());
+    return retVal;
+  }
+  return std::nullopt;
+}
+
+std::optional<PhotonCamera::DistortionMatrix> PhotonCamera::GetDistCoeffs() {
  auto distCoeffs = cameraDistortionSubscriber.Get();
-  if (distCoeffs.size() == 5) {
-    cv::Mat retVal(5, 1, CV_64FC1);
-    for (int i = 0; i < 5; i++) {
-      retVal.at<double>(i, 0) = distCoeffs[i];
-    }
+  auto bound = distCoeffs.size();
+  if (bound > 0 && bound <= 8) {
+    PhotonCamera::DistortionMatrix retVal =
+        PhotonCamera::DistortionMatrix::Zero();
+
+    Eigen::Map<const Eigen::VectorXd> map(distCoeffs.data(), bound);
+    retVal.block(0, 0, bound, 1) = map;
+
    return retVal;
  }
  return std::nullopt;
--- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
@@ -50,6 +50,9 @@
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"

+#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
+#include <opencv2/core/eigen.hpp>
+
 namespace photon {

 namespace detail {
@@ -126,8 +129,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
 }

 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
-    const PhotonPipelineResult& result, std::optional<cv::Mat> cameraMatrixData,
-    std::optional<cv::Mat> cameraDistCoeffs) {
+    const PhotonPipelineResult& result,
+    std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
+    std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs) {
  // Time in the past -- give up, since the following if expects times > 0
  if (result.GetTimestamp() < 0_s) {
    return std::nullopt;
@@ -152,8 +156,10 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
 }

 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
-    PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
-    std::optional<cv::Mat> cameraDistCoeffs, PoseStrategy strategy) {
+    const PhotonPipelineResult& result,
+    std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
+    std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs,
+    PoseStrategy strategy) {
  std::optional<EstimatedRobotPose> ret = std::nullopt;

  switch (strategy) {
@@ -371,8 +377,9 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
 }

 std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
-    PhotonPipelineResult result, std::optional<cv::Mat> camMat,
-    std::optional<cv::Mat> distCoeffs) {
+    PhotonPipelineResult result,
+    std::optional<PhotonCamera::CameraMatrix> camMat,
+    std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
  if (result.MultiTagResult().result.isPresent) {
    const auto field2camera = result.MultiTagResult().result.best;

@@ -388,8 +395,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
 }

 std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
-    PhotonPipelineResult result, std::optional<cv::Mat> camMat,
-    std::optional<cv::Mat> distCoeffs) {
+    PhotonPipelineResult result,
+    std::optional<PhotonCamera::CameraMatrix> camMat,
+    std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
  using namespace frc;

  // Need at least 2 targets
@@ -433,8 +441,15 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
  cv::Mat const rvec(3, 1, cv::DataType<double>::type);
  cv::Mat const tvec(3, 1, cv::DataType<double>::type);

-  cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(),
-               rvec, tvec, false, cv::SOLVEPNP_SQPNP);
+  {
+    cv::Mat cameraMatCV(camMat->rows(), camMat->cols(), CV_64F);
+    cv::eigen2cv(*camMat, cameraMatCV);
+    cv::Mat distCoeffsMatCV(distCoeffs->rows(), distCoeffs->cols(), CV_64F);
+    cv::eigen2cv(*distCoeffs, distCoeffsMatCV);
+
+    cv::solvePnP(objectPoints, imagePoints, cameraMatCV, distCoeffsMatCV, rvec,
+                 tvec, false, cv::SOLVEPNP_SQPNP);
+  }

  const Pose3d pose = detail::ToPose3d(tvec, rvec);

--- a/photon-lib/src/main/native/include/photon/PhotonCamera.h
+++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h
@@ -149,8 +149,23 @@ class PhotonCamera {
   */
  const std::string_view GetCameraName() const;

-  std::optional<cv::Mat> GetCameraMatrix();
-  std::optional<cv::Mat> GetDistCoeffs();
+  using CameraMatrix = Eigen::Matrix<double, 3, 3>;
+  using DistortionMatrix = Eigen::Matrix<double, 8, 1>;
+
+  /**
+   * @brief Get the camera calibration matrix, in standard OpenCV form
+   *
+   * @return std::optional<cv::Mat>
+   */
+  std::optional<CameraMatrix> GetCameraMatrix();
+
+  /**
+   * @brief Get the camera calibration distortion coefficients, in OPENCV8 form.
+   * Higher order terms are set to zero.
+   *
+   * @return std::optional<cv::Mat>
+   */
+  std::optional<DistortionMatrix> GetDistCoeffs();

  static void SetVersionCheckEnabled(bool enabled);

--- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
+++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
@@ -213,8 +213,8 @@ class PhotonPoseEstimator {
   */
  std::optional<EstimatedRobotPose> Update(
      const PhotonPipelineResult& result,
-      std::optional<cv::Mat> cameraMatrixData,
-      std::optional<cv::Mat> coeffsData);
+      std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
+      std::optional<PhotonCamera::DistortionMatrix> coeffsData);

  inline std::shared_ptr<PhotonCamera> GetCamera() { return camera; }

@@ -236,8 +236,10 @@ class PhotonPoseEstimator {
  inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }

  std::optional<EstimatedRobotPose> Update(
-      PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
-      std::optional<cv::Mat> coeffsData, PoseStrategy strategy);
+      const PhotonPipelineResult& result,
+      std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
+      std::optional<PhotonCamera::DistortionMatrix> coeffsData,
+      PoseStrategy strategy);

  /**
   * Return the estimated position of the robot with the lowest position
@@ -278,8 +280,9 @@ class PhotonPoseEstimator {
   * @return the estimated position of the robot in the FCS
   */
  std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
-      PhotonPipelineResult result, std::optional<cv::Mat> camMat,
-      std::optional<cv::Mat> distCoeffs);
+      PhotonPipelineResult result,
+      std::optional<PhotonCamera::CameraMatrix> camMat,
+      std::optional<PhotonCamera::DistortionMatrix> distCoeffs);

  /**
   * Return the pose calculation using all targets in view in the same PNP()
@@ -289,8 +292,9 @@ class PhotonPoseEstimator {
   timestamp of this estimation.
   */
  std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
-      PhotonPipelineResult result, std::optional<cv::Mat> camMat,
-      std::optional<cv::Mat> distCoeffs);
+      PhotonPipelineResult result,
+      std::optional<PhotonCamera::CameraMatrix> camMat,
+      std::optional<PhotonCamera::DistortionMatrix> distCoeffs);

  /**
   * Return the average of the best target poses using ambiguity as weight.
--- a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h
+++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h
@@ -60,8 +60,8 @@ class SimCameraProperties {
    frc::Rotation2d fovHeight{
        units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}};

-    Eigen::Matrix<double, 5, 1> newDistCoeffs;
-    newDistCoeffs << 0, 0, 0, 0, 0;
+    Eigen::Matrix<double, 8, 1> newDistCoeffs =
+        Eigen::Matrix<double, 8, 1>::Zero();

    double cx = width / 2.0 - 0.5;
    double cy = height / 2.0 - 0.5;
@@ -76,7 +76,7 @@ class SimCameraProperties {

  void SetCalibration(int width, int height,
                      const Eigen::Matrix<double, 3, 3>& newCamIntrinsics,
-                      const Eigen::Matrix<double, 5, 1>& newDistCoeffs) {
+                      const Eigen::Matrix<double, 8, 1>& newDistCoeffs) {
    resWidth = width;
    resHeight = height;
    camIntrinsics = newCamIntrinsics;
@@ -151,7 +151,7 @@ class SimCameraProperties {

  Eigen::Matrix<double, 3, 3> GetIntrinsics() const { return camIntrinsics; }

-  Eigen::Matrix<double, 5, 1> GetDistCoeffs() const { return distCoeffs; }
+  Eigen::Matrix<double, 8, 1> GetDistCoeffs() const { return distCoeffs; }

  units::hertz_t GetFPS() const { return 1 / frameSpeed; }

@@ -377,9 +377,9 @@ class SimCameraProperties {
        (Eigen::MatrixXd(3, 3) << 328.2733242048587, 0.0, 164.8190261141906,
         0.0, 318.0609794305216, 123.8633838438093, 0.0, 0.0, 1.0)
            .finished(),
-        Eigen::Matrix<double, 5, 1>{
+        Eigen::Matrix<double, 8, 1>{
            0.09957946553445934, -0.9166265114485799, 0.0019519890627236526,
-            -0.0036071725380870333, 1.5627234622420942});
+            -0.0036071725380870333, 1.5627234622420942, 0, 0, 0});
    prop.SetCalibError(0.21, 0.0124);
    prop.SetFPS(30_Hz);
    prop.SetAvgLatency(30_ms);
@@ -394,9 +394,9 @@ class SimCameraProperties {
        (Eigen::MatrixXd(3, 3) << 669.1428078983059, 0.0, 322.53377249329213,
         0.0, 646.9843137061716, 241.26567383784163, 0.0, 0.0, 1.0)
            .finished(),
-        Eigen::Matrix<double, 5, 1>{
+        Eigen::Matrix<double, 8, 1>{
            0.12788470750464645, -1.2350335805796528, 0.0024990767286192732,
-            -0.0026958287600230705, 2.2951386729115537});
+            -0.0026958287600230705, 2.2951386729115537, 0, 0, 0});
    prop.SetCalibError(0.26, 0.046);
    prop.SetFPS(15_Hz);
    prop.SetAvgLatency(65_ms);
@@ -411,9 +411,9 @@ class SimCameraProperties {
        (Eigen::MatrixXd(3, 3) << 511.22843367007755, 0.0, 323.62049380211096,
         0.0, 514.5452336723849, 261.8827920543568, 0.0, 0.0, 1.0)
            .finished(),
-        Eigen::Matrix<double, 5, 1>{0.1917469998873756, -0.5142936883324216,
+        Eigen::Matrix<double, 8, 1>{0.1917469998873756, -0.5142936883324216,
                                    0.012461562046896614, 0.0014084973492408186,
-                                    0.35160648971214437});
+                                    0.35160648971214437, 0, 0, 0});
    prop.SetCalibError(0.25, 0.05);
    prop.SetFPS(15_Hz);
    prop.SetAvgLatency(35_ms);
@@ -428,9 +428,9 @@ class SimCameraProperties {
        (Eigen::MatrixXd(3, 3) << 769.6873145148892, 0.0, 486.1096609458122,
         0.0, 773.8164483705323, 384.66071662358354, 0.0, 0.0, 1.0)
            .finished(),
-        Eigen::Matrix<double, 5, 1>{0.189462064814501, -0.49903003669627627,
+        Eigen::Matrix<double, 8, 1>{0.189462064814501, -0.49903003669627627,
                                    0.007468423590519429, 0.002496885298683693,
-                                    0.3443122090208624});
+                                    0.3443122090208624, 0, 0, 0});
    prop.SetCalibError(0.35, 0.10);
    prop.SetFPS(10_Hz);
    prop.SetAvgLatency(50_ms);
@@ -445,9 +445,9 @@ class SimCameraProperties {
        (Eigen::MatrixXd(3, 3) << 1011.3749416937393, 0.0, 645.4955139388737,
         0.0, 1008.5391755084075, 508.32877656020196, 0.0, 0.0, 1.0)
            .finished(),
-        Eigen::Matrix<double, 5, 1>{0.13730101577061535, -0.2904345656989261,
+        Eigen::Matrix<double, 8, 1>{0.13730101577061535, -0.2904345656989261,
                                    8.32475714507539E-4, -3.694397782014239E-4,
-                                    0.09487962227027584});
+                                    0.09487962227027584, 0, 0, 0});
    prop.SetCalibError(0.37, 0.06);
    prop.SetFPS(7_Hz);
    prop.SetAvgLatency(60_ms);
@@ -463,7 +463,7 @@ class SimCameraProperties {
  int resWidth;
  int resHeight;
  Eigen::Matrix<double, 3, 3> camIntrinsics;
-  Eigen::Matrix<double, 5, 1> distCoeffs;
+  Eigen::Matrix<double, 8, 1> distCoeffs;
  double avgErrorPx;
  double errorStdDevPx;
  units::second_t frameSpeed{0};
--- a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
+++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
@@ -22,6 +22,9 @@
 * SOFTWARE.
 */

+#include <chrono>
+#include <thread>
+
 #include "gtest/gtest.h"
 #include "photon/PhotonUtils.h"
 #include "photon/simulation/VisionSystemSim.h"
@@ -251,6 +254,7 @@ TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) {
          frc::Translation3d{},
          frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}});
  visionSysSim.Update(robotPose);
+
  ASSERT_TRUE(camera.GetLatestResult().HasTargets());
  ASSERT_NEAR(GetParam().to<double>(),
              camera.GetLatestResult().GetBestTarget().GetPitch(), 0.25);
@@ -418,11 +422,8 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
      {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}});
  visionSysSim.Update(robotPose);

-  Eigen::Matrix<double, 3, 3> camEigen;
-  cv::cv2eigen(camera.GetCameraMatrix().value(), camEigen);
-
-  Eigen::Matrix<double, 5, 1> distEigen;
-  cv::cv2eigen(camera.GetDistCoeffs().value(), distEigen);
+  Eigen::Matrix<double, 3, 3> camEigen = camera.GetCameraMatrix().value();
+  Eigen::Matrix<double, 8, 1> distEigen = camera.GetDistCoeffs().value();

  auto camResults = camera.GetLatestResult();
  auto targetSpan = camResults.GetTargets();
--- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
@@ -263,7 +263,7 @@ public final class OpenCVHelp {
     */
    public static Point[] projectPoints(
            Matrix<N3, N3> cameraMatrix,
-            Matrix<N5, N1> distCoeffs,
+            Matrix<N8, N1> distCoeffs,
            RotTrlTransform3d camRt,
            List<Translation3d> objectTranslations) {
        // translate to opencv classes
@@ -302,7 +302,7 @@ public final class OpenCVHelp {
     * @return The undistorted image points
     */
    public static Point[] undistortPoints(
-            Matrix<N3, N3> cameraMatrix, Matrix<N5, N1> distCoeffs, Point[] points) {
+            Matrix<N3, N3> cameraMatrix, Matrix<N8, N1> distCoeffs, Point[] points) {
        var distMat = new MatOfPoint2f(points);
        var undistMat = new MatOfPoint2f();
        var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
@@ -404,7 +404,7 @@ public final class OpenCVHelp {
     */
    public static PNPResult solvePNP_SQUARE(
            Matrix<N3, N3> cameraMatrix,
-            Matrix<N5, N1> distCoeffs,
+            Matrix<N8, N1> distCoeffs,
            List<Translation3d> modelTrls,
            Point[] imagePoints) {
        // solvepnp inputs
@@ -511,7 +511,7 @@ public final class OpenCVHelp {
     */
    public static PNPResult solvePNP_SQPNP(
            Matrix<N3, N3> cameraMatrix,
-            Matrix<N5, N1> distCoeffs,
+            Matrix<N8, N1> distCoeffs,
            List<Translation3d> objectTrls,
            Point[] imagePoints) {
        try {
--- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
@@ -68,7 +68,7 @@ public class VisionEstimation {
     */
    public static PNPResult estimateCamPosePNP(
            Matrix<N3, N3> cameraMatrix,
-            Matrix<N5, N1> distCoeffs,
+            Matrix<N8, N1> distCoeffs,
            List<PhotonTrackedTarget> visTags,
            AprilTagFieldLayout tagLayout,
            TargetModel tagModel) {
--- a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
+++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
@@ -122,7 +122,7 @@ static std::vector<cv::Point3f> RotationToRVec(

 static std::vector<cv::Point2f> ProjectPoints(
    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
-    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    const Eigen::Matrix<double, 8, 1>& distCoeffs,
    const RotTrlTransform3d& camRt,
    const std::vector<frc::Translation3d>& objectTranslations) {
  std::vector<cv::Point3f> objectPoints = TranslationToTVec(objectTranslations);
@@ -184,7 +184,7 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {

 [[maybe_unused]] static photon::PNPResult SolvePNP_Square(
    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
-    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    const Eigen::Matrix<double, 8, 1>& distCoeffs,
    std::vector<frc::Translation3d> modelTrls,
    std::vector<cv::Point2f> imagePoints) {
  modelTrls = ReorderCircular(modelTrls, true, -1);
@@ -252,7 +252,7 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {

 [[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP(
    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
-    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    const Eigen::Matrix<double, 8, 1>& distCoeffs,
    std::vector<frc::Translation3d> modelTrls,
    std::vector<cv::Point2f> imagePoints) {
  std::vector<cv::Point3f> objectMat = TranslationToTVec(modelTrls);
--- a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
+++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
@@ -48,7 +48,7 @@ static std::vector<frc::AprilTag> GetVisibleLayoutTags(

 static PNPResult EstimateCamPosePNP(
    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
-    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    const Eigen::Matrix<double, 8, 1>& distCoeffs,
    const std::vector<PhotonTrackedTarget>& visTags,
    const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
  if (visTags.size() == 0) {
--- a/shared/config.gradle
+++ b/shared/config.gradle
@@ -33,6 +33,9 @@ model {
    binaries {
        withType(NativeBinarySpec).all {
            nativeUtils.usePlatformArguments(it)
+            if (it.toolChain instanceof GccCompatibleToolChain) {
+                it.cppCompiler.args << "-Wno-deprecated-enum-enum-conversion"
+            }
        }
    }
 }