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)
private void reset()
{
if (frame.getCamFrameNumber() != (lastCamFrameId + 1))
{
if (lastCamFrameId != 0)
{
log.warn("Non-consecutive cam frame for cam {}: {} -> {}", frame.getCameraId(), lastCamFrameId,
frame.getCamFrameNumber());
}
frameIntervalFilter.reset();
}
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;
......@@ -47,15 +49,15 @@ public class ViewportArchitect
@Configurable(defValue = "DYNAMICALLY", comment = "Method to be used to construct viewports.")
private static EViewportConstruction viewportConstruction = EViewportConstruction.DYNAMICALLY;
@Configurable(defValue = "50.0", comment = "Update rate for viewport changes to base station (per camera). [Hz]")
private static double reportRate = 50.0;
static
{
ConfigRegistration.registerClass("vision", ViewportArchitect.class);
}
private enum EViewportConstruction
{
DYNAMICALLY,
......@@ -63,11 +65,11 @@ public class ViewportArchitect
FROM_CAM_POS,
FROM_CAM_PROJECTION,
}
/**
* Update architect with new geometry.
*
*
* @param geometry
*/
public void newCameraGeometry(final CamGeometry geometry)
......@@ -77,57 +79,56 @@ public class ViewportArchitect
for (CamCalibration calib : geometry.getCalibrations().values())
{
int camId = calib.getCameraId();
// check if the camera calibration changed => someone playing around with vision
if (viewports.containsKey(camId) && !viewports.get(camId).calib.similarTo(calib))
{
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));
}
field = new Viewport(Vector2f.ZERO_VECTOR, geometry.getField().getFieldWithBoundary());
}
/**
* Update with new detection frame. Adjusts dynamic viewports.
*
*
* @param frame
*/
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());
allPositions.add(viewport.dynamicMax);
allPositions.add(viewport.dynamicMin);
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 minX = allPositions.stream().mapToDouble(IVector2::x).min().orElse(viewport.dynamicMin.x());
double minY = allPositions.stream().mapToDouble(IVector2::y).min().orElse(viewport.dynamicMin.y());
viewport.dynamicMax.setX(maxX);
viewport.dynamicMax.setY(maxY);
viewport.dynamicMin.setX(minX);
viewport.dynamicMin.setY(minY);
if (field == null)
{
return;
}
switch (viewportConstruction)
{
case DYNAMICALLY:
......@@ -145,11 +146,11 @@ public class ViewportArchitect
default:
break;
}
for (Entry<Integer, Viewport> entry : viewports.entrySet())
{
double timeSinceLastReport = (frame.gettCapture() - entry.getValue().lastReportTimestamp) * 1e-9;
if (timeSinceLastReport > (1.0 / reportRate))
{
notifyViewportUpdated(entry.getKey(), entry.getValue().getRectangle());
......@@ -157,8 +158,14 @@ public class ViewportArchitect
}
}
}
public void updateCameras(Set<Integer> cameraIds)
{
viewports.keySet().removeIf(id -> !cameraIds.contains(id));
}
/**
* @param observer
*/
......@@ -166,8 +173,8 @@ public class ViewportArchitect
{
observers.add(observer);
}
/**
* @param observer
*/
......@@ -175,47 +182,47 @@ public class ViewportArchitect
{
observers.remove(observer);
}
private void notifyViewportUpdated(final int cameraId, final IRectangle viewport)
{
observers.forEach(o -> o.onViewportUpdated(cameraId, viewport));
}
private void adjustViewportsFromFieldSize(final Viewport field)
{
if (viewports.isEmpty())
{
return;
}
Viewport start = viewports.values().iterator().next();
int numCamerasUp = countViewports(0, start, EDirection.UP);
int numCamerasDown = countViewports(0, start, EDirection.DOWN);
int numCamerasLeft = countViewports(0, start, EDirection.LEFT);
int numCamerasRight = countViewports(0, start, EDirection.RIGHT);
int numCamsY = 1 + numCamerasUp + numCamerasDown;
int numCamsX = 1 + numCamerasLeft + numCamerasRight;
IVector2 fieldSize = field.max.subtractNew(field.min);
IVector2 step = fieldSize.multiplyNew(Vector2.fromXY(1.0 / numCamsX, 1.0 / numCamsY));
for (Viewport primary : viewports.values())
{
double column = Math.floor(primary.center.x() / step.x());
double row = Math.floor(primary.center.y() / step.y());
primary.min.setX((column * step.x()) - (maxViewportOverlap / 2.0));
primary.min.setY((row * step.y()) - (maxViewportOverlap / 2.0));
primary.max.setX(((column + 1) * step.x()) + (maxViewportOverlap / 2.0));
primary.max.setY(((row + 1) * step.y()) + (maxViewportOverlap / 2.0));
}
}
private void adjustViewportsFromCameraPos(final Viewport field)
{
for (Viewport primary : viewports.values())
......@@ -224,41 +231,41 @@ public class ViewportArchitect
Optional<Viewport> down = nextViewport(primary, EDirection.DOWN);
Optional<Viewport> left = nextViewport(primary, EDirection.LEFT);
Optional<Viewport> right = nextViewport(primary, EDirection.RIGHT);
if (up.isPresent())
{
double center = (primary.center.y() + up.get().center.y()) / 2.0;
primary.max.setY(center + (maxViewportOverlap / 2.0));
} else
{
primary.max.setY(field.max.y());
}
if (down.isPresent())
{
double center = (primary.center.y() + down.get().center.y()) / 2.0;
primary.min.setY(center - (maxViewportOverlap / 2.0));
} else
{
primary.min.setY(field.min.y());
}
if (left.isPresent())
{
double center = (primary.center.x() + left.get().center.x()) / 2.0;
primary.min.setX(center - (maxViewportOverlap / 2.0));
} else
{
primary.min.setX(field.min.x());
}
if (right.isPresent())
{
double center = (primary.center.x() + right.get().center.x()) / 2.0;
primary.max.setX(center + (maxViewportOverlap / 2.0));
} else
{
......@@ -266,8 +273,8 @@ public class ViewportArchitect
}
}
}
private void adjustViewportsFromPrincipalProjection(final Viewport field)
{
for (Viewport primary : viewports.values())
......@@ -276,41 +283,41 @@ public class ViewportArchitect
Optional<Viewport> down = nextViewport(primary, EDirection.DOWN);
Optional<Viewport> left = nextViewport(primary, EDirection.LEFT);
Optional<Viewport> right = nextViewport(primary, EDirection.RIGHT);
if (up.isPresent())
{
double center = (primary.principalProjection.y() + up.get().principalProjection.y()) / 2.0;
primary.max.setY(center + (maxViewportOverlap / 2.0));
} else
{
primary.max.setY(field.max.y());
}
if (down.isPresent())
{
double center = (primary.principalProjection.y() + down.get().principalProjection.y()) / 2.0;
primary.min.setY(center - (maxViewportOverlap / 2.0));
} else
{
primary.min.setY(field.min.y());
}
if (left.isPresent())
{
double center = (primary.principalProjection.x() + left.get().principalProjection.x()) / 2.0;
primary.min.setX(center - (maxViewportOverlap / 2.0));
} else
{
primary.min.setX(field.min.x());
}
if (right.isPresent())
{
double center = (primary.principalProjection.x() + right.get().principalProjection.x()) / 2.0;
primary.max.setX(center + (maxViewportOverlap / 2.0));
} else
{
......@@ -318,8 +325,8 @@ public class ViewportArchitect
}
}
}
private void adjustViewportsDynamically(final Viewport field)
{
for (Viewport primary : viewports.values())
......@@ -328,41 +335,41 @@ public class ViewportArchitect
Optional<Viewport> down = nextViewport(primary, EDirection.DOWN);
Optional<Viewport> left = nextViewport(primary, EDirection.LEFT);
Optional<Viewport> right = nextViewport(primary, EDirection.RIGHT);
if (up.isPresent())
{
double center = (primary.dynamicMax.y() + up.get().dynamicMin.y()) / 2.0;
primary.max.setY(center + (maxViewportOverlap / 2.0));
} else
{
primary.max.setY(field.max.y());
}
if (down.isPresent())
{
double center = (primary.dynamicMin.y() + down.get().dynamicMax.y()) / 2.0;
primary.min.setY(center - (maxViewportOverlap / 2.0));
} else
{
primary.min.setY(field.min.y());
}
if (left.isPresent())
{
double center = (primary.dynamicMin.x() + left.get().dynamicMax.x()) / 2.0;
primary.min.setX(center - (maxViewportOverlap / 2.0));
} else
{
primary.min.setX(field.min.x());
}
if (right.isPresent())
{
double center = (primary.dynamicMax.x() + right.get().dynamicMin.x()) / 2.0;
primary.max.setX(center + (maxViewportOverlap / 2.0));
} else
{
......@@ -370,11 +377,11 @@ public class ViewportArchitect
}
}
}
/**
* Get viewport of a specific camera.
*
*
* @param camId
* @return
*/
......@@ -385,10 +392,11 @@ public class ViewportArchitect
{
return null;
}
return viewports.get(camId).getRectangle();
return viewport.getRectangle();
}
private enum EDirection
{
UP,
......@@ -396,7 +404,7 @@ public class ViewportArchitect
LEFT,
RIGHT
}
private static class Viewport
{
private final IVector2 center;
......@@ -407,8 +415,8 @@ public class ViewportArchitect
private Vector2 dynamicMin;
private Vector2 dynamicMax;
private long lastReportTimestamp;
private Viewport(final IVector2 center, final IVector2 principalProjection, final CamCalibration calib)
{
this.center = center;
......@@ -419,8 +427,8 @@ public class ViewportArchitect
dynamicMin = Vector2.copy(center);
dynamicMax = Vector2.copy(center);
}
private Viewport(final IVector2 center, final IRectangle rect)
{
this.center = center;
......@@ -431,30 +439,30 @@ public class ViewportArchitect
dynamicMin = Vector2.copy(center);
dynamicMax = Vector2.copy(center);
}
private IRectangle getRectangle()
{
return Rectangle.fromPoints(min, max);
}
private IRectangle getDynamicRectangle()
{
return Rectangle.fromPoints(dynamicMin, dynamicMax);
}
}
private Optional<Viewport> nextViewport(final Viewport origin, final EDirection direction)
{
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());
switch (direction)
{
case UP:
......@@ -481,47 +489,43 @@ public class ViewportArchitect
return Optional.empty();
}
}
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);
}
/**
* Viewport info shapes.
*
*
* @return
*/
public List<IDrawableShape> getInfoShapes()
{
List<IDrawableShape> shapes = new ArrayList<>();