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] 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));
     }