From 6f876f56755ca8c7b093e4e446602cd3649c6e33 Mon Sep 17 00:00:00 2001 From: amquake Date: Sat, 12 Aug 2023 00:41:31 -0700 Subject: [PATCH] spotless --- .../common/util/math/MathUtils.java | 34 +++++++++---------- .../vision/pipe/impl/Draw3dTargetsPipe.java | 2 +- .../vision/pipe/impl/SolvePNPPipe.java | 3 +- .../vision/pipeline/AprilTagPipeline.java | 6 ++-- 4 files changed, 23 insertions(+), 22 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java index 0862b8717b..cd645e1fbf 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java @@ -17,6 +17,7 @@ package org.photonvision.common.util.math; +import edu.wpi.first.apriltag.AprilTagPoseEstimate; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.CoordinateSystem; import edu.wpi.first.math.geometry.Pose3d; @@ -27,7 +28,6 @@ import java.util.Arrays; import java.util.List; import org.opencv.core.Mat; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; public class MathUtils { MathUtils() {} @@ -142,22 +142,20 @@ public static double lerp(double startValue, double endValue, double t) { /** * OpenCV uses the EDN coordinate system, but WPILib uses NWU. Converts a camera-to-target * transformation from EDN to NWU. - * - *

Note: The detected target's rvec and tvec perform a rotation-translation transformation which - * converts points in the target's coordinate system to the camera's. This means applying - * the transformation to the target point (0,0,0) for example would give the target's center - * relative to the camera. Conveniently, if we make a translation-rotation transformation out - * of these components instead, we get the transformation from the camera to the target.

- * + * + *

Note: The detected target's rvec and tvec perform a rotation-translation transformation + * which converts points in the target's coordinate system to the camera's. This means applying + * the transformation to the target point (0,0,0) for example would give the target's center + * relative to the camera. Conveniently, if we make a translation-rotation transformation out of + * these components instead, we get the transformation from the camera to the target. + * * @param cameraToTarget3d A camera-to-target Transform3d in EDN. * @return A camera-to-target Transform3d in NWU. */ public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTarget3d) { // TODO: Refactor into new pipe? return CoordinateSystem.convert( - cameraToTarget3d, - CoordinateSystem.EDN(), - CoordinateSystem.NWU()); + cameraToTarget3d, CoordinateSystem.EDN(), CoordinateSystem.NWU()); } /* @@ -166,7 +164,7 @@ public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTar * center out the camera lens. The x-axis is to the right in the image taken by the camera, and * y is down. The tag's coordinate frame is centered at the center of the tag, with x-axis to the * right, y-axis down, and z-axis into the tag." - * + * * This means our detected transformation will be in EDN. Our subsequent uses of the transformation, * however, assume the tag's z-axis point away from the tag instead of into it. This means we * have to correct the transformation's rotation. @@ -175,12 +173,12 @@ public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTar new Rotation3d(VecBuilder.fill(0, 1, 0), Units.degreesToRadians(180)); /** - * AprilTag returns a camera-to-tag transform in EDN, but the tag's z-axis points into - * the tag instead of away from it and towards the camera. This means we have to correct - * the transformation's rotation. - * - * @param pose The Transform3d with translation and rotation directly from the - * {@link AprilTagPoseEstimate}. + * AprilTag returns a camera-to-tag transform in EDN, but the tag's z-axis points into the tag + * instead of away from it and towards the camera. This means we have to correct the + * transformation's rotation. + * + * @param pose The Transform3d with translation and rotation directly from the {@link + * AprilTagPoseEstimate}. */ public static Transform3d convertApriltagtoOpenCV(Transform3d pose) { var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation()); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java index b27320c7cd..09ec84e6e5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java @@ -182,7 +182,7 @@ protected Void process(Pair> in) { axisPoints.get(1), ColorHelper.colorToScalar(Color.RED), 3); - + // box edges perpendicular to tag for (int i = 0; i < bottomPoints.size(); i++) { Imgproc.line( diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java index 496ebfca1e..6a887d3a18 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java @@ -97,7 +97,8 @@ private void calculateTargetPose(TrackedTarget target) { VecBuilder.fill(rVec.get(0, 0)[0], rVec.get(1, 0)[0], rVec.get(2, 0)[0]), Core.norm(rVec)); - Transform3d camToTarget = MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation)); + Transform3d camToTarget = + MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation)); target.setBestCameraToTarget3d(camToTarget); target.setAltCameraToTarget3d(new Transform3d()); } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index c1cc876b93..59c0833320 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -144,8 +144,10 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting new TargetCalculationParameters( false, null, null, null, null, frameStaticProperties)); - var correctedBestPose = MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d()); - var correctedAltPose = MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d()); + var correctedBestPose = + MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d()); + var correctedAltPose = + MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d()); target.setBestCameraToTarget3d( new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));