From 1aac80c63a5a75ff5dce20ea400055057d362959 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sun, 5 Jan 2020 22:29:41 -0800
Subject: [PATCH] Update box for 2020

---
 .../pipeline/pipes/DrawSolvePNPPipe.java      | 21 +++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

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 563553f2e..2dcf6d115 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
@@ -23,7 +23,8 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
 
     public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
         setConfig(settings);
-        setObjectBox(14.5, 6, 2);
+//        setObjectBox(14.5, 6, 2);
+        set2020Box();
     }
 
     public void setObjectBox(double targetWidth, double targetHeight, double targetDepth) {
@@ -42,6 +43,20 @@ 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(-9.819867, -17, 0),
+                new Point3(9.819867, -17, 0),
+                new Point3(16.25, 0, 0),
+                new Point3(-16.25, 0, -6),
+                new Point3(-9.819867, -17, -6),
+                new Point3(9.819867, -17, -6),
+                new Point3(16.25, 0, -6)
+        );
+    }
+
     private Mat cameraMatrix = new Mat();
     private MatOfDouble distortionCoefficients = new MatOfDouble();
 
@@ -58,13 +73,15 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
         this.distortionCoefficients = distortionMatrix_;
     }
 
+    MatOfPoint2f imagePoints = new MatOfPoint2f();
+
     @Override
     public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> targets) {
         long processStartNanos = System.nanoTime();
 
         var image = targets.getLeft();
         for(var it : targets.getRight()) {
-            MatOfPoint2f imagePoints = new MatOfPoint2f();
+
             try {
                 Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat() , 0);
             } catch (Exception e) {