diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java index d05ad15638..f05f75e31a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java @@ -146,8 +146,10 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) new TargetCalculationParameters( false, null, null, null, null, frameStaticProperties)); - var correctedBestPose = MathUtils.convertOpenCVtoPhotonPose(target.getBestCameraToTarget3d()); - var correctedAltPose = MathUtils.convertOpenCVtoPhotonPose(target.getAltCameraToTarget3d()); + var correctedBestPose = + MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d()); + var correctedAltPose = + MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d()); target.setBestCameraToTarget3d( new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation())); diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java index 1e29f6ee7f..1f1729c53e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -19,6 +19,7 @@ import edu.wpi.first.apriltag.AprilTagDetection; import edu.wpi.first.apriltag.AprilTagPoseEstimate; import edu.wpi.first.math.geometry.Transform3d; +import java.util.ArrayList; import java.util.HashMap; import java.util.List; import org.opencv.core.CvType;