From 6fe62f8e7d057b2610c7543875d94dfecfd68972 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Fri, 21 Feb 2020 12:17:23 -0800
Subject: [PATCH] Add 8 point target, track 4 more corners

---
 chameleon-client/src/assets/FRCtargets.json   |  6 ++++-
 .../vision/pipeline/pipes/SolvePNPPipe.java   | 22 ++++++++++++-------
 2 files changed, 19 insertions(+), 9 deletions(-)

diff --git a/chameleon-client/src/assets/FRCtargets.json b/chameleon-client/src/assets/FRCtargets.json
index 55aa10fb6..0b8b2841f 100644
--- a/chameleon-client/src/assets/FRCtargets.json
+++ b/chameleon-client/src/assets/FRCtargets.json
@@ -3,7 +3,11 @@
     [-19.625, 0],
     [-9.819867, -17],
     [9.819867, -17],
-    [19.625,0]
+    [19.625,0],
+    [17.316,       0],
+    [8.665867,   -15],
+    [-9.819867,  -15],
+    [-17.316,      0]
   ],
   "2020 Loading Station": [
     [-3.54,5.5],
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 fd4d301bb..373c1a41c 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
@@ -162,11 +162,11 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
 
         // algorithm from team 4915
 
-        // Contour perimiter
+        // Contour perimeter
         var peri = Imgproc.arcLength(target.rawContour, true);
         // approximating a shape around the contours
         // Can be tuned to allow/disallow hulls
-        // Approx is the number of verticies
+        // Approx is the number of vertices
         // Ramer–Douglas–Peucker algorithm
         Imgproc.approxPolyDP(target.rawContour, polyOutput, 0.01 * peri, true);
 
@@ -190,17 +190,23 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         try {
 
             // 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().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get();
+            var 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();
 
+            polyList = new ArrayList<>(polyList);
+            polyList.removeAll(List.of(tl, tr, bl, br));
+
+            var tl2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get();
+            var tr2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get();
+
+            var bl2 = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get();
+            var br2 = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get();
+
             target2020ResultMat.release();
-            target2020ResultMat.fromList(List.of(tl, bl, br, tr));
+            target2020ResultMat.fromList(List.of(tl, bl, br, tr, tr2, br2, bl2, tl2));
 
             return target2020ResultMat;
         } catch (NoSuchElementException e) {