diff --git a/.gitignore b/.gitignore index 721d1619a..e0aad645c 100644 --- a/.gitignore +++ b/.gitignore @@ -154,4 +154,4 @@ photon-server/src/main/resources/nativelibraries/apriltag/* photonlib-java-examples/*/vendordeps/* photonlib-cpp-examples/*/vendordeps/* -networktables.json +*/networktables.json diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index cd14a685b..3823ab6d7 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -180,6 +180,15 @@ public class NTDataPublisher implements CVPipelineResultConsumer { ts.bestTargetPosY.set(0); } + var fsp = result.inputAndOutputFrame.frameStaticProperties; + if (fsp.cameraCalibration != null) { + ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr()); + ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getExtrinsicsArr()); + } else { + ts.cameraIntrinsicsPublisher.accept(new double[] {}); + ts.cameraDistortionPublisher.accept(new double[] {}); + } + ts.heartbeatPublisher.set(heartbeatCounter++); // TODO...nt4... is this needed? diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java index 7df950533..4b4ae5a8c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java @@ -44,6 +44,10 @@ public class CameraCalibrationCoefficients implements Releasable { @JsonProperty("standardDeviation") public final double standardDeviation; + @JsonIgnore private final double[] intrinsicsArr = new double[9]; + + @JsonIgnore private final double[] extrinsicsArr = new double[5]; + @JsonCreator public CameraCalibrationCoefficients( @JsonProperty("resolution") Size resolution, @@ -56,6 +60,10 @@ public class CameraCalibrationCoefficients implements Releasable { this.distCoeffs = distCoeffs; this.perViewErrors = perViewErrors; this.standardDeviation = standardDeviation; + + // do this once so gets are quick + getCameraIntrinsicsMat().get(0, 0, intrinsicsArr); + getDistCoeffsMat().get(0, 0, extrinsicsArr); } @JsonIgnore @@ -68,6 +76,16 @@ public class CameraCalibrationCoefficients implements Releasable { return distCoeffs.getAsMatOfDouble(); } + @JsonIgnore + public double[] getIntrinsicsArr() { + return intrinsicsArr; + } + + @JsonIgnore + public double[] getExtrinsicsArr() { + return extrinsicsArr; + } + @JsonIgnore public double[] getPerViewErrors() { return perViewErrors; diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java index 60b4a524b..fca4fdf26 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java @@ -23,6 +23,7 @@ import java.util.Arrays; import org.opencv.core.CvType; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; +import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.vision.opencv.Releasable; public class JsonMat implements Releasable { @@ -106,4 +107,9 @@ public class JsonMat implements Releasable { public void release() { getAsMat().release(); } + + public Packet populatePacket(Packet packet) { + packet.encode(this.data); + return packet; + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java index ee205d588..f03db3a60 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java @@ -61,10 +61,12 @@ public class FrameStaticProperties { imageArea = this.imageWidth * this.imageHeight; + // Todo -- if we have calibration, use it's center point? centerX = ((double) this.imageWidth / 2) - 0.5; centerY = ((double) this.imageHeight / 2) - 0.5; centerPoint = new Point(centerX, centerY); + // TODO if we have calibration use it here instead // pinhole model calculations DoubleCouple horizVertViews = calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight); diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 60e42b176..741117401 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -22,6 +22,14 @@ targetCompatibility = JavaVersion.VERSION_11 wpilibTools.deps.wpilibVersion = wpilibVersion println("Buidling for wpilib ${wpilibTools.deps.wpilibVersion}") +// From wpilib shared/config.gradle: +// NativeUtils adds the following OpenCV warning suppression for Linux, but not +// for macOS +// https://github.com/opencv/opencv/issues/20269 +nativeUtils.platformConfigs.osxuniversal.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion") +nativeUtils.platformConfigs.linuxathena.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion") +// nativeUtils.platformConfigs.linuxx64.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion") + // Apply Java configuration dependencies { implementation project(":photon-targeting") @@ -34,6 +42,11 @@ dependencies { implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + testImplementation wpilibTools.deps.wpilibJava("cscore") + implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-java:$opencvVersion" + implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-jni:$opencvVersion:$jniPlatform" + implementation "org.ejml:ejml-simple:0.41" + // Junit testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2") testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2") @@ -66,6 +79,7 @@ model { } nativeUtils.useRequiredLibrary(it, "wpilib_shared") nativeUtils.useRequiredLibrary(it, "apriltag_shared") + nativeUtils.useRequiredLibrary(it, "opencv_shared") } } testSuites { @@ -82,6 +96,7 @@ model { nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") nativeUtils.useRequiredLibrary(it, "apriltag_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") + nativeUtils.useRequiredLibrary(it, "opencv_shared") } } } @@ -147,3 +162,4 @@ testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") testNativeConfig.dependencies.add wpilibTools.deps.wpilib("hal") testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") +testNativeConfig.dependencies.add wpilibTools.deps.cscore() diff --git a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java index 44846b7ae..383887cf6 100644 --- a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java +++ b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java @@ -25,6 +25,8 @@ package org.photonvision; import edu.wpi.first.math.geometry.Pose3d; +import java.util.List; +import org.photonvision.targeting.PhotonTrackedTarget; /** An estimated pose based on pipeline result */ public class EstimatedRobotPose { @@ -34,14 +36,19 @@ public class EstimatedRobotPose { /** The estimated time the frame used to derive the robot pose was taken */ public final double timestampSeconds; + /** A list of the targets used to compute this pose */ + public final List targetsUsed; + /** * Constructs an EstimatedRobotPose * * @param estimatedPose estimated pose * @param timestampSeconds timestamp of the estimate */ - public EstimatedRobotPose(Pose3d estimatedPose, double timestampSeconds) { + public EstimatedRobotPose( + Pose3d estimatedPose, double timestampSeconds, List targetsUsed) { this.estimatedPose = estimatedPose; this.timestampSeconds = timestampSeconds; + this.targetsUsed = targetsUsed; } } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index d4be529ef..aaa52bd6b 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -24,9 +24,14 @@ package org.photonvision; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.*; import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoubleArraySubscriber; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.IntegerEntry; import edu.wpi.first.networktables.IntegerPublisher; @@ -39,6 +44,7 @@ import edu.wpi.first.networktables.RawSubscriber; import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import java.util.Optional; import java.util.Set; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.hardware.VisionLEDMode; @@ -64,6 +70,8 @@ public class PhotonCamera { IntegerPublisher pipelineIndexRequest, ledModeRequest; IntegerSubscriber pipelineIndexState, ledModeState; IntegerSubscriber heartbeatEntry; + private DoubleArraySubscriber cameraIntrinsicsSubscriber; + private DoubleArraySubscriber cameraDistortionSubscriber; public void close() { rawBytesEntry.close(); @@ -84,6 +92,8 @@ public class PhotonCamera { ledModeRequest.close(); ledModeState.close(); pipelineIndexRequest.close(); + cameraIntrinsicsSubscriber.close(); + cameraDistortionSubscriber.close(); } private final String path; @@ -133,6 +143,8 @@ public class PhotonCamera { ledModeRequest = mainTable.getIntegerTopic("ledModeRequest").publish(); ledModeState = mainTable.getIntegerTopic("ledModeState").subscribe(-1); versionEntry = mainTable.getStringTopic("version").subscribe(""); + cameraIntrinsicsSubscriber = mainTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null); + cameraDistortionSubscriber = mainTable.getDoubleArrayTopic("cameraDistortion").subscribe(null); m_topicNameSubscriber = new MultiSubscriber( @@ -305,6 +317,20 @@ public class PhotonCamera { return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC; } + public Optional> getCameraMatrix() { + var cameraMatrix = cameraIntrinsicsSubscriber.get(); + if (cameraMatrix != null && cameraMatrix.length == 9) { + return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix)); + } else return Optional.empty(); + } + + public Optional> getDistCoeffs() { + var distCoeffs = cameraDistortionSubscriber.get(); + if (distCoeffs != null && distCoeffs.length == 5) { + return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs)); + } else return Optional.empty(); + } + private void verifyVersion() { if (!VERSION_CHECK_ENABLED) return; diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 9f196d74d..ec0498287 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -24,6 +24,7 @@ package org.photonvision; +import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; @@ -37,8 +38,10 @@ import java.util.HashSet; import java.util.List; import java.util.Optional; import java.util.Set; +import org.photonvision.estimation.VisionEstimation; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; /** * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a @@ -61,11 +64,15 @@ public class PhotonPoseEstimator { CLOSEST_TO_LAST_POSE, /** Choose the Pose with the lowest ambiguity. */ - AVERAGE_BEST_TARGETS + AVERAGE_BEST_TARGETS, + + /** Use all visible tags to compute a single pose estimate.. */ + MULTI_TAG_PNP } private AprilTagFieldLayout fieldTags; - private PoseStrategy strategy; + private PoseStrategy primaryStrategy; + private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; private final PhotonCamera camera; private Transform3d robotToCamera; @@ -93,7 +100,7 @@ public class PhotonPoseEstimator { PhotonCamera camera, Transform3d robotToCamera) { this.fieldTags = fieldTags; - this.strategy = strategy; + this.primaryStrategy = strategy; this.camera = camera; this.robotToCamera = robotToCamera; } @@ -121,8 +128,8 @@ public class PhotonPoseEstimator { * * @return the strategy */ - public PoseStrategy getStrategy() { - return strategy; + public PoseStrategy getPrimaryStrategy() { + return primaryStrategy; } /** @@ -130,8 +137,23 @@ public class PhotonPoseEstimator { * * @param strategy the strategy to set */ - public void setStrategy(PoseStrategy strategy) { - this.strategy = strategy; + public void setPrimaryStrategy(PoseStrategy strategy) { + this.primaryStrategy = strategy; + } + + /** + * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must + * NOT be MULTI_TAG_PNP + * + * @param strategy the strategy to set + */ + public void setMultiTagFallbackStrategy(PoseStrategy strategy) { + if (strategy == PoseStrategy.MULTI_TAG_PNP) { + DriverStation.reportWarning( + "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", null); + strategy = PoseStrategy.LOWEST_AMBIGUITY; + } + this.multiTagFallbackStrategy = strategy; } /** @@ -229,8 +251,13 @@ public class PhotonPoseEstimator { return Optional.empty(); } + return update(cameraResult, this.primaryStrategy); + } + + private Optional update( + PhotonPipelineResult cameraResult, PoseStrategy strat) { Optional estimatedPose; - switch (strategy) { + switch (strat) { case LOWEST_AMBIGUITY: estimatedPose = lowestAmbiguityStrategy(cameraResult); break; @@ -245,6 +272,9 @@ public class PhotonPoseEstimator { case AVERAGE_BEST_TARGETS: estimatedPose = averageBestTargetsStrategy(cameraResult); break; + case MULTI_TAG_PNP: + estimatedPose = multiTagPNPStrategy(cameraResult); + break; default: DriverStation.reportError( "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false); @@ -258,6 +288,55 @@ public class PhotonPoseEstimator { return estimatedPose; } + private Optional multiTagPNPStrategy(PhotonPipelineResult result) { + // Arrays we need declared up front + var visCorners = new ArrayList(); + var knownVisTags = new ArrayList(); + var fieldToCams = new ArrayList(); + var fieldToCamsAlt = new ArrayList(); + + if (result.getTargets().size() < 2) { + // Run fallback strategy instead + update(result, this.multiTagFallbackStrategy); + } + + for (var target : result.getTargets()) { + visCorners.addAll(target.getDetectedCorners()); + Pose3d tagPose = fieldTags.getTagPose(target.getFiducialId()).get(); + + // actual layout poses of visible tags -- not exposed, so have to recreate + knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose)); + + fieldToCams.add(tagPose.transformBy(target.getBestCameraToTarget().inverse())); + fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse())); + } + + var cameraMatrixOpt = camera.getCameraMatrix(); + var distCoeffsOpt = camera.getDistCoeffs(); + boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); + + // multi-target solvePNP + if (hasCalibData) { + var cameraMatrix = cameraMatrixOpt.get(); + var distCoeffs = distCoeffsOpt.get(); + var pnpResults = + VisionEstimation.estimateCamPosePNP(cameraMatrix, distCoeffs, visCorners, knownVisTags); + var best = + new Pose3d() + .plus(pnpResults.best) // field-to-camera + .plus(robotToCamera.inverse()); // field-to-robot + // var alt = new Pose3d() + // .plus(pnpResults.alt) // field-to-camera + // .plus(robotToCamera.inverse()); // field-to-robot + + return Optional.of( + new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets())); + } else { + // TODO fallback strategy? Should we just always do solvePNP? + return Optional.empty(); + } + } + /** * Return the estimated position of the robot with the lowest position ambiguity from a List of * pipeline results. @@ -299,7 +378,8 @@ public class PhotonPoseEstimator { .get() .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds())); + result.getTimestampSeconds(), + result.getTargets())); } /** @@ -352,7 +432,8 @@ public class PhotonPoseEstimator { .get() .transformBy(target.getAlternateCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds()); + result.getTimestampSeconds(), + result.getTargets()); } if (bestTransformDelta < smallestHeightDifference) { @@ -363,7 +444,8 @@ public class PhotonPoseEstimator { .get() .transformBy(target.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds()); + result.getTimestampSeconds(), + result.getTargets()); } } @@ -424,12 +506,14 @@ public class PhotonPoseEstimator { if (altDifference < smallestPoseDelta) { smallestPoseDelta = altDifference; lowestDeltaPose = - new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds()); + new EstimatedRobotPose( + altTransformPosition, result.getTimestampSeconds(), result.getTargets()); } if (bestDifference < smallestPoseDelta) { smallestPoseDelta = bestDifference; lowestDeltaPose = - new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds()); + new EstimatedRobotPose( + bestTransformPosition, result.getTimestampSeconds(), result.getTargets()); } } return Optional.ofNullable(lowestDeltaPose); @@ -471,7 +555,8 @@ public class PhotonPoseEstimator { .get() .transformBy(target.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds())); + result.getTimestampSeconds(), + result.getTargets())); } totalAmbiguity += 1.0 / target.getPoseAmbiguity(); @@ -502,7 +587,8 @@ public class PhotonPoseEstimator { } return Optional.of( - new EstimatedRobotPose(new Pose3d(transform, rotation), result.getTimestampSeconds())); + new EstimatedRobotPose( + new Pose3d(transform, rotation), result.getTimestampSeconds(), result.getTargets())); } /** diff --git a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java index 1823c4875..5289bbf68 100644 --- a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java @@ -24,6 +24,10 @@ package org.photonvision; +import edu.wpi.first.math.Matrix; +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.networktables.NetworkTableInstance; import java.util.Arrays; import java.util.List; @@ -52,6 +56,28 @@ public class SimPhotonCamera { ts.updateEntries(); } + /** + * Publishes the camera intrinsics matrix. The matrix should be in the form: spotless:off + * fx 0 cx + * 0 fy cy + * 0 0 1 + * @param cameraMatrix The cam matrix + * spotless:on + */ + public void setCameraIntrinsicsMat(Matrix cameraMatrix) { + ts.cameraIntrinsicsPublisher.set(cameraMatrix.getData()); + } + + /** + * Publishes the camera distortion matrix. The matrix should be in the form [k1 k2 p1 p2 k3]. See + * more: https://docs.opencv.org/3.4/d4/d94/tutorial_camera_calibration.html + * + * @param distortionMat The distortion mat + */ + public void setCameraDistortionMat(Matrix distortionMat) { + ts.cameraDistortionPublisher.set(distortionMat.getData()); + } + /** * Constructs a Simulated PhotonCamera from the name of the camera. * diff --git a/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java b/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java new file mode 100644 index 000000000..afcc43b0c --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java @@ -0,0 +1,64 @@ +/* + * MIT License + * + * Copyright (c) 2022 PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.photonvision.estimation; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; + +/** Holds various helper geometries describing the relation between camera and target. */ +public class CameraTargetRelation { + public final Pose3d camPose; + public final Transform3d camToTarg; + public final double camToTargDist; + public final double camToTargDistXY; + public final Rotation2d camToTargYaw; + public final Rotation2d camToTargPitch; + /** Angle from the camera's relative x-axis */ + public final Rotation2d camToTargAngle; + + public final Transform3d targToCam; + public final Rotation2d targToCamYaw; + public final Rotation2d targToCamPitch; + /** Angle from the target's relative x-axis */ + public final Rotation2d targToCamAngle; + + public CameraTargetRelation(Pose3d cameraPose, Pose3d targetPose) { + this.camPose = cameraPose; + camToTarg = new Transform3d(cameraPose, targetPose); + camToTargDist = camToTarg.getTranslation().getNorm(); + camToTargDistXY = + Math.hypot(camToTarg.getTranslation().getX(), camToTarg.getTranslation().getY()); + camToTargYaw = new Rotation2d(camToTarg.getX(), camToTarg.getY()); + camToTargPitch = new Rotation2d(camToTargDistXY, -camToTarg.getZ()); + camToTargAngle = + new Rotation2d(Math.hypot(camToTargYaw.getRadians(), camToTargPitch.getRadians())); + targToCam = new Transform3d(targetPose, cameraPose); + targToCamYaw = new Rotation2d(targToCam.getX(), targToCam.getY()); + targToCamPitch = new Rotation2d(camToTargDistXY, -targToCam.getZ()); + targToCamAngle = + new Rotation2d(Math.hypot(targToCamYaw.getRadians(), targToCamPitch.getRadians())); + } +} diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java new file mode 100644 index 000000000..4b6429319 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -0,0 +1,518 @@ +/* + * MIT License + * + * Copyright (c) 2022 PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.photonvision.estimation; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.CoordinateSystem; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.util.RuntimeLoader; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import org.ejml.simple.SimpleMatrix; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.MatOfInt; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point; +import org.opencv.core.Point3; +import org.opencv.core.Rect; +import org.opencv.core.RotatedRect; +import org.opencv.imgproc.Imgproc; +import org.photonvision.targeting.TargetCorner; + +public final class OpenCVHelp { + static { + try { + var loader = + new RuntimeLoader<>( + Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class); + loader.loadLibrary(); + } catch (Exception e) { + throw new RuntimeException("Failed to load native libraries!", e); + } + } + + public static MatOfDouble matrixToMat(SimpleMatrix matrix) { + var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F); + mat.put(0, 0, matrix.getDDRM().getData()); + var wrappedMat = new MatOfDouble(); + mat.convertTo(wrappedMat, CvType.CV_64F); + mat.release(); + return wrappedMat; + } + + public static Matrix matToMatrix(Mat mat) { + double[] data = new double[(int) mat.total() * mat.channels()]; + var doubleMat = new Mat(mat.rows(), mat.cols(), CvType.CV_64F); + mat.convertTo(doubleMat, CvType.CV_64F); + doubleMat.get(0, 0, data); + return new Matrix<>(new SimpleMatrix(mat.rows(), mat.cols(), true, data)); + } + + /** + * Creates a new {@link MatOfPoint3f} with these 3d translations. The opencv tvec is a vector with + * three elements representing {x, y, z} in the EDN coordinate system. + * + * @param translations The translations to convert into a MatOfPoint3f + */ + public static MatOfPoint3f translationToTvec(Translation3d... translations) { + Point3[] points = new Point3[translations.length]; + for (int i = 0; i < translations.length; i++) { + var trl = + CoordinateSystem.convert(translations[i], CoordinateSystem.NWU(), CoordinateSystem.EDN()); + points[i] = new Point3(trl.getX(), trl.getY(), trl.getZ()); + } + return new MatOfPoint3f(points); + } + /** + * Returns a new 3d translation from this {@link Mat}. The opencv tvec is a vector with three + * elements representing {x, y, z} in the EDN coordinate system. + * + * @param tvecInput The tvec to create a Translation3d from + */ + public static Translation3d tvecToTranslation(Mat tvecInput) { + float[] data = new float[3]; + var wrapped = new Mat(tvecInput.rows(), tvecInput.cols(), CvType.CV_32F); + tvecInput.convertTo(wrapped, CvType.CV_32F); + wrapped.get(0, 0, data); + wrapped.release(); + return CoordinateSystem.convert( + new Translation3d(data[0], data[1], data[2]), + CoordinateSystem.EDN(), + CoordinateSystem.NWU()); + } + + /** + * Creates a new {@link MatOfPoint3f} with this 3d rotation. The opencv rvec Mat is a vector with + * three elements representing the axis scaled by the angle in the EDN coordinate system. (angle = + * norm, and axis = rvec / norm) + * + * @param rotation The rotation to convert into a MatOfPoint3f + */ + public static MatOfPoint3f rotationToRvec(Rotation3d rotation) { + rotation = rotationNWUtoEDN(rotation); + return new MatOfPoint3f(new Point3(rotation.getQuaternion().toRotationVector().getData())); + } + /** + * Returns a 3d rotation from this {@link Mat}. The opencv rvec Mat is a vector with three + * elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm, + * and axis = rvec / norm) + * + * @param rvecInput The rvec to create a Rotation3d from + */ + public static Rotation3d rvecToRotation(Mat rvecInput) { + float[] data = new float[3]; + var wrapped = new Mat(rvecInput.rows(), rvecInput.cols(), CvType.CV_32F); + rvecInput.convertTo(wrapped, CvType.CV_32F); + wrapped.get(0, 0, data); + wrapped.release(); + Vector axis = new Vector<>(Nat.N3()); + axis.set(0, 0, data[0]); + axis.set(1, 0, data[1]); + axis.set(2, 0, data[2]); + return rotationEDNtoNWU(new Rotation3d(axis.div(axis.norm()), axis.norm())); + } + + public static TargetCorner averageCorner(List corners) { + if (corners == null || corners.size() == 0) return null; + + var pointMat = targetCornersToMat(corners); + Core.reduce(pointMat, pointMat, 0, Core.REDUCE_AVG); + var avgPt = matToTargetCorners(pointMat)[0]; + pointMat.release(); + return avgPt; + } + + public static MatOfPoint2f targetCornersToMat(List corners) { + return targetCornersToMat(corners.toArray(TargetCorner[]::new)); + } + + public static MatOfPoint2f targetCornersToMat(TargetCorner... corners) { + var points = new Point[corners.length]; + for (int i = 0; i < corners.length; i++) { + points[i] = new Point(corners[i].x, corners[i].y); + } + return new MatOfPoint2f(points); + } + + public static TargetCorner[] pointsToTargetCorners(Point... points) { + var corners = new TargetCorner[points.length]; + for (int i = 0; i < points.length; i++) { + corners[i] = new TargetCorner(points[i].x, points[i].y); + } + return corners; + } + + public static TargetCorner[] matToTargetCorners(MatOfPoint2f matInput) { + var corners = new TargetCorner[(int) matInput.total()]; + float[] data = new float[(int) matInput.total() * matInput.channels()]; + matInput.get(0, 0, data); + for (int i = 0; i < corners.length; i++) { + corners[i] = new TargetCorner(data[0 + 2 * i], data[1 + 2 * i]); + } + return corners; + } + + /** + * Reorders the list, optionally indexing backwards and wrapping around to the last element after + * the first, and shifting all indices in the direction of indexing. + * + *

e.g. + * + *

({1,2,3}, false, 1) == {2,3,1} + * + *

({1,2,3}, true, 0) == {1,3,2} + * + *

({1,2,3}, true, 1) == {3,2,1} + * + * @param Element type + * @param elements + * @param backwards If indexing should happen in reverse (0, size-1, size-2, ...) + * @param shiftStart How much the inital index should be shifted (instead of starting at index 0, + * start at shiftStart, negated if backwards) + * @return Reordered list + */ + public static List reorderCircular(List elements, boolean backwards, int shiftStart) { + int size = elements.size(); + int dir = backwards ? -1 : 1; + var reordered = new ArrayList<>(elements); + for (int i = 0; i < size; i++) { + int index = (i * dir + shiftStart * dir) % size; + if (index < 0) index = size + index; + reordered.set(i, elements.get(index)); + } + return reordered; + } + + /** + * Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN, + * this would be XYZ {0, -1, 0} in NWU. + */ + private static Rotation3d rotationEDNtoNWU(Rotation3d rot) { + return CoordinateSystem.convert( + new Rotation3d(), CoordinateSystem.NWU(), CoordinateSystem.EDN()) + .plus(CoordinateSystem.convert(rot, CoordinateSystem.EDN(), CoordinateSystem.NWU())); + } + /** + * Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN, + * this would be XYZ {0, -1, 0} in NWU. + */ + private static Rotation3d rotationNWUtoEDN(Rotation3d rot) { + return CoordinateSystem.convert( + new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU()) + .plus(CoordinateSystem.convert(rot, CoordinateSystem.NWU(), CoordinateSystem.EDN())); + } + + /** + * Project object points from the 3d world into the 2d camera image. The camera + * properties(intrinsics, distortion) determine the results of this projection. + * + * @param cameraMatrix the camera intrinsics matrix in standard opencv form + * @param distCoeffs the camera distortion matrix in standard opencv form + * @param camPose The current camera pose in the 3d world + * @param objectTranslations The 3d points to be projected + * @return The 2d points in pixels which correspond to the image of the 3d points on the camera + */ + public static List projectPoints( + Matrix cameraMatrix, + Matrix distCoeffs, + Pose3d camPose, + List objectTranslations) { + // translate to opencv classes + var objectPoints = translationToTvec(objectTranslations.toArray(new Translation3d[0])); + // opencv rvec/tvec describe a change in basis from world to camera + var basisChange = RotTrlTransform3d.makeRelativeTo(camPose); + var rvec = rotationToRvec(basisChange.getRotation()); + var tvec = translationToTvec(basisChange.getTranslation()); + var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage()); + var distCoeffsMat = matrixToMat(distCoeffs.getStorage()); + var imagePoints = new MatOfPoint2f(); + // project to 2d + Calib3d.projectPoints(objectPoints, rvec, tvec, cameraMatrixMat, distCoeffsMat, imagePoints); + + // turn 2d point Mat into TargetCorners + var corners = matToTargetCorners(imagePoints); + + // release our Mats from native memory + objectPoints.release(); + rvec.release(); + tvec.release(); + cameraMatrixMat.release(); + distCoeffsMat.release(); + imagePoints.release(); + + return Arrays.asList(corners); + } + + /** + * Undistort 2d image points using a given camera's intrinsics and distortion. + * + *

2d image points from projectPoints(CameraProperties, Pose3d, List) projectPoints} will + * naturally be distorted, so this operation is important if the image points need to be directly + * used (e.g. 2d yaw/pitch). + * + * @param cameraMatrix the camera intrinsics matrix in standard opencv form + * @param distCoeffs the camera distortion matrix in standard opencv form + * @param corners The distorted image points + * @return The undistorted image points + */ + public static List undistortPoints( + Matrix cameraMatrix, Matrix distCoeffs, List corners) { + var points_in = targetCornersToMat(corners); + var points_out = new MatOfPoint2f(); + var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage()); + var distCoeffsMat = matrixToMat(distCoeffs.getStorage()); + + Calib3d.undistortImagePoints(points_in, points_out, cameraMatrixMat, distCoeffsMat); + var corners_out = matToTargetCorners(points_out); + + points_in.release(); + points_out.release(); + cameraMatrixMat.release(); + distCoeffsMat.release(); + + return Arrays.asList(corners_out); + } + + /** + * Gets the (upright) rectangle which bounds this contour. + * + *

Note that rectangle size and position are stored with ints and do not have sub-pixel + * accuracy. + * + * @param corners The corners/points to be bounded + * @return Rectangle bounding the given corners + */ + public static Rect getBoundingRect(List corners) { + var corn = targetCornersToMat(corners); + var rect = Imgproc.boundingRect(corn); + corn.release(); + return rect; + } + /** + * Gets the rotated rectangle with minimum area which bounds this contour. + * + *

Note that rectangle size and position are stored with doubles and have sub-pixel accuracy. + * + * @param corners The corners/points to be bounded + * @return Rotated rectangle bounding the given corners + */ + public static RotatedRect getMinAreaRect(List corners) { + var corn = targetCornersToMat(corners); + var rect = Imgproc.minAreaRect(corn); + corn.release(); + return rect; + } + /** + * Get the area in pixels of this target's contour. It's important to note that this may be + * different from the area of the bounding rectangle around the contour. + * + * @param corners The corners defining this contour + * @return Area in pixels (units of corner x/y) + */ + public static double getContourAreaPx(List corners) { + var temp = targetCornersToMat(corners); + var corn = new MatOfPoint(temp.toArray()); + temp.release(); + + // outputHull gives us indices (of corn) that make a convex hull contour + var outputHull = new MatOfInt(); + Imgproc.convexHull(corn, outputHull); + int[] indices = outputHull.toArray(); + outputHull.release(); + var tempPoints = corn.toArray(); + var points = tempPoints.clone(); + for (int i = 0; i < indices.length; i++) { + points[i] = tempPoints[indices[i]]; + } + corn.fromArray(points); + // calculate area of the (convex hull) contour + double area = Imgproc.contourArea(corn); + corn.release(); + return area; + } + + /** + * Finds the transformation(s) that map the camera's pose to the target pose. The camera's pose + * relative to the target is determined by the supplied 3d points of the target's model and their + * associated 2d points imaged by the camera. + * + *

For planar targets, there may be an alternate solution which is plausible given the 2d image + * points. This has an associated "ambiguity" which describes the ratio of reprojection error + * between the "best" and "alternate" solution. + * + *

This method is intended for use with individual AprilTags, and will not work unless 4 points + * are provided. + * + * @param cameraMatrix the camera intrinsics matrix in standard opencv form + * @param distCoeffs the camera distortion matrix in standard opencv form + * @param modelTrls The translations of the object corners. These should have the object pose as + * their origin. These must come in a specific, pose-relative order (in NWU): + *

    + *
  • Point 0: [0, -squareLength / 2, squareLength / 2] + *
  • Point 1: [0, squareLength / 2, squareLength / 2] + *
  • Point 2: [0, squareLength / 2, -squareLength / 2] + *
  • Point 3: [0, -squareLength / 2, -squareLength / 2] + *
+ * + * @param imageCorners The projection of these 3d object points into the 2d camera image. The + * order should match the given object point translations. + * @return The resulting transformation that maps the camera pose to the target pose and the + * ambiguity if an alternate solution is available. + */ + public static PNPResults solvePNP_SQUARE( + Matrix cameraMatrix, + Matrix distCoeffs, + List modelTrls, + List imageCorners) { + // IPPE_SQUARE expects our corners in a specific order + modelTrls = reorderCircular(modelTrls, true, -1); + imageCorners = reorderCircular(imageCorners, true, -1); + // translate to opencv classes + var objectPoints = translationToTvec(modelTrls.toArray(new Translation3d[0])); + var imagePoints = targetCornersToMat(imageCorners); + var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage()); + var distCoeffsMat = matrixToMat(distCoeffs.getStorage()); + var rvecs = new ArrayList(); + var tvecs = new ArrayList(); + var rvec = Mat.zeros(3, 1, CvType.CV_32F); + var tvec = Mat.zeros(3, 1, CvType.CV_32F); + var reprojectionError = new Mat(); + // calc rvecs/tvecs and associated reprojection error from image points + Calib3d.solvePnPGeneric( + objectPoints, + imagePoints, + cameraMatrixMat, + distCoeffsMat, + rvecs, + tvecs, + false, + Calib3d.SOLVEPNP_IPPE_SQUARE, + rvec, + tvec, + reprojectionError); + + float[] errors = new float[2]; + reprojectionError.get(0, 0, errors); + // convert to wpilib coordinates + var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0))); + + Transform3d alt = null; + if (tvecs.size() > 1) { + alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1))); + } + + // release our Mats from native memory + objectPoints.release(); + imagePoints.release(); + cameraMatrixMat.release(); + distCoeffsMat.release(); + for (var v : rvecs) v.release(); + for (var v : tvecs) v.release(); + rvec.release(); + tvec.release(); + reprojectionError.release(); + + if (alt != null) return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]); + else return new PNPResults(best, errors[0]); + } + /** + * Finds the transformation that maps the camera's pose to the target pose. The camera's pose + * relative to the target is determined by the supplied 3d points of the target's model and their + * associated 2d points imaged by the camera. + * + *

This method is intended for use with multiple targets and has no alternate solutions. There + * must be at least 3 points. + * + * @param cameraMatrix the camera intrinsics matrix in standard opencv form + * @param distCoeffs the camera distortion matrix in standard opencv form + * @param objectTrls The translations of the object corners, relative to the field. + * @param imageCorners The projection of these 3d object points into the 2d camera image. The + * order should match the given object point translations. + * @return The resulting transformation that maps the camera pose to the target pose. If the 3d + * model points are supplied relative to the origin, this transformation brings the camera to + * the origin. + */ + public static PNPResults solvePNP_SQPNP( + Matrix cameraMatrix, + Matrix distCoeffs, + List objectTrls, + List imageCorners) { + // translate to opencv classes + var objectPoints = translationToTvec(objectTrls.toArray(new Translation3d[0])); + var imagePoints = targetCornersToMat(imageCorners); + var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage()); + var distCoeffsMat = matrixToMat(distCoeffs.getStorage()); + var rvecs = new ArrayList(); + var tvecs = new ArrayList(); + var rvec = Mat.zeros(3, 1, CvType.CV_32F); + var tvec = Mat.zeros(3, 1, CvType.CV_32F); + var reprojectionError = new Mat(); + // calc rvec/tvec from image points + Calib3d.solvePnPGeneric( + objectPoints, + imagePoints, + cameraMatrixMat, + distCoeffsMat, + rvecs, + tvecs, + false, + Calib3d.SOLVEPNP_SQPNP, + rvec, + tvec, + reprojectionError); + + float[] error = new float[1]; + reprojectionError.get(0, 0, error); + // convert to wpilib coordinates + var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0))); + + // release our Mats from native memory + objectPoints.release(); + imagePoints.release(); + cameraMatrixMat.release(); + distCoeffsMat.release(); + for (var v : rvecs) v.release(); + for (var v : tvecs) v.release(); + rvec.release(); + tvec.release(); + reprojectionError.release(); + + return new PNPResults(best, error[0]); + } +} diff --git a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java new file mode 100644 index 000000000..4359b20c2 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java @@ -0,0 +1,72 @@ +/* + * MIT License + * + * Copyright (c) 2022 PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.photonvision.estimation; + +import edu.wpi.first.math.geometry.Transform3d; + +/** + * The best estimated transformation from solvePnP, and possibly an alternate transformation + * depending on the solvePNP method. If an alternate solution is present, the ambiguity value + * represents the ratio of reprojection error in the best solution to the alternate (best / + * alternate). + * + *

Note that the coordinate frame of these transforms depends on the implementing solvePnP + * method. + */ +public class PNPResults { + public final Transform3d best; + public final double bestReprojErr; + + /** + * Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal + * to the best solution. + */ + public final Transform3d alt; + /** If no alternate solution is found, this is bestReprojErr */ + public final double altReprojErr; + /** If no alternate solution is found, this is 0 */ + public final double ambiguity; + + public PNPResults() { + this(new Transform3d(), new Transform3d(), 0, 0, 0); + } + + public PNPResults(Transform3d best, double bestReprojErr) { + this(best, best, 0, bestReprojErr, bestReprojErr); + } + + public PNPResults( + Transform3d best, + Transform3d alt, + double ambiguity, + double bestReprojErr, + double altReprojErr) { + this.best = best; + this.alt = alt; + this.ambiguity = ambiguity; + this.bestReprojErr = bestReprojErr; + this.altReprojErr = altReprojErr; + } +} diff --git a/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java new file mode 100644 index 000000000..5d5c8474f --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java @@ -0,0 +1,114 @@ +/* + * MIT License + * + * Copyright (c) 2022 PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.photonvision.estimation; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import java.util.stream.Collectors; + +/** + * Represents a transformation that first rotates a pose around the origin, and then translates it. + */ +public class RotTrlTransform3d { + private final Translation3d trl; + private final Rotation3d rot; + + public RotTrlTransform3d() { + this(new Rotation3d(), new Translation3d()); + } + /** + * Creates a rotation-translation transformation from a Transform3d. + * + *

Applying this transformation to poses will preserve their current origin-to-pose transform + * as if the origin was transformed by these components. + * + * @param trf The origin transformation + */ + public RotTrlTransform3d(Transform3d trf) { + this(trf.getRotation(), trf.getTranslation()); + } + /** + * A rotation-translation transformation. + * + *

Applying this transformation to poses will preserve their current origin-to-pose transform + * as if the origin was transformed by these components. + * + * @param rot The rotation component + * @param trl The translation component + */ + public RotTrlTransform3d(Rotation3d rot, Translation3d trl) { + this.rot = rot; + this.trl = trl; + } + /** + * The rotation-translation transformation that makes poses in the world consider this pose as the + * new origin, or change the basis to this pose. + * + * @param pose The new origin + */ + public static RotTrlTransform3d makeRelativeTo(Pose3d pose) { + return new RotTrlTransform3d(pose.getRotation(), pose.getTranslation()).inverse(); + } + + /** The inverse of this transformation. Applying the inverse will "undo" this transformation. */ + public RotTrlTransform3d inverse() { + var inverseRot = rot.unaryMinus(); + var inverseTrl = trl.rotateBy(inverseRot).unaryMinus(); + return new RotTrlTransform3d(inverseRot, inverseTrl); + } + + /** This transformation as a Transform3d (as if of the origin) */ + public Transform3d getTransform() { + return new Transform3d(trl, rot); + } + /** The translation component of this transformation */ + public Translation3d getTranslation() { + return trl; + } + /** The rotation component of this transformation */ + public Rotation3d getRotation() { + return rot; + } + + public Translation3d apply(Translation3d trl) { + return apply(new Pose3d(trl, new Rotation3d())).getTranslation(); + } + ; + + public List applyTrls(List trls) { + return trls.stream().map(t -> apply(t)).collect(Collectors.toList()); + } + + public Pose3d apply(Pose3d pose) { + return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl), pose.getRotation().plus(rot)); + } + + public List applyPoses(List poses) { + return poses.stream().map(p -> apply(p)).collect(Collectors.toList()); + } +} diff --git a/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java new file mode 100644 index 000000000..7cbd8291e --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java @@ -0,0 +1,116 @@ +/* + * MIT License + * + * Copyright (c) 2022 PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.photonvision.estimation; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import java.util.ArrayList; +import java.util.List; +import java.util.stream.Collectors; + +/** Describes the 3d model of a target. */ +public class TargetModel { + /** + * Translations of this target's vertices relative to its pose. If this target is spherical, this + * list has one translation with x == radius. + */ + public final List vertices; + + public final boolean isPlanar; + public final boolean isSpherical; + + public static final TargetModel kTag16h5 = + new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6)); + + /** Creates a rectangular, planar target model given the width and height. */ + public TargetModel(double widthMeters, double heightMeters) { + this.vertices = + List.of( + // this order is relevant for AprilTag compatibility + new Translation3d(0, -widthMeters / 2.0, -heightMeters / 2.0), + new Translation3d(0, widthMeters / 2.0, -heightMeters / 2.0), + new Translation3d(0, widthMeters / 2.0, heightMeters / 2.0), + new Translation3d(0, -widthMeters / 2.0, heightMeters / 2.0)); + this.isPlanar = true; + this.isSpherical = false; + } + + /** + * Creates a spherical target model which has similar dimensions when viewed from any angle. This + * model will only have one vertex which has x == radius. + */ + public TargetModel(double diameterMeters) { + this.vertices = List.of(new Translation3d(diameterMeters / 2.0, 0, 0)); + this.isPlanar = false; + this.isSpherical = true; + } + + /** + * Creates a target model from arbitrary 3d vertices. Automatically determines if the given + * vertices are planar(x == 0). More than 2 vertices must be given. + * + * @param vertices Translations representing the vertices of this target model relative to its + * pose. + */ + public TargetModel(List vertices) { + this.isSpherical = false; + if (vertices == null || vertices.size() <= 2) { + vertices = new ArrayList<>(); + this.isPlanar = false; + } else { + boolean cornersPlanar = true; + for (Translation3d corner : vertices) { + if (corner.getX() != 0) cornersPlanar = false; + } + this.isPlanar = cornersPlanar; + } + this.vertices = vertices; + } + + /** + * This target's vertices offset from its field pose. + * + *

Note: If this target is spherical, only one vertex radius meters in front of the pose is + * returned. + */ + public List getFieldVertices(Pose3d targetPose) { + return vertices.stream() + .map(t -> targetPose.plus(new Transform3d(t, new Rotation3d())).getTranslation()) + .collect(Collectors.toList()); + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj instanceof TargetModel) { + var o = (TargetModel) obj; + return vertices.equals(o.vertices) && isPlanar == o.isPlanar && isSpherical == o.isSpherical; + } + return false; + } +} diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java new file mode 100644 index 000000000..0618b8ed8 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -0,0 +1,168 @@ +/* + * MIT License + * + * Copyright (c) 2022 PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.photonvision.estimation; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.ArrayList; +import java.util.List; +import org.photonvision.targeting.TargetCorner; + +public class VisionEstimation { + public static final TargetModel kTagModel = + new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6)); + + /** + * Performs solvePNP using 3d-2d point correspondences to estimate the field-to-camera + * transformation. If only one tag is visible, the result may have an alternate solution. + * + *

Note: The returned transformation is from the field origin to the camera pose! + * + * @param cameraMatrix the camera intrinsics matrix in standard opencv form + * @param distCoeffs the camera distortion matrix in standard opencv form + * @param corners The visible tag corners in the 2d image + * @param knownTags The known tag field poses corresponding to the visible tag IDs + * @return The transformation that maps the field origin to the camera pose + */ + public static PNPResults estimateCamPosePNP( + Matrix cameraMatrix, + Matrix distCoeffs, + List corners, + List knownTags) { + if (knownTags == null + || corners == null + || corners.size() != knownTags.size() * 4 + || knownTags.size() == 0) { + return new PNPResults(); + } + // single-tag pnp + if (corners.size() == 4) { + var camToTag = + OpenCVHelp.solvePNP_SQUARE( + cameraMatrix, distCoeffs, kTagModel.getFieldVertices(knownTags.get(0).pose), corners); + var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse()); + var altPose = new Pose3d(); + if (camToTag.ambiguity != 0) + altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse()); + + var bestTagToCam = camToTag.best.inverse(); + SmartDashboard.putNumberArray( + "multiTagBest_internal", + new double[] { + bestTagToCam.getX(), + bestTagToCam.getY(), + bestTagToCam.getZ(), + bestTagToCam.getRotation().getQuaternion().getW(), + bestTagToCam.getRotation().getQuaternion().getX(), + bestTagToCam.getRotation().getQuaternion().getY(), + bestTagToCam.getRotation().getQuaternion().getZ() + }); + + var o = new Pose3d(); + return new PNPResults( + new Transform3d(o, bestPose), + new Transform3d(o, altPose), + camToTag.ambiguity, + camToTag.bestReprojErr, + camToTag.altReprojErr); + } + // multi-tag pnp + else { + var objectTrls = new ArrayList(); + for (var tag : knownTags) objectTrls.addAll(kTagModel.getFieldVertices(tag.pose)); + var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners); + // var camToOrigin = OpenCVHelp.solveTagsPNPRansac(prop, objectTrls, corners); + return new PNPResults( + camToOrigin.best.inverse(), + camToOrigin.alt.inverse(), + camToOrigin.ambiguity, + camToOrigin.bestReprojErr, + camToOrigin.altReprojErr); + } + } + + // /** + // * The best estimated transformation to the target, and possibly an alternate + // * transformation + // * depending on the solvePNP method. If an alternate solution is present, the + // * ambiguity value + // * represents the ratio of reprojection error in the best solution to the + // * alternate (best / alternate). + // */ + // public static class PNPResults { + // public final Transform3d best; + // public final double bestReprojErr; + + // /** + // * Alternate, ambiguous solution from solvepnp. This may be empty + // * if no alternate solution is found. + // */ + // public final Transform3d alt; + // /** If no alternate solution is found, this is 0 */ + // public final double altReprojErr; + + // /** If no alternate solution is found, this is 0 */ + // public final double ambiguity; + + // public PNPResults() { + // this(new Transform3d(), new Transform3d(), 0, 0, 0); + // } + + // public PNPResults( + // Transform3d best, Transform3d alt, + // double ambiguity, double bestReprojErr, double altReprojErr) { + // this.best = best; + // this.alt = alt; + // this.ambiguity = ambiguity; + // this.bestReprojErr = bestReprojErr; + // this.altReprojErr = altReprojErr; + // } + // } + + /** + * The best estimated transformation (Rotation-translation composition) that maps a set of + * translations onto another with point correspondences, and its RMSE. + */ + public static class SVDResults { + public final RotTrlTransform3d trf; + /** If the result is invalid, this value is -1 */ + public final double rmse; + + public SVDResults() { + this(new RotTrlTransform3d(), -1); + } + + public SVDResults(RotTrlTransform3d trf, double rmse) { + this.trf = trf; + this.rmse = rmse; + } + } +} diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index d0732cd04..18c950278 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -26,6 +26,7 @@ #include #include +#include #include "PhotonVersion.h" #include "photonlib/Packet.h" @@ -57,6 +58,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, ledModePub(mainTable->GetIntegerTopic("ledMode").Publish()), ledModeSub(mainTable->GetIntegerTopic("ledMode").Subscribe(0)), versionEntry(mainTable->GetStringTopic("version").Subscribe("")), + cameraIntrinsicsSubscriber( + rootTable->GetDoubleArrayTopic("cameraIntrinsics").Subscribe({})), + cameraDistortionSubscriber( + rootTable->GetDoubleArrayTopic("cameraDistortion").Subscribe({})), driverModeSubscriber( rootTable->GetBooleanTopic("driverMode").Subscribe(false)), driverModePublisher( @@ -119,12 +124,26 @@ LEDMode PhotonCamera::GetLEDMode() const { return static_cast(static_cast(ledModeSub.Get())); } +std::optional PhotonCamera::GetCameraMatrix() { + auto camCoeffs = cameraIntrinsicsSubscriber.Get(); + if (camCoeffs.size() == 9) { + // clone should deal with ownership concerns? not sure + return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone(); + } + return std::nullopt; +} + void PhotonCamera::SetLEDMode(LEDMode mode) { ledModePub.Set(static_cast(static_cast(mode))); } -const std::string_view PhotonCamera::GetCameraName() const { - return m_cameraName; +std::optional PhotonCamera::GetDistCoeffs() { + auto distCoeffs = cameraDistortionSubscriber.Get(); + if (distCoeffs.size() == 5) { + // clone should deal with ownership concerns? not sure + return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone(); + } + return std::nullopt; } void PhotonCamera::VerifyVersion() { diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index 16ef76611..c9e59c49e 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -36,6 +36,9 @@ #include #include #include +#include +#include +#include #include #include "photonlib/PhotonCamera.h" @@ -43,6 +46,16 @@ #include "photonlib/PhotonTrackedTarget.h" namespace photonlib { + +namespace detail { +cv::Point3d ToPoint3d(const frc::Translation3d& translation); +std::optional> CalcTagCorners( + int tagID, const frc::AprilTagFieldLayout& aprilTags); +frc::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec); +cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX, + units::meter_t cornerY, frc::Pose3d tagPose); +} // namespace detail + PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, PoseStrategy strat, PhotonCamera&& cam, frc::Transform3d robotToCamera) @@ -53,6 +66,17 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, lastPose(frc::Pose3d()), referencePose(frc::Pose3d()) {} +void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { + if (strategy == MULTI_TAG_PNP) { + FRC_ReportError( + frc::warn::Warning, + "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", + ""); + strategy = LOWEST_AMBIGUITY; + } + multiTagFallbackStrategy = strategy; +} + std::optional PhotonPoseEstimator::Update() { auto result = camera.GetLatestResult(); return Update(result); @@ -64,6 +88,11 @@ std::optional PhotonPoseEstimator::Update( return std::nullopt; } + return Update(result, this->strategy); +} + +std::optional PhotonPoseEstimator::Update( + PhotonPipelineResult result, PoseStrategy strategy) { std::optional ret = std::nullopt; switch (strategy) { @@ -83,6 +112,9 @@ std::optional PhotonPoseEstimator::Update( case AVERAGE_BEST_TARGETS: ret = AverageBestTargetsStrategy(result); break; + case ::photonlib::MULTI_TAG_PNP: + ret = MultiTagPnpStrategy(result); + break; default: FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!", ""); @@ -127,7 +159,7 @@ std::optional PhotonPoseEstimator::LowestAmbiguityStrategy( fiducialPose.value() .TransformBy(bestTarget.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - result.GetTimestamp()}; + result.GetTimestamp(), result.GetTargets()}; } std::optional @@ -147,14 +179,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy( target.GetFiducialId()); continue; } - frc::Pose3d targetPose = fiducialPose.value(); + frc::Pose3d const targetPose = fiducialPose.value(); - units::meter_t alternativeDifference = units::math::abs( + units::meter_t const alternativeDifference = units::math::abs( m_robotToCamera.Z() - targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) .Z()); - units::meter_t bestDifference = units::math::abs( + units::meter_t const bestDifference = units::math::abs( m_robotToCamera.Z() - targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z()); @@ -163,14 +195,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy( pose = EstimatedRobotPose{ targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - result.GetTimestamp()}; + result.GetTimestamp(), result.GetTargets()}; } if (bestDifference < smallestHeightDifference) { smallestHeightDifference = bestDifference; pose = EstimatedRobotPose{ targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - result.GetTimestamp()}; + result.GetTimestamp(), result.GetTargets()}; } } @@ -204,9 +236,9 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy( targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()); - units::meter_t alternativeDifference = units::math::abs( + units::meter_t const alternativeDifference = units::math::abs( referencePose.Translation().Distance(altPose.Translation())); - units::meter_t bestDifference = units::math::abs( + units::meter_t const bestDifference = units::math::abs( referencePose.Translation().Distance(bestPose.Translation())); if (alternativeDifference < smallestDifference) { smallestDifference = alternativeDifference; @@ -221,7 +253,113 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy( } } - return EstimatedRobotPose{pose, stateTimestamp}; + return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets()}; +} + +std::optional> detail::CalcTagCorners( + int tagID, const frc::AprilTagFieldLayout& aprilTags) { + if (auto tagPose = aprilTags.GetTagPose(tagID); tagPose.has_value()) { + return std::array{TagCornerToObjectPoint(-3_in, -3_in, *tagPose), + TagCornerToObjectPoint(+3_in, -3_in, *tagPose), + TagCornerToObjectPoint(+3_in, +3_in, *tagPose), + TagCornerToObjectPoint(-3_in, +3_in, *tagPose)}; + } else { + return std::nullopt; + } +} + +cv::Point3d detail::ToPoint3d(const frc::Translation3d& translation) { + return cv::Point3d(-translation.Y().value(), -translation.Z().value(), + +translation.X().value()); +} + +cv::Point3d detail::TagCornerToObjectPoint(units::meter_t cornerX, + units::meter_t cornerY, + frc::Pose3d tagPose) { + frc::Translation3d cornerTrans = + tagPose.Translation() + + frc::Translation3d(0.0_m, cornerX, cornerY).RotateBy(tagPose.Rotation()); + return ToPoint3d(cornerTrans); +} + +frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { + using namespace frc; + using namespace units; + + cv::Mat R; + cv::Rodrigues(rvec, R); // R is 3x3 + + R = R.t(); // rotation of inverse + cv::Mat tvecI = -R * tvec; // translation of inverse + + Vectord<3> tv; + tv[0] = +tvecI.at(2, 0); + tv[1] = -tvecI.at(0, 0); + tv[2] = -tvecI.at(1, 0); + Vectord<3> rv; + rv[0] = +rvec.at(2, 0); + rv[1] = -rvec.at(0, 0); + rv[2] = +rvec.at(1, 0); + + return Pose3d(Translation3d(meter_t{tv[0]}, meter_t{tv[1]}, meter_t{tv[2]}), + Rotation3d( + // radian_t{rv[0]}, + // radian_t{rv[1]}, + // radian_t{rv[2]} + rv, radian_t{rv.norm()})); +} + +std::optional PhotonPoseEstimator::MultiTagPnpStrategy( + PhotonPipelineResult result) { + using namespace frc; + + if (!result.HasTargets() || result.GetTargets().size() < 2) { + return Update(result, this->multiTagFallbackStrategy); + } + + auto const targets = result.GetTargets(); + + // List of corners mapped from 3d space (meters) to the 2d camera screen + // (pixels). + std::vector objectPoints; + std::vector imagePoints; + + // Add all target corners to main list of corners + for (auto target : targets) { + int id = target.GetFiducialId(); + if (auto const tagCorners = detail::CalcTagCorners(id, aprilTags); + tagCorners.has_value()) { + auto const targetCorners = target.GetDetectedCorners(); + for (size_t cornerIdx = 0; cornerIdx < 4; ++cornerIdx) { + imagePoints.emplace_back(targetCorners[cornerIdx].first, + targetCorners[cornerIdx].second); + objectPoints.emplace_back((*tagCorners)[cornerIdx]); + } + } + } + + if (imagePoints.empty()) { + return std::nullopt; + } + + // Use OpenCV ITERATIVE solver + cv::Mat const rvec(3, 1, cv::DataType::type); + cv::Mat const tvec(3, 1, cv::DataType::type); + + auto const camMat = camera.GetCameraMatrix(); + auto const distCoeffs = camera.GetDistCoeffs(); + if (!camMat || !distCoeffs) { + return std::nullopt; + } + + cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(), + rvec, tvec, false, cv::SOLVEPNP_SQPNP); + + Pose3d const pose = detail::ToPose3d(tvec, rvec); + + return photonlib::EstimatedRobotPose( + pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(), + result.GetTargets()); } std::optional @@ -247,7 +385,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) { return EstimatedRobotPose{ targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - result.GetLatency()}; + result.GetTimestamp(), result.GetTargets()}; } totalAmbiguity += 1. / target.GetPoseAmbiguity(); @@ -261,12 +399,12 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) { for (std::pair>& pair : tempPoses) { - double weight = (1. / pair.second.first) / totalAmbiguity; + double const weight = (1. / pair.second.first) / totalAmbiguity; transform = transform + pair.first.Translation() * weight; rotation = rotation + pair.first.Rotation() * weight; } return EstimatedRobotPose{frc::Pose3d(transform, rotation), - result.GetTimestamp()}; + result.GetTimestamp(), result.GetTargets()}; } } // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h index 2c641ebc4..3f8c27571 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -40,6 +41,10 @@ #include "photonlib/PhotonPipelineResult.h" +namespace cv { +class Mat; +} // namespace cv + namespace photonlib { enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 }; @@ -144,6 +149,9 @@ class PhotonCamera { */ const std::string_view GetCameraName() const; + std::optional GetCameraMatrix(); + std::optional GetDistCoeffs(); + /** * Returns whether the latest target result has targets. * This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should @@ -178,6 +186,9 @@ class PhotonCamera { nt::IntegerSubscriber ledModeSub; nt::StringSubscriber versionEntry; + nt::DoubleArraySubscriber cameraIntrinsicsSubscriber; + nt::DoubleArraySubscriber cameraDistortionSubscriber; + nt::BooleanSubscriber driverModeSubscriber; nt::BooleanPublisher driverModePublisher; nt::IntegerSubscriber ledModeSubscriber; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h index 367043d73..01c91778e 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h @@ -33,13 +33,18 @@ #include "photonlib/PhotonCamera.h" #include "photonlib/PhotonPipelineResult.h" +namespace cv { +class Mat; +} // namespace cv + namespace photonlib { -enum PoseStrategy : int { - LOWEST_AMBIGUITY, +enum PoseStrategy { + LOWEST_AMBIGUITY = 0, CLOSEST_TO_CAMERA_HEIGHT, CLOSEST_TO_REFERENCE_POSE, CLOSEST_TO_LAST_POSE, - AVERAGE_BEST_TARGETS + AVERAGE_BEST_TARGETS, + MULTI_TAG_PNP }; struct EstimatedRobotPose { @@ -49,8 +54,14 @@ struct EstimatedRobotPose { * the same timebase as the RoboRIO FPGA Timestamp */ units::second_t timestamp; - EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_) - : estimatedPose(pose_), timestamp(time_) {} + /** A list of the targets used to compute this pose */ + wpi::SmallVector targetsUsed; + + EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_, + std::span targets) + : estimatedPose(pose_), + timestamp(time_), + targetsUsed(targets.data(), targets.data() + targets.size()) {} }; /** @@ -100,6 +111,14 @@ class PhotonPoseEstimator { */ inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; } + /** + * Set the Position Estimation Strategy used in multi-tag mode when + * only one tag can be seen. Must NOT be MULTI_TAG_PNP + * + * @param strategy the strategy to set + */ + void SetMultiTagFallbackStrategy(PoseStrategy strategy); + /** * Return the reference position that is being used by the estimator. * @@ -159,6 +178,7 @@ class PhotonPoseEstimator { private: frc::AprilTagFieldLayout aprilTags; PoseStrategy strategy; + PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY; PhotonCamera camera; frc::Transform3d m_robotToCamera; @@ -166,6 +186,9 @@ class PhotonPoseEstimator { frc::Pose3d lastPose; frc::Pose3d referencePose; + std::optional Update(PhotonPipelineResult result, + PoseStrategy strategy); + /** * Return the estimated position of the robot with the lowest position * ambiguity from a List of pipeline results. @@ -199,12 +222,21 @@ class PhotonPoseEstimator { std::optional ClosestToReferencePoseStrategy( PhotonPipelineResult result); + /** + * Return the pose calculation using all targets in view in the same PNP() + calculation + + * @return the estimated position of the robot in the FCS and the estimated + timestamp of this estimation. + */ + std::optional MultiTagPnpStrategy( + PhotonPipelineResult result); + /** * Return the average of the best target poses using ambiguity as weight. * @return the estimated position of the robot in the FCS and the estimated - timestamp of this - * estimation. + timestamp of this estimation. */ std::optional AverageBestTargetsStrategy( PhotonPipelineResult result); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index c16bff640..209d4a1c0 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -29,7 +29,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.hal.JNIWrapper; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -157,8 +156,6 @@ class PhotonPoseEstimatorTest { @Test void testClosestToCameraHeightStrategy() { - ArrayList> cameras = new ArrayList<>(); - PhotonCameraInjector cameraOne = new PhotonCameraInjector(); cameraOne.result = new PhotonPipelineResult( @@ -474,8 +471,6 @@ class PhotonPoseEstimatorTest { @Test void averageBestPoses() { - ArrayList> cameras = new ArrayList<>(); - PhotonCameraInjector cameraOne = new PhotonCameraInjector(); cameraOne.result = new PhotonPipelineResult( diff --git a/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java b/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java new file mode 100644 index 000000000..61e1d060b --- /dev/null +++ b/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java @@ -0,0 +1,101 @@ +/* + * MIT License + * + * Copyright (c) 2022 PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.photonvision.estimation; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.cscore.CameraServerCvJNI; +import edu.wpi.first.hal.JNIWrapper; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.net.WPINetJNI; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTablesJNI; +import edu.wpi.first.util.CombinedRuntimeLoader; +import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.io.IOException; +import org.junit.jupiter.api.BeforeAll; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; + +public class ApriltagWorkbenchTest { + @BeforeAll + public static void setUp() { + JNIWrapper.Helper.setExtractOnStaticLoad(false); + WPIUtilJNI.Helper.setExtractOnStaticLoad(false); + NetworkTablesJNI.Helper.setExtractOnStaticLoad(false); + WPINetJNI.Helper.setExtractOnStaticLoad(false); + CameraServerCvJNI.Helper.setExtractOnStaticLoad(false); + + try { + CombinedRuntimeLoader.loadLibraries( + ApriltagWorkbenchTest.class, + "wpiutiljni", + "ntcorejni", + "wpinetjni", + "wpiHaljni", + "cscorejnicvstatic"); + } catch (Exception e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + + // No version check for testing + PhotonCamera.setVersionCheckEnabled(false); + } + + // @Test + public void testMeme() throws IOException, InterruptedException { + NetworkTableInstance instance = NetworkTableInstance.getDefault(); + instance.stopServer(); + // set the NT server if simulating this code. + // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]" + // for coprocessor + instance.setServer("localhost"); + instance.startClient4("myRobot"); + + var robotToCamera = new Transform3d(); + var cam = new PhotonCamera("WPI2023"); + var tagLayout = + AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); + + var pe = + new PhotonPoseEstimator( + tagLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, cam, robotToCamera); + + var field = new Field2d(); + SmartDashboard.putData(field); + + while (!Thread.interrupted()) { + Thread.sleep(500); + + var ret = pe.update(); + System.out.println(ret); + + field.setRobotPose(ret.get().estimatedPose.toPose2d()); + } + } +} diff --git a/photon-server/networktables.json b/photon-server/networktables.json new file mode 100644 index 000000000..fe51488c7 --- /dev/null +++ b/photon-server/networktables.json @@ -0,0 +1 @@ +[] diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index ec3d4935d..08a8dca19 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -5,6 +5,11 @@ apply from: "${rootDir}/shared/common.gradle" dependencies { implementation wpilibTools.deps.wpilibJava("wpimath") implementation "org.ejml:ejml-simple:0.41" + + // Junit + testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2") + testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2") + testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2") } tasks.withType(JavaCompile) { @@ -19,4 +24,8 @@ java { withSourcesJar() } +test { + useJUnitPlatform() +} + apply from: "publish.gradle" diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java index 9464dabf1..02a8f0774 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java @@ -182,4 +182,18 @@ public class Packet { } return packetData[readPos++] == 1; } + + public void encode(double[] data) { + for (double d : data) { + encode(d); + } + } + + public double[] decodeDoubleArray(int len) { + double[] ret = new double[len]; + for (int i = 0; i < len; i++) { + ret[i] = decodeDouble(); + } + return ret; + } } diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 8107b4d17..5a0d31eae 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -64,6 +64,10 @@ public class NTTopicSet { public IntegerTopic heartbeatTopic; public IntegerPublisher heartbeatPublisher; + // Camera Calibration + public DoubleArrayPublisher cameraIntrinsicsPublisher; + public DoubleArrayPublisher cameraDistortionPublisher; + public void updateEntries() { rawBytesEntry = subTable @@ -93,6 +97,9 @@ public class NTTopicSet { heartbeatTopic = subTable.getIntegerTopic("heartbeat"); heartbeatPublisher = heartbeatTopic.publish(); + + cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish(); + cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish(); } @SuppressWarnings("DuplicatedCode") @@ -113,5 +120,10 @@ public class NTTopicSet { if (targetSkewEntry != null) targetSkewEntry.close(); if (bestTargetPosX != null) bestTargetPosX.close(); if (bestTargetPosY != null) bestTargetPosY.close(); + + if (heartbeatPublisher != null) heartbeatPublisher.close(); + + if (cameraIntrinsicsPublisher != null) cameraIntrinsicsPublisher.close(); + if (cameraDistortionPublisher != null) cameraDistortionPublisher.close(); } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index b4b82a58e..f2a529245 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -19,7 +19,6 @@ package org.photonvision.targeting; import java.util.ArrayList; import java.util.List; -import java.util.Objects; import org.photonvision.common.dataflow.structures.Packet; /** Represents a pipeline result from a PhotonCamera. */ @@ -123,21 +122,6 @@ public class PhotonPipelineResult { return new ArrayList<>(targets); } - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - PhotonPipelineResult that = (PhotonPipelineResult) o; - boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0; - boolean targetsMatch = that.targets.equals(targets); - return latencyMatch && targetsMatch; - } - - @Override - public int hashCode() { - return Objects.hash(latencyMillis, targets); - } - /** * Populates the fields of the pipeline result from the packet. * @@ -178,4 +162,33 @@ public class PhotonPipelineResult { // Return the packet. return packet; } + + @Override + public int hashCode() { + final int prime = 31; + int result = 1; + result = prime * result + ((targets == null) ? 0 : targets.hashCode()); + long temp; + temp = Double.doubleToLongBits(latencyMillis); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(timestampSeconds); + result = prime * result + (int) (temp ^ (temp >>> 32)); + return result; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + PhotonPipelineResult other = (PhotonPipelineResult) obj; + if (targets == null) { + if (other.targets != null) return false; + } else if (!targets.equals(other.targets)) return false; + if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) + return false; + if (Double.doubleToLongBits(timestampSeconds) + != Double.doubleToLongBits(other.timestampSeconds)) return false; + return true; + } } diff --git a/photonlib-cpp-examples/apriltagExample/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/apriltagExample/.wpilib/wpilib_preferences.json new file mode 100644 index 000000000..cf67d2d09 --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2023", + "teamNumber": 5 +} diff --git a/photonlib-cpp-examples/apriltagExample/networktables.json b/photonlib-cpp-examples/apriltagExample/networktables.json new file mode 100644 index 000000000..fe51488c7 --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/networktables.json @@ -0,0 +1 @@ +[] diff --git a/photonlib-cpp-examples/apriltagExample/simgui-window.json b/photonlib-cpp-examples/apriltagExample/simgui-window.json new file mode 100644 index 000000000..79d676b16 --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/simgui-window.json @@ -0,0 +1,81 @@ +{ + "Docking": { + "Data": [] + }, + "MainWindow": { + "GLOBAL": { + "fps": "120", + "height": "880", + "maximized": "0", + "style": "0", + "userScale": "2", + "width": "1652", + "xpos": "268", + "ypos": "82" + } + }, + "Table": { + "0x542B5671,2": { + "Column 0 Width": "391", + "Column 1 Width": "156", + "RefScale": "13" + } + }, + "Window": { + "###/SmartDashboard/Field": { + "Collapsed": "0", + "Pos": "514,2", + "Size": "517,341" + }, + "###FMS": { + "Collapsed": "0", + "Pos": "36,663", + "Size": "283,146" + }, + "###Joysticks": { + "Collapsed": "0", + "Pos": "373,500", + "Size": "796,206" + }, + "###Keyboard 0 Settings": { + "Collapsed": "0", + "Pos": "149,98", + "Size": "300,560" + }, + "###NetworkTables": { + "Collapsed": "0", + "Pos": "663,464", + "Size": "750,365" + }, + "###NetworkTables Info": { + "Collapsed": "0", + "Pos": "520,330", + "Size": "750,145" + }, + "###Other Devices": { + "Collapsed": "0", + "Pos": "1025,20", + "Size": "250,695" + }, + "###System Joysticks": { + "Collapsed": "0", + "Pos": "5,350", + "Size": "192,218" + }, + "###Timing": { + "Collapsed": "0", + "Pos": "5,150", + "Size": "135,127" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Robot State": { + "Collapsed": "0", + "Pos": "5,20", + "Size": "92,99" + } + } +} diff --git a/photonlib-cpp-examples/apriltagExample/simgui.json b/photonlib-cpp-examples/apriltagExample/simgui.json new file mode 100644 index 000000000..bb09beb6e --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/simgui.json @@ -0,0 +1,40 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d" + }, + "windows": { + "/SmartDashboard/Field": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "CameraPublisher": { + "open": true + }, + "SmartDashboard": { + "open": true + }, + "photonvision": { + "open": true, + "photonvision": { + "open": true + } + } + } + }, + "NetworkTables Info": { + "Clients": { + "open": true + }, + "Server": { + "open": true + }, + "visible": true + } +} diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h index e45517b8f..6f1a22968 100644 --- a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h +++ b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h @@ -23,6 +23,7 @@ */ #pragma once + #include #include @@ -35,8 +36,8 @@ class PhotonCameraWrapper { public: photonlib::PhotonPoseEstimator m_poseEstimator{ frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp), - photonlib::LOWEST_AMBIGUITY, - std::move(photonlib::PhotonCamera{"WPI2023"}), frc::Transform3d{}}; + photonlib::MULTI_TAG_PNP, std::move(photonlib::PhotonCamera{"WPI2023"}), + frc::Transform3d{}}; inline std::optional Update( frc::Pose2d estimatedPose) { diff --git a/shared/config.gradle b/shared/config.gradle index aa248f283..104faaec0 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -9,8 +9,8 @@ nativeUtils.wpi.configureDependencies { wpiVersion = wpilibVersion wpimathVersion = wpilibVersion niLibVersion = "2023.3.0" - opencvVersion = opencvVersion - googleTestVersion = "1.11.0-4" + opencvVersion = "4.6.0-4" + googleTestVersion = "1.12.1-1" imguiVersion = "1.86-1" }