diff --git a/photon-client/src/store/index.js b/photon-client/src/store/index.js
index 3d9a04419..caa8705f1 100644
--- a/photon-client/src/store/index.js
+++ b/photon-client/src/store/index.js
@@ -79,6 +79,7 @@ export default new Vuex.Store({
pipelineResults: [
{
fps: 0,
+ latency: 0,
targets: [{
// Available in both 2D and 3D
pitch: 0,
diff --git a/photon-client/src/views/PipelineView.vue b/photon-client/src/views/PipelineView.vue
index ea304a08e..dcc701121 100644
--- a/photon-client/src/views/PipelineView.vue
+++ b/photon-client/src/views/PipelineView.vue
@@ -77,7 +77,8 @@
:scale="75"
@click="onImageClick"
/>
- FPS:{{ parseFloat(fps).toFixed(2) }}
+ FPS: {{ parseFloat(fps).toFixed(2) }}
+ Latency: {{ parseFloat(latency).toFixed(2) }}ms
@@ -185,6 +186,11 @@
get() {
return this.$store.getters.currentCameraFPS;
}
+ },
+ latency: {
+ get() {
+ return this.$store.getters.currentPipelineResults.latency;
+ }
}
},
methods: {
diff --git a/photon-server/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java b/photon-server/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
new file mode 100644
index 000000000..332959fe7
--- /dev/null
+++ b/photon-server/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * A class that implements a moving-window median filter. Useful for reducing measurement noise,
+ * especially with processes that generate occasional, extreme outliers (such as values from
+ * vision processing, LIDAR, or ultrasonic sensors).
+ */
+public class MedianFilter {
+ private final CircularBuffer m_valueBuffer;
+ private final List m_orderedValues;
+ private final int m_size;
+
+ /**
+ * Creates a new MedianFilter.
+ *
+ * @param size The number of samples in the moving window.
+ */
+ public MedianFilter(int size) {
+ // Circular buffer of values currently in the window, ordered by time
+ m_valueBuffer = new CircularBuffer(size);
+ // List of values currently in the window, ordered by value
+ m_orderedValues = new ArrayList<>(size);
+ // Size of rolling window
+ m_size = size;
+ }
+
+ /**
+ * Calculates the moving-window median for the next value of the input stream.
+ *
+ * @param next The next input value.
+ * @return The median of the moving window, updated to include the next value.
+ */
+ public double calculate(double next) {
+ // Find insertion point for next value
+ int index = Collections.binarySearch(m_orderedValues, next);
+
+ // Deal with binarySearch behavior for element not found
+ if (index < 0) {
+ index = Math.abs(index + 1);
+ }
+
+ // Place value at proper insertion point
+ m_orderedValues.add(index, next);
+
+ int curSize = m_orderedValues.size();
+
+ // If buffer is at max size, pop element off of end of circular buffer
+ // and remove from ordered list
+ if (curSize > m_size) {
+ m_orderedValues.remove(m_valueBuffer.removeLast());
+ curSize = curSize - 1;
+ }
+
+ // Add next value to circular buffer
+ m_valueBuffer.addFirst(next);
+
+ if (curSize % 2 == 1) {
+ // If size is odd, return middle element of sorted list
+ return m_orderedValues.get(curSize / 2);
+ } else {
+ // If size is even, return average of middle elements
+ return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
+ }
+ }
+
+ /**
+ * Resets the filter, clearing the window of all elements.
+ */
+ public void reset() {
+ m_orderedValues.clear();
+ m_valueBuffer.clear();
+ }
+}
diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
index 478ba91ee..c99326b94 100644
--- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
+++ b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
@@ -18,6 +18,7 @@
package org.photonvision.vision.processes;
import com.fasterxml.jackson.core.JsonProcessingException;
+import edu.wpi.first.wpilibj.MedianFilter;
import java.util.*;
import org.apache.commons.lang3.tuple.Pair;
import org.photonvision.common.configuration.CameraConfiguration;
@@ -60,7 +61,9 @@ public class VisionModule {
private final NTDataPublisher ntConsumer;
private final int moduleIndex;
private final MJPGFrameConsumer uiStreamer;
- private long lastUpdateTimestamp = -1;
+ private long lastUIResultUpdateTime = 0;
+ private long lastRunTime = 0;
+ private MedianFilter fpsAverager = new MedianFilter(10);
private MJPGFrameConsumer dashboardStreamer;
@@ -100,12 +103,21 @@ public class VisionModule {
addResultConsumer(
result -> {
var now = System.currentTimeMillis();
- if (lastUpdateTimestamp + 1000.0 / 15.0 > now) return;
+
+ var fps = fpsAverager.calculate(1000.0 / (now - lastRunTime));
+ lastRunTime = now;
+
+ // only update the UI at 15hz
+ if (lastUIResultUpdateTime + 1000.0 / 15.0 > now) return;
var uiMap = new HashMap>();
var dataMap = new HashMap();
- dataMap.put("fps", 1000.0 / result.getLatencyMillis());
+
+ dataMap.put("fps", fps);
+ dataMap.put("latency", result.getLatencyMillis());
+
var targets = result.targets;
+
var uiTargets = new ArrayList>();
for (var t : targets) {
uiTargets.add(t.toHashMap());
@@ -123,7 +135,7 @@ public class VisionModule {
logger.error(Arrays.toString(e.getStackTrace()));
}
- lastUpdateTimestamp = now;
+ lastUIResultUpdateTime = now;
});
setPipeline(visionSource.getSettables().getConfiguration().currentPipelineIndex);