Add VO - Needs debugging

README.md

@@ -3,8 +3,8 @@ This is a project that aims to improve FRC vision accuracy using Visual Odometry
 
 Things to accomplish:
 - [x] Get custom pipeline running
-- [ ] Basic VO (look at https://github.com/thehummingbird/robotics_demos/blob/main/monocular_vo)
+- [needs debugging] Basic VO (look at https://github.com/thehummingbird/robotics_demos/blob/main/monocular_vo)
 - [ ] Inertial odometry using pidgen and sensor fusion (look at https://www.thinkautonomous.ai/blog/visual-inertial-odometry/)
-- [ ] Sensor fusion for physical odometry from RoboRiO
+- [?] Sensor fusion for physical odometry from RoboRiO
 - [ ] Mesh based localisation? https://youtube.com/watch?v=vMsV04emXHU https://dlnext.acm.org/doi/10.1007/978-3-031-20047-2_34
 - [ ] Optimise and refine.

photon-client/src/components/dashboard/tabs/CustomTestTab.vue

@@ -25,37 +25,59 @@ const interactiveCols = computed(() =>
 <template>
   <div v-if="currentPipelineSettings.pipelineType === PipelineType.CustomTest">
     <pv-slider
-      v-model="currentPipelineSettings.test1"
+      v-model="currentPipelineSettings.featureThreshold"
       class="pt-2"
       :slider-cols="interactiveCols"
-      label="Test 1"
+      label="Feature Threshold"
-      tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
+      tooltip="The sharpness required in a feature for FAST to detect it"
       :min="0"
-      :max="10"
+      :max="100"
       :step="1"
-      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ test1: value }, false)"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ featureThreshold: value }, false)"
     />
     <pv-slider
-      v-model="currentPipelineSettings.test2"
+      v-model="currentPipelineSettings.minFeatures"
       class="pt-2"
       :slider-cols="interactiveCols"
-      label="Test 2"
+      label="Minimum Features"
-      tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
+      tooltip="Minimum amount of features for calculation of camera translation"
       :min="0"
-      :max="10"
+      :max="1000"
       :step="1"
-      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ test2: value }, false)"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ minFeatures: value }, false)"
     />
     <pv-slider
-        v-model="currentPipelineSettings.test3"
+      v-model="currentPipelineSettings.imageDifferenceThreshold"
       class="pt-2"
       :slider-cols="interactiveCols"
-        label="Test 3"
+      label="Image Difference Threshold"
-        tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
+      tooltip="Minimum amount of difference between points to update position"
       :min="0"
-        :max="10"
+      :max="1000"
       :step="1"
-        @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ test3: value }, false)"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ imageDifferenceThreshold: value }, false)"
+    />
+    <pv-slider
+      v-model="currentPipelineSettings.essentialMatProb"
+      class="pt-2"
+      :slider-cols="interactiveCols"
+      label="Essential Matrix Prob"
+      tooltip="'Prob' for cv2.findEssentialMat"
+      :min="0"
+      :max="1"
+      :step="0.001"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ essentialMatProb: value }, false)"
+    />
+    <pv-slider
+      v-model="currentPipelineSettings.essentialMatThreshold"
+      class="pt-2"
+      :slider-cols="interactiveCols"
+      label="Essential Matrix Threshold"
+      tooltip="'Threshold' for cv2.findEssentialMat"
+      :min="0"
+      :max="1"
+      :step="0.001"
+      @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ essentialMatThreshold: value }, false)"
     />
   </div>
 </template>

photon-client/src/types/PipelineTypes.ts

@@ -319,9 +319,25 @@ export const DefaultObjectDetectionPipelineSettings: ObjectDetectionPipelineSett
 
 export interface CustomTestPipelineSettings extends PipelineSettings {
   pipelineType: PipelineType.CustomTest;
-  test1: number;
+
-  test2: number;
+  featureThreshold: number
-  test3: number;
+  minFeatures: number,
+  imageDifferenceThreshold: number,
+  essentialMatProb: number
+  essentialMatThreshold: number,
+
+  hammingDist: number;
+  numIterations: number;
+  decimate: number;
+  blur: number;
+  decisionMargin: number;
+  refineEdges: boolean;
+  debug: boolean;
+  threads: number;
+  tagFamily: AprilTagFamily;
+  doMultiTarget: boolean;
+  doSingleTargetAlways: boolean;
+
 }
 export type ConfigurableCustomTestPipelineSettings = Partial<
   Omit<CustomTestPipelineSettings, "pipelineType">
