diff --git a/steps/src/dom.js b/steps/src/dom.js
index a22d14f..8864b93 100644
--- a/steps/src/dom.js
+++ b/steps/src/dom.js
@@ -23,39 +23,25 @@ export const speedSlider = document.getElementById("speed-slider");
export const speedDisplay = document.getElementById("speed-display");
export const featureToggles = document.getElementById("feature-toggles");
export const toggleSnrColor = document.getElementById("toggle-snr-color");
-export const toggleClusterColor = document.getElementById(
- "toggle-cluster-color"
-);
+export const toggleClusterColor = document.getElementById("toggle-cluster-color");
export const toggleInlierColor = document.getElementById("toggle-inlier-color");
-export const toggleStationaryColor = document.getElementById(
- "toggle-stationary-color"
-);
+export const toggleStationaryColor = document.getElementById("toggle-stationary-color");
export const toggleVelocity = document.getElementById("toggle-velocity");
export const toggleTracks = document.getElementById("toggle-tracks");
export const toggleEgoSpeed = document.getElementById("toggle-ego-speed");
export const toggleFrameNorm = document.getElementById("toggle-frame-norm");
-export const toggleDebugOverlay = document.getElementById(
- "toggle-debug-overlay"
-);
+export const toggleDebugOverlay = document.getElementById("toggle-debug-overlay");
export const egoSpeedDisplay = document.getElementById("ego-speed-display");
export const canSpeedDisplay = document.getElementById("can-speed-display");
export const debugOverlay = document.getElementById("debug-overlay");
-export const toggleDebug2Overlay = document.getElementById(
- "toggle-debug2-overlay"
-);
+export const toggleDebug2Overlay = document.getElementById("toggle-debug2-overlay");
export const snrMinInput = document.getElementById("snr-min-input");
export const snrMaxInput = document.getElementById("snr-max-input");
export const applySnrBtn = document.getElementById("apply-snr-btn");
-export const autoOffsetIndicator = document.getElementById(
- "auto-offset-indicator"
-);
+export const autoOffsetIndicator = document.getElementById("auto-offset-indicator");
export const clearCacheBtn = document.getElementById("clear-cache-btn");
-export const speedGraphContainer = document.getElementById(
- "speed-graph-container"
-);
-export const speedGraphPlaceholder = document.getElementById(
- "speed-graph-placeholder"
-);
+export const speedGraphContainer = document.getElementById("speed-graph-container");
+export const speedGraphPlaceholder = document.getElementById("speed-graph-placeholder");
export const modalContainer = document.getElementById("modal-container");
export const modalOverlay = document.getElementById("modal-overlay");
export const modalContent = document.getElementById("modal-content");
@@ -63,6 +49,8 @@ export const modalText = document.getElementById("modal-text");
export const modalOkBtn = document.getElementById("modal-ok-btn");
export const modalCancelBtn = document.getElementById("modal-cancel-btn");
export const toggleCloseUp = document.getElementById("toggle-close-up");
+export const togglePredictedPos = document.getElementById("toggle-predicted-pos");
+export const toggleCovariance = document.getElementById("toggle-covariance");
//----------------------UPDATE FRAME Function----------------------//
// Updates the UI to reflect the current radar frame and synchronizes video playback.
@@ -71,7 +59,8 @@ export function updateFrame(frame, forceVideoSeek) {
!appState.vizData ||
frame < 0 ||
frame >= appState.vizData.radarFrames.length
- ) // Exit if no visualization data or invalid frame.
+ )
+ // Exit if no visualization data or invalid frame.
return; // Exit if no visualization data or invalid frame
appState.currentFrame = frame;
timelineSlider.value = appState.currentFrame;
@@ -79,7 +68,8 @@ export function updateFrame(frame, forceVideoSeek) {
appState.vizData.radarFrames.length
}`;
const frameData = appState.vizData.radarFrames[appState.currentFrame];
- if (toggleEgoSpeed.checked && frameData) { // Update ego speed display if enabled.
+ if (toggleEgoSpeed.checked && frameData) {
+ // Update ego speed display if enabled.
const egoVy_kmh = (frameData.egoVelocity[1] * 3.6).toFixed(1); // Convert m/s to km/h and format
egoSpeedDisplay.textContent = `Ego: ${egoVy_kmh} km/h`;
egoSpeedDisplay.classList.remove("hidden");
@@ -100,8 +90,10 @@ export function updateFrame(frame, forceVideoSeek) {
const offsetMs = parseFloat(offsetInput.value) || 0;
const targetRadarTimeMs = frameData.timestampMs;
const targetVideoTimeSec = (targetRadarTimeMs - offsetMs) / 1000;
- if (targetVideoTimeSec >= 0 && targetVideoTimeSec <= videoPlayer.duration) { // Ensure target time is within video duration
- if (Math.abs(videoPlayer.currentTime - targetVideoTimeSec) > 0.05) { // Check for significant drift
+ if (targetVideoTimeSec >= 0 && targetVideoTimeSec <= videoPlayer.duration) {
+ // Ensure target time is within video duration
+ if (Math.abs(videoPlayer.currentTime - targetVideoTimeSec) > 0.05) {
+ // Check for significant drift
videoPlayer.currentTime = targetVideoTimeSec; // Seek video if drift is significant
}
// MODIFIED: Use the calculated target time for our updates, not the stale videoPlayer.currentTime
@@ -117,7 +109,8 @@ export function updateFrame(frame, forceVideoSeek) {
// --- End of fix ---
if (appState.p5_instance) appState.p5_instance.redraw(); // Redraw radar sketch
- if (appState.speedGraphInstance && !appState.isPlaying) // Redraw speed graph if not playing.
+ if (appState.speedGraphInstance && !appState.isPlaying)
+ // Redraw speed graph if not playing.
appState.speedGraphInstance.redraw();
}
@@ -172,7 +165,7 @@ export function updateDebugOverlay(currentMediaTime) {
// If at least one is checked, show the overlay
debugOverlay.classList.remove("hidden"); // Show debug overlay.
let content = [];
-
+
// --- Logic for the original debug overlay ---
if (isDebug1Visible) {
content.push(`--- Basic Info ---`);
@@ -206,7 +199,7 @@ export function updateDebugOverlay(currentMediaTime) {
.replace("Z", "")}`
); // Format and display radar absolute time
}
- }
+ }
// --- Logic for the new advanced debug overlay ---
if (isDebug2Visible) {
diff --git a/steps/src/drawUtils.js b/steps/src/drawUtils.js
index 4ddabd5..a6c79ee 100644
--- a/steps/src/drawUtils.js
+++ b/steps/src/drawUtils.js
@@ -516,3 +516,38 @@ export function handleCloseUpDisplay(p, plotScales) {
p.pop();
}
}
+
+export function drawCovarianceEllipse(p, position, covarianceP, plotScales) {
+ const pPos = [
+ [covarianceP[0][0], covarianceP[0][1]],
+ [covarianceP[1][0], covarianceP[1][1]],
+ ];
+
+ const a = pPos[0][0];
+ const b = pPos[0][1];
+ const d = pPos[1][1];
+ const trace = a + d;
+ const determinant = a * d - b * b;
+
+ const lambda1 = trace / 2 + Math.sqrt(Math.pow(trace, 2) / 4 - determinant);
+ const lambda2 = trace / 2 - Math.sqrt(Math.pow(trace, 2) / 4 - determinant);
+
+ const chi2 = 5.991;
+ const majorAxis = Math.sqrt(chi2 * lambda1);
+ const minorAxis = Math.sqrt(chi2 * lambda2);
+
+ let eigenvector = [1, 0];
+ if (b !== 0) {
+ eigenvector = [lambda1 - d, b];
+ }
+ const angle = Math.atan2(eigenvector[1], eigenvector[0]);
+
+ p.push();
+ p.noFill();
+ p.stroke(255, 0, 0, 150);
+ p.strokeWeight(1);
+ p.translate(position[0] * plotScales.plotScaleX, position[1] * plotScales.plotScaleY);
+ p.rotate(angle);
+ p.ellipse(0, 0, majorAxis * 2 * plotScales.plotScaleX, minorAxis * 2 * plotScales.plotScaleY);
+ p.pop();
+}
\ No newline at end of file
diff --git a/steps/src/p5/radarSketch.js b/steps/src/p5/radarSketch.js
index d52b410..8cda624 100644
--- a/steps/src/p5/radarSketch.js
+++ b/steps/src/p5/radarSketch.js
@@ -6,7 +6,13 @@ import {
RADAR_Y_MAX,
RADAR_Y_MIN,
} from "../constants.js";
-import { canvasContainer, toggleSnrColor, toggleTracks } from "../dom.js";
+import {
+ canvasContainer,
+ toggleSnrColor,
+ toggleTracks,
+ togglePredictedPos,
+ toggleCovariance,
+} from "../dom.js";
import {
drawStaticRegionsToBuffer,
drawAxes,
@@ -15,7 +21,8 @@ import {
drawTrajectories,
drawTrackMarkers,
snrColors,
- handleCloseUpDisplay, // BUG FIX 1: Import the close-up handler
+ handleCloseUpDisplay,
+ drawCovarianceEllipse, // BUG FIX 1: Import the close-up handler
} from "../drawUtils.js";
export const radarSketch = function (p) {
@@ -89,6 +96,44 @@ export const radarSketch = function (p) {
if (toggleTracks.checked) {
drawTrajectories(p, plotScales);
drawTrackMarkers(p, plotScales);
+
+ if (toggleCovariance.checked) {
+ for (const track of appState.vizData.tracks) {
+ const log = track.historyLog.find(
+ (log) => log.frameIdx === appState.currentFrame + 1
+ );
+ if (log && log.covarianceP) {
+ const pos = log.predictedPosition;
+ if (pos && pos[0] !== null) {
+ drawCovarianceEllipse(p, pos, log.covarianceP, plotScales);
+ }
+ }
+ }
+ }
+
+ if (togglePredictedPos.checked) {
+ for (const track of appState.vizData.tracks) {
+ const log = track.historyLog.find(
+ (log) => log.frameIdx === appState.currentFrame + 1
+ );
+ if (
+ log &&
+ log.predictedPosition &&
+ log.predictedPosition[0] !== null
+ ) {
+ const pos = log.predictedPosition;
+ const x = pos[0] * plotScales.plotScaleX;
+ const y = pos[1] * plotScales.plotScaleY;
+
+ p.push();
+ p.stroke(255, 0, 0); // Red for predicted
+ p.strokeWeight(2);
+ p.line(x - 4, y - 4, x + 4, y + 4);
+ p.line(x + 4, y - 4, x - 4, y + 4);
+ p.pop();
+ }
+ }
+ }
}
// Draw the point cloud for the current frame
drawPointCloud(p, frameData.pointCloud, plotScales);