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 ...@@ -141,6 +141,7 @@ public class CamFilter
*/ */
public void update(final CamDetectionFrame frame, final FilteredVisionFrame lastFilteredFrame) public void update(final CamDetectionFrame frame, final FilteredVisionFrame lastFilteredFrame)
{ {
checkForNonConsecutiveFrames(frame);
CamDetectionFrame adjustedFrame = adjustTCapture(frame); CamDetectionFrame adjustedFrame = adjustTCapture(frame);
processRobots(adjustedFrame, lastFilteredFrame.getBots()); processRobots(adjustedFrame, lastFilteredFrame.getBots());
...@@ -203,20 +204,19 @@ public class CamFilter ...@@ -203,20 +204,19 @@ public class CamFilter
} }
private CamDetectionFrame adjustTCapture(final CamDetectionFrame frame) private void reset()
{ {
if (frame.getCamFrameNumber() != (lastCamFrameId + 1)) frameIntervalFilter.reset();
{ lastKnownBallPosition = Vector2f.ZERO_VECTOR;
if (lastCamFrameId != 0) lastBallVisibleTimestamp = 0;
{ robots.clear();
log.warn("Non-consecutive cam frame for cam {}: {} -> {}", frame.getCameraId(), lastCamFrameId, balls.clear();
frame.getCamFrameNumber()); ballHistory.clear();
} }
frameIntervalFilter.reset();
}
lastCamFrameId = frame.getCamFrameNumber();
private CamDetectionFrame adjustTCapture(final CamDetectionFrame frame)
{
if ((frame.getCamFrameNumber() % FRAME_FILTER_DIVIDER) == 0) if ((frame.getCamFrameNumber() % FRAME_FILTER_DIVIDER) == 0)
{ {
frameIntervalFilter.addSample(frame.getCamFrameNumber(), frame.gettCapture()); frameIntervalFilter.addSample(frame.getCamFrameNumber(), frame.gettCapture());
...@@ -230,6 +230,26 @@ public class CamFilter ...@@ -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) private List<RobotCollisionShape> getRobotCollisionShapes(final List<FilteredVisionBot> mergedRobots)
{ {
List<RobotCollisionShape> shapes = new ArrayList<>(); List<RobotCollisionShape> shapes = new ArrayList<>();
......
...@@ -21,10 +21,12 @@ import edu.tigers.sumatra.math.vector.Vector2f; ...@@ -21,10 +21,12 @@ import edu.tigers.sumatra.math.vector.Vector2f;
import java.awt.Color; import java.awt.Color;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Comparator;
import java.util.List; import java.util.List;
import java.util.Map; import java.util.Map;
import java.util.Map.Entry; import java.util.Map.Entry;
import java.util.Optional; import java.util.Optional;
import java.util.Set;
import java.util.concurrent.ConcurrentSkipListMap; import java.util.concurrent.ConcurrentSkipListMap;
import java.util.concurrent.CopyOnWriteArrayList; import java.util.concurrent.CopyOnWriteArrayList;
import java.util.stream.Collectors; import java.util.stream.Collectors;
...@@ -47,15 +49,15 @@ public class ViewportArchitect ...@@ -47,15 +49,15 @@ public class ViewportArchitect
@Configurable(defValue = "DYNAMICALLY", comment = "Method to be used to construct viewports.") @Configurable(defValue = "DYNAMICALLY", comment = "Method to be used to construct viewports.")
private static EViewportConstruction viewportConstruction = EViewportConstruction.DYNAMICALLY; private static EViewportConstruction viewportConstruction = EViewportConstruction.DYNAMICALLY;
@Configurable(defValue = "50.0", comment = "Update rate for viewport changes to base station (per camera). [Hz]") @Configurable(defValue = "50.0", comment = "Update rate for viewport changes to base station (per camera). [Hz]")
private static double reportRate = 50.0; private static double reportRate = 50.0;
static static
{ {
ConfigRegistration.registerClass("vision", ViewportArchitect.class); ConfigRegistration.registerClass("vision", ViewportArchitect.class);
} }
private enum EViewportConstruction private enum EViewportConstruction
{ {
DYNAMICALLY, DYNAMICALLY,
...@@ -63,11 +65,11 @@ public class ViewportArchitect ...@@ -63,11 +65,11 @@ public class ViewportArchitect
FROM_CAM_POS, FROM_CAM_POS,
FROM_CAM_PROJECTION, FROM_CAM_PROJECTION,
} }
/** /**
* Update architect with new geometry. * Update architect with new geometry.
* *
* @param geometry * @param geometry
*/ */
public void newCameraGeometry(final CamGeometry geometry) public void newCameraGeometry(final CamGeometry geometry)
...@@ -77,57 +79,56 @@ public class ViewportArchitect ...@@ -77,57 +79,56 @@ public class ViewportArchitect
for (CamCalibration calib : geometry.getCalibrations().values()) for (CamCalibration calib : geometry.getCalibrations().values())
{ {
int camId = calib.getCameraId(); int camId = calib.getCameraId();
// check if the camera calibration changed => someone playing around with vision // check if the camera calibration changed => someone playing around with vision
if (viewports.containsKey(camId) && !viewports.get(camId).calib.similarTo(calib)) if (viewports.containsKey(camId) && !viewports.get(camId).calib.similarTo(calib))
{ {
viewports.remove(camId); 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)); calib.imageToField(calib.getPrincipalPoint(), 0), calib));
} }
field = new Viewport(Vector2f.ZERO_VECTOR, geometry.getField().getFieldWithBoundary()); field = new Viewport(Vector2f.ZERO_VECTOR, geometry.getField().getFieldWithBoundary());
} }
/** /**
* Update with new detection frame. Adjusts dynamic viewports. * Update with new detection frame. Adjusts dynamic viewports.
* *
* @param frame * @param frame
*/ */
public void newDetectionFrame(final CamDetectionFrame frame) public void newDetectionFrame(final CamDetectionFrame frame)
{ {
if (!viewports.containsKey(frame.getCameraId())) Viewport viewport = viewports.get(frame.getCameraId());
if (viewport == null)
{ {
return; return;
} }
Viewport viewport = viewports.get(frame.getCameraId());
List<IVector2> allPositions = frame.getRobots().stream() List<IVector2> allPositions = frame.getRobots().stream()
.map(CamRobot::getPos) .map(CamRobot::getPos)
.collect(Collectors.toList()); .collect(Collectors.toList());
allPositions.add(viewport.dynamicMax); allPositions.add(viewport.dynamicMax);
allPositions.add(viewport.dynamicMin); allPositions.add(viewport.dynamicMin);
double maxX = allPositions.stream().mapToDouble(IVector2::x).max().orElse(viewport.dynamicMax.x()); double maxX = allPositions.stream().mapToDouble(IVector2::x).max().orElse(viewport.dynamicMax.x());
double maxY = allPositions.stream().mapToDouble(IVector2::y).max().orElse(viewport.dynamicMax.y()); double maxY = allPositions.stream().mapToDouble(IVector2::y).max().orElse(viewport.dynamicMax.y());
double minX = allPositions.stream().mapToDouble(IVector2::x).min().orElse(viewport.dynamicMin.x()); double minX = allPositions.stream().mapToDouble(IVector2::x).min().orElse(viewport.dynamicMin.x());
double minY = allPositions.stream().mapToDouble(IVector2::y).min().orElse(viewport.dynamicMin.y()); double minY = allPositions.stream().mapToDouble(IVector2::y).min().orElse(viewport.dynamicMin.y());
viewport.dynamicMax.setX(maxX); viewport.dynamicMax.setX(maxX);
viewport.dynamicMax.setY(maxY); viewport.dynamicMax.setY(maxY);
viewport.dynamicMin.setX(minX); viewport.dynamicMin.setX(minX);
viewport.dynamicMin.setY(minY); viewport.dynamicMin.setY(minY);
if (field == null) if (field == null)
{ {
return; return;
} }
switch (viewportConstruction) switch (viewportConstruction)
{ {
case DYNAMICALLY: case DYNAMICALLY:
...@@ -145,11 +146,11 @@ public class ViewportArchitect ...@@ -145,11 +146,11 @@ public class ViewportArchitect
default: default:
break; break;
} }
for (Entry<Integer, Viewport> entry : viewports.entrySet()) for (Entry<Integer, Viewport> entry : viewports.entrySet())
{ {
double timeSinceLastReport = (frame.gettCapture() - entry.getValue().lastReportTimestamp) * 1e-9; double timeSinceLastReport = (frame.gettCapture() - entry.getValue().lastReportTimestamp) * 1e-9;
if (timeSinceLastReport > (1.0 / reportRate)) if (timeSinceLastReport > (1.0 / reportRate))
{ {
notifyViewportUpdated(entry.getKey(), entry.getValue().getRectangle()); notifyViewportUpdated(entry.getKey(), entry.getValue().getRectangle());
...@@ -157,8 +158,14 @@ public class ViewportArchitect ...@@ -157,8 +158,14 @@ public class ViewportArchitect
} }
} }
} }
public void updateCameras(Set<Integer> cameraIds)
{
viewports.keySet().removeIf(id -> !cameraIds.contains(id));
}
/** /**
* @param observer * @param observer
*/ */
...@@ -166,8 +173,8 @@ public class ViewportArchitect ...@@ -166,8 +173,8 @@ public class ViewportArchitect
{ {
observers.add(observer); observers.add(observer);
} }
/** /**
* @param observer * @param observer
*/ */
...@@ -175,47 +182,47 @@ public class ViewportArchitect ...@@ -175,47 +182,47 @@ public class ViewportArchitect
{ {
observers.remove(observer); observers.remove(observer);
} }
private void notifyViewportUpdated(final int cameraId, final IRectangle viewport) private void notifyViewportUpdated(final int cameraId, final IRectangle viewport)
{ {
observers.forEach(o -> o.onViewportUpdated(cameraId, viewport)); observers.forEach(o -> o.onViewportUpdated(cameraId, viewport));
} }
private void adjustViewportsFromFieldSize(final Viewport field) private void adjustViewportsFromFieldSize(final Viewport field)
{ {
if (viewports.isEmpty()) if (viewports.isEmpty())
{ {
return; return;
} }
Viewport start = viewports.values().iterator().next(); Viewport start = viewports.values().iterator().next();
int numCamerasUp = countViewports(0, start, EDirection.UP); int numCamerasUp = countViewports(0, start, EDirection.UP);
int numCamerasDown = countViewports(0, start, EDirection.DOWN); int numCamerasDown = countViewports(0, start, EDirection.DOWN);
int numCamerasLeft = countViewports(0, start, EDirection.LEFT); int numCamerasLeft = countViewports(0, start, EDirection.LEFT);
int numCamerasRight = countViewports(0, start, EDirection.RIGHT); int numCamerasRight = countViewports(0, start, EDirection.RIGHT);
int numCamsY = 1 + numCamerasUp + numCamerasDown; int numCamsY = 1 + numCamerasUp + numCamerasDown;
int numCamsX = 1 + numCamerasLeft + numCamerasRight; int numCamsX = 1 + numCamerasLeft + numCamerasRight;
IVector2 fieldSize = field.max.subtractNew(field.min); IVector2 fieldSize = field.max.subtractNew(field.min);
IVector2 step = fieldSize.multiplyNew(Vector2.fromXY(1.0 / numCamsX, 1.0 / numCamsY)); IVector2 step = fieldSize.multiplyNew(Vector2.fromXY(1.0 / numCamsX, 1.0 / numCamsY));
for (Viewport primary : viewports.values()) for (Viewport primary : viewports.values())
{ {
double column = Math.floor(primary.center.x() / step.x()); double column = Math.floor(primary.center.x() / step.x());
double row = Math.floor(primary.center.y() / step.y()); double row = Math.floor(primary.center.y() / step.y());
primary.min.setX((column * step.x()) - (maxViewportOverlap / 2.0)); primary.min.setX((column * step.x()) - (maxViewportOverlap / 2.0));
primary.min.setY((row * step.y()) - (maxViewportOverlap / 2.0)); primary.min.setY((row * step.y()) - (maxViewportOverlap / 2.0));
primary.max.setX(((column + 1) * step.x()) + (maxViewportOverlap / 2.0)); primary.max.setX(((column + 1) * step.x()) + (maxViewportOverlap / 2.0));
primary.max.setY(((row + 1) * step.y()) + (maxViewportOverlap / 2.0)); primary.max.setY(((row + 1) * step.y()) + (maxViewportOverlap / 2.0));
} }
} }
private void adjustViewportsFromCameraPos(final Viewport field) private void adjustViewportsFromCameraPos(final Viewport field)
{ {
for (Viewport primary : viewports.values()) for (Viewport primary : viewports.values())
...@@ -224,41 +231,41 @@ public class ViewportArchitect ...@@ -224,41 +231,41 @@ public class ViewportArchitect
Optional<Viewport> down = nextViewport(primary, EDirection.DOWN); Optional<Viewport> down = nextViewport(primary, EDirection.DOWN);
Optional<Viewport> left = nextViewport(primary, EDirection.LEFT); Optional<Viewport> left = nextViewport(primary, EDirection.LEFT);
Optional<Viewport> right = nextViewport(primary, EDirection.RIGHT); Optional<Viewport> right = nextViewport(primary, EDirection.RIGHT);
if (up.isPresent()) if (up.isPresent())
{ {
double center = (primary.center.y() + up.get().center.y()) / 2.0; double center = (primary.center.y() + up.get().center.y()) / 2.0;
primary.max.setY(center + (maxViewportOverlap / 2.0)); primary.max.setY(center + (maxViewportOverlap / 2.0));
} else } else
{ {
primary.max.setY(field.max.y()); primary.max.setY(field.max.y());
} }
if (down.isPresent()) if (down.isPresent())
{ {
double center = (primary.center.y() + down.get().center.y()) / 2.0; double center = (primary.center.y() + down.get().center.y()) / 2.0;
primary.min.setY(center - (maxViewportOverlap / 2.0)); primary.min.setY(center - (maxViewportOverlap / 2.0));
} else } else
{ {
primary.min.setY(field.min.y()); primary.min.setY(field.min.y());
} }
if (left.isPresent()) if (left.isPresent())
{ {
double center = (primary.center.x() + left.get().center.x()) / 2.0; double center = (primary.center.x() + left.get().center.x()) / 2.0;
primary.min.setX(center - (maxViewportOverlap / 2.0)); primary.min.setX(center - (maxViewportOverlap / 2.0));
} else } else
{ {
primary.min.setX(field.min.x()); primary.min.setX(field.min.x());
} }
if (right.isPresent()) if (right.isPresent())
{ {
double center = (primary.center.x() + right.get().center.x()) / 2.0; double center = (primary.center.x() + right.get().center.x()) / 2.0;
primary.max.setX(center + (maxViewportOverlap / 2.0)); primary.max.setX(center + (maxViewportOverlap / 2.0));
} else } else
{ {
...@@ -266,8 +273,8 @@ public class ViewportArchitect ...@@ -266,8 +273,8 @@ public class ViewportArchitect
} }
} }
} }
private void adjustViewportsFromPrincipalProjection(final Viewport field) private void adjustViewportsFromPrincipalProjection(final Viewport field)
{ {
for (Viewport primary : viewports.values()) for (Viewport primary : viewports.values())
...@@ -276,41 +283,41 @@ public class ViewportArchitect ...@@ -276,41 +283,41 @@ public class ViewportArchitect
Optional<Viewport> down = nextViewport(primary, EDirection.DOWN); Optional<Viewport> down = nextViewport(primary, EDirection.DOWN);
Optional<Viewport> left = nextViewport(primary, EDirection.LEFT); Optional<Viewport> left = nextViewport(primary, EDirection.LEFT);
Optional<Viewport> right = nextViewport(primary, EDirection.RIGHT); Optional<Viewport> right = nextViewport(primary, EDirection.RIGHT);
if (up.isPresent()) if (up.isPresent())
{ {
double center = (primary.principalProjection.y() + up.get().principalProjection.y()) / 2.0; double center = (primary.principalProjection.y() + up.get().principalProjection.y()) / 2.0;
primary.max.setY(center + (maxViewportOverlap / 2.0)); primary.max.setY(center + (maxViewportOverlap / 2.0));
} else } else
{ {
primary.max.setY(field.max.y()); primary.max.setY(field.max.y());
} }
if (down.isPresent()) if (down.isPresent())
{ {
double center = (primary.principalProjection.y() + down.get().principalProjection.y()) / 2.0; double center = (primary.principalProjection.y() + down.get().principalProjection.y()) / 2.0;
primary.min.setY(center - (maxViewportOverlap / 2.0)); primary.min.setY(center - (maxViewportOverlap / 2.0));
} else } else
{ {
primary.min.setY(field.min.y()); primary.min.setY(field.min.y());
} }
if (left.isPresent()) if (left.isPresent())
{ {
double center = (primary.principalProjection.x() + left.get().principalProjection.x()) / 2.0; double center = (primary.principalProjection.x() + left.get().principalProjection.x()) / 2.0;
primary.min.setX(center - (maxViewportOverlap / 2.0)); primary.min.setX(center - (maxViewportOverlap / 2.0));
} else } else
{ {
primary.min.setX(field.min.x()); primary.min.setX(field.min.x());
} }
if (right.isPresent()) if (right.isPresent())
{ {
double center = (primary.principalProjection.x() + right.get().principalProjection.x()) / 2.0; double center = (primary.principalProjection.x() + right.get().principalProjection.x()) / 2.0;
primary.max.setX(center + (maxViewportOverlap / 2.0)); primary.max.setX(center + (maxViewportOverlap / 2.0));
} else } else
{ {
...@@ -318,8 +325,8 @@ public class ViewportArchitect ...@@ -318,8 +325,8 @@ public class ViewportArchitect
} }
} }
}