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);