From 2ce8a8dad9bd201c90c6329e2952f35011f24b4d Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Fri, 3 Jan 2020 16:00:18 -0800
Subject: [PATCH] Avoid allocating a new mat in solvePNP, perf. increases in
 group contors pipe

---
 .../vision/pipeline/pipes/GroupContoursPipe.java      |  4 ++++
 .../vision/pipeline/pipes/SolvePNPPipe.java           | 11 +++++------
 2 files changed, 9 insertions(+), 6 deletions(-)

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 0cb2cf31b..42cfd6d96 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
@@ -41,6 +41,7 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
     public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<MatOfPoint> input) {
         long processStartNanos = System.nanoTime();
 
+        groupedContours.forEach(StandardCVPipeline.TrackedTarget::release);
         groupedContours.clear();
 
         if (input.size() > (group.equals(TargetGroup.Single) ? 0 : 1)) {
@@ -104,6 +105,9 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
                                 
                                 groupedContours.add(target);
 
+                                firstContour.release();
+                                secondContour.release();
+
                                 // skip the next contour because it's been grouped already
                                 i += 1;
                             }
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 7157ca51c..21d4798bc 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
@@ -36,6 +36,7 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
     Comparator<Point> leftRightComparator = Comparator.comparingDouble(point -> point.x);
     Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
     private double distanceDivisor = 1.0;
+    Mat scaledTvec = new Mat();
 
     public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) {
         super();
@@ -331,22 +332,20 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
 
         // This should be pzero_world = numpy.matmul(rot_inv, -tvec)
 //        pzero_world  = rot_inv.mul(matScale(tVec, -1));
-        Core.gemm(rot_inv, matScale(tVec, -1), 1, kMat, 0, pzero_world);
+        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
-        //noinspection UnnecessaryLocalVariable
-        var targetDistance = distance * 25.4 / 1000d / distanceDivisor; // meters or whatever the calibration was in
+        var targetDistance = distance * 25.4 / 1000d / distanceDivisor; // This should be meters
 
         var targetLocation = new Translation2d(targetDistance * FastMath.cos(targetAngle), targetDistance * FastMath.sin(targetAngle));
         target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation));
         target.rVector = rVec;
         target.tVector = tVec;
 
-//        System.out.println("Found target at pose: " + target.cameraRelativePose.toString());
-
         return target;
     }
 
@@ -358,7 +357,7 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
      */
     public Mat matScale(Mat src, double factor) {
         Mat dst = new Mat(src.rows(),src.cols(),src.type());
-        Scalar s = new Scalar(factor);
+        Scalar s = new Scalar(factor); // TODO check if we need to add more elements to this
         Core.multiply(src, s, dst);
         return dst;
     }