From 293edfe770302390076ff68ccfbbd1c86b8d79a7 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Wed, 26 Feb 2020 09:54:21 -0800
Subject: [PATCH] Add accuracy to solvePNP approx poly dp

---
 .../pipeline/impl/StandardCVPipelineSettings.java      |  2 +-
 .../vision/pipeline/pipes/SolvePNPPipe.java            | 10 +++++++---
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipelineSettings.java
index c81a42133..c58494850 100644
--- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipelineSettings.java
+++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipelineSettings.java
@@ -33,7 +33,7 @@ public class StandardCVPipelineSettings extends CVPipelineSettings {
 
     // 3d stuff
     public MatOfPoint3f targetCornerMat = new MatOfPoint3f();
-    public Number accuracy = 2;
+    public Number accuracy = 5;
     private static MatOfPoint3f hexTargetMat = new MatOfPoint3f();
 
     static {
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 1cda49459..c637018f0 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
@@ -38,6 +38,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
     MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f();
     MatOfPoint2f polyOutput = new MatOfPoint2f();
     private Mat greyImg = new Mat();
+    private double accuracyPercentage = 0.2;
 
     public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) {
         super();
@@ -88,6 +89,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         // TODO add proper year differentiation
         tilt_angle = tilt.getRadians();
         this.objPointsMat = settings.targetCornerMat;
+        this.accuracyPercentage = settings.accuracy.doubleValue();
     }
 
     private void setCameraCoeffs(CameraCalibrationConfig settings) {
@@ -114,7 +116,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         Imgproc.cvtColor(image, greyImg, Imgproc.COLOR_BGR2GRAY);
         poseList.clear();
         for(var target: targets) {
-            var corners = find2020VisionTarget(target);//, imageTargetPair.getRight()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target);
+            var corners = find2020VisionTarget(target, accuracyPercentage);//, imageTargetPair.getRight()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target);
 //            var corners = findCorner2019(target);
             if(corners == null) continue;
 
@@ -165,7 +167,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
      * @param target the target.
      * @return The four outermost tape corners.
      */
-    private MatOfPoint2f find2020VisionTarget(StandardCVPipeline.TrackedTarget target) {
+    private MatOfPoint2f find2020VisionTarget(StandardCVPipeline.TrackedTarget target, double accuracyPercentage) {
         if(target.rawContour.cols() < 1) return null;
 
         var centroid = target.minAreaRect.center;
@@ -179,7 +181,9 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         // Can be tuned to allow/disallow hulls
         // Approx is the number of vertices
         // Ramer–Douglas–Peucker algorithm
-        Imgproc.approxPolyDP(target.rawContour, polyOutput, 0.01 * peri, true);
+        // we want a number between 0 and 0.16 out of a percentage from 0 to 100
+        // so take accuracy and divide by 600
+        Imgproc.approxPolyDP(target.rawContour, polyOutput, accuracyPercentage / 600.0 * peri, true);
 
         var area = Imgproc.moments(polyOutput);