From 08a03b31783d3af5e7572f739d3fdac6fd27ccda Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sun, 23 Feb 2020 22:11:07 -0800
Subject: [PATCH 1/5] reproject corners in solvePNP test

---
 .../vision/pipeline/SolvePNPtest.java         | 21 +++++++++++++++----
 1 file changed, 17 insertions(+), 4 deletions(-)

diff --git a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
index 66d237210..9fc54dbba 100644
--- a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
+++ b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
@@ -54,16 +54,17 @@ public class SolvePNPtest {
         // make a tvec and rvec
 //        positive x to the right, positive y to the bottom, positive z away from the image
         // tvec example:
-//        [45.92180012026041;
-//        24.94979431168253;
-//        88.00905002660249]
+//        [-3.377348429632199;
+//        9.132802434424915;
+//        67.79662519667924]
+
         // rvec:
 //        [1.990867004634147;
 //        -0.1508389335122144;
 //        -1.552061845576413]
 
         Mat tvec = new Mat(3, 1, 6);
-        tvec.put(0, 0, 0, 0, 200); // 10ft away?
+        tvec.put(0, 0, 0, 40, 200); // 10ft away?
         Mat rvec = new Mat(3, 1, 6);
         rvec.put(0, 0, 0, 0, 0);
 
@@ -75,6 +76,18 @@ public class SolvePNPtest {
             Imgproc.circle(blank, p, 3, new Scalar(0, 0, 255), 4);
         }
 
+        // go backwards to solvePNP
+        Mat rvec_ = new Mat(), tvec_ = new Mat();
+        Calib3d.solvePnP(target, imagePoints, calibration.getCameraMatrixAsMat(), calibration.getDistortionCoeffsAsMat(), rvec_, tvec_);
+
+        // what we projected
+        var initTvec = tvec.dump();
+        var initRvec = rvec.dump();
+
+        // what solvePNP gives
+        var retedTvec = tvec_.dump();
+        var rettedRvec = rvec_.dump();
+
         displayImage(mat2BufferedImage(blank));
     }
 

From b0661d32156ad459014a96b7876f054beb51cc48 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Mon, 24 Feb 2020 00:04:35 -0800
Subject: [PATCH 2/5] add corner sub pixel detection

assuming your resolution is high enough this should work well for helping out approx poly DP.
---
 .../pipeline/impl/StandardCVPipeline.java     |   2 +-
 .../vision/pipeline/pipes/SolvePNPPipe.java   | 136 ++++--------------
 2 files changed, 27 insertions(+), 111 deletions(-)

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 82bfabd5b..836f33b39 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
@@ -200,7 +200,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
 
         if (settings.is3D) {
             // once we've sorted our targets, perform solvePNP. The number of "best targets" is limited by the above pipe
-            Pair<List<TrackedTarget>, Long> solvePNPResult = solvePNPPipe.run(Pair.of(collect2dTargetsResult.getLeft(), hsvResult.getLeft()));
+            Pair<List<TrackedTarget>, Long> solvePNPResult = solvePNPPipe.run(Pair.of(collect2dTargetsResult.getLeft(), rotateFlipResult.getLeft()));
             totalPipelineTimeNanos += solvePNPResult.getRight();
 
             Pair<Mat, Long> draw3dContoursResult = drawSolvePNPPipe.run(Pair.of(outputMatResult.getLeft(), solvePNPResult.getLeft()));
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 c83345846..c9a3bab09 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
@@ -14,7 +14,6 @@ import org.opencv.core.*;
 import org.opencv.imgproc.Imgproc;
 
 import java.util.*;
-import java.util.stream.Collectors;
 
 public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, Mat>, List<StandardCVPipeline.TrackedTarget>> {
 
@@ -33,6 +32,9 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
     Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
     private double distanceDivisor = 1.0;
     Mat scaledTvec = new Mat();
+    MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f();
+    MatOfPoint2f polyOutput = new MatOfPoint2f();
+    private Mat greyImg = new Mat();
 
     public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) {
         super();
@@ -105,10 +107,16 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
     public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(Pair<List<StandardCVPipeline.TrackedTarget>, Mat> imageTargetPair) {
         long processStartNanos = System.nanoTime();
         var targets = imageTargetPair.getLeft();
+        var image = imageTargetPair.getRight();
         poseList.clear();
         for(var target: targets) {
             var corners = find2020VisionTarget(target);//, imageTargetPair.getRight()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target);
+//            var corners = findCorner2019(target);
             if(corners == null) continue;
+
+            // refine the estimate
+            corners = refineCornerEstimateSubPix(corners, image);
+
             var pose = calculatePose(corners, target);
             if(pose != null) poseList.add(pose);
         }
@@ -145,10 +153,6 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         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.
@@ -210,8 +214,6 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         }
     }
 
-    MatOfPoint2f polyOutput = new MatOfPoint2f();
-
     /**
      * Find the target using the outermost tape corners and a dual target.
      * @param target the target.
@@ -290,115 +292,29 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         return boundingBoxResultMat;
     }
 
-    MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f();
-    MatOfPoint2f goodFeaturesResultMat = new MatOfPoint2f();
+    // Set the needed parameters to find the refined corners
+    Size winSize = new Size(5, 5);
+    Size zeroZone = new Size(-1, -1); // we don't need a zero zone
+    TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 50, 0.001);
 
-    private Mat dstNorm = new Mat();
-    private Mat dstNormScaled = new Mat();
-    List<Point> tempCornerList = new ArrayList<>();
+    private boolean shouldRefineCorners = true;
 
     /**
-     * Find the corners in an image.
-     * @param targetImage the image to find corners in.
-     * @return the corners found in the image.
+     * Refine an estimated corner position using the cornerSubPixel algorithm.
+     *
+     * TODO should this be here or before the points are chosen? 
+     *
+     * @param corners the corners detected -- this mat is modified!
+     * @param img the image taken by the camera as color
+     * @return the updated mat, same as the corner mat passed in.
      */
-    @Deprecated
-    private List<Point> findCornerHarris(Mat targetImage) {
+    private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat img) {
+        if(!shouldRefineCorners) return corners; // just return
 
-        // convert the image to greyscale
-        var gray = new Mat();
-        Imgproc.cvtColor(targetImage, gray, Imgproc.COLOR_BGR2GRAY);
-        Mat dst = Mat.zeros(targetImage.size(), CvType.CV_8U);
+        Imgproc.cvtColor(img, greyImg, Imgproc.COLOR_BGR2GRAY);
+        Imgproc.cornerSubPix(greyImg, corners, winSize, zeroZone, criteria);
 
-        // constants
-        final int blockSize = 2;
-        final int apertureSize = 3;
-        final double k = 0.04;
-        final int threshold = 200;
-
-        /// Detecting corners
-        Imgproc.cornerHarris(gray, dst, blockSize, apertureSize, k);
-
-        /// Normalizing
-        Core.normalize(dst, dstNorm, 0, 255, Core.NORM_MINMAX);
-        Core.convertScaleAbs(dstNorm, dstNormScaled);
-
-        /// Drawing a circle around corners
-        float[] dstNormData = new float[(int) (dstNorm.total() * dstNorm.channels())];
-        dstNorm.get(0, 0, dstNormData);
-
-        tempCornerList.clear();
-        for (int i = 0; i < dstNorm.rows(); i++) {
-            for (int j = 0; j < dstNorm.cols(); j++) {
-                if ((int) dstNormData[i * dstNorm.cols() + j] > threshold) {
-                    tempCornerList.add(new Point(j, i));
-                }
-            }
-        }
-
-        return tempCornerList;
-    }
-
-    @Deprecated
-    private MatOfPoint2f findGoodFeaturesToTrack2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
-
-        // start by looking for corners
-        var points__ = findBoundingBoxCorners(target).toList();
-        var xList = points__.stream().map(it -> it.x).sorted(Double::compare).collect(Collectors.toList());
-        var yList = points__.stream().map(it -> it.y).sorted(Double::compare).collect(Collectors.toList());
-
-        var boundingTl = new Point(
-            xList.get(0), yList.get(0)
-        );
-        var boundingBr = new Point (
-            xList.get(2), yList.get(2)
-        );
-        System.out.println("tl/br:\n" + boundingTl.toString() + "\n" + boundingBr.toString());
-
-        var slightlyBiggerTl = new Point(
-            Math.max(0, boundingTl.x - 5),
-            Math.max(0, boundingTl.y - 5)
-        );
-
-        var slightlyBiggerBr = new Point(
-            Math.min(srcImage.rows(), boundingBr.x + 5),
-            Math.min(srcImage.cols(), boundingBr.y + 5)
-        );
-        var rect = new Rect(slightlyBiggerTl, slightlyBiggerBr);
-
-        var croppedImage = srcImage.submat(rect);
-        var corners = new MatOfPoint();
-        Imgproc.goodFeaturesToTrack(croppedImage, corners, 0,0.01,5);
-
-        List<Point> cornerList = new ArrayList<>(corners.toList());
-//        if(cornerList.size() != 8 && cornerList.size() != 4) return null;
-        cornerList.sort(leftRightComparator);
-
-        cornerList = cornerList.stream().map(point ->
-            new Point(point.x + slightlyBiggerTl.x, point.y + slightlyBiggerTl.y))
-            .collect(Collectors.toList());
-
-        // of these, we want the two leftmost and two rightmost points
-        var left1 = cornerList.get(0);
-        var left2 = cornerList.get(1);
-        var right1 = cornerList.get(0);
-        var right2 = cornerList.get(1);
-
-        // TODO maximize distance from the center rather than naively assume the leftmost and rightmost
-        // will have to do per quadrant
-
-        var leftOrder = left1.y < left2.y;
-        var rightOrder = right1.y < right2.y;
-
-        var tl = leftOrder ? left1 : left2;
-        var bl = !leftOrder ? left1 : left2;
-        var tr = rightOrder ? right1 : right2;
-        var br = !rightOrder ? right1 : right2;
-
-        goodFeaturesResultMat.release();
-        goodFeaturesResultMat.fromList(List.of(tl, bl, br, tr));
-
-        return goodFeaturesResultMat;
+        return corners;
     }
 
     private StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) {

From cdd01c4b191924de5ab7cf6ff2d84034d6bb91fa Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Mon, 24 Feb 2020 09:10:17 -0800
Subject: [PATCH 3/5] Increase window size

---
 .../com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 c9a3bab09..8a7467fb1 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
@@ -293,7 +293,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
     }
 
     // Set the needed parameters to find the refined corners
