Resolve "Remove vanished cameras after some timeout"

Closes #1606

See merge request main/Sumatra!1347

sumatra-commit: a86952de9cdb05ac38987bebd36377f4ac894e3e

modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/CamFilter.java

@@ -141,6 +141,7 @@ public class CamFilter
 	 */
 	public void update(final CamDetectionFrame frame, final FilteredVisionFrame lastFilteredFrame)
 	{
+		checkForNonConsecutiveFrames(frame);
 		CamDetectionFrame adjustedFrame = adjustTCapture(frame);
 
 		processRobots(adjustedFrame, lastFilteredFrame.getBots());
@@ -203,20 +204,19 @@ public class CamFilter
 	}
 
 
-	private CamDetectionFrame adjustTCapture(final CamDetectionFrame frame)
-	{
-		if (frame.getCamFrameNumber() != (lastCamFrameId + 1))
+	private void reset()
 	{
-			if (lastCamFrameId != 0)
-			{
-				log.warn("Non-consecutive cam frame for cam {}: {} -> {}", frame.getCameraId(), lastCamFrameId,
-						frame.getCamFrameNumber());
-			}
 		frameIntervalFilter.reset();
+		lastKnownBallPosition = Vector2f.ZERO_VECTOR;
+		lastBallVisibleTimestamp = 0;
+		robots.clear();
+		balls.clear();
+		ballHistory.clear();
 	}
 
-		lastCamFrameId = frame.getCamFrameNumber();
 
+	private CamDetectionFrame adjustTCapture(final CamDetectionFrame frame)
+	{
 		if ((frame.getCamFrameNumber() % FRAME_FILTER_DIVIDER) == 0)
 		{
 			frameIntervalFilter.addSample(frame.getCamFrameNumber(), frame.gettCapture());
@@ -230,6 +230,26 @@ public class CamFilter
 	}
 
 
+	private void checkForNonConsecutiveFrames(CamDetectionFrame frame)
+	{
+		if (frame.getCamFrameNumber() != (lastCamFrameId + 1))
+		{
+			if (lastCamFrameId != 0)
+			{
+				log.warn("Non-consecutive cam frame for cam {}: {} -> {}", frame.getCameraId(), lastCamFrameId,
+						frame.getCamFrameNumber());
+			}
+			if (Math.abs(frame.getCamFrameNumber() - lastCamFrameId + 1) > 10)
+			{
+				log.info("Resetting cam filter for cam {}", camId);
+				reset();
+			}
+		}
+
+		lastCamFrameId = frame.getCamFrameNumber();
+	}
+
+
 	private List<RobotCollisionShape> getRobotCollisionShapes(final List<FilteredVisionBot> mergedRobots)
 	{
 		List<RobotCollisionShape> shapes = new ArrayList<>();

modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/ViewportArchitect.java

@@ -21,10 +21,12 @@ import edu.tigers.sumatra.math.vector.Vector2f;
 
 import java.awt.Color;
 import java.util.ArrayList;
+import java.util.Comparator;
 import java.util.List;
 import java.util.Map;
 import java.util.Map.Entry;
 import java.util.Optional;
+import java.util.Set;
 import java.util.concurrent.ConcurrentSkipListMap;
 import java.util.concurrent.CopyOnWriteArrayList;
 import java.util.stream.Collectors;
@@ -84,7 +86,7 @@ public class ViewportArchitect
 				viewports.remove(camId);
 			}
 
-			viewports.putIfAbsent(camId, new Viewport(calib.getCameraPosition().getXYVector(),
+			viewports.computeIfAbsent(camId, id -> new Viewport(calib.getCameraPosition().getXYVector(),
 					calib.imageToField(calib.getPrincipalPoint(), 0), calib));
 		}
 
@@ -99,13 +101,12 @@ public class ViewportArchitect
 	 */
 	public void newDetectionFrame(final CamDetectionFrame frame)
 	{
-		if (!viewports.containsKey(frame.getCameraId()))
+		Viewport viewport = viewports.get(frame.getCameraId());
+		if (viewport == null)
 		{
 			return;
 		}
 
-		Viewport viewport = viewports.get(frame.getCameraId());
-		
 		List<IVector2> allPositions = frame.getRobots().stream()
 				.map(CamRobot::getPos)
 				.collect(Collectors.toList());
@@ -159,6 +160,12 @@ public class ViewportArchitect
 	}
 
 
+	public void updateCameras(Set<Integer> cameraIds)
+	{
+		viewports.keySet().removeIf(id -> !cameraIds.contains(id));
+	}
+
+
 	/**
 	 * @param observer
 	 */
@@ -386,9 +393,10 @@ public class ViewportArchitect
 			return null;
 		}
 
-		return viewports.get(camId).getRectangle();
+		return viewport.getRectangle();
 	}
 
+
 	private enum EDirection
 	{
 		UP,
@@ -451,7 +459,7 @@ public class ViewportArchitect
 		final IVector2 start = origin.center;
 		List<Viewport> closestFour = viewports.values().stream()
 				.filter(v -> !v.equals(origin))
-				.sorted((v1, v2) -> Double.compare(v1.center.distanceToSqr(start), v2.center.distanceToSqr(start)))
+				.sorted(Comparator.comparingDouble(v -> v.center.distanceToSqr(start)))
 				.limit(4)
 				.collect(Collectors.toList());
 
@@ -486,12 +494,7 @@ public class ViewportArchitect
 	private int countViewports(final int startValue, final Viewport origin, final EDirection direction)
 	{
 		Optional<Viewport> next = nextViewport(origin, direction);
-		if (next.isPresent())
-		{
-			return countViewports(startValue + 1, next.get(), direction);
-		}
-		
-		return startValue;
+		return next.map(viewport -> countViewports(startValue + 1, viewport, direction)).orElse(startValue);
 	}
 
 
@@ -519,6 +522,7 @@ public class ViewportArchitect
 		return shapes;
 	}
 
+
 	/**
 	 * ViewportArchitect Observer interface.
 	 *

modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/VisionFilterImpl.java

@@ -162,6 +162,10 @@ public class VisionFilterImpl extends AVisionFilter
 
 	private FilteredVisionFrame constructFilteredVisionFrame(FilteredVisionFrame lastFrame)
 	{
+		// remove old camera filters
+		long avgTimestamp = (long) cams.values().stream().mapToLong(CamFilter::getTimestamp).average().orElse(0);
+		cams.values().removeIf(f -> Math.abs(avgTimestamp - f.getTimestamp()) / 1e9 > 0.5);
+
 		long timestamp = cams.values().stream().mapToLong(CamFilter::getTimestamp).max().orElse(lastFrame.getTimestamp());
 
 		// use newest timestamp to prevent negative delta time in filtered frames
@@ -200,6 +204,9 @@ public class VisionFilterImpl extends AVisionFilter
 		// forward frame for inspection
 		qualityInspector.inspectFilteredVisionFrame(frame);
 
+		// Update active cameras in viewport architect
+		viewportArchitect.updateCameras(cams.keySet());
+
 		// add debug and info shapes for visualizer
 		frame.getShapeMap().get(EVisionFilterShapesLayer.VIEWPORT_SHAPES).addAll(viewportArchitect.getInfoShapes());
 		frame.getShapeMap().get(EVisionFilterShapesLayer.QUALITY_SHAPES).addAll(qualityInspector.getInfoShapes());
@@ -289,9 +296,10 @@ public class VisionFilterImpl extends AVisionFilter
 		for (CamCalibration c : geometry.getCalibrations().values())
 		{
 			int camId = c.getCameraId();
-			if (cams.containsKey(camId))
+			CamFilter camFilter = cams.get(camId);
+			if (camFilter != null)
 			{
-				cams.get(camId).update(c);
+				camFilter.update(c);
 			}
 		}
 

modules/sumatra-gui-visualizer/src/main/java/edu/tigers/sumatra/visualizer/VisualizerPresenter.java

 /*
- * Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 
 package edu.tigers.sumatra.visualizer;
@@ -381,6 +381,7 @@ public class VisualizerPresenter extends ASumatraViewPresenter implements IRobot
 	public void onNewCamDetectionFrame(final ExtendedCamDetectionFrame frame)
 	{
 		camFrames.put(frame.getCameraId(), frame);
+		camFrames.values().removeIf(f -> Math.abs(frame.gettCapture() - f.gettCapture()) / 1e9 > 1);
 	}