From 061385f0b660bed3b1565bb17434a614199e07d8 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Thu, 16 Jul 2020 17:50:40 -0700
Subject: [PATCH] Add skew calculation support (#36)

This is a breaking change to PhotonLib.
---
 .../src/views/PipelineViews/TargetsTab.vue    |   4 +
 .../structures/SimplePipelineResult.java      | 110 ------------------
 .../structures/SimpleTrackedTarget.java       |  97 ---------------
 .../pipeline/result/SimplePipelineResult.java |   4 +-
 .../pipeline/result/SimpleTrackedTarget.java  |  10 +-
 .../vision/target/TargetCalculations.java     |  11 ++
 .../vision/target/TrackedTarget.java          |   8 +-
 .../vision/target/TargetCalculationsTest.java |  78 +++++++++++++
 8 files changed, 107 insertions(+), 215 deletions(-)
 delete mode 100644 photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimplePipelineResult.java
 delete mode 100644 photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimpleTrackedTarget.java

diff --git a/photon-client/src/views/PipelineViews/TargetsTab.vue b/photon-client/src/views/PipelineViews/TargetsTab.vue
index c87f1f812..b6324c86b 100644
--- a/photon-client/src/views/PipelineViews/TargetsTab.vue
+++ b/photon-client/src/views/PipelineViews/TargetsTab.vue
@@ -25,6 +25,9 @@
                 <th class="text-center">
                   Yaw
                 </th>
+                <th class="text-center">
+                  Skew
+                </th>
               </template>
               <th class="text-center">
                 Area
@@ -51,6 +54,7 @@
               <template v-if="!is3D">
                 <td>{{ parseFloat(value.pitch).toFixed(2) }}</td>
                 <td>{{ parseFloat(value.yaw).toFixed(2) }}</td>
+                <td>{{ parseFloat(value.skew).toFixed(2) }}</td>
               </template>
               <td>{{ parseFloat(value.area).toFixed(2) }}</td>
               <template v-if="is3D">
diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimplePipelineResult.java b/photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimplePipelineResult.java
deleted file mode 100644
index 34e4fe637..000000000
--- a/photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimplePipelineResult.java
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- * Copyright (C) 2020 Photon Vision.
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <https://www.gnu.org/licenses/>.
- */
-
-package org.photonvision.common.dataflow.structures;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Objects;
-import org.photonvision.vision.pipeline.result.CVPipelineResult;
-
-public class SimplePipelineResult extends BytePackable {
-
-    private double latencyMillis;
-    private boolean hasTargets;
-    public final List<SimpleTrackedTarget> targets = new ArrayList<>();
-
-    public SimplePipelineResult() {}
-
-    public SimplePipelineResult(
-            double latencyMillis, boolean hasTargets, List<SimpleTrackedTarget> targets) {
-        this.latencyMillis = latencyMillis;
-        this.hasTargets = hasTargets;
-        this.targets.addAll(targets);
-    }
-
-    public SimplePipelineResult(CVPipelineResult origResult) {
-        update(origResult);
-    }
-
-    public void update(CVPipelineResult origResult) {
-        latencyMillis = origResult.getLatencyMillis();
-        hasTargets = origResult.hasTargets();
-        targets.clear();
-        for (var origTarget : origResult.targets) {
-            targets.add(new SimpleTrackedTarget(origTarget));
-        }
-    }
-
-    public double getLatencyMillis() {
-        return latencyMillis;
-    }
-
-    public boolean hasTargets() {
-        return hasTargets;
-    }
-
-    @Override
-    public boolean equals(Object o) {
-        if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
-        SimplePipelineResult that = (SimplePipelineResult) o;
-        boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0;
-        boolean hasTargetsMatch = that.hasTargets == hasTargets;
-        boolean targetsMatch = that.targets.equals(targets);
-        return latencyMatch && hasTargetsMatch && targetsMatch;
-    }
-
-    @Override
-    public int hashCode() {
-        return Objects.hash(latencyMillis, hasTargets, targets);
-    }
-
-    @Override
-    public byte[] toByteArray() {
-        bufferPosition = 0;
-        int bufferSize =
-                8 + 1 + 1 + (targets.size() * 48); // latencyMillis + hasTargets + targetCount + targets
-        var buff = new byte[bufferSize];
-
-        bufferData(latencyMillis, buff);
-        bufferData(hasTargets, buff);
-        bufferData((byte) targets.size(), buff);
-        for (var target : targets) {
-            bufferData(target.toByteArray(), buff);
-        }
-
-        return buff;
-    }
-
-    @Override
-    public void fromByteArray(byte[] src) {
-        bufferPosition = 0;
-
-        latencyMillis = unbufferDouble(src);
-        hasTargets = unbufferBoolean(src);
-        byte targetCount = unbufferByte(src);
-
-        targets.clear();
-        for (int i = 0; i < targetCount; i++) {
-            var target = new SimpleTrackedTarget();
-            target.fromByteArray(unbufferBytes(src, 48));
-            bufferPosition += 48;
-            targets.add(target);
-        }
-    }
-}
diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimpleTrackedTarget.java b/photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimpleTrackedTarget.java
deleted file mode 100644
index bff7431b5..000000000
--- a/photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimpleTrackedTarget.java
+++ /dev/null
@@ -1,97 +0,0 @@
-/*
- * Copyright (C) 2020 Photon Vision.
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <https://www.gnu.org/licenses/>.
- */
-
-package org.photonvision.common.dataflow.structures;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import java.util.Objects;
-import org.photonvision.vision.target.TrackedTarget;
-
-public class SimpleTrackedTarget extends BytePackable {
-    public static final int PACK_SIZE_BYTES = Double.BYTES * 6;
-
-    private double yaw;
-    private double pitch;
-    private double area;
-    private Pose2d robotRelativePose = new Pose2d();
-
-    public SimpleTrackedTarget() {}
-
-    public SimpleTrackedTarget(double yaw, double pitch, double area, Pose2d pose) {
-        this.yaw = yaw;
-        this.pitch = pitch;
-        this.area = area;
-        robotRelativePose = pose;
-    }
-
-    public SimpleTrackedTarget(TrackedTarget origTarget) {
-        yaw = origTarget.getYaw();
-        pitch = origTarget.getPitch();
-        area = origTarget.getArea();
-        robotRelativePose = origTarget.getRobotRelativePose();
-    }
-
-    @Override
-    public boolean equals(Object o) {
-        if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
-        SimpleTrackedTarget that = (SimpleTrackedTarget) o;
-        return Double.compare(that.yaw, yaw) == 0
-                && Double.compare(that.pitch, pitch) == 0
-                && Double.compare(that.area, area) == 0
-                && Objects.equals(robotRelativePose, that.robotRelativePose);
-    }
-
-    @Override
-    public int hashCode() {
-        return Objects.hash(yaw, pitch, area, robotRelativePose);
-    }
-
-    @Override
-    public byte[] toByteArray() {
-        resetBufferPosition();
-        byte[] data = new byte[PACK_SIZE_BYTES];
-
-        bufferData(yaw, data);
-        bufferData(pitch, data);
-        bufferData(area, data);
-        bufferData(robotRelativePose.getTranslation().getX(), data);
-        bufferData(robotRelativePose.getTranslation().getY(), data);
-        bufferData(robotRelativePose.getRotation().getDegrees(), data);
-
-        return data;
-    }
-
-    @Override
-    public void fromByteArray(byte[] src) {
-        resetBufferPosition();
-        if (src.length < PACK_SIZE_BYTES) {
-            // TODO: log error?
-            return;
-        }
-
-        yaw = unbufferDouble(src);
-        pitch = unbufferDouble(src);
-        area = unbufferDouble(src);
-
-        var poseX = unbufferDouble(src);
-        var poseY = unbufferDouble(src);
-        var poseR = unbufferDouble(src);
-        robotRelativePose = new Pose2d(poseX, poseY, Rotation2d.fromDegrees(poseR));
-    }
-}
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java
index 8e76769ae..51a0aa77b 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java
@@ -68,8 +68,8 @@ public class SimplePipelineResult extends BytePackable {
     @Override
     public byte[] toByteArray() {
         bufferPosition = 0;
-        int bufferSize =
-                8 + 1 + 1 + (targets.size() * 48); // latencyMillis + hasTargets + targetCount + targets
+        // latencyMillis + hasTargets + targetCount + targets
+        int bufferSize = 8 + 1 + 1 + (targets.size() * 8 * SimpleTrackedTarget.PACK_SIZE_BYTES);
         var buff = new byte[bufferSize];
 
         bufferData(latencyMillis, buff);
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
index bec38e5b3..3a6ba5429 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
@@ -23,24 +23,26 @@ import java.util.Objects;
 import org.photonvision.vision.target.TrackedTarget;
 
 public class SimpleTrackedTarget extends BytePackable {
-    public static final int PACK_SIZE_BYTES = Double.BYTES * 6;
+    public static final int PACK_SIZE_BYTES = Double.BYTES * 7;
 
     private double yaw;
     private double pitch;
     private double area;
+    private double skew;
     private Pose2d robotRelativePose = new Pose2d();
 
     public SimpleTrackedTarget() {}
 
-    public SimpleTrackedTarget(double yaw, double pitch, double area, Pose2d pose) {
+    public SimpleTrackedTarget(double yaw, double pitch, double area, double skew, Pose2d pose) {
         this.yaw = yaw;
         this.pitch = pitch;
         this.area = area;
+        this.skew = skew;
         robotRelativePose = pose;
     }
 
     public SimpleTrackedTarget(TrackedTarget t) {
-        this(t.getYaw(), t.getPitch(), t.getArea(), t.getRobotRelativePose());
+        this(t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getRobotRelativePose());
     }
 
     public double getYaw() {
@@ -83,6 +85,7 @@ public class SimpleTrackedTarget extends BytePackable {
         bufferData(yaw, data);
         bufferData(pitch, data);
         bufferData(area, data);
+        bufferData(skew, data);
         if (robotRelativePose != null) {
             bufferData(robotRelativePose.getTranslation().getX(), data);
             bufferData(robotRelativePose.getTranslation().getY(), data);
@@ -107,6 +110,7 @@ public class SimpleTrackedTarget extends BytePackable {
         yaw = unbufferDouble(src);
         pitch = unbufferDouble(src);
         area = unbufferDouble(src);
+        skew = unbufferByte(src);
 
         var poseX = unbufferDouble(src);
         var poseY = unbufferDouble(src);
diff --git a/photon-server/src/main/java/org/photonvision/vision/target/TargetCalculations.java b/photon-server/src/main/java/org/photonvision/vision/target/TargetCalculations.java
index 98d7aab67..7b721ddaa 100644
--- a/photon-server/src/main/java/org/photonvision/vision/target/TargetCalculations.java
+++ b/photon-server/src/main/java/org/photonvision/vision/target/TargetCalculations.java
@@ -35,6 +35,17 @@ public class TargetCalculations {
                 FastMath.atan((offsetCenterY - targetCenterY) / verticalFocalLength));
     }
 
+    @SuppressWarnings("DuplicatedCode")
+    public static double calculateSkew(boolean isLandscape, RotatedRect minAreaRect) {
+        // https://namkeenman.wordpress.com/2015/12/18/open-cv-determine-angle-of-rotatedrect-minarearect/
+        var angle = minAreaRect.angle;
+
+        if (isLandscape && minAreaRect.size.width < minAreaRect.size.height) angle += 90;
+        else if (!isLandscape && minAreaRect.size.height < minAreaRect.size.width) angle += 90;
+
+        return angle;
+    }
+
     public static Point calculateTargetOffsetPoint(
             boolean isLandscape, TargetOffsetPointEdge offsetRegion, RotatedRect minAreaRect) {
         Point[] vertices = new Point[4];
diff --git a/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
index a4d954cbc..d1a3cc19a 100644
--- a/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
+++ b/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
@@ -42,6 +42,7 @@ public class TrackedTarget implements Releasable {
     private double m_pitch;
     private double m_yaw;
     private double m_area;
+    private double m_skew;
 
     private Pose2d m_robotRelativePose = new Pose2d();
 
@@ -79,9 +80,8 @@ public class TrackedTarget implements Releasable {
         return m_yaw;
     }
 
-    private double getSkew() {
-        // TODO skew;
-        return 5940;
+    public double getSkew() {
+        return m_skew;
     }
 
     public double getArea() {
@@ -116,6 +116,8 @@ public class TrackedTarget implements Releasable {
                 TargetCalculations.calculateYaw(
                         m_targetOffsetPoint.x, m_robotOffsetPoint.x, params.horizontalFocalLength);
         m_area = m_mainContour.getMinAreaRect().size.area() / params.imageArea;
+
+        m_skew = TargetCalculations.calculateSkew(params.isLandscape, getMinAreaRect());
     }
 
     @Override
diff --git a/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
index c0c2cf29f..809aba903 100644
--- a/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
+++ b/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
@@ -23,6 +23,7 @@ import org.apache.commons.math3.util.FastMath;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.opencv.core.*;
+import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.common.util.numbers.DoubleCouple;
 import org.photonvision.vision.frame.FrameStaticProperties;
@@ -93,4 +94,81 @@ public class TargetCalculationsTest {
         assertEquals(4.3, result.x, 0.33, "Target offset x not as expected");
         assertEquals(2.5, result.y, 0.05, "Target offset Y not as expected");
     }
+
+    @Test
+    public void testSkewCalculation() {
+        // Setup
+        var isLandscape = true;
+        var rect = new RotatedRect(new Point(), new Size(10, 5), -10);
+
+        // Compute min area rect
+        var points = new Point[4];
+        rect.points(points);
+        var mat2f = new MatOfPoint2f(points);
+        var minAreaRect = Imgproc.minAreaRect(mat2f);
+
+        // Assert result
+        var result = TargetCalculations.calculateSkew(isLandscape, minAreaRect);
+        assertEquals(-10, result, 0.01);
+
+        // Setup
+        isLandscape = true;
+        rect = new RotatedRect(new Point(), new Size(10, 5), -70);
+
+        // Compute min area rect
+        points = new Point[4];
+        rect.points(points);
+        mat2f.release();
+        mat2f = new MatOfPoint2f(points);
+        minAreaRect = Imgproc.minAreaRect(mat2f);
+
+        // Assert result
+        result = TargetCalculations.calculateSkew(isLandscape, minAreaRect);
+        assertEquals(-70, result, 0.01);
+
+        // Setup
+        isLandscape = false;
+        rect = new RotatedRect(new Point(), new Size(5, 10), 10);
+
+        // Compute min area rect
+        points = new Point[4];
+        rect.points(points);
+        mat2f.release();
+        mat2f = new MatOfPoint2f(points);
+        minAreaRect = Imgproc.minAreaRect(mat2f);
+
+        // Assert result
+        result = TargetCalculations.calculateSkew(isLandscape, minAreaRect);
+        assertEquals(10, result, 0.01);
+
+        // Setup
+        isLandscape = false;
+        rect = new RotatedRect(new Point(), new Size(5, 10), 70);
+
+        // Compute min area rect
+        points = new Point[4];
+        rect.points(points);
+        mat2f.release();
+        mat2f = new MatOfPoint2f(points);
+        minAreaRect = Imgproc.minAreaRect(mat2f);
+
+        // Assert result
+        result = TargetCalculations.calculateSkew(isLandscape, minAreaRect);
+        assertEquals(70, result, 0.01);
+
+        // Setup
+        isLandscape = false;
+        rect = new RotatedRect(new Point(), new Size(5, 10), -70);
+
+        // Compute min area rect
+        points = new Point[4];
+        rect.points(points);
+        mat2f.release();
+        mat2f = new MatOfPoint2f(points);
+        minAreaRect = Imgproc.minAreaRect(mat2f);
+
+        // Assert result
+        result = TargetCalculations.calculateSkew(isLandscape, minAreaRect);
+        assertEquals(-70, result, 0.01);
+    }
 }