diff --git a/chameleon-server/src/main/java/com/chameleonvision/util/Helpers.java b/chameleon-server/src/main/java/com/chameleonvision/util/Helpers.java
index d63db5428..f63f5d1e7 100644
--- a/chameleon-server/src/main/java/com/chameleonvision/util/Helpers.java
+++ b/chameleon-server/src/main/java/com/chameleonvision/util/Helpers.java
@@ -12,7 +12,7 @@ public class Helpers {
     }
 
     public static Scalar colorToScalar(Color color) {
-        return new Scalar(color.getRed(), color.getGreen(), color.getBlue());
+        return new Scalar(color.getBlue(), color.getGreen(), color.getRed());
     }
 
     public static HashMap VideoModeToHashMap(VideoMode videoMode) {
diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java
index 2dcf6d115..4095453ee 100644
--- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java
+++ b/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);
@@ -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
diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java
index 2851bfa6f..f3d37ad90 100644
--- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java
+++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java
@@ -137,6 +137,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 +159,45 @@ 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 +245,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 +335,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());