From b07323133020331581c7eb8df84281f8ad9df2a4 Mon Sep 17 00:00:00 2001
From: oriagranat9 <oriagranat9@gmail.com>
Date: Sun, 26 Jan 2020 22:25:40 -0800
Subject: [PATCH] Bugfixes in solve pnp drawing

* bugfixed

* Fix unit mismatch in frctargets

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
---
 chameleon-client/src/assets/FRCtargets.json   |  8 +--
 .../pipeline/impl/StandardCVPipeline.java     |  3 +-
 .../pipeline/pipes/DrawSolvePNPPipe.java      | 59 ++++---------------
 3 files changed, 19 insertions(+), 51 deletions(-)

diff --git a/chameleon-client/src/assets/FRCtargets.json b/chameleon-client/src/assets/FRCtargets.json
index 77165c08b..55aa10fb6 100644
--- a/chameleon-client/src/assets/FRCtargets.json
+++ b/chameleon-client/src/assets/FRCtargets.json
@@ -6,10 +6,10 @@
     [19.625,0]
   ],
   "2020 Loading Station": [
-    [-0.09,0.14],
-    [-0.09,-0.14],
-    [0.09,-0.14],
-    [0.09,0.14]
+    [-3.54,5.5],
+    [-3.54,-5.5],
+    [3.54,-5.5],
+    [3.54,5.5]
   ],
   "2019 Dual Target": [
     [-7.75, 3],
diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
index bfa0eb790..ddf275730 100644
--- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
+++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
@@ -135,10 +135,11 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
             if (solvePNPPipe == null)
                 solvePNPPipe = new SolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
             if (drawSolvePNPPipe == null)
-                drawSolvePNPPipe = new DrawSolvePNPPipe(cameraCapture.getCurrentCalibrationData());
+                drawSolvePNPPipe = new DrawSolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData());
 
             solvePNPPipe.setConfig(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
             drawSolvePNPPipe.setConfig(cameraCapture.getCurrentCalibrationData());
+            drawSolvePNPPipe.setConfig(settings);
 
         }
 
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 f2511ffc3..8a8fed749 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
@@ -19,70 +19,37 @@ import java.util.stream.Collectors;
 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) {
+    public DrawSolvePNPPipe(StandardCVPipelineSettings standardCVPipelineSettings, CameraCalibrationConfig settings) {
         setConfig(settings);
-//        setObjectBox(14.5, 6, 2);
-        set2020Box();
+        setBox(standardCVPipelineSettings.targetCornerMat);
     }
 
-    public void setObjectBox(double targetWidth, double targetHeight, double targetDepth) {
-        // implementation from 5190 Green Hope Falcons
 
+    private void setBox(MatOfPoint3f mat) {
         boxCornerMat.release();
-        boxCornerMat = new MatOfPoint3f(
-                new Point3(-targetWidth/2d, -targetHeight/2d, 0),
-                new Point3(-targetWidth/2d, targetHeight/2d, 0),
-                new Point3(targetWidth/2d, targetHeight/2d, 0),
-                new Point3(targetWidth/2d, -targetHeight/2d, 0),
-                new Point3(-targetWidth/2d, -targetHeight/2d, -targetDepth),
-                new Point3(-targetWidth/2d, targetHeight/2d, -targetDepth),
-                new Point3(targetWidth/2d, targetHeight/2d, -targetDepth),
-                new Point3(targetWidth/2d, -targetHeight/2d, -targetDepth)
-        );
-    }
-
-    private MatOfPoint3f lastCornerListPtr = null;
-    public void setConfig(StandardCVPipelineSettings settings) {
-//        boxCornerMat.fromList(settings.targetCornerMat);
-
-        if(settings.targetCornerMat == lastCornerListPtr) {
-            lastCornerListPtr = settings.targetCornerMat;
-            return;
-        }
-
-        var list = settings.targetCornerMat.toList();
+        var list = mat.toList();
         var auxList = list.stream().map(it -> new Point3(it.x, it.y, it.z - 6)).collect(Collectors.toList());
         var finalList = new ArrayList<>(list);
         finalList.addAll(auxList);
         boxCornerMat.fromList(finalList);
     }
 
-    public void set2020Box() {
-        boxCornerMat.release();
-        boxCornerMat.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),
-                new Point3(-19.625, 0, -6),
-                new Point3(-9.819867, -17, -6),
-                new Point3(9.819867, -17, -6),
-                new Point3(19.625, 0, -6)
-        ));
+
+    public void setConfig(StandardCVPipelineSettings settings) {
+        setBox(settings.targetCornerMat);
     }
 
     private Mat cameraMatrix = new Mat();
     private MatOfDouble distortionCoefficients = new MatOfDouble();
 
     public void setConfig(CameraCalibrationConfig config) {
-        if(config == null) {
+        if (config == null) {
             System.err.println("got passed a null config! Returning...");
             return;
         }
@@ -101,17 +68,17 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
         long processStartNanos = System.nanoTime();
 
         var image = targets.getLeft();
-        for(var it : targets.getRight()) {
+        for (var it : targets.getRight()) {
 
             try {
-                Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat() , 0);
+                Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat(), 0);
             } catch (Exception e) {
                 e.printStackTrace();
             }
             var pts = imagePoints.toList();
 
             // draw left and right targets if possible
-            if(it.leftRightDualTargetPair != null) {
+            if (it.leftRightDualTargetPair != null) {
                 var left = it.leftRightDualTargetPair.getLeft();
                 var right = it.leftRightDualTargetPair.getRight();
                 Imgproc.rectangle(image, left.tl(), left.br(), new Scalar(200, 200, 0), 4);
@@ -120,7 +87,7 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
 
             // draw poly dp
             var list = it.approxPoly.toList();
-            for(int i = 0; i < list.size(); i++) {
+            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, 2);
             }
@@ -129,7 +96,7 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
             Imgproc.circle(image, it.minAreaRect.center, 5, red);
 
             // draw corners
-            for(int i = 0; i < it.imageCornerPoints.rows(); i++) {
+            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);
             }