From 844d10335ca0ceb8e99dd2acb5075d2f7f210557 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Thu, 2 Jan 2020 09:53:44 -0800
Subject: [PATCH] Add method to find most extreme per-quadrant points for
 solvePNP

---
 .../pipeline/impl/StandardCVPipeline.java     |  1 +
 .../pipeline/pipes/GroupContoursPipe.java     |  5 +++
 .../vision/pipeline/pipes/SolvePNPPipe.java   | 39 ++++++++++++++++++-
 3 files changed, 44 insertions(+), 1 deletion(-)

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 f3f05cfe8..af727867a 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
@@ -253,6 +253,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
         public Mat tVector = new Mat();
         public MatOfPoint2f imageCornerPoints = new MatOfPoint2f();
         public Pair<Rect, Rect> leftRightDualTargetPair = null;
+        public Pair<RotatedRect, RotatedRect> leftRightRotatedRect = null;
     }
 
 
diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java
index 9fd9f31b0..9d96a053c 100644
--- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java
+++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java
@@ -91,10 +91,15 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
                                 var target = new StandardCVPipeline.TrackedTarget();
                                 target.minAreaRect = rect;
                                 target.contour = contour;
+
                                 target.leftRightDualTargetPair =
                                         Pair.of(Imgproc.boundingRect(firstContour),
                                                 Imgproc.boundingRect(secondContour));
 
+                                target.leftRightRotatedRect =
+                                        Pair.of(Imgproc.minAreaRect(new MatOfPoint2f(firstContour.toArray())),
+                                                Imgproc.minAreaRect(new MatOfPoint2f(secondContour.toArray())));
+
                                 groupedContours.add(target);
                             }
                         } catch (IndexOutOfBoundsException e) {
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 db0384807..7157ca51c 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
@@ -120,6 +120,42 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
         return points;
     }
 
+    private MatOfPoint2f findCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) {
+        if(target.leftRightRotatedRect == null) return null;
+
+        var centroid = target.minAreaRect.center;
+
+        var left = target.leftRightRotatedRect.getLeft();
+        var right = target.leftRightRotatedRect.getRight();
+
+        // flip if the "left" target is to the right
+        if(left.center.x > right.center.x) {
+            var temp = left;
+            left = right;
+            right = temp;
+        }
+
+        var leftPoints = new Point[4];
+        left.points(leftPoints);
+        var rightPoints = new Point[4];
+        right.points(rightPoints);
+        ArrayList<Point> combinedList = new ArrayList<>(List.of(leftPoints));
+        combinedList.addAll(List.of(rightPoints));
+
+        // start looking in the top left quadrant
+        Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
+
+        var tl = combinedList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get();
+        var tr = combinedList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get();
+        var bl = combinedList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get();
+        var br = combinedList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get();
+
+        boundingBoxResultMat.release();
+        boundingBoxResultMat.fromList(List.of(tl, bl, br, tr));
+
+        return boundingBoxResultMat;
+    }
+
     private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) {
 
 //        List<Pair<MatOfPoint2f, CVPipeline2d.Target2d>> list = new ArrayList<>();
@@ -202,7 +238,8 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
         return tempCornerList;
     }
 
-    private MatOfPoint2f findCorners2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
+    @Deprecated
+    private MatOfPoint2f findGoodFeaturesToTrack2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
 
 //        Imgproc.approxPolyDP(new MatOfPoint2f(max_contour.toArray()),approx,epsilon,true);