@@ -337,10 +353,24 @@ export const DefaultCustomTestPipelineSettings: CustomTestPipelineSettings = {
   cameraAutoExposure: true,
   ledMode: false,
 
+  featureThreshold: 1,
+  minFeatures: 500,
+  imageDifferenceThreshold: 150,
+  essentialMatProb: 0.999,
+  essentialMatThreshold: 1.,
 
-  test1: 1,
+
-  test2: 2,
+  hammingDist: 0,
-  test3: 3
+  numIterations: 40,
+  decimate: 1,
+  blur: 0,
+  decisionMargin: 35,
+  refineEdges: true,
+  debug: false,
+  threads: 4,
+  tagFamily: AprilTagFamily.Family36h11,
+  doMultiTarget: false,
+  doSingleTargetAlways: false
 };
 
 export interface Calibration3dPipelineSettings extends PipelineSettings {

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

@@ -0,0 +1,50 @@
+package org.photonvision.vision.pipe.impl;
+
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.photonvision.vision.pipeline.CustomTestPipelineSettings;
+import org.photonvision.vision.pipeline.PipelineType;
+
+public class VisualOdometryParams {
+    public int featureThreshold = 1;
+    public int minFeatures = 500;
+    public int imageDifferenceThreshold = 150;
+    public double essentialMatProb = 0.999;
+    public double essentialMatThreshold = 1.;
+    public double focal = 1.;
+    public Point pp = new Point();
+    public Mat cam_mat = new Mat();
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = super.hashCode();
+        long temp;
+
+        temp = Double.doubleToLongBits(featureThreshold);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(minFeatures);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(imageDifferenceThreshold);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(essentialMatProb);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(essentialMatThreshold);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (!super.equals(obj)) return false;
+        if (getClass() != obj.getClass()) return false;
+        VisualOdometryParams other = (VisualOdometryParams) obj;
+        if(featureThreshold != other.featureThreshold) return false;
+        if(minFeatures != other.minFeatures) return false;
+        if(essentialMatProb != other.essentialMatProb) return false;
+        if(essentialMatThreshold != other.essentialMatThreshold) return false;
+        return true;
+    }
+}

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