-    Size winSize = new Size(5, 5);
+    Size winSize = new Size(12, 12);
     Size zeroZone = new Size(-1, -1); // we don't need a zero zone
     TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 50, 0.001);
 

From ee13c0b2f9c3d98c23b288957945bdfe82d4bd43 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Mon, 24 Feb 2020 22:12:42 -0800
Subject: [PATCH 4/5] Use goodFeaturesToTrack, update test

---
 .../vision/pipeline/pipes/SolvePNPPipe.java   | 57 ++++++++++++++++---
 .../vision/pipeline/SolvePNPtest.java         | 19 +++++--
 2 files changed, 63 insertions(+), 13 deletions(-)

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 8a7467fb1..1340e83e7 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
@@ -14,6 +14,7 @@ import org.opencv.core.*;
 import org.opencv.imgproc.Imgproc;
 
 import java.util.*;
+import java.util.stream.Collectors;
 
 public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, Mat>, List<StandardCVPipeline.TrackedTarget>> {
 
@@ -108,14 +109,18 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         long processStartNanos = System.nanoTime();
         var targets = imageTargetPair.getLeft();
         var image = imageTargetPair.getRight();
+        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 = findCorner2019(target);
             if(corners == null) continue;
 
+            // use best features to track
+            corners = refineCornersByBestTrack(corners, greyImg, target);
+
             // refine the estimate
-            corners = refineCornerEstimateSubPix(corners, image);
+            corners = refineCornerEstimateSubPix(corners, greyImg);
 
             var pose = calculatePose(corners, target);
             if(pose != null) poseList.add(pose);
@@ -172,7 +177,7 @@ 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.02 * peri, true);
+        Imgproc.approxPolyDP(target.rawContour, polyOutput, 0.01 * peri, true);
 
         var area = Imgproc.moments(polyOutput);
 
@@ -292,10 +297,46 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         return boundingBoxResultMat;
     }
 
+    MatOfPoint2f goodFeatureToTrackRetval = new MatOfPoint2f();
+
+    private MatOfPoint2f refineCornersByBestTrack(MatOfPoint2f corners, Mat greyImg, StandardCVPipeline.TrackedTarget target) {
+
+        MatOfPoint approxf1 = new MatOfPoint();
+        var origCornerList = new ArrayList<>(corners.toList());
+        approxf1.fromList(origCornerList.stream()
+                .map(it -> new Point(it.x - target.boundingRect.x, it.y - target.boundingRect.y))
+                .collect(Collectors.toList())
+        );
+        var croppedImage = greyImg.submat(target.boundingRect);
+
+        Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.01, 5);
+
+        // at this point corners is still unmodified so let's map it
+        List<Point> tempList = new ArrayList<>();
+
+        // shift all points back into global pose
+        var reshiftedList = approxf1.toList().stream().map(it -> new Point(it.x + target.boundingRect.x, it.y + target.boundingRect.y))
+                .collect(Collectors.toList());
+        for(Point p: origCornerList) {
+            // find the goodFeaturesToTrack corner closest to me
+            var closestPoint = reshiftedList.stream().min(Comparator.comparingDouble(p_ -> distanceBetween(p_, p)));
+            if(closestPoint.isEmpty()) {
+                tempList.add(p);
+                reshiftedList.remove(p);
+            } else {
+                tempList.add(closestPoint.get());
+                reshiftedList.remove(closestPoint.get());
+            }
+        }
+
+        goodFeatureToTrackRetval.fromList(tempList);
+        return goodFeatureToTrackRetval;
+    }
+
     // Set the needed parameters to find the refined corners
