Undistort corners in umich pose estimation (#699)

* Undistort corners in umich pose estimation

Add tag corner unit test

Delete hellooo.jpg

Update Draw3dTargetsPipe.java

Update FileFrameProvider.java

* Update AprilTagTest.java

photon-core/src/main/java/org/photonvision/common/util/TestUtils.java

@@ -196,7 +196,8 @@ public class TestUtils {
 
     public enum ApriltagTestImages {
         kRobots,
-        kTag1_640_480;
+        kTag1_640_480,
+        kTag_corner_1280;
 
         public final Path path;
 
@@ -302,6 +303,7 @@ public class TestUtils {
 
     private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";
     private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json";
+    public static final String LIMELIGHT_480P_CAL_FILE = "limelight_1280_720.json";
 
     public static CameraCalibrationCoefficients getCoeffs(String filename, boolean testMode) {
         try {

photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java

@@ -21,6 +21,11 @@ import edu.wpi.first.apriltag.AprilTagDetection;
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
 import edu.wpi.first.apriltag.AprilTagPoseEstimator;
 import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
+import java.util.Arrays;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipe.CVPipe;
 
 public class AprilTagPoseEstimatorPipe
@@ -37,8 +42,55 @@ public class AprilTagPoseEstimatorPipe
         super();
     }
 
+    MatOfPoint2f temp = new MatOfPoint2f();
+
     @Override
     protected AprilTagPoseEstimate process(AprilTagDetection in) {
+        // Save the corner points of our detection to an array
+        Point corners[] = new Point[4];
+        for (int i = 0; i < 4; i++) {
+            corners[i] = new Point(in.getCornerX(i), in.getCornerY(i));
+        }
+        System.out.println("Before: " + Arrays.toString(corners));
+        // And shove into our matofpoints
+        temp.fromArray(corners);
+
+        System.out.println("Size " + temp.size().toString());
+
+        // Probably overwrites what was in temp before. I hope
+        Calib3d.undistortPoints(
+                temp,
+                temp,
+                params.calibration.getCameraIntrinsicsMat(),
+                params.calibration.getDistCoeffsMat());
+
+        // Save out undistorted corners
+        corners = temp.toArray();
+
+        // Apriltagdetection expects an array in form [x1 y1 x2 y2 ...]
+        var fixedCorners = new double[8];
+        for (int i = 0; i < 4; i++) {
+            // https://stackoverflow.com/questions/8499984/how-to-undistort-points-in-camera-shot-coordinates-and-obtain-corresponding-undi
+            // perform transformation.
+            // In fact this is equivalent to multiplication to camera matrix
+
+            fixedCorners[i * 2] = corners[i].x * params.config.fx + params.config.cx;
+            fixedCorners[i * 2 + 1] = corners[i].y * params.config.fy + params.config.cy;
+        }
+        System.out.println("After: " + Arrays.toString(fixedCorners));
+
+        // Create a new Detection with the fixed corners
+        var corrected =
+                new AprilTagDetection(
+                        in.getFamily(),
+                        in.getId(),
+                        in.getHamming(),
+                        in.getDecisionMargin(),
+                        in.getHomography(),
+                        in.getCenterX(),
+                        in.getCenterY(),
+                        fixedCorners);
+
         return m_poseEstimator.estimateOrthogonalIteration(in, params.nIters);
     }
 
@@ -57,11 +109,14 @@ public class AprilTagPoseEstimatorPipe
 
     public static class AprilTagPoseEstimatorPipeParams {
         final AprilTagPoseEstimator.Config config;
+        final CameraCalibrationCoefficients calibration;
         final int nIters;
 
-        public AprilTagPoseEstimatorPipeParams(Config config, int nIters) {
+        public AprilTagPoseEstimatorPipeParams(
+                Config config, CameraCalibrationCoefficients cal, int nIters) {
             this.config = config;
             this.nIters = nIters;
+            this.calibration = cal;
         }
 
         @Override

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dAprilTagsPipe.java

@@ -30,6 +30,7 @@ public class Draw3dAprilTagsPipe extends Draw3dTargetsPipe {
                 FrameDivisor divisor) {
             super(shouldDraw, cameraCalibrationCoefficients, targetModel, divisor);
             this.shouldDrawHull = false;
+            this.redistortPoints = true;
         }
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java

@@ -93,7 +93,12 @@ public class Draw3dTargetsPipe
                         params.cameraCalibrationCoefficients.getDistCoeffsMat(),
                         tempMat,
                         jac);
-                // Distort the points so they match the image they're being overlaid on
+
+                if (params.redistortPoints) {
+                    // Distort the points so they match the image they're being overlaid on
+                    distortPoints(tempMat, tempMat);
+                }
+
                 var bottomPoints = tempMat.toList();
 
                 Calib3d.projectPoints(
@@ -104,6 +109,12 @@ public class Draw3dTargetsPipe
                         params.cameraCalibrationCoefficients.getDistCoeffsMat(),
                         tempMat,
                         jac);
+
+                if (params.redistortPoints) {
+                    // Distort the points so they match the image they're being overlaid on
+                    distortPoints(tempMat, tempMat);
+                }
+
                 var topPoints = tempMat.toList();
 
                 dividePointList(bottomPoints);
@@ -290,6 +301,8 @@ public class Draw3dTargetsPipe
         public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
         public final FrameDivisor divisor;
 
+        public boolean redistortPoints = false;
+
         public Draw3dContoursParams(
                 boolean shouldDraw,
                 CameraCalibrationCoefficients cameraCalibrationCoefficients,

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -97,7 +97,9 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
 
                 poseEstimatorPipe.setParams(
                         new AprilTagPoseEstimatorPipeParams(
-                                new Config(tagWidth, fx, fy, cx, cy), settings.numIterations));
+                                new Config(tagWidth, fx, fy, cx, cy),
+                                frameStaticProperties.cameraCalibration,
+                                settings.numIterations));
             }
         }
     }

photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java

@@ -18,7 +18,6 @@
 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;
@@ -33,7 +32,7 @@ import org.photonvision.vision.target.TrackedTarget;
 
 public class AprilTagTest {
     @BeforeEach
-    public void Init() throws IOException {
+    public void Init() {
         TestUtils.loadLibraries();
     }
 
@@ -67,22 +66,21 @@ public class AprilTagTest {
 
         // Draw on input
         var outputPipe = new OutputStreamPipeline();
-        outputPipe.process(
-                pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+        var ret =
+                outputPipe.process(
+                        pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
 
-        TestUtils.showImage(
-                pipelineResult.inputAndOutputFrame.colorImage.getMat(), "Pipeline output", 999999);
+        TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
 
         // these numbers are not *accurate*, but they are known and expected
         var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
         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);
+        Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 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 to be forward, or -X in world space
         Assertions.assertEquals(
@@ -94,6 +92,49 @@ public class AprilTagTest {
         Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1);
     }
 
+    @Test
+    public void testApriltagDistorted() {
+        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;
+        pipeline.getSettings().tagFamily = AprilTagFamily.kTag16h5;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag_corner_1280, false),
+                        TestUtils.WPI2020Image.FOV,
+                        TestUtils.getCoeffs(TestUtils.LIMELIGHT_480P_CAL_FILE, false));
+        frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
+
+        CVPipelineResult pipelineResult;
+        try {
+            pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+            printTestResults(pipelineResult);
+        } catch (RuntimeException e) {
+            // For now, will throw coz rotation3d ctor
+            return;
+        }
+
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        var ret =
+                outputPipe.process(
+                        pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+
+        TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
+
+        // these numbers are not *accurate*, but they are known and expected
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
+        Assertions.assertEquals(4, pose.getTranslation().getX(), 0.2);
+        Assertions.assertEquals(2, pose.getTranslation().getY(), 0.2);
+        Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);
+    }
+
     private static void printTestResults(CVPipelineResult pipelineResult) {
         double fps = 1000 / pipelineResult.getLatencyMillis();
         System.out.println(