@@ -0,0 +1,246 @@
+package org.photonvision.vision.pipe.impl;
+
+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.geometry.Translation3d;
+import org.opencv.core.*;
+import org.opencv.features2d.FastFeatureDetector;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.vision.pipe.CVPipe;
+
+import java.util.ArrayList;
+import java.util.LinkedList;
+import java.util.List;
+
+import static org.opencv.calib3d.Calib3d.*;
+import static org.opencv.core.Core.meanStdDev;
+import static org.opencv.features2d.Features2d.drawKeypoints;
+import static org.opencv.video.Video.calcOpticalFlowPyrLK;
+
+public class VisualOdometryPipe extends CVPipe<Mat, Transform3d, VisualOdometryParams>{
+    private static final Scalar WHITE = new Scalar(255, 255, 255);
+    private static final Scalar BLUE = new Scalar(255, 0, 0);
+    private static final Scalar RED = new Scalar(0, 0, 255);
+    public boolean hasReset = false;
+
+    FastFeatureDetector fast = FastFeatureDetector.create();
+
+
+    MatOfByte status = new MatOfByte();
+
+    Mat E = new Mat();
+    Mat R = new Mat();
+    Mat t = new Mat();
+
+//    Mat R_f = new Mat();
+//    Mat t_f = new Mat();
+
+    Mat prevImage = new Mat();
+    MatOfPoint2f prevFeatures = new MatOfPoint2f();
+    MatOfPoint2f currFeatures = new MatOfPoint2f();
+
+    List<Point> currFeaturesList, prevFeaturesList;
+
+//    private Transform3d lastPosition = new Transform3d();
+
+    public void release() {
+        status.release();
+        E.release();
+        R.release();
+        t.release();
+        prevImage.release();
+        prevFeatures.release();
+        currFeatures.release();;
+        currFeatures = null;
+        prevFeatures = null;
+    }
+
+    @Override
+    protected Transform3d process(Mat frame_mat) {
+        Transform3d tf = estimate(frame_mat);
+
+        if(keypoints != null)
+            drawKeypoints(frame_mat, keypoints, frame_mat, RED);
+
+        if(currFeaturesList != null && prevFeaturesList != null)
+            for(int i = 0; i < currFeaturesList.size(); i++){
+                Point curPoint = currFeaturesList.get(i);
+                Point lastPoint = prevFeaturesList.get(i);
+
+                Imgproc.circle(frame_mat, curPoint, 5, WHITE, 1);
+                Imgproc.line(frame_mat, curPoint, lastPoint, BLUE, 1);
+            }
+
+        if(tf != null) {
+            return tf;
+        }
+        return new Transform3d();
+    }
+
+
+    private double featureTracking(Mat prevImage, Mat currImage, MatOfPoint2f prevFeatures, MatOfPoint2f currFeatures, MatOfByte status){
+        // 트래킹에 실패한 포인트들은 버린다.
+        MatOfFloat err = new MatOfFloat();
+        Size winSize = new Size(21, 21);
+        TermCriteria termcrit = new TermCriteria(TermCriteria.COUNT+TermCriteria.EPS, 30, 0.01);
+
+        calcOpticalFlowPyrLK(prevImage, currImage, prevFeatures, currFeatures, status, err, winSize, 3, termcrit, 0, 0.01);
+
+        double weight = 0;
+        // KLT 트래킹에 실패하거나 프레임 바깥으로 벗어난 포인트들은 버린다.
+        int indexCorrection = 0;
+        byte[] statusArray = status.toArray();
+        Point[] currFeaturesArray = currFeatures.toArray();
+        Point[] prevFeaturesArray = prevFeatures.toArray();
+
+        currFeaturesList = new LinkedList<>(currFeatures.toList());
+        prevFeaturesList = new LinkedList<>(prevFeatures.toList());
+
+        for(int i = 0; i < statusArray.length; i++){
+            Point pt = currFeaturesArray[i];
+
+            if((statusArray[i] == 0) || (pt.x < 0) || (pt.y < 0)){
+                prevFeaturesList.remove(i - indexCorrection);
+                currFeaturesList.remove(i - indexCorrection);
+
+                indexCorrection++;
+            } else {
+                Point before = prevFeaturesList.get(i - indexCorrection);
+                Point after = currFeaturesList.get(i - indexCorrection);
+                weight += (after.x - before.x) * (after.x - before.x) + (after.y - before.y)*(after.y - before.y);
+//                Point before = prevFeaturesArray[i - indexCorrection];
+//                weight += ((int)pt.x - before.x)*((int)pt.x - before.x) + ((int)pt.y - before.y)*((int)pt.y - before.y);
+            }
+        }
+
+        // currFeatures, prevFeatures를 필터한 특징점으로 교체함
+        currFeatures.fromList(currFeaturesList);
+        prevFeatures.fromList(prevFeaturesList);
+
+        if(prevFeaturesArray.length == 0)
+            return 0;
+        return weight / prevFeaturesArray.length;
+    }
+
+    MatOfKeyPoint keypoints;
+
+    private MatOfPoint2f featureDetection(Mat image) {
+//        System.out.println("New points!");
+        keypoints = new MatOfKeyPoint();
+
+        fast.detect(image, keypoints);
+
+        KeyPoint[] kps = keypoints.toArray();
+        ArrayList<Point> arrayOfPoints = new ArrayList<>();
+
+        for(int i = 0; i < kps.length; i++){
+            arrayOfPoints.add(kps[i].pt);
+        }
+
+        MatOfPoint2f matOfPoint = new MatOfPoint2f();
+        matOfPoint.fromList(arrayOfPoints);
+
+        return matOfPoint;
+    }
+
+    private Transform3d estimate(Mat currImage) {
+        Point[] prevFeaturesArray = prevFeatures.toArray();
+
+        if(prevImage.empty()){
+            prevImage = currImage.clone();
+            prevFeatures = featureDetection(prevImage);
+            System.out.println(4);
+            return null;
+        }  else if (prevFeaturesArray.length < params.minFeatures) {
+            prevFeatures = featureDetection(prevImage);
+            prevImage = currImage.clone();
+//            if(prevFeaturesArray.length <= 0){
+//                System.out.println("Can't detect features.");
+//            }
+            System.out.println(3);
+            return null;
+        }
+
+        double weight = featureTracking(prevImage, currImage, prevFeatures, currFeatures, status);
+
+        if(prevFeaturesArray.length <= 0){
+            prevFeatures = featureDetection(currImage);
+            prevImage = currImage.clone();
+            System.out.println(2);
+            return null;
+        }
+
+        if(weight < params.imageDifferenceThreshold) {
+            prevImage = currImage.clone();
+            currFeatures.copyTo(prevFeatures);
+            System.out.println(1);
+            return null;
+        }
+
+        try {
+            E = findEssentialMat(prevFeatures, currFeatures, params.cam_mat);// params.focal, params.pp);//, RANSAC, params.essentialMatProb, params.essentialMatThreshold);
+//            System.out.println(E.size());
+
+            double sum =0;
+            for(int x =0; x<E.width(); x++)
+                for(int y = 0; y<E.height(); y++)
+                    sum += E.get(y,x)[0];
+
+            System.out.println(sum);
+
+
+//            System.out.println("npoints = " + prevFeatures.checkVector(2));
+//            System.out.println("mask.checkVector(1) = " + status.checkVector(1));
+            recoverPose(E, currFeatures, prevFeatures, R, t);//, params.cam_mat);// params.focal, params.pp);//, status);
+        } catch (Exception e){
+            prevImage = currImage.clone();
+            currFeatures.copyTo(prevFeatures);
+            e.printStackTrace();
+            return null;
+        }
+
+//        if(R_f.empty()) {
+//            R_f = R.clone();
+//            t_f = t.clone();
+//        } else {
+////            Mat tmp = new Mat();
+////            Core.gemm(R_f, t, 1, new Mat(), 0, tmp);
+////            Core.add(t_f, tmp, t_f);
+////            Core.multiply(R, R_f, R_f);
+////            Core.gemm(R, R_f, 1, new Mat(), 0, R_f, 0);
+//        }
+
+        prevImage = currImage.clone();
+        currFeatures.copyTo(prevFeatures);
+
+//        System.out.println(R.size());
+
+        double pos_x = t.get(0,0)[0];
+        double pos_y = t.get(1,0)[0];
+        double pos_z = t.get(2,0)[0];
+//
+        double roll  = Math.atan2( R.get(2,1)[0],R.get(2,2)[0]) * Math.PI / 180;
+        double pitch = Math.atan2(-R.get(2,0)[0],Math.sqrt(Math.pow(R.get(2,1)[0],2)+Math.pow(R.get(2,2)[0],2))) * Math.PI / 180;
+        double yaw   = Math.atan2 (R.get(1,0)[0],R.get(0,0)[0]) * Math.PI / 180;
+
+
+//        double rot_w = Math.sqrt(1.0 + R.get(0,0)[0] + R.get(1,1)[0] + R.get(2,2)[0]) / 2.0;
+//        double rot_x = (R.get(2,1)[0] - R.get(1,2)[0]) / (rot_w*4);
+//        double rot_y = (R.get(0,2)[0] - R.get(2,0)[0]) / (rot_w*4);
+//        double rot_z = (R.get(1,0)[0] - R.get(0,1)[0]) / (rot_w*4);
+
+        return new Transform3d(
+//                new Translation3d(),
+                new Translation3d(pos_x, pos_y, pos_z),
+//                new Rotation3d()
+                new Rotation3d(roll, pitch, yaw)
+        );
+    }
+
+    public void reset() {
+//        prevImage.release();
+        prevImage = new Mat();
+        System.out.println("Reset");
+    }
+}

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