-    Size winSize = new Size(12, 12);
+    Size winSize = new Size(4, 4);
     Size zeroZone = new Size(-1, -1); // we don't need a zero zone
-    TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 50, 0.001);
+    TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 90, 0.001);
 
     private boolean shouldRefineCorners = true;
 
@@ -305,19 +346,17 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
      * TODO should this be here or before the points are chosen? 
      *
      * @param corners the corners detected -- this mat is modified!
-     * @param img the image taken by the camera as color
+     * @param greyImg the image taken by the camera as color
      * @return the updated mat, same as the corner mat passed in.
      */
-    private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat img) {
+    private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat greyImg) {
         if(!shouldRefineCorners) return corners; // just return
-
-        Imgproc.cvtColor(img, greyImg, Imgproc.COLOR_BGR2GRAY);
         Imgproc.cornerSubPix(greyImg, corners, winSize, zeroZone, criteria);
 
         return corners;
     }
 
-    private StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) {
+    public StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) {
         if(objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) {
             System.err.println("can't do solvePNP with invalid params!");
             return null;
diff --git a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
index 9fc54dbba..104a89dfb 100644
--- a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
+++ b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
@@ -40,8 +40,8 @@ public class SolvePNPtest {
         var settings = new StandardCVPipelineSettings();
         var calibration = new CameraCalibrationConfig(
                 new Size(640, 480),
-                new JsonMat(3, 3, 6, new double[] { 926.1016601017006, 0.0, 437.22446072361055, 0.0, 918.2612433944396, 137.8989492231747, 0.0, 0.0, 1.0 }),
-                new JsonMat(1, 5, 6, new double[] { -0.001917838248173303, 0.0059895823594355, -0.035282888419499406, 0.04373249383460662, 0.1732528905031391 }),
+                new JsonMat(3, 3, 6, new double[] { 1126.1154452525066, 0.0, 666.4172679761178, 0.0, 1088.0425532065287, 335.37748454259633, 0.0, 0.0, 1.0 }),
+                new JsonMat(1, 5, 6, new double[] { 0.07253724845871252, -0.664268685338307, -0.0011224914177033868, 4.8323234488098423E-4, 1.1731498589436031 }),
                 1.056
                 );
 
@@ -64,17 +64,24 @@ public class SolvePNPtest {
 //        -1.552061845576413]
 
         Mat tvec = new Mat(3, 1, 6);
-        tvec.put(0, 0, 0, 40, 200); // 10ft away?
         Mat rvec = new Mat(3, 1, 6);
-        rvec.put(0, 0, 0, 0, 0);
+//        tvec.put(0, 0, 1.032188152287021,  -3.78145690753876, 52.32713732614368);
+//        rvec.put(0, 0, -3.084531365719034, -0.1446574541579896,  -0.1297813889017779);
+        tvec.put(0, 0, 1.75, -6, 75.2);
+        rvec.put(0, 0, 2.79, 0.23, -0.0388);
 
         MatOfPoint2f imagePoints = new MatOfPoint2f();
         Calib3d.projectPoints(target, rvec, tvec, calibration.getCameraMatrixAsMat(), calibration.getDistortionCoeffsAsMat(), imagePoints, new Mat(), 0);
         var projectedPts = imagePoints.toList();
 
+        // draw circles
         for(var p: projectedPts) {
             Imgproc.circle(blank, p, 3, new Scalar(0, 0, 255), 4);
         }
+        Imgproc.line(blank, projectedPts.get(0), projectedPts.get(1), new Scalar(255, 0, 0));
+        Imgproc.line(blank, projectedPts.get(1), projectedPts.get(2), new Scalar(255, 0, 0));
+        Imgproc.line(blank, projectedPts.get(2), projectedPts.get(3), new Scalar(255, 0, 0));
+        Imgproc.line(blank, projectedPts.get(3), projectedPts.get(0), new Scalar(255, 0, 0));
 
         // go backwards to solvePNP
         Mat rvec_ = new Mat(), tvec_ = new Mat();
@@ -88,6 +95,10 @@ public class SolvePNPtest {
         var retedTvec = tvec_.dump();
         var rettedRvec = rvec_.dump();
 
+        var target_ = new StandardCVPipeline.TrackedTarget();
+        pipe.calculatePose(imagePoints, target_);
+        System.out.println(target_.cameraRelativePose);
+
         displayImage(mat2BufferedImage(blank));
     }
 

From bef79cb8421b44ba0da2311e8cdcf02165102bde Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Wed, 26 Feb 2020 08:52:14 -0800
Subject: [PATCH 5/5] Temporarily disable goodFeaturesToTrack

---
 .../vision/pipeline/pipes/SolvePNPPipe.java   | 41 ++++++++++++-------
 1 file changed, 27 insertions(+), 14 deletions(-)

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 1340e83e7..1cda49459 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
@@ -4,6 +4,8 @@ import com.chameleonvision.config.CameraCalibrationConfig;
 import com.chameleonvision.vision.pipeline.Pipe;
 import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
 import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -116,8 +118,8 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
 //            var corners = findCorner2019(target);
             if(corners == null) continue;
 
-            // use best features to track
-            corners = refineCornersByBestTrack(corners, greyImg, target);
+//            // use best features to track
+//            corners = refineCornersByBestTrack(corners, greyImg, target);
 
             // refine the estimate
             corners = refineCornerEstimateSubPix(corners, greyImg);
@@ -309,7 +311,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         );
         var croppedImage = greyImg.submat(target.boundingRect);
 
-        Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.01, 5);
+        Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.1, 5);
 
         // at this point corners is still unmodified so let's map it
         List<Point> tempList = new ArrayList<>();
@@ -356,6 +358,9 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
         return corners;
     }
 
+    NetworkTableEntry tvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard").getEntry("tvec");
+    NetworkTableEntry rvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard").getEntry("rvec");
+
     public StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) {
         if(objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) {
             System.err.println("can't do solvePNP with invalid params!");
@@ -371,35 +376,43 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
             return null;
         }
 
+        tvecE.setString(tVec.dump());
+        rvecE.setString(rVec.dump());
+
         // Algorithm from team 5190 Green Hope Falcons
 
 //        var tilt_angle = 0.0; // TODO add to settings
 
+        // the left/right distance to the target, unchanged by tilt
         var x = tVec.get(0, 0)[0];
-        var z = FastMath.sin(tilt_angle) * tVec.get(1, 0)[0] + tVec.get(2, 0)[0] *  FastMath.cos(tilt_angle);
 
-        // distance in the horizontal plane between camera and target
-        var distance = FastMath.sqrt(x * x + z * z);
+        // Z distance in the flat plane is given by
+        // Z_field = z cos theta + y sin theta
+        var z = tVec.get(2, 0)[0] *  FastMath.cos(tilt_angle) + tVec.get(1, 0)[0] * FastMath.sin(tilt_angle);
 
-        // horizontal angle between center camera and target
-        @SuppressWarnings("SuspiciousNameCombination")
-        var angle1 = FastMath.atan2(x, z);
+        // find skew of the target relative to the camera
+        // from ligerbots:
+        // rot, _ = cv2.Rodrigues(rvec)
+        // rot_inv = rot.transpose()
+        // pzero_world = numpy.matmul(rot_inv, -tvec)
+        // angle2 = math.atan2(pzero_world[0][0], pzero_world[2][0]
 
         Calib3d.Rodrigues(rVec, rodriguez);
         Core.transpose(rodriguez, rot_inv); // rodrigurz.t()
 
-        // This should be pzero_world = numpy.matmul(rot_inv, -tvec)
-//        pzero_world  = rot_inv.mul(matScale(tVec, -1));
         scaledTvec = matScale(tVec, -1);
         Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world);
 
         var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]);
 
-        var targetAngle = -angle1; // radians
         var targetRotation = -angle2; // radians
-        var targetDistance = distance * 25.4 / 1000d / distanceDivisor; // This should be meters
 
-        var targetLocation = new Translation2d(targetDistance * FastMath.cos(targetAngle), targetDistance * FastMath.sin(targetAngle));
+        // We want a vector that is X forward and Y left.
+        // We have a Z_field (out of the camera projected onto the field), and an X left/right.
+        // so Z_field becomes X, and X becomes Y
+
+        //noinspection SuspiciousNameCombination
+        var targetLocation = new Translation2d(z, x).times(25.4 / 1000d / distanceDivisor);
         target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation));
         target.rVector = rVec;
         target.tVector = tVec;