revert commit

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

@@ -34,20 +34,18 @@ protected List<AprilTagDetection> process(List<AprilTagDetection> in) {
         List<AprilTagDetection> results = new ArrayList<>();
 
         for (AprilTagDetection detection : in) {
-            double[] offsetCorners = offsetCorners(detection);
-            offsetHomography(reformatCorners(offsetCorners));
-
             results.add(
                     new AprilTagDetection(
                             detection.getFamily(),
                             detection.getId(),
                             detection.getHamming(),
                             detection.getDecisionMargin(),
-                            offsetHomography(reformatCorners(offsetCorners)),
+                            offsetHomography(detection),
                             detection.getCenterX() + this.params.x,
                             detection.getCenterY() + this.params.y,
-                            offsetCorners));
+                            offsetCorners(detection)));
         }
+
         return results;
     }
 
@@ -63,87 +61,6 @@ private double[] offsetHomography(AprilTagDetection detection) {
         return h;
     }
 
-    private double[] offsetHomography(double[][] corners) {
-        double corr_arr[][] = new double[4][4];
-
-        for (int i = 0; i < 4; i++) {
-            corr_arr[i][0] = (i == 0 || i == 3) ? -1 : 1;
-            corr_arr[i][1] = (i == 0 || i == 1) ? -1 : 1;
-            corr_arr[i][2] = corners[i][0];
-            corr_arr[i][3] = corners[i][1];
-        }
-
-        // New homography from tag to original is H' = T_o * H
-        return homography_compute2(corr_arr);
-    }
-
-    static double[] homography_compute2(double c[][]) {
-        double A[] = {
-            c[0][0], c[0][1], 1, 0, 0, 0, -c[0][0] * c[0][2], -c[0][1] * c[0][2], c[0][2], 0, 0, 0,
-                    c[0][0], c[0][1], 1, -c[0][0] * c[0][3], -c[0][1] * c[0][3], c[0][3],
-            c[1][0], c[1][1], 1, 0, 0, 0, -c[1][0] * c[1][2], -c[1][1] * c[1][2], c[1][2], 0, 0, 0,
-                    c[1][0], c[1][1], 1, -c[1][0] * c[1][3], -c[1][1] * c[1][3], c[1][3],
-            c[2][0], c[2][1], 1, 0, 0, 0, -c[2][0] * c[2][2], -c[2][1] * c[2][2], c[2][2], 0, 0, 0,
-                    c[2][0], c[2][1], 1, -c[2][0] * c[2][3], -c[2][1] * c[2][3], c[2][3],
-            c[3][0], c[3][1], 1, 0, 0, 0, -c[3][0] * c[3][2], -c[3][1] * c[3][2], c[3][2], 0, 0, 0,
-                    c[3][0], c[3][1], 1, -c[3][0] * c[3][3], -c[3][1] * c[3][3], c[3][3],
-        };
-
-        double epsilon = 1e-10;
-
-        // Eliminate.
-        for (int col = 0; col < 8; col++) {
-            // Find best row to swap with.
-            double max_val = 0;
-            int max_val_idx = -1;
-            for (int row = col; row < 8; row++) {
-                double val = Math.abs(A[row * 9 + col]);
-                if (val > max_val) {
-                    max_val = val;
-                    max_val_idx = row;
-                }
-            }
-
-            if (max_val_idx < 0) {
-                return null;
-            }
-
-            if (max_val < epsilon) {
-                System.out.print("WRN: Matrix is singular.\n");
-                return null;
-            }
-
-            // Swap to get best row.
-            if (max_val_idx != col) {
-                for (int i = col; i < 9; i++) {
-                    double tmp = A[col * 9 + i];
-                    A[col * 9 + i] = A[max_val_idx * 9 + i];
-                    A[max_val_idx * 9 + i] = tmp;
-                }
-            }
-
-            // Do eliminate.
-            for (int i = col + 1; i < 8; i++) {
-                double f = A[i * 9 + col] / A[col * 9 + col];
-                A[i * 9 + col] = 0;
-                for (int j = col + 1; j < 9; j++) {
-                    A[i * 9 + j] -= f * A[col * 9 + j];
-                }
-            }
-        }
-
-        // Back solve.
-        for (int col = 7; col >= 0; col--) {
-            double sum = 0;
-            for (int i = col + 1; i < 8; i++) {
-                sum += A[col * 9 + i] * A[i * 9 + 8];
-            }
-            A[col * 9 + 8] = (A[col * 9 + 8] - sum) / A[col * 9 + col];
-        }
-        double[] H = {A[8], A[17], A[26], A[35], A[44], A[53], A[62], A[71], 1};
-        return H;
-    }
-
     private double[] offsetCorners(AprilTagDetection detection) {
         return new double[] {
             detection.getCornerX(0) + this.params.x,
@@ -156,21 +73,4 @@ private double[] offsetCorners(AprilTagDetection detection) {
             detection.getCornerY(3) + this.params.y
         };
     }
-
-    private double[][] reformatCorners(double[] corners) {
-        double[][] nCorners = new double[4][2];
-
-        nCorners[0][0] = corners[6];
-        nCorners[0][1] = corners[7];
-
-        nCorners[1][0] = corners[0];
-        nCorners[1][1] = corners[1];
-
-        nCorners[2][0] = corners[2];
-        nCorners[2][1] = corners[3];
-
-        nCorners[3][0] = corners[4];
-        nCorners[3][1] = corners[5];
-        return nCorners;
-    }
 }

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

@@ -152,12 +152,12 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
         sumPipeNanosElapsed += croppedFrame.nanosElapsed;
 
         CVPipeResult<List<AprilTagDetection>> tagDetectionPipeResult;
-        tagDetectionPipeResult = aprilTagDetectionPipe.run(frame.processedImage);
+        tagDetectionPipeResult = aprilTagDetectionPipe.run(croppedFrame.output);
         sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
         croppedFrame.output.release();
 
-        // tagDetectionPipeResult = uncropPipe.run(tagDetectionPipeResult.output);
-        // sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
+        tagDetectionPipeResult = uncropPipe.run(tagDetectionPipeResult.output);
+        sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
 
         List<AprilTagDetection> detections = tagDetectionPipeResult.output;
         List<AprilTagDetection> usedDetections = new ArrayList<>();