Commit b3787b4a authored by AndreR's avatar AndreR Committed by TIGERs GitLab
Browse files

Resolve "Remove vanished cameras after some timeout"

Closes #1606

See merge request main/Sumatra!1347

sumatra-commit: a86952de9cdb05ac38987bebd36377f4ac894e3e
parent d50d1f35
Pipeline #8748 passed with stage
in 4 minutes and 11 seconds
......@@ -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<>();
......
......@@ -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.
*
......
......@@ -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);
}
}
......
/*
* 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);
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment