diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index acc5887c0..4727d813a 100644 --- 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 diff --git a/build.gradle b/build.gradle index 67077550f..f44dbe4ff 100644 --- 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") + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java index ac4dc7a6e..0ed64b82c 100644 --- 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]; diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 54173a41b..aa92025fe 100644 --- 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> getDistCoeffs() { + /** + * The camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms are set + * to 0 + */ + public Optional> 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(); } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 6b217c9b7..2460288d0 100644 --- 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 update( PhotonPipelineResult cameraResult, Optional> cameraMatrix, - Optional> distCoeffs) { + Optional> 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 update( PhotonPipelineResult cameraResult, Optional> cameraMatrix, - Optional> distCoeffs, + Optional> distCoeffs, PoseStrategy strat) { Optional estimatedPose; switch (strat) { @@ -418,7 +418,7 @@ public class PhotonPoseEstimator { private Optional multiTagOnCoprocStrategy( PhotonPipelineResult result, Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { + Optional> distCoeffsOpt) { if (result.getMultiTagResult().estimatedPose.isPresent) { var best_tf = result.getMultiTagResult().estimatedPose.best; var best = @@ -440,7 +440,7 @@ public class PhotonPoseEstimator { private Optional multiTagOnRioStrategy( PhotonPipelineResult result, Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { + Optional> distCoeffsOpt) { boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); // cannot run multitagPNP, use fallback strategy if (!hasCalibData || result.getTargets().size() < 2) { diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 074b3aa17..1e069ac06 100644 --- 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 camIntrinsics; - private Matrix distCoeffs; + private Matrix 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 camIntrinsics, Matrix distCoeffs) { + int resWidth, int resHeight, Matrix camIntrinsics, Matrix distCoeffs) { this.resWidth = resWidth; this.resHeight = resHeight; this.camIntrinsics = camIntrinsics; @@ -280,7 +282,7 @@ public class SimCameraProperties { return camIntrinsics.copy(); } - public Vector getDistCoeffs() { + public Vector 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); diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 91c5c7a5c..0a8809c36 100644 --- 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(static_cast(ledModeSub.Get())); } -std::optional 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(i, j) = camCoeffs[(j * 3) + i]; - } - } - return retVal; - } - return std::nullopt; -} - void PhotonCamera::SetLEDMode(LEDMode mode) { ledModePub.Set(static_cast(mode)); } @@ -181,13 +167,26 @@ const std::string_view PhotonCamera::GetCameraName() const { return cameraName; } -std::optional PhotonCamera::GetDistCoeffs() { +std::optional PhotonCamera::GetCameraMatrix() { + auto camCoeffs = cameraIntrinsicsSubscriber.Get(); + if (camCoeffs.size() == 9) { + PhotonCamera::CameraMatrix retVal = + Eigen::Map(camCoeffs.data()); + return retVal; + } + return std::nullopt; +} + +std::optional 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(i, 0) = distCoeffs[i]; - } + auto bound = distCoeffs.size(); + if (bound > 0 && bound <= 8) { + PhotonCamera::DistortionMatrix retVal = + PhotonCamera::DistortionMatrix::Zero(); + + Eigen::Map map(distCoeffs.data(), bound); + retVal.block(0, 0, bound, 1) = map; + return retVal; } return std::nullopt; diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index d14394b99..2571eafe2 100644 --- 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 + namespace photon { namespace detail { @@ -126,8 +129,9 @@ std::optional PhotonPoseEstimator::Update( } std::optional PhotonPoseEstimator::Update( - const PhotonPipelineResult& result, std::optional cameraMatrixData, - std::optional cameraDistCoeffs) { + const PhotonPipelineResult& result, + std::optional cameraMatrixData, + std::optional 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 PhotonPoseEstimator::Update( } std::optional PhotonPoseEstimator::Update( - PhotonPipelineResult result, std::optional cameraMatrixData, - std::optional cameraDistCoeffs, PoseStrategy strategy) { + const PhotonPipelineResult& result, + std::optional cameraMatrixData, + std::optional cameraDistCoeffs, + PoseStrategy strategy) { std::optional ret = std::nullopt; switch (strategy) { @@ -371,8 +377,9 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { } std::optional PhotonPoseEstimator::MultiTagOnCoprocStrategy( - PhotonPipelineResult result, std::optional camMat, - std::optional distCoeffs) { + PhotonPipelineResult result, + std::optional camMat, + std::optional distCoeffs) { if (result.MultiTagResult().result.isPresent) { const auto field2camera = result.MultiTagResult().result.best; @@ -388,8 +395,9 @@ std::optional PhotonPoseEstimator::MultiTagOnCoprocStrategy( } std::optional PhotonPoseEstimator::MultiTagOnRioStrategy( - PhotonPipelineResult result, std::optional camMat, - std::optional distCoeffs) { + PhotonPipelineResult result, + std::optional camMat, + std::optional distCoeffs) { using namespace frc; // Need at least 2 targets @@ -433,8 +441,15 @@ std::optional PhotonPoseEstimator::MultiTagOnRioStrategy( cv::Mat const rvec(3, 1, cv::DataType::type); cv::Mat const tvec(3, 1, cv::DataType::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); diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index f588c340d..cd1752ed0 100644 --- 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 GetCameraMatrix(); - std::optional GetDistCoeffs(); + using CameraMatrix = Eigen::Matrix; + using DistortionMatrix = Eigen::Matrix; + + /** + * @brief Get the camera calibration matrix, in standard OpenCV form + * + * @return std::optional + */ + std::optional GetCameraMatrix(); + + /** + * @brief Get the camera calibration distortion coefficients, in OPENCV8 form. + * Higher order terms are set to zero. + * + * @return std::optional + */ + std::optional GetDistCoeffs(); static void SetVersionCheckEnabled(bool enabled); diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index 2c863330b..c9c36bd0d 100644 --- 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 Update( const PhotonPipelineResult& result, - std::optional cameraMatrixData, - std::optional coeffsData); + std::optional cameraMatrixData, + std::optional coeffsData); inline std::shared_ptr GetCamera() { return camera; } @@ -236,8 +236,10 @@ class PhotonPoseEstimator { inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; } std::optional Update( - PhotonPipelineResult result, std::optional cameraMatrixData, - std::optional coeffsData, PoseStrategy strategy); + const PhotonPipelineResult& result, + std::optional cameraMatrixData, + std::optional 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 MultiTagOnCoprocStrategy( - PhotonPipelineResult result, std::optional camMat, - std::optional distCoeffs); + PhotonPipelineResult result, + std::optional camMat, + std::optional 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 MultiTagOnRioStrategy( - PhotonPipelineResult result, std::optional camMat, - std::optional distCoeffs); + PhotonPipelineResult result, + std::optional camMat, + std::optional distCoeffs); /** * Return the average of the best target poses using ambiguity as weight. diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h index 4632ef5d4..e67315406 100644 --- 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 newDistCoeffs; - newDistCoeffs << 0, 0, 0, 0, 0; + Eigen::Matrix newDistCoeffs = + Eigen::Matrix::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& newCamIntrinsics, - const Eigen::Matrix& newDistCoeffs) { + const Eigen::Matrix& newDistCoeffs) { resWidth = width; resHeight = height; camIntrinsics = newCamIntrinsics; @@ -151,7 +151,7 @@ class SimCameraProperties { Eigen::Matrix GetIntrinsics() const { return camIntrinsics; } - Eigen::Matrix GetDistCoeffs() const { return distCoeffs; } + Eigen::Matrix 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{ + Eigen::Matrix{ 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{ + Eigen::Matrix{ 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{0.1917469998873756, -0.5142936883324216, + Eigen::Matrix{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{0.189462064814501, -0.49903003669627627, + Eigen::Matrix{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{0.13730101577061535, -0.2904345656989261, + Eigen::Matrix{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 camIntrinsics; - Eigen::Matrix distCoeffs; + Eigen::Matrix distCoeffs; double avgErrorPx; double errorStdDevPx; units::second_t frameSpeed{0}; diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp index 667b6f53a..e3db87318 100644 --- a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -22,6 +22,9 @@ * SOFTWARE. */ +#include +#include + #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(), 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 camEigen; - cv::cv2eigen(camera.GetCameraMatrix().value(), camEigen); - - Eigen::Matrix distEigen; - cv::cv2eigen(camera.GetDistCoeffs().value(), distEigen); + Eigen::Matrix camEigen = camera.GetCameraMatrix().value(); + Eigen::Matrix distEigen = camera.GetDistCoeffs().value(); auto camResults = camera.GetLatestResult(); auto targetSpan = camResults.GetTargets(); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index e2413a4d2..e23392379 100644 --- 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 cameraMatrix, - Matrix distCoeffs, + Matrix distCoeffs, RotTrlTransform3d camRt, List objectTranslations) { // translate to opencv classes @@ -302,7 +302,7 @@ public final class OpenCVHelp { * @return The undistorted image points */ public static Point[] undistortPoints( - Matrix cameraMatrix, Matrix distCoeffs, Point[] points) { + Matrix cameraMatrix, Matrix 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 cameraMatrix, - Matrix distCoeffs, + Matrix distCoeffs, List modelTrls, Point[] imagePoints) { // solvepnp inputs @@ -511,7 +511,7 @@ public final class OpenCVHelp { */ public static PNPResult solvePNP_SQPNP( Matrix cameraMatrix, - Matrix distCoeffs, + Matrix distCoeffs, List objectTrls, Point[] imagePoints) { try { diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index e1e270fc3..6d50d355d 100644 --- 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 cameraMatrix, - Matrix distCoeffs, + Matrix distCoeffs, List visTags, AprilTagFieldLayout tagLayout, TargetModel tagModel) { diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h index 2d79e661e..d72e2dc2c 100644 --- 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 RotationToRVec( static std::vector ProjectPoints( const Eigen::Matrix& cameraMatrix, - const Eigen::Matrix& distCoeffs, + const Eigen::Matrix& distCoeffs, const RotTrlTransform3d& camRt, const std::vector& objectTranslations) { std::vector 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& cameraMatrix, - const Eigen::Matrix& distCoeffs, + const Eigen::Matrix& distCoeffs, std::vector modelTrls, std::vector 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& cameraMatrix, - const Eigen::Matrix& distCoeffs, + const Eigen::Matrix& distCoeffs, std::vector modelTrls, std::vector imagePoints) { std::vector objectMat = TranslationToTVec(modelTrls); diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h index cb3ae3bdc..08a418e6e 100644 --- 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 GetVisibleLayoutTags( static PNPResult EstimateCamPosePNP( const Eigen::Matrix& cameraMatrix, - const Eigen::Matrix& distCoeffs, + const Eigen::Matrix& distCoeffs, const std::vector& visTags, const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) { if (visTags.size() == 0) { diff --git a/shared/config.gradle b/shared/config.gradle index 4044d7e3f..e6fde7f46 100644 --- 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" + } } } }