Delete old Aruco

Author: Gold856

photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java

@@ -25,20 +25,16 @@
 import org.opencv.core.Size;
 import org.opencv.core.TermCriteria;
 import org.opencv.imgproc.Imgproc;
-import org.opencv.objdetect.Objdetect;
 import org.photonvision.jni.ArucoNanoV5Detector;
 import org.photonvision.jni.ArucoNanoV5Detector.DetectionResult;
 import org.photonvision.vision.aruco.ArucoDetectionResult;
-import org.photonvision.vision.aruco.PhotonArucoDetector;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.pipe.CVPipe;
 
 public class ArucoDetectionPipe
         extends CVPipe<CVMat, List<ArucoDetectionResult>, ArucoDetectionPipeParams>
         implements Releasable {
-    // ArucoDetector wrapper class
-    private final PhotonArucoDetector photonDetector = new PhotonArucoDetector();
 
     // Ratio multiplied with image size and added to refinement window size
     private static final double kRefineWindowImageRatio = 0.004;
@@ -101,33 +97,9 @@ protected List<ArucoDetectionResult> process(CVMat in) {
 
     @Override
     public void setParams(ArucoDetectionPipeParams newParams) {
-        if (this.params == null || !this.params.equals(newParams)) {
-            System.out.println("Changing tag family to " + newParams.tagFamily);
-            photonDetector
-                    .getDetector()
-                    .setDictionary(Objdetect.getPredefinedDictionary(newParams.tagFamily));
-            var detectParams = photonDetector.getParams();
-
-            detectParams.set_adaptiveThreshWinSizeMin(newParams.threshMinSize);
-            detectParams.set_adaptiveThreshWinSizeStep(newParams.threshStepSize);
-            detectParams.set_adaptiveThreshWinSizeMax(newParams.threshMaxSize);
-            detectParams.set_adaptiveThreshConstant(newParams.threshConstant);
-
-            detectParams.set_errorCorrectionRate(newParams.errorCorrectionRate);
-
-            detectParams.set_useAruco3Detection(newParams.useAruco3);
-            detectParams.set_minSideLengthCanonicalImg(newParams.aruco3MinCanonicalImgSide);
-            detectParams.set_minMarkerLengthRatioOriginalImg((float) newParams.aruco3MinMarkerSideRatio);
-
-            photonDetector.setParams(detectParams);
-        }
-
         super.setParams(newParams);
     }
 
-    public PhotonArucoDetector getPhotonDetector() {
-        return photonDetector;
-    }
 
     private void drawCornerRefineWindow(Mat outputMat, Point corner, int windowSize) {
         int thickness = (int) (Math.ceil(Math.max(outputMat.cols(), outputMat.rows()) * 0.003));
@@ -146,6 +118,5 @@ public static List<ArucoDetectionResult> detect(Mat in) {
 
     @Override
     public void release() {
-        photonDetector.release();
     }
 }

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

@@ -30,13 +30,6 @@ public class ArucoDetectionPipeParams {
     public int threshMaxSize = 91;
     public double threshConstant = 10;
 
-    /**
-     * Bits allowed to be corrected, expressed as a ratio of the tag families theoretical maximum.
-     *
-     * <p>E.g. 36h11 = 11 * errorCorrectionRate = Max error bits
-     */
-    public double errorCorrectionRate = 0;
-
     /**
      * If obtained corners should be iteratively refined. This should always be on for 3D estimation.
      */
@@ -53,22 +46,6 @@ public class ArucoDetectionPipeParams {
 
     public boolean debugRefineWindow = false;
 
-    /**
-     * If the 'Aruco3' speedup should be used. This is similar to AprilTag's 'decimate' value, but
-     * automatically determined with the given parameters.
-     *
-     * <p>T_i = aruco3MinMarkerSideRatio, and T_c = aruco3MinCanonicalImgSide
-     *
-     * <p>Scale factor = T_c / (T_c + T_i * max(img_width, img_height))
-     */
-    public boolean useAruco3 = false;
-
-    /** Minimum side length of markers expressed as a ratio of the largest image dimension. */
-    public double aruco3MinMarkerSideRatio = 0.02;
-
-    /** Minimum side length of the canonical image (marker after undoing perspective distortion). */
-    public int aruco3MinCanonicalImgSide = 32;
-
     @Override
     public boolean equals(Object o) {
         if (this == o) return true;
@@ -79,13 +56,9 @@ public boolean equals(Object o) {
                 && threshStepSize == that.threshStepSize
                 && threshMaxSize == that.threshMaxSize
                 && threshConstant == that.threshConstant
-                && errorCorrectionRate == that.errorCorrectionRate
                 && useCornerRefinement == that.useCornerRefinement
                 && refinementMaxIterations == that.refinementMaxIterations
-                && refinementMinErrorPx == that.refinementMinErrorPx
-                && useAruco3 == that.useAruco3
-                && aruco3MinMarkerSideRatio == that.aruco3MinMarkerSideRatio
-                && aruco3MinCanonicalImgSide == that.aruco3MinCanonicalImgSide;
+                && refinementMinErrorPx == that.refinementMinErrorPx;
     }
 
     @Override
@@ -96,12 +69,8 @@ public int hashCode() {
                 threshStepSize,
                 threshMaxSize,
                 threshConstant,
-                errorCorrectionRate,
                 useCornerRefinement,
                 refinementMaxIterations,
-                refinementMinErrorPx,
-                useAruco3,
-                aruco3MinMarkerSideRatio,
-                aruco3MinCanonicalImgSide);
+                refinementMinErrorPx);
     }
 }

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

@@ -71,13 +71,13 @@ private Rotation3d rvecToRotation3d(Mat mat) {
 
     @Override
     protected AprilTagPoseEstimate process(ArucoDetectionResult in) {
-        // We receive 2d corners as (BL, BR, TR, TL) but we want (BR, BL, TL, TR)
+        // We receive 2d corners as (TL, TR, BR, BL) but we want (BR, BL, TL, TR)
         double[] xCorn = in.getXCorners();
         double[] yCorn = in.getYCorners();
-        imagePoints.put(0, 0, new float[] {(float) xCorn[1], (float) yCorn[1]});
-        imagePoints.put(1, 0, new float[] {(float) xCorn[0], (float) yCorn[0]});
-        imagePoints.put(2, 0, new float[] {(float) xCorn[3], (float) yCorn[3]});
-        imagePoints.put(3, 0, new float[] {(float) xCorn[2], (float) yCorn[2]});
+        imagePoints.put(0, 0, new float[] {(float) xCorn[2], (float) yCorn[2]});
+        imagePoints.put(1, 0, new float[] {(float) xCorn[3], (float) yCorn[3]});
+        imagePoints.put(2, 0, new float[] {(float) xCorn[0], (float) yCorn[0]});
+        imagePoints.put(3, 0, new float[] {(float) xCorn[1], (float) yCorn[1]});
 
         float[] reprojErrors = new float[2];
         // very rarely the solvepnp solver returns NaN results, so we retry with slight noise added

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

@@ -15,23 +15,6 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 
-/*
- * Copyright (C) Photon Vision.
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <https://www.gnu.org/licenses/>.
- */
-
 package org.photonvision.vision.pipeline;
 
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
@@ -116,9 +99,6 @@ protected void setPipeParamsImpl() {
         params.useCornerRefinement = settings.useCornerRefinement;
         params.refinementMaxIterations = settings.refineNumIterations;
         params.refinementMinErrorPx = settings.refineMinErrorPx;
-        params.useAruco3 = settings.useAruco3;
-        params.aruco3MinMarkerSideRatio = settings.aruco3MinMarkerSideRatio;
-        params.aruco3MinCanonicalImgSide = settings.aruco3MinCanonicalImgSide;
         arucoDetectionPipe.setParams(params);
 
         if (frameStaticProperties.cameraCalibration != null) {

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

@@ -35,10 +35,6 @@ public class ArucoPipelineSettings extends AdvancedPipelineSettings {
     public int refineNumIterations = 30;
     public double refineMinErrorPx = 0.005;
 
-    public boolean useAruco3 = false;
-    public double aruco3MinMarkerSideRatio = 0.02;
-    public int aruco3MinCanonicalImgSide = 32;
-
     public boolean doMultiTarget = false;
     public boolean doSingleTargetAlways = false;
 

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

@@ -19,11 +19,14 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import java.io.IOException;
+
 import edu.wpi.first.math.geometry.Translation3d;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.TestUtils;
+import org.photonvision.jni.PhotonTargetingJniLoader;
 import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
@@ -32,8 +35,9 @@
 
 public class ArucoPipelineTest {
     @BeforeEach
-    public void setup() {
+    public void setup() throws IOException {
         TestUtils.loadLibraries();
+        PhotonTargetingJniLoader.load();
         ConfigManager.getInstance().load();
     }
 
@@ -51,9 +55,9 @@ public void testApriltagFacingCamera() {
 
         var frameProvider =
                 new FileFrameProvider(
-                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_640_480, false),
-                        TestUtils.WPI2020Image.FOV,
-                        TestUtils.get2020LifeCamCoeffs(false));
+                        TestUtils.getTestImagesPath(false).resolve(TestUtils.WPI2024Images.kSpeakerCenter_143in.path),
+                        TestUtils.WPI2024Images.FOV,
+                        TestUtils.get2023LifeCamCoeffs(true));
         frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
 
         CVPipelineResult pipelineResult;
@@ -69,24 +73,24 @@ public void testApriltagFacingCamera() {
         TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
 
         // these numbers are not *accurate*, but they are known and expected
-        var target = pipelineResult.targets.get(0);
+        var target = pipelineResult.targets.get(1); // tag 4
 
         // Test corner order
         var corners = target.getTargetCorners();
-        assertEquals(260, corners.get(0).x, 10);
-        assertEquals(245, corners.get(0).y, 10);
-        assertEquals(315, corners.get(1).x, 10);
-        assertEquals(245, corners.get(1).y, 10);
-        assertEquals(315, corners.get(2).x, 10);
-        assertEquals(190, corners.get(2).y, 10);
-        assertEquals(260, corners.get(3).x, 10);
-        assertEquals(190, corners.get(3).y, 10);
+        assertEquals(650, corners.get(3).x, 10);
+        assertEquals(540, corners.get(3).y, 10);
+        assertEquals(690, corners.get(2).x, 10);
+        assertEquals(540, corners.get(2).y, 10);
+        assertEquals(690, corners.get(1).x, 10);
+        assertEquals(500, corners.get(1).y, 10);
+        assertEquals(650, corners.get(0).x, 10);
+        assertEquals(500, corners.get(0).y, 10);
 
         var pose = target.getBestCameraToTarget3d();
         // Test pose estimate translation
-        assertEquals(2, pose.getTranslation().getX(), 0.2);
-        assertEquals(0.1, pose.getTranslation().getY(), 0.2);
-        assertEquals(0.0, pose.getTranslation().getZ(), 0.2);
+        assertEquals(4.8, pose.getTranslation().getX(), 0.2);
+        assertEquals(-0.2, pose.getTranslation().getY(), 0.2);
+        assertEquals(-0.7, pose.getTranslation().getZ(), 0.2);
 
         // Test pose estimate rotation
         // We expect the object axes to be in NWU, with the x-axis coming out of the tag