From 4d5904dd6d9ad6f7b11c1698bbd0167098edf286 Mon Sep 17 00:00:00 2001
From: Chris Gerth <gerth2@users.noreply.github.com>
Date: Sun, 9 Oct 2022 20:30:16 -0500
Subject: [PATCH] Stream content reorg. (#489)

Revised stream and target draw logic to divide the streams by "Raw" and "Processed" and only draw the results on the "Processed" stream.

Should allow for input sterams to be recorded for raw camera input, and output for debug info.
---
 photon-client/src/views/PipelineView.vue      | 10 ++---
 .../vision/pipeline/AprilTagPipeline.java     | 10 ++---
 .../vision/pipeline/OutputStreamPipeline.java | 40 +++++++++++++------
 3 files changed, 37 insertions(+), 23 deletions(-)

diff --git a/photon-client/src/views/PipelineView.vue b/photon-client/src/views/PipelineView.vue
index c1fd52a09..911d9c6c1 100644
--- a/photon-client/src/views/PipelineView.vue
+++ b/photon-client/src/views/PipelineView.vue
@@ -36,7 +36,7 @@
                 <span class="pr-1">{{ Math.round($store.state.pipelineResults.fps) }}&nbsp;FPS &ndash;</span>
                 <span v-if="!fpsTooLow">{{ Math.min(Math.round($store.state.pipelineResults.latency), 9999) }} ms latency</span>
                 <span v-else-if="!$store.getters.currentPipelineSettings.inputShouldShow">HSV thresholds are too broad; narrow them for better performance</span>
-                <span v-else>stop viewing the color stream for better performance</span>
+                <span v-else>stop viewing the raw stream for better performance</span>
               </v-chip>
               <v-switch
                 v-model="driverMode"
@@ -136,15 +136,15 @@
                     color="secondary"
                     class="fill"
                   >
-                    <v-icon>mdi-palette</v-icon>
-                    <span>Normal</span>
+                    <v-icon>mdi-import</v-icon>
+                    <span>Raw</span>
                   </v-btn>
                   <v-btn
                     color="secondary"
                     class="fill"
                   >
-                    <v-icon>mdi-compare</v-icon>
-                    <span>Threshold</span>
+                    <v-icon>mdi-export</v-icon>
+                    <span>Processed</span>
                   </v-btn>
                 </v-btn-toggle>
               </v-col>
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
index b6c0a0ced..7b89a6648 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
@@ -117,9 +117,13 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
             sumPipeNanosElapsed += rotateImageResult.nanosElapsed;
         }
 
+        var inputFrame = new Frame(new CVMat(rawInputMat), frameStaticProperties);
+
         grayscalePipeResult = grayscalePipe.run(rawInputMat);
         sumPipeNanosElapsed += grayscalePipeResult.nanosElapsed;
 
+        var outputFrame = new Frame(new CVMat(grayscalePipeResult.output), frameStaticProperties);
+
         List<TrackedTarget> targetList;
         CVPipeResult<List<DetectionResult>> tagDetectionPipeResult;
 
@@ -127,7 +131,6 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         aprilTagDetectionPipe.setNativePoseEstimationEnabled(settings.solvePNPEnabled);
 
         tagDetectionPipeResult = aprilTagDetectionPipe.run(grayscalePipeResult.output);
-        grayscalePipeResult.output.release();
         sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
 
         targetList = new ArrayList<>();
@@ -150,11 +153,6 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
-        var inputFrame = new Frame(new CVMat(rawInputMat), frameStaticProperties);
-        // empty output frame
-        var outputFrame =
-                Frame.emptyFrame(frameStaticProperties.imageWidth, frameStaticProperties.imageHeight);
-
         return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, outputFrame, inputFrame);
     }
 }
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java
index f1547fd46..1a9eacdab 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java
@@ -120,42 +120,58 @@ public class OutputStreamPipeline {
             pipeProfileNanos[2] = 0;
         }
 
-        // Draw 2D Crosshair on input and output
-        var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
+        // Draw 2D Crosshair on output
+        var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
         sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
 
         if (!(settings instanceof AprilTagPipelineSettings)) {
+            // If we're processing anything other than Apriltags...
+
             var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
             sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
 
-            // Draw 3D Targets on input and output if necessary
             if (settings.solvePNPEnabled
                     || (settings.solvePNPEnabled
                             && settings instanceof ColoredShapePipelineSettings
                             && ((ColoredShapePipelineSettings) settings).contourShape == ContourShape.Circle)) {
-                var drawOnInputResult = draw3dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+                // Draw 3D Targets on input and output if possible
+                pipeProfileNanos[5] = 0;
+                pipeProfileNanos[6] = 0;
+                pipeProfileNanos[7] = 0;
 
                 var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
             } else {
-                pipeProfileNanos[7] = 0;
-                pipeProfileNanos[8] = 0;
-
-                var draw2dTargetsOnInput = draw2dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+                // Only draw 2d targets
+                pipeProfileNanos[5] = 0;
 
                 var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
+
+                pipeProfileNanos[7] = 0;
+                pipeProfileNanos[8] = 0;
             }
 
         } else {
+            // If we are doing apriltags...
             if (settings.solvePNPEnabled) {
-                var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(inMat, targetsToDraw));
+                // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
+                pipeProfileNanos[5] = 0;
+                pipeProfileNanos[6] = 0;
+
+                var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+
+                pipeProfileNanos[8] = 0;
+
             } else {
-                var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(inMat, targetsToDraw));
+                // Draw 2d apriltag markers
+                var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+
+                pipeProfileNanos[6] = 0;
+                pipeProfileNanos[7] = 0;
+                pipeProfileNanos[8] = 0;
             }
         }