@@ -1,38 +1,48 @@
 package org.photonvision.vision.pipeline;
 
-import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.apriltag.AprilTagDetection;
-import org.opencv.calib3d.Calib3d;
+import edu.wpi.first.apriltag.AprilTagDetector;
+import edu.wpi.first.apriltag.AprilTagPoseEstimate;
+import edu.wpi.first.apriltag.AprilTagPoseEstimator;
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.util.Units;
 import org.opencv.core.*;
 import org.opencv.core.Point;
-import org.opencv.features2d.*;
 import org.opencv.imgproc.Imgproc;
+import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.ColorHelper;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.estimation.TargetModel;
+import org.photonvision.targeting.MultiTargetPNPResult;
+import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.pipe.CVPipe;
-import org.photonvision.vision.pipe.impl.BlurDetectionPipe;
+import org.photonvision.vision.pipe.impl.*;
-import org.photonvision.vision.pipe.impl.BlurPipe;
-import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TrackedTarget;
 
 import java.awt.*;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.Optional;
 
-import static org.opencv.calib3d.Calib3d.findEssentialMat;
-import static org.opencv.calib3d.Calib3d.recoverPose;
-import static org.opencv.core.CvType.CV_16U;
 import static org.opencv.features2d.Features2d.drawKeypoints;
 import static org.opencv.video.Video.calcOpticalFlowPyrLK;
 
 public class CustomTestPipeline extends CVPipeline<CVPipelineResult, CustomTestPipelineSettings> {
     private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.GREYSCALE;
     private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
-    private final BlurDetectionPipe blurDetectionPipe = new BlurDetectionPipe();
 
-    private final BlurPipe blurPipe = new BlurPipe();
+    private final AprilTagDetectionPipe aprilTagDetectionPipe = new AprilTagDetectionPipe();
+    private final AprilTagPoseEstimatorPipe singleTagPoseEstimatorPipe = new AprilTagPoseEstimatorPipe();
+    private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe();
 
+    private final VisualOdometryPipe visualOdometryPipe = new VisualOdometryPipe();
+
+    private List<TrackedTarget> previousAprilTags;
+
+    Mat cam_mat;
     public CustomTestPipeline() {
         super(PROCESSING_TYPE);
         settings = new CustomTestPipelineSettings();
@@ -45,27 +55,101 @@ public class CustomTestPipeline extends CVPipeline<CVPipelineResult, CustomTestP
 
     @Override
     protected void setPipeParamsImpl() {
-//        super.setPipeParamsImpl();
+
-        this.released = false;
+        VisualOdometryParams VOConfig = new VisualOdometryParams();
-        blurPipe.setParams(new BlurPipe.BlurParams(5));
+        VOConfig.featureThreshold = settings.featureThreshold;
-//        if(frameStaticProperties.cameraCalibration != null) {
+        VOConfig.minFeatures = settings.minFeatures;
-//            visualOdometry = new VisualOdometry(frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat());
+        VOConfig.imageDifferenceThreshold = settings.imageDifferenceThreshold;
-//        }
+        VOConfig.essentialMatProb = settings.essentialMatProb;
-//        blurDetectionPipe.setParams(new BlurDetectionPipe.BlurDetectionParams(100));
+        VOConfig.essentialMatThreshold = settings.essentialMatThreshold;
+
+        if(frameStaticProperties.cameraCalibration != null) {
+            cam_mat = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
+
+            double fx = cam_mat.get(0,0)[0];
+            double fy = cam_mat.get(1,1)[0];
+
+            double x = cam_mat.get(0,2)[0];
+            double y = cam_mat.get(1,2)[0];
+
+            double width = frameStaticProperties.imageWidth;
+            double height = frameStaticProperties.imageHeight;
+
+            VOConfig.cam_mat = cam_mat;
+
+//            VOConfig.focal = 2;
+//            VOConfig.pp.x = x;
+//            VOConfig.pp.y = y;
+        }
+
+        visualOdometryPipe.setParams(VOConfig);
+
+
+
+
+
+        // Sanitize thread count - not supported to have fewer than 1 threads
+        settings.threads = Math.max(1, settings.threads);
+
+        // for now, hard code tag width based on enum value
+        // 2023/other: best guess is 6in
+        double tagWidth = Units.inchesToMeters(6);
+        TargetModel tagModel = TargetModel.kAprilTag16h5;
+        if (settings.tagFamily == AprilTagFamily.kTag36h11) {
+            // 2024 tag, 6.5in
+            tagWidth = Units.inchesToMeters(6.5);
+            tagModel = TargetModel.kAprilTag36h11;
+        }
+
+        var config = new AprilTagDetector.Config();
+        config.numThreads = settings.threads;
+        config.refineEdges = settings.refineEdges;
+        config.quadSigma = (float) settings.blur;
+        config.quadDecimate = settings.decimate;
+
+        var quadParams = new AprilTagDetector.QuadThresholdParameters();
+        // 5 was the default minClusterPixels in WPILib prior to 2025
+        // increasing it causes detection problems when decimate > 1
+        quadParams.minClusterPixels = 5;
+        // these are the same as the values in WPILib 2025
+        // setting them here to prevent upstream changes from changing behavior of the detector
+        quadParams.maxNumMaxima = 10;
+        quadParams.criticalAngle = 45 * Math.PI / 180.0;
+        quadParams.maxLineFitMSE = 10.0f;
+        quadParams.minWhiteBlackDiff = 5;
+        quadParams.deglitch = false;
+
+        aprilTagDetectionPipe.setParams(
+                new AprilTagDetectionPipeParams(settings.tagFamily, config, quadParams));
+
+        if (frameStaticProperties.cameraCalibration != null) {
+            var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
+            if (cameraMatrix != null && cameraMatrix.rows() > 0) {
+                var cx = cameraMatrix.get(0, 2)[0];
+                var cy = cameraMatrix.get(1, 2)[0];
+                var fx = cameraMatrix.get(0, 0)[0];
+                var fy = cameraMatrix.get(1, 1)[0];
+
+                singleTagPoseEstimatorPipe.setParams(
+                        new AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams(
+                                new AprilTagPoseEstimator.Config(tagWidth, fx, fy, cx, cy),
+                                frameStaticProperties.cameraCalibration,
+                                settings.numIterations));
+
+                // TODO global state ew
+                var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
+                multiTagPNPPipe.setParams(
+                        new MultiTargetPNPPipe.MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel));
+            }
+        }
     }
 
 
     private static final Scalar FONT_COLOR = ColorHelper.colorToScalar(Color.RED);
 
-    AKAZE detector = AKAZE.create();
-
-    MatOfKeyPoint prev_points;
-
     @Override
     protected CVPipelineResult process(Frame frame, CustomTestPipelineSettings settings) {
-        long total_proc_time = 0;
+        long sumPipeNanosElapsed = 0L;
-
-        String status = "ERROR";
 
         if (frame.type != FrameThresholdType.GREYSCALE) {
             // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up
@@ -79,86 +163,154 @@ public class CustomTestPipeline extends CVPipeline<CVPipelineResult, CustomTestP
             return new CVPipelineResult(frame.sequenceID, 0, 0, List.of(), frame);
         }
 
-        Mat frame_mat = frame.processedImage.getMat();
+//        Transform3d tf = visualOdometryPipe.run(frame.processedImage.getMat()).output;
-
-        MatOfKeyPoint points = new MatOfKeyPoint();
-        detector.detect(frame_mat, points);
-        points.convertTo(points, CV_16U);
-
-        if(prev_points != null && points.depth() > 0 && prev_points.depth() > 0){
-//            System.out.println("Calc!");
-            Mat cam_mat = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
-            Mat E, R = new Mat(), t = new Mat(), mask = new Mat();
-
-            int npoints = points.checkVector(2);
-
-            System.out.println("npoints >= 0" + (npoints >= 0));
-            System.out.println("points2.checkVector(2) == npoints" + (prev_points.checkVector(2) == npoints));
-            System.out.println("points1.type() == points2.type()" + (points.type() == prev_points.type()));
-            E = findEssentialMat(prev_points, points, cam_mat, Calib3d.RANSAC);
-
-
-//            recoverPose(E, points, prev_points, cam_mat, R, t, mask);
 //
+//        var fps = calculateFPSPipe.run(null).output;
 //
+//        List<TrackedTarget> result = new ArrayList<>();
 //
+//        TrackedTarget target = new TrackedTarget();
+//        target.setBestCameraToTarget3d(tf);
+////        target.tran
 //
-//            System.out.println(t.width() + ", " + t.height());
+//        result.add(target);
-////            System.out.println(pos.get(0,3)[0] + ", " + pos.get(1,3)[0] + ", " + pos.get(2,3)[0]);
+//
-        }
+//        return new CVPipelineResult(frame.sequenceID, total_proc_time, fps, result, frame);
-        prev_points = points;
 
+        CVPipe.CVPipeResult<List<AprilTagDetection>> tagDetectionPipeResult;
+        tagDetectionPipeResult = aprilTagDetectionPipe.run(frame.processedImage);
+        sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
 
+        List<AprilTagDetection> detections = tagDetectionPipeResult.output;
+        List<AprilTagDetection> usedDetections = new ArrayList<>();
+        List<TrackedTarget> targetList = new ArrayList<>();
 
-        drawKeypoints(frame_mat, points, frame_mat);
+        // Filter out detections based on pipeline settings
+        for (AprilTagDetection detection : detections) {
+            // TODO this should be in a pipe, not in the top level here (Matt)
+            if (detection.getDecisionMargin() < settings.decisionMargin) continue;
+            if (detection.getHamming() > settings.hammingDist) continue;
 
-//        Transform3d pos =
+            usedDetections.add(detection);
 
+            // Populate target list for multitag
+            // (TODO: Address circular dependencies. Multitag only requires corners and IDs, this should
+            // not be necessary.)
+            TrackedTarget target =
+                    new TrackedTarget(
+                            detection,
+                            null,
+                            new TrackedTarget.TargetCalculationParameters(
+                                    false, null, null, null, null, frameStaticProperties));
 
-        var fps = calculateFPSPipe.run(null).output;
+            targetList.add(target);
-
-        return new CVPipelineResult(frame.sequenceID, total_proc_time, fps, List.of(), frame);
         }
 
-    public void featureTracking(Mat img_1, Mat img_2, MatOfPoint2f points1, MatOfPoint2f points2, MatOfByte status) {
+        // Do multi-tag pose estimation
-
+        Optional<MultiTargetPNPResult> multiTagResult = Optional.empty();
-  // this function automatically gets rid of points for which tracking fails
+        if (settings.solvePNPEnabled && settings.doMultiTarget) {
-
+            var multiTagOutput = multiTagPNPPipe.run(targetList);
-  MatOfFloat err = new MatOfFloat();
+            sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
-  Size window_size = new Size(21, 21);
+            multiTagResult = multiTagOutput.output;
-  TermCriteria term_criteria = new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, 0.01);
-
-  calcOpticalFlowPyrLK(img_1, img_2, points1, points2, status, err, window_size, 3, term_criteria, 0, 0.001);
-
-  // getting rid of points for which the KLT tracking failed or those who have gone outside the frame
-//  int index_correction = 0;
-//  for (int i = 0; i < status.depth(); i++) {
-//    Point pt = points2.toArray()[i - index_correction];
-//    if ((status.toArray()[i] == 0) || (pt.x < 0) || (pt.y < 0))
-//    {
-//      if ((pt.x < 0) || (pt.y < 0))
-//      {
-//        status.toArray()[i] = 0;
-//      }
-//      points1
-//      points1.erase(points1.begin() + (i - index_correction));
-//      points2.erase(points2.begin() + (i - index_correction));
-//      index_correction++;
-//    }
-//  }
         }
 
-    void featureDetection(Mat img_1, MatOfKeyPoint points1)  {
+        // Do single-tag pose estimation
-      // uses FAST for feature detections
+        if (settings.solvePNPEnabled) {
-//      MatOfKeyPoint keypoints_1;
+            // Clear target list that was used for multitag so we can add target transforms
-//      int fast_threshold = 20;
+            targetList.clear();
-//      boolean non_max_suppression = true;
+            // TODO global state again ew
-      detector.detect(img_1, points1);
+            var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
+
+            for (AprilTagDetection detection : usedDetections) {
+                AprilTagPoseEstimate tagPoseEstimate = null;
+                // Do single-tag estimation when "always enabled" or if a tag was not used for multitag
+                if (settings.doSingleTargetAlways
+                        || !(multiTagResult.isPresent()
+                                && multiTagResult.get().fiducialIDsUsed.contains((short) detection.getId()))) {
+                    var poseResult = singleTagPoseEstimatorPipe.run(detection);
+                    sumPipeNanosElapsed += poseResult.nanosElapsed;
+                    tagPoseEstimate = poseResult.output;
+                }
+
+                // If single-tag estimation was not done, this is a multi-target tag from the layout
+                if (tagPoseEstimate == null && multiTagResult.isPresent()) {
+                    // compute this tag's camera-to-tag transform using the multitag result
+                    var tagPose = atfl.getTagPose(detection.getId());
+                    if (tagPose.isPresent()) {
+                        var camToTag =
+                                new Transform3d(
+                                        new Pose3d().plus(multiTagResult.get().estimatedPose.best), tagPose.get());
+                        // match expected AprilTag coordinate system
+                        camToTag =
+                                CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());
+                        // (AprilTag expects Z axis going into tag)
+                        camToTag =
+                                new Transform3d(
+                                        camToTag.getTranslation(),
+                                        new Rotation3d(0, Math.PI, 0).plus(camToTag.getRotation()));
+                        tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0);
+                    }
+                }
+
+                // populate the target list
+                // Challenge here is that TrackedTarget functions with OpenCV Contour
+                TrackedTarget target =
+                        new TrackedTarget(
+                                detection,
+                                tagPoseEstimate,
+                                new TrackedTarget.TargetCalculationParameters(
+                                        false, null, null, null, null, frameStaticProperties));
+
+                var correctedBestPose =
+                        MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d());
+                var correctedAltPose =
+                        MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d());
+
+                target.setBestCameraToTarget3d(
+                        new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+                target.setAltCameraToTarget3d(
+                        new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
+
+                targetList.add(target);
+            }
+        }
+
+        if(!targetList.isEmpty()) {
+            previousAprilTags = targetList;
+            visualOdometryPipe.hasReset = false;
+        } else if(previousAprilTags != null) {
+            if(!visualOdometryPipe.hasReset){
+                visualOdometryPipe.reset();
+                visualOdometryPipe.hasReset = true;
+            }
+
+            CVPipe.CVPipeResult<Transform3d> VOResult = visualOdometryPipe.run(frame.processedImage.getMat());
+            sumPipeNanosElapsed += VOResult.nanosElapsed;
+
+//            System.out.println("X: " + VOResult.output.getTranslation());
+
+            for(int i = 0; i < previousAprilTags.size(); i++){
+                TrackedTarget old = previousAprilTags.get(i);
+                old.setBestCameraToTarget3d(old.getBestCameraToTarget3d().plus(VOResult.output));
+
+            }
+
+        }
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
+        return new CVPipelineResult(
+                frame.sequenceID, sumPipeNanosElapsed, fps, previousAprilTags, frame);
+
     }
 
 
     @Override
     public void release() {
+        aprilTagDetectionPipe.release();
+        singleTagPoseEstimatorPipe.release();
+        visualOdometryPipe.release();
         super.release();
     }
 }

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

@@ -1,30 +1,62 @@
 package org.photonvision.vision.pipeline;
 
 import com.fasterxml.jackson.annotation.JsonTypeName;
+import org.photonvision.vision.apriltag.AprilTagFamily;
+import org.photonvision.vision.target.TargetModel;
 
 @JsonTypeName("CustomTestPipelineSettings")
 public class CustomTestPipelineSettings extends AdvancedPipelineSettings {
-    public int test1 = 1;
+    public int featureThreshold = 1;
-    public int test2 = 2;
+    public int minFeatures = 500;
-    public int test3 = 3;
+    public int imageDifferenceThreshold = 150;
+    public double essentialMatProb = 0.999;
+    public double essentialMatThreshold = 1.;
+
+    public AprilTagFamily tagFamily = AprilTagFamily.kTag36h11;
+    public int decimate = 1;
+    public double blur = 0;
+    public int threads = 4; // Multiple threads seems to be better performance on most platforms
+    public boolean debug = false;
+    public boolean refineEdges = true;
+    public int numIterations = 40;
+    public int hammingDist = 0;
+    public int decisionMargin = 35;
+    public boolean doMultiTarget = false;
+    public boolean doSingleTargetAlways = false;
+
 
     public CustomTestPipelineSettings() {
         super();
         pipelineType = PipelineType.CustomTest;
     }
+
     @Override
     public int hashCode() {
         final int prime = 31;
         int result = super.hashCode();
-        long temp1;
+        long temp;
-        temp1 = Double.doubleToLongBits(test1);
+
-        result = prime * result + (int) (temp1 ^ (temp1 >>> 32));
+        temp = Double.doubleToLongBits(featureThreshold);
-        long temp2;
+        result = prime * result + (int) (temp ^ (temp >>> 32));
-        temp2 = Double.doubleToLongBits(test1);
+        temp = Double.doubleToLongBits(minFeatures);
-        result = prime * result + (int) (temp2^ (temp2 >>> 32));
+        result = prime * result + (int) (temp ^ (temp >>> 32));
-        long temp3;
+        temp = Double.doubleToLongBits(imageDifferenceThreshold);
-        temp3 = Double.doubleToLongBits(test1);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
-        result = prime * result + (int) (temp3 ^ (temp3 >>> 32));
+        temp = Double.doubleToLongBits(essentialMatProb);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(essentialMatThreshold);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        result = prime * result + threads;
+        result = prime * result + (debug ? 1231 : 1237);
+        result = prime * result + (refineEdges ? 1231 : 1237);
+        result = prime * result + numIterations;
+        result = prime * result + hammingDist;
+        result = prime * result + decisionMargin;
+        result = prime * result + (doMultiTarget ? 1231 : 1237);
+        result = prime * result + (doSingleTargetAlways ? 1231 : 1237);
+
         return result;
     }
 
@@ -34,9 +66,23 @@ public class CustomTestPipelineSettings extends AdvancedPipelineSettings {
         if (!super.equals(obj)) return false;
         if (getClass() != obj.getClass()) return false;
         CustomTestPipelineSettings other = (CustomTestPipelineSettings) obj;
-        if(test1 != ((CustomTestPipelineSettings) obj).test1) return false;
+        if(featureThreshold != other.featureThreshold) return false;
-        if(test2 != ((CustomTestPipelineSettings) obj).test2) return false;
+        if(minFeatures != other.minFeatures) return false;
-        if(test3 != ((CustomTestPipelineSettings) obj).test3) return false;
+        if(essentialMatProb != other.essentialMatProb) return false;
+        if(essentialMatThreshold != other.essentialMatThreshold) return false;
+
+        if (tagFamily != other.tagFamily) return false;
+        if (decimate != other.decimate) return false;
+        if (Double.doubleToLongBits(blur) != Double.doubleToLongBits(other.blur)) return false;
+        if (threads != other.threads) return false;
+        if (debug != other.debug) return false;
+        if (refineEdges != other.refineEdges) return false;
+        if (numIterations != other.numIterations) return false;
+        if (hammingDist != other.hammingDist) return false;
+        if (decisionMargin != other.decisionMargin) return false;
+        if (doMultiTarget != other.doMultiTarget) return false;
+        if (doSingleTargetAlways != other.doSingleTargetAlways) return false;
+
         return true;
     }
 }