2.2 (#52)

## features
-- added pi cam support (from wpilib v2020.2.2)
-- added gain slider for ps3 eye
-- added custom pnp model upload
-- network tables will try to reconnect if no connection
-- re did network tables key and added new values 


## bug fixes
-- fixed solve pnp not detection
-- lowered minimum fps for some camera
-- fixed vision hang bug

chameleon-server/src/main/java/com/chameleonvision/vision/VisionManager.java

@@ -15,9 +15,9 @@ import org.opencv.videoio.VideoCapture;
 import java.util.*;
 import java.util.stream.Collectors;
 
+@SuppressWarnings("rawtypes")
 public class VisionManager {
-    private VisionManager() {
-    }
+    private VisionManager() {}
 
     private static final LinkedHashMap<String, UsbCameraInfo> usbCameraInfosByCameraName = new LinkedHashMap<>();
     private static final LinkedList<FullCameraConfiguration> loadedCameraConfigs = new LinkedList<>();

chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java

@@ -4,6 +4,7 @@ import com.chameleonvision.Debug;
 import com.chameleonvision.config.CameraCalibrationConfig;
 import com.chameleonvision.config.CameraConfig;
 import com.chameleonvision.config.ConfigManager;
+import com.chameleonvision.networktables.NetworkTablesManager;
 import com.chameleonvision.scripting.ScriptEventType;
 import com.chameleonvision.scripting.ScriptManager;
 import com.chameleonvision.config.FullCameraConfiguration;
@@ -32,10 +33,10 @@ import java.util.concurrent.LinkedBlockingDeque;
 import java.util.stream.Collectors;
 
 
+@SuppressWarnings("rawtypes")
 public class VisionProcess {
 
-    private final USBCameraCapture cameraCapture;
-//    private final CameraStreamerRunnable streamRunnable;
+    public final USBCameraCapture cameraCapture;
     private final VisionProcessRunnable visionRunnable;
     private final CameraConfig fileConfig;
     public final CameraStreamer cameraStreamer;
@@ -43,8 +44,6 @@ public class VisionProcess {
 
     private volatile CVPipelineResult lastPipelineResult;
 
-    private BlockingQueue<Mat> streamFrameQueue = new LinkedBlockingDeque<>(1);
-
     // network table stuff
     private final NetworkTable defaultTable;
     private NetworkTableInstance tableInstance;
@@ -59,6 +58,12 @@ public class VisionProcess {
     private NetworkTableEntry ntLatencyEntry;
     private NetworkTableEntry ntValidEntry;
     private NetworkTableEntry ntPoseEntry;
+    private NetworkTableEntry ntFittedHeightEntry;
+    private NetworkTableEntry ntFittedWidthEntry;
+    private NetworkTableEntry ntBoundingHeightEntry;
+    private NetworkTableEntry ntBoundingWidthEntry;
+    private NetworkTableEntry ntTargetRotation;
+
     private ObjectMapper objectMapper = new ObjectMapper();
 
     private long lastUIUpdateMs = 0;
@@ -72,7 +77,6 @@ public class VisionProcess {
 
         // Thread to put frames on the dashboard
         this.cameraStreamer = new CameraStreamer(cameraCapture, config.cameraConfig.name, pipelineManager.getCurrentPipeline().settings.streamDivisor);
-//        this.streamRunnable = new CameraStreamerRunnable(30, cameraStreamer);
 
         // Thread to process vision data
         this.visionRunnable = new VisionProcessRunnable();
@@ -110,21 +114,26 @@ public class VisionProcess {
 
     public void setCameraNickname(String newName) {
         getCamera().getProperties().setNickname(newName);
-        var newTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + newName);
-        resetNT(newTable);
+        NetworkTable camTable = NetworkTablesManager.kRootTable.getSubTable(newName);
+        resetNT(camTable);
     }
 
-    private void initNT(NetworkTable newTable) {
-        tableInstance = newTable.getInstance();
-        ntPipelineEntry = newTable.getEntry("pipeline");
-        ntDriverModeEntry = newTable.getEntry("driver_mode");
-        ntPitchEntry = newTable.getEntry("pitch");
-        ntYawEntry = newTable.getEntry("yaw");
-        ntAreaEntry = newTable.getEntry("area");
-        ntLatencyEntry = newTable.getEntry("latency");
-        ntValidEntry = newTable.getEntry("is_valid");
-        ntAuxListEntry = newTable.getEntry("aux_targets");
-        ntPoseEntry = newTable.getEntry("pose");
+    private void initNT(NetworkTable camTable) {
+        tableInstance = camTable.getInstance();
+        ntPipelineEntry = camTable.getEntry("pipeline");
+        ntDriverModeEntry = camTable.getEntry("driverMode");
+        ntPitchEntry = camTable.getEntry("targetPitch");
+        ntYawEntry = camTable.getEntry("targetYaw");
+        ntAreaEntry = camTable.getEntry("targetArea");
+        ntLatencyEntry = camTable.getEntry("latency");
+        ntValidEntry = camTable.getEntry("isValid");
+        ntAuxListEntry = camTable.getEntry("auxTargets");
+        ntPoseEntry = camTable.getEntry("targetPose");
+        ntFittedHeightEntry = camTable.getEntry("targetFittedHeight");
+        ntFittedWidthEntry = camTable.getEntry("targetFittedWidth");
+        ntBoundingHeightEntry = camTable.getEntry("targetBoundingHeight");
+        ntBoundingWidthEntry = camTable.getEntry("targetBoundingWidth");
+        ntTargetRotation = camTable.getEntry("targetRotation");
         ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
         ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
         ntDriverModeEntry.setBoolean(false);
@@ -157,7 +166,6 @@ public class VisionProcess {
     }
 
     public void setDriverModeEntry(boolean isDriverMode) {
-
         // if it's null, we haven't even started the program yet, so just return
         // otherwise, set it.
         if (ntDriverModeEntry != null) {
@@ -176,7 +184,7 @@ public class VisionProcess {
                 HashMap<String, Object> WebSend = new HashMap<>();
                 HashMap<String, Object> point = new HashMap<>();
                 HashMap<String, Object> pointMap = new HashMap<>();
-                ArrayList<Object> webTargets = new ArrayList<Object>();
+                ArrayList<Object> webTargets = new ArrayList<>();
                 List<Double> center = new ArrayList<>();
 
 
@@ -203,9 +211,8 @@ public class VisionProcess {
                             }
                             center.add(bestTarget.minAreaRect.center.x);
                             center.add(bestTarget.minAreaRect.center.y);
-
                         } catch (ClassCastException ignored) {
-
+                          
                         }
                     } else {
                         pointMap.put("pitch", null);
@@ -236,17 +243,23 @@ public class VisionProcess {
 
                 //noinspection unchecked
                 List<StandardCVPipeline.TrackedTarget> targets = (List<StandardCVPipeline.TrackedTarget>) data.targets;
+                StandardCVPipeline.TrackedTarget bestTarget = targets.get(0);
                 ntLatencyEntry.setDouble(MathHandler.roundTo(data.processTime * 1e-6, 3));
-                ntPitchEntry.setDouble(targets.get(0).pitch);
-                ntYawEntry.setDouble(targets.get(0).yaw);
-                ntAreaEntry.setDouble(targets.get(0).area);
+                ntPitchEntry.setDouble(bestTarget.pitch);
+                ntYawEntry.setDouble(bestTarget.yaw);
+                ntAreaEntry.setDouble(bestTarget.area);
+                ntBoundingHeightEntry.setDouble(bestTarget.boundingRect.height);
+                ntBoundingWidthEntry.setDouble(bestTarget.boundingRect.width);
+                ntFittedHeightEntry.setDouble(bestTarget.minAreaRect.size.height);
+                ntFittedWidthEntry.setDouble(bestTarget.minAreaRect.size.width);
+                ntTargetRotation.setDouble(bestTarget.minAreaRect.angle);
                 try {
                     Pose2d targetPose = targets.get(0).cameraRelativePose;
                     double[] targetArray = {targetPose.getTranslation().getX(), targetPose.getTranslation().getY(), targetPose.getRotation().getDegrees()};
                     ntPoseEntry.setDoubleArray(targetArray);
 //                    ntPoseEntry.setString(objectMapper.writeValueAsString(targets.get(0).cameraRelativePose));
                     ntAuxListEntry.setString(objectMapper.writeValueAsString(targets.stream()
-                            .map(it -> List.of(it.pitch, it.yaw, it.area, it.cameraRelativePose))
+                            .map(it -> List.of(it.pitch, it.yaw, it.area, it.boundingRect.width, it.boundingRect.height, it.minAreaRect.size.width, it.minAreaRect.size.height, it.minAreaRect.angle, it.cameraRelativePose))
                             .collect(Collectors.toList())));
                 } catch (JsonProcessingException e) {
                     e.printStackTrace();
@@ -260,7 +273,6 @@ public class VisionProcess {
             }
         }
         tableInstance.flush();
-
     }
 
     public void setVideoMode(VideoMode newMode) {
@@ -343,8 +355,6 @@ public class VisionProcess {
                 }
 
                 try {
-//                    streamFrameQueue.clear();
-//                    streamFrameQueue.add(lastPipelineResult.outputMat);
                     var currentTime = System.currentTimeMillis();
                     if ((currentTime - lastStreamTimeMs) / 1000d > 1.0 / 30.0) {
                         cameraStreamer.runStream(lastPipelineResult.outputMat);
@@ -372,38 +382,5 @@ public class VisionProcess {
             temp /= 7.0;
             return temp;
         }
-
-    }
-
-    private class CameraStreamerRunnable extends LoopingRunnable {
-
-        final CameraStreamer streamer;
-        private Mat bufferMat = new Mat();
-
-        private CameraStreamerRunnable(int cameraFPS, CameraStreamer streamer) {
-            // add 2 FPS to allow for a bit of overhead
-            super(1000L / (cameraFPS + 2));
-            this.streamer = streamer;
-        }
-
-        @Override
-        protected void process() {
-            if (!streamFrameQueue.isEmpty()) {
-                try {
-
-                    bufferMat = streamFrameQueue.take();
-
-                    try {
-                        streamer.runStream(bufferMat);
-                        bufferMat.release();
-                    } catch (Exception e) {
-                        // do nothing
-                    }
-
-                } catch (InterruptedException e) {
-                    e.printStackTrace();
-                }
-            }
-        }
     }
 }

chameleon-server/src/main/java/com/chameleonvision/vision/camera/USBCameraCapture.java

@@ -2,6 +2,7 @@ package com.chameleonvision.vision.camera;
 
 import com.chameleonvision.config.CameraCalibrationConfig;
 import com.chameleonvision.config.FullCameraConfiguration;
+import com.chameleonvision.util.Helpers;
 import edu.wpi.cscore.CvSink;
 import edu.wpi.cscore.UsbCamera;
 import edu.wpi.cscore.VideoException;
@@ -13,6 +14,7 @@ import org.opencv.core.Size;
 import org.opencv.imgcodecs.Imgcodecs;
 
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 public class USBCameraCapture implements CameraCapture {
@@ -30,6 +32,12 @@ public class USBCameraCapture implements CameraCapture {
         cvSink = CameraServer.getInstance().getVideo(baseCamera);
         properties = new USBCaptureProperties(baseCamera, config);
 
+        var videoModes = properties.getVideoModes();
+        if(videoModes.size() < 1) {
+            throw new VideoException("0 video modes are valid! Full list provided by camera: \n\n"
+            + Arrays.stream(baseCamera.enumerateVideoModes()).map(Helpers::VideoModeToHashMap).toString()  );
+        }
+
         int videoMode = properties.videoModes.size() - 1 <= config.videomode ? config.videomode : 0;
         setVideoMode(videoMode);
     }

chameleon-server/src/main/java/com/chameleonvision/vision/camera/USBCaptureProperties.java

@@ -17,15 +17,15 @@ public class USBCaptureProperties extends CaptureProperties {
     public static final double DEFAULT_FOV = 70;
     private static final int DEFAULT_EXPOSURE = 50;
     private static final int DEFAULT_BRIGHTNESS = 50;
-    private static final int MINIMUM_FPS = 30;
+    private static final int MINIMUM_FPS = 21;
     private static final int MINIMUM_WIDTH = 320;
     private static final int MINIMUM_HEIGHT = 200;
     private static final int MAX_INIT_MS = 1500;
 
-    private static final int PS3EYE_VID = 1415;
-    private static final int PS3EYE_PID = 2000;
+    private static final int PS3EYE_VID = 0x1415;
+    private static final int PS3EYE_PID = 0x2000;
 
-    private static final List<VideoMode.PixelFormat> ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG);
+    private static final List<VideoMode.PixelFormat> ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG, VideoMode.PixelFormat.kBGR);
 
     private static final Predicate<VideoMode> kMinFPSPredicate = (videoMode -> videoMode.fps >= MINIMUM_FPS);
     private static final Predicate<VideoMode> kMinSizePredicate = (videoMode -> videoMode.width >= MINIMUM_WIDTH && videoMode.height >= MINIMUM_HEIGHT);

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/PipelineManager.java

@@ -118,7 +118,8 @@ public class PipelineManager {
     }
 
     public void setCurrentPipeline(int index) {
-        CVPipeline newPipeline=null;
+        CVPipeline newPipeline = null;
+
         if (index == DRIVERMODE_INDEX) {
             newPipeline = driverModePipeline;
 
@@ -165,6 +166,11 @@ public class PipelineManager {
                 ntIndexEntry.setDouble(index);
             }
         }
+
+        // gain setting quirk
+        if (!parentProcess.cameraCapture.getProperties().isPS3Eye) {
+            getCurrentPipeline().settings.gain = -1;
+        }
     }
 
     public void addPipeline(CVPipelineSettings setting) {

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java

@@ -235,7 +235,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
             System.out.printf("full pipeline run time was %.3fms (%.2fFPS)\n", truePipelineTimeMillis, truePipelineFPS);
         }
 
-        memManager.run();
+//        memManager.run();
 
         resultCache = new StandardCVPipelineResult(collect2dTargetsResult.getLeft(), outputMat, totalPipelineTimeNanos);
         return resultCache;
@@ -260,6 +260,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
         public double area = 0.0;
         public Point point = new Point();
         public RotatedRect minAreaRect;
+        public Rect boundingRect;
 
         // 3d stuff
         public Pose2d cameraRelativePose = new Pose2d();

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipelineSettings.java

@@ -4,10 +4,7 @@ import com.chameleonvision.vision.enums.*;
 import com.chameleonvision.vision.pipeline.CVPipelineSettings;
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import edu.wpi.first.wpilibj.util.Units;
-import org.opencv.core.Mat;
-import org.opencv.core.MatOfDouble;
-import org.opencv.core.Point;
-import org.opencv.core.Point3;
+import org.opencv.core.*;
 
 import java.util.Arrays;
 import java.util.List;
@@ -20,7 +17,7 @@ public class StandardCVPipelineSettings extends CVPipelineSettings {
     public boolean dilate = false;
     public List<Number> area = Arrays.asList(0.0, 100.0);
     public List<Number> ratio = Arrays.asList(0.0, 20.0);
-    public List<Number> extent = Arrays.asList(0, 100);
+    public List<Number> extent = Arrays.asList(0.0, 100.0);
     public Number speckle = 5;
     public boolean isBinary = false;
     public SortMode sortMode = SortMode.Largest;
@@ -35,7 +32,22 @@ public class StandardCVPipelineSettings extends CVPipelineSettings {
     public double dualTargetCalibrationB = 0;
 
     // 3d stuff
-    public double targetWidth = 15.5, targetHeight = 6.0;
+    public MatOfPoint3f targetCornerMat = new MatOfPoint3f();
+    private static MatOfPoint3f hexTargetMat = new MatOfPoint3f();
+
+    static {
+        hexTargetMat.fromList(List.of(
+            new Point3(-19.625, 0, 0),
+            new Point3(-9.819867, -17, 0),
+            new Point3(9.819867, -17, 0),
+            new Point3(19.625, 0, 0)));
+    }
+
+    public StandardCVPipelineSettings() {
+        super();
+        hexTargetMat.copyTo(targetCornerMat);
+    }
+
 
     public boolean is3D = false;
 }

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java

@@ -16,10 +16,12 @@ import java.util.List;
 public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
 
     private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
+    private MatOfPoint tempMatOfPoints = new MatOfPoint();
 
     public Scalar green = Helpers.colorToScalar(Color.GREEN);
     public Scalar blue = Helpers.colorToScalar(Color.BLUE);
     public Scalar red = Helpers.colorToScalar(Color.RED);
+    public Scalar orange = Helpers.colorToScalar(Color.orange);
 
     public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
         setConfig(settings);
@@ -46,14 +48,14 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
     public void set2020Box() {
         boxCornerMat.release();
         boxCornerMat = new MatOfPoint3f(
-                new Point3(-16.25, 0, 0),
+                new Point3(-19.625, 0, 0),
                 new Point3(-9.819867, -17, 0),
                 new Point3(9.819867, -17, 0),
-                new Point3(16.25, 0, 0),
-                new Point3(-16.25, 0, -6),
+                new Point3(19.625, 0, 0),
+                new Point3(-19.625, 0, -6),
                 new Point3(-9.819867, -17, -6),
                 new Point3(9.819867, -17, -6),
-                new Point3(16.25, 0, -6)
+                new Point3(19.625, 0, -6)
         );
     }
 
@@ -97,17 +99,20 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
                 Imgproc.rectangle(image, right.tl(), right.br(), new Scalar(200, 200, 0), 2);
             }
 
-            // draw corners
-            for(int i = 0; i < it.imageCornerPoints.rows(); i++) {
-                var point = new Point(it.imageCornerPoints.get(i, 0));
-                Imgproc.circle(image, point, 4, green, 5);
-            }
-
             // draw poly dp
             var list = it.approxPoly.toList();
             for(int i = 0; i < list.size(); i++) {
                 var next = (i == list.size() - 1) ? list.get(0) : list.get(i + 1);
-                Imgproc.line(image, list.get(i), next, red, 3);
+                Imgproc.line(image, list.get(i), next, red, 2);
+            }
+
+            // draw center
+            Imgproc.circle(image, it.minAreaRect.center, 5, red);
+
+            // draw corners
+            for(int i = 0; i < it.imageCornerPoints.rows(); i++) {
+                var point = new Point(it.imageCornerPoints.get(i, 0));
+                Imgproc.circle(image, point, 4, green, 5);
             }
 
             // sketch out floor

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java

@@ -7,6 +7,7 @@ import com.chameleonvision.vision.pipeline.Pipe;
 import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.*;
+import org.opencv.core.Point;
 import org.opencv.imgproc.Imgproc;
 import org.opencv.imgproc.Moments;
 
@@ -60,9 +61,11 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
                         contourBuffer.fromArray(c.toArray());
                         if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
                             RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
+                            Rect boundingRect = Imgproc.boundingRect(contourBuffer);
                             var target = new StandardCVPipeline.TrackedTarget();
                             target.minAreaRect = rect;
                             target.rawContour = contourBuffer;
+                            target.boundingRect = boundingRect;
                             groupedContours.add(target);
                         }
                     });
@@ -90,9 +93,10 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
 
                             if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
                                 RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
+                                Rect boundingRect = Imgproc.boundingRect(contourBuffer);
                                 var target = new StandardCVPipeline.TrackedTarget();
                                 target.minAreaRect = rect;
-
+                                target.boundingRect = boundingRect;
                                 // find left and right bouding rectangles
                                 target.leftRightDualTargetPair =
                                         Pair.of(Imgproc.boundingRect(firstContour),

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java

@@ -48,10 +48,11 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         if(isHighGoal) {
             // tl, bl, br, tr is the order
             List<Point3> corners = List.of(
-                new Point3(-16.25, 0, 0),
+
+                new Point3(-19.625, 0, 0),
                 new Point3(-9.819867, -17, 0),
                 new Point3(9.819867, -17, 0),
-                new Point3(16.25, 0, 0));
+                new Point3(19.625, 0, 0));
             setObjectCorners(corners);
         } else {
             setBoundingBoxTarget(7, 11);
@@ -81,6 +82,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
 //        setBoundingBoxTarget(settings.targetWidth, settings.targetHeight);
         // TODO add proper year differentiation
         tilt_angle = tilt.getRadians();
+        this.objPointsMat = settings.targetCornerMat;
     }
 
     private void setCameraCoeffs(CameraCalibrationConfig settings) {
@@ -137,6 +139,16 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         return points;
     }
 
+    MatOfPoint2f target2020ResultMat = new MatOfPoint2f();
+
+    private double distanceBetween(Point a, Point b) {
+        return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2));
+    }
+
+    MatOfInt tempInt = new MatOfInt();
+    MatOfPoint2f tempMat2f = new MatOfPoint2f();
+    MatOfPoint tempMatOfPoint = new MatOfPoint();
+
     /**
      * Find the target using the outermost tape corners and a 2020 target.
      * @param target the target.
@@ -149,25 +161,46 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
 
         var contour = target.rawContour;
+        var convex = tempInt;
+        tempMatOfPoint.fromList(contour.toList());
+        Imgproc.convexHull(tempMatOfPoint, convex);
         var combinedList = contour.toList();
 
         // approx poly dp time
-        Imgproc.approxPolyDP(contour, polyOutput, 3, true);
+
+        Point[] contourArray = contour.toArray();
+        Point[] hullPoints = new Point[convex.rows()];
+        List<Integer> hullContourIdxList = convex.toList();
+        for (int i = 0; i < hullContourIdxList.size(); i++) {
+            hullPoints[i] = contourArray[hullContourIdxList.get(i)];
+        }
+        tempMat2f.fromArray(hullPoints);
+
+        Imgproc.approxPolyDP(tempMat2f, polyOutput, 5, true);
+
         var polyList = polyOutput.toList();
 
         polyOutput.copyTo(target.approxPoly);
 
-        // start looking in the top left quadrant
+        // left top, left bottom, right bottom, right top
+        var boundingBoxCorners = findBoundingBoxCorners(target).toList();
+
         try {
-            var tl = polyList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get();
-            var tr = polyList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get();
+
+            // top left and top right are the poly corners closest to the bouding box tl and tr
+            Point tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get();
+            Point tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get();
+
+
+//            var tl = polyList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get();
+//            var tr = polyList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get();
             var bl = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get();
             var br = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get();
 
-            boundingBoxResultMat.release();
-            boundingBoxResultMat.fromList(List.of(tl, bl, br, tr));
+            target2020ResultMat.release();
+            target2020ResultMat.fromList(List.of(tl, bl, br, tr));
 
-            return boundingBoxResultMat;
+            return target2020ResultMat;
         } catch (NoSuchElementException e) {
             return null;
         }
@@ -215,6 +248,11 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         return boundingBoxResultMat;
     }
 
+    /**
+     *
+     * @param target the target to find the corners of.
+     * @return the corners. left top, left bottom, right bottom, right top
+     */
     private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) {
 
 //        List<Pair<MatOfPoint2f, CVPipeline2d.Target2d>> list = new ArrayList<>();
@@ -300,8 +338,6 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
     @Deprecated
     private MatOfPoint2f findGoodFeaturesToTrack2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
 
-//        Imgproc.approxPolyDP(new MatOfPoint2f(max_contour.toArray()),approx,epsilon,true);
-
         // start by looking for corners
         var points__ = findBoundingBoxCorners(target).toList();
         var xList = points__.stream().map(it -> it.x).sorted(Double::compare).collect(Collectors.toList());
@@ -313,7 +349,6 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         var boundingBr = new Point (
             xList.get(2), yList.get(2)
         );
-
         System.out.println("tl/br:\n" + boundingTl.toString() + "\n" + boundingBr.toString());
 
         var slightlyBiggerTl = new Point(