diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 1e2db3d24a..8bbdaf5744 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -370,7 +370,7 @@ private Optional multiTagPNPStrategy( var pnpResults = VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags); + cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags); // try fallback strategy if solvePNP fails for some reason if (!pnpResults.isPresent) return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); diff --git a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java index 0f851c1de4..89adf94bcf 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java @@ -36,10 +36,18 @@ * method. */ public class PNPResults { - /** If this result is valid. A false value indicates there was an error in estimation, and this result should not be used. */ + /** + * If this result is valid. A false value indicates there was an error in estimation, and this + * result should not be used. + */ public final boolean isPresent; - /** The best-fit transform. The coordinate frame of this transform depends on the method which gave this result. */ + + /** + * The best-fit transform. The coordinate frame of this transform depends on the method which gave + * this result. + */ public final Transform3d best; + /** Reprojection error of the best solution, in pixels */ public final double bestReprojErr; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java index 8bc6f93436..dad9eed5b0 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -68,7 +68,8 @@ public static List getVisibleLayoutTags( * @param distCoeffs The camera distortion matrix in standard opencv form * @param visTags The visible tags reported by PV * @param tagLayout The known tag layout on the field - * @return The transformation that maps the field origin to the camera pose. Ensure the {@link PNPResults} are present before utilizing them. + * @return The transformation that maps the field origin to the camera pose. Ensure the {@link + * PNPResults} are present before utilizing them. */ public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, @@ -91,7 +92,9 @@ public static PNPResults estimateCamPosePNP( // single-tag pnp if (visTags.size() == 1) { - var camToTag = OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners); + var camToTag = + OpenCVHelp.solvePNP_SQUARE( + cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners); if (!camToTag.isPresent) return new PNPResults(); var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse()); var altPose = new Pose3d(); @@ -100,12 +103,11 @@ public static PNPResults estimateCamPosePNP( var o = new Pose3d(); return new PNPResults( - new Transform3d(o, bestPose), - new Transform3d(o, altPose), - camToTag.ambiguity, - camToTag.bestReprojErr, - camToTag.altReprojErr - ); + new Transform3d(o, bestPose), + new Transform3d(o, altPose), + camToTag.ambiguity, + camToTag.bestReprojErr, + camToTag.altReprojErr); } // multi-tag pnp else { @@ -114,12 +116,11 @@ public static PNPResults estimateCamPosePNP( var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners); if (!camToOrigin.isPresent) return new PNPResults(); return new PNPResults( - camToOrigin.best.inverse(), - camToOrigin.alt.inverse(), - camToOrigin.ambiguity, - camToOrigin.bestReprojErr, - camToOrigin.altReprojErr - ); + camToOrigin.best.inverse(), + camToOrigin.alt.inverse(), + camToOrigin.ambiguity, + camToOrigin.bestReprojErr, + camToOrigin.altReprojErr); } } } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index d68ffd93f7..81a20d0d36 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -352,10 +352,10 @@ public PhotonPipelineResult process( if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP pnpSim = OpenCVHelp.solvePNP_SQUARE( - prop.getIntrinsics(), - prop.getDistCoeffs(), - tgt.getModel().vertices, - noisyTargetCorners); + prop.getIntrinsics(), + prop.getDistCoeffs(), + tgt.getModel().vertices, + noisyTargetCorners); if (!pnpSim.isPresent) continue; centerRot = prop.getPixelRot( diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java index 999149d42a..594439e847 100644 --- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -194,10 +194,7 @@ public void testSolvePNP_SQUARE() { prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); var pnpSim = OpenCVHelp.solvePNP_SQUARE( - prop.getIntrinsics(), - prop.getDistCoeffs(), - target.getModel().vertices, - targetCorners); + prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); } @@ -225,10 +222,7 @@ public void testSolvePNP_SQPNP() { prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); var pnpSim = OpenCVHelp.solvePNP_SQPNP( - prop.getIntrinsics(), - prop.getDistCoeffs(), - target.getModel().vertices, - targetCorners); + prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners); var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); assertSame(actualRelation.camToTarg, estRelation.camToTarg); }