diff --git a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java index ba8c988c9..57310f0f4 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java @@ -33,7 +33,6 @@ public class TestUtils { try { CameraServerCvJNI.forceLoad(); // PicamJNI.forceLoad(); - // AprilTagJNI.forceLoad(); } catch (IOException ex) { // ignored } @@ -165,7 +164,8 @@ public class TestUtils { } public enum ApriltagTestImages { - kRobots; + kRobots, + kTag1_640_480; public final Path path; @@ -233,6 +233,10 @@ public class TestUtils { return getTestImagesPath(testMode).resolve(image.path); } + public static Path getApriltagImagePath(ApriltagTestImages image, boolean testMode) { + return getTestImagesPath(testMode).resolve(image.path); + } + public static Path getPowercellImagePath(PowercellTestImages image, boolean testMode) { return getPowercellPath(testMode).resolve(image.path); } 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 759038a36..b2e697310 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 @@ -19,14 +19,17 @@ package org.photonvision.common.util.math; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.CoordinateSystem; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.util.WPIUtilJNI; import java.util.Arrays; import java.util.List; +import org.opencv.core.Mat; public class MathUtils { MathUtils() {} @@ -159,25 +162,33 @@ public class MathUtils { // TODO: Refactor into new pipe? public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) { // CameraToTarget _should_ be in opencv-land EDN - - var pose = - CoordinateSystem.convert( - new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU()); - - return pose; + return CoordinateSystem.convert( + new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU()); } - public static Pose3d convertApriltagtoPhotonPose(Transform3d cameraToTarget3d) { - // CameraToTarget _should_ be in opencv-land EDN - var pose = - CoordinateSystem.convert( - new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU()); + /* + * The AprilTag pose rotation outputs are X left, Y down, Z away from the tag with the tag facing + * the camera upright and the camera facing the target parallel to the floor. But our OpenCV + * solvePNP code would have X left, Y up, Z towards the camera with the target facing the camera + * and both parallel to the floor. So we apply a base rotation to the rotation component of the + * apriltag pose to make it consistent with the EDN system that OpenCV uses, internally a 180 + * rotation about the X axis + */ + private static final Rotation3d APRILTAG_BASE_ROTATION = + new Rotation3d(VecBuilder.fill(1, 0, 0), Units.degreesToRadians(180)); - // Apply an extra rotation so that at zero pose, X ls left, Y is up, and Z is towards the camera - // to a camera facing along the +X axis of the field parallel with the ground plane - // So we need a 180 flip about X axis - var newRotation = pose.getRotation().rotateBy(new Rotation3d(0, Math.PI, 0)); + /** + * Apply a 180 degree rotation about X to the rotation component of a given Apriltag pose. This + * aligns it with the OpenCV poses we use in other places. + */ + public static Transform3d convertApriltagtoOpenCV(Transform3d pose) { + var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation()); + return new Transform3d(pose.getTranslation(), ocvRotation); + } - return new Pose3d(pose.getTranslation(), newRotation); + public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) { + var angle = rotation.getAngle(); + var axis = rotation.getAxis().times(angle); + rvecOutput.put(0, 0, axis.getData()); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java b/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java index ff55b04f4..2826b6868 100644 --- a/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java @@ -19,8 +19,8 @@ package org.photonvision.vision.apriltag; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; -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.Arrays; @@ -81,11 +81,11 @@ public class DetectionResult { return error2; } - public Pose3d getPoseResult1() { + public Transform3d getPoseResult1() { return poseResult1; } - public Pose3d getPoseResult2() { + public Transform3d getPoseResult2() { return poseResult2; } @@ -96,9 +96,9 @@ public class DetectionResult { double centerX, centerY; double[] corners; - Pose3d poseResult1; + Transform3d poseResult1; double error1; - Pose3d poseResult2; + Transform3d poseResult2; double error2; public DetectionResult( @@ -125,12 +125,12 @@ public class DetectionResult { this.error1 = err1; this.poseResult1 = - new Pose3d( + new Transform3d( new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]), new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr))); this.error2 = err2; this.poseResult2 = - new Pose3d( + new Transform3d( new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]), new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr))); } 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 d24865ff5..4d1317730 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 @@ -25,7 +25,9 @@ import org.opencv.calib3d.Calib3d; import org.opencv.core.Mat; 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.imgproc.Imgproc; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; @@ -132,8 +134,51 @@ public class Draw3dTargetsPipe ColorHelper.colorToScalar(Color.orange), 3); } + + // Draw X, Y and Z axis + MatOfPoint3f pointMat = new MatOfPoint3f(); + var list = + List.of( + new Point3(0, 0, 0), + new Point3(0.2, 0, 0), + new Point3(0, 0.2, 0), + new Point3(0, 0, 0.2)); + pointMat.fromList(list); + + Calib3d.projectPoints( + pointMat, + target.getCameraRelativeRvec(), + target.getCameraRelativeTvec(), + params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(), + params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(), + tempMat, + jac); + var axisPoints = tempMat.toList(); + dividePointList(axisPoints); + + // Red = x, green y, blue z + Imgproc.line( + in.getLeft(), + axisPoints.get(0), + axisPoints.get(1), + ColorHelper.colorToScalar(Color.RED), + 3); + Imgproc.line( + in.getLeft(), + axisPoints.get(0), + axisPoints.get(2), + ColorHelper.colorToScalar(Color.GREEN), + 3); + Imgproc.line( + in.getLeft(), + axisPoints.get(0), + axisPoints.get(3), + ColorHelper.colorToScalar(Color.BLUE), + 3); + tempMat.release(); jac.release(); + pointMat.release(); } // draw corners 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 63ff35cb5..b6c0a0ced 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 @@ -140,7 +140,7 @@ public class AprilTagPipeline extends CVPipeline. + */ + +package org.photonvision.vision.pipeline; + +import edu.wpi.first.math.geometry.Translation3d; +import java.io.IOException; +import java.util.stream.Collectors; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.photonvision.common.util.TestUtils; +import org.photonvision.vision.apriltag.AprilTagJNI; +import org.photonvision.vision.camera.QuirkyCamera; +import org.photonvision.vision.frame.provider.FileFrameProvider; +import org.photonvision.vision.pipeline.result.CVPipelineResult; +import org.photonvision.vision.target.TargetModel; +import org.photonvision.vision.target.TrackedTarget; + +public class AprilTagTest { + @BeforeEach + public void Init() throws IOException { + TestUtils.loadLibraries(); + AprilTagJNI.forceLoad(); + } + + @Test + public void testApriltagFacingCamera() { + var pipeline = new AprilTagPipeline(); + + pipeline.getSettings().inputShouldShow = true; + pipeline.getSettings().outputShouldDraw = true; + pipeline.getSettings().solvePNPEnabled = true; + pipeline.getSettings().cornerDetectionAccuracyPercentage = 4; + pipeline.getSettings().cornerDetectionUseConvexHulls = true; + pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag; + + var frameProvider = + new FileFrameProvider( + TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_640_480, false), + TestUtils.WPI2020Image.FOV, + TestUtils.get2020LifeCamCoeffs(false)); + + CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera); + printTestResults(pipelineResult); + + // Draw on input + var outputPipe = new OutputStreamPipeline(); + outputPipe.process( + pipelineResult.inputFrame, + pipelineResult.outputFrame, + pipeline.getSettings(), + pipelineResult.targets); + + TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999); + + // these numbers are not *accurate*, but they are known and expected + var pose = pipelineResult.targets.get(0).getCameraToTarget3d(); + Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2); + Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2); + Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2); + + var objX = new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY(); + var objY = new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ(); + var objZ = new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX(); + System.out.printf("Object x %.2f y %.2f z %.2f\n", objX, objY, objZ); + + // We expect the object X axis to be to the right, or negative-Y in world space + Assertions.assertEquals( + -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY(), 0.08); + // We expect the object Y axis to be up, or +Z in world space + Assertions.assertEquals( + 1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ(), 0.08); + // We expect the object Z axis to towards the camera, or negative-X in world space + Assertions.assertEquals( + -1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX(), 0.08); + } + + private static void printTestResults(CVPipelineResult pipelineResult) { + double fps = 1000 / pipelineResult.getLatencyMillis(); + System.out.println( + "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)"); + System.out.println("Found " + pipelineResult.targets.size() + " valid targets"); + System.out.println( + "Found targets at " + + pipelineResult.targets.stream() + .map(TrackedTarget::getCameraToTarget3d) + .collect(Collectors.toList())); + } +} diff --git a/test-resources/testimages/apriltag/tag1_640_480.jpg b/test-resources/testimages/apriltag/tag1_640_480.jpg new file mode 100644 index 000000000..a18957d65 Binary files /dev/null and b/test-resources/testimages/apriltag/tag1_640_480.jpg differ