Commit 96274f08 authored by NicolaiO's avatar NicolaiO 🐼 Committed by TIGERs GitLab
Browse files

Resolve "Improve kick (spin) detection robustness"

Closes #1639

See merge request main/Sumatra!1383

sumatra-commit: b7e5a81690e5b42e5dbe843e3663f05f8996861b
parent 85f3fee4
Pipeline #16691 failed with stage
in 4 minutes and 9 seconds
......@@ -272,7 +272,8 @@ public class BallFilterPreprocessor
if (flatEstimator == null)
{
flatEstimator = new StraightKickEstimator(kickEvent, filteredBallHistory);
flatEstimator = new StraightKickEstimator(kickEvent,
filteredBallHistory.stream().collect(Collectors.toList()));
log.debug("Spawned flat estimator");
}
......@@ -287,7 +288,8 @@ public class BallFilterPreprocessor
(lastBestEstimator.getFitResult().get().getKickPos().distanceTo(kickEvent.getPosition()) > 500.0))
{
// large angle deviation or some distance away from last kick, spawn new estimator
flatEstimator = new StraightKickEstimator(kickEvent, filteredBallHistory);
flatEstimator = new StraightKickEstimator(kickEvent,
filteredBallHistory.stream().collect(Collectors.toList()));
log.debug("Spawned flat estimator due to angle/pos deviation");
} else
{
......
......@@ -39,7 +39,6 @@ import org.apache.logging.log4j.Logger;
import java.awt.Color;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Comparator;
import java.util.List;
import java.util.Optional;
......@@ -87,7 +86,7 @@ public class StraightKickEstimator implements IKickEstimator
*
* @param event Initial kick event from detector.
*/
public StraightKickEstimator(final KickEvent event, Collection<FilteredVisionBall> filteredBalls)
public StraightKickEstimator(final KickEvent event, List<FilteredVisionBall> filteredBalls)
{
List<CamBall> camBalls = event.getRecordsSinceKick().stream()
.map(r -> r.getLatestCamBall().get())
......@@ -100,8 +99,7 @@ public class StraightKickEstimator implements IKickEstimator
camBalls.remove(0);
}
var ballStateAtKick = filteredBalls.stream()
.min(Comparator.comparingLong(b -> Math.abs(b.getTimestamp() - event.getTimestamp())));
var ballStateAtKick = getBallStateAtKick(event, filteredBalls);
records.addAll(camBalls);
allRecords.addAll(camBalls);
......@@ -126,6 +124,39 @@ public class StraightKickEstimator implements IKickEstimator
}
private Optional<FilteredVisionBall> getBallStateAtKick(final KickEvent event,
final List<FilteredVisionBall> filteredBalls)
{
// go through balls backward in time until one is found with a plausible inbound state
for (int i = filteredBalls.size() - 1; i > 1; i--)
{
FilteredVisionBall last = filteredBalls.get(i);
FilteredVisionBall prev = filteredBalls.get(i - 1);
// travel direction away from kicking robot, this cannot be the inbound ball state
boolean travelDirectionAwayFromKicker =
last.getVel().getXYVector().angleToAbs(Vector2.fromAngle(event.getBotDirection())).orElse(0.0)
< AngleMath.PI_HALF;
// The previous ball has at least a 25% greater velocity than the current one.
// This is too much for normal ball deceleration, it probably hit the kicking bot already
boolean rapidlyDeceleratingBall = prev.getVel().getLength2() > last.getVel().getLength2() * 1.25;
// If ball speed is increasing this state is already past the kick
boolean increasingBallSpeed = prev.getVel().getLength2() < last.getVel().getLength2();
if (travelDirectionAwayFromKicker || rapidlyDeceleratingBall || increasingBallSpeed)
{
continue;
}
return Optional.of(last);
}
return Optional.empty();
}
@Override
public void addCamBall(final CamBall record)
{
......
......@@ -12,6 +12,7 @@ import edu.tigers.sumatra.math.pose.Pose;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3;
import edu.tigers.sumatra.math.vector.Vector2;
import edu.tigers.sumatra.math.vector.Vector2f;
import edu.tigers.sumatra.vision.data.FilteredVisionBall;
import edu.tigers.sumatra.vision.data.KickSolverResult;
import edu.tigers.sumatra.vision.kick.estimators.EBallModelIdentType;
......@@ -51,8 +52,7 @@ public class FlatKickSolverNonLin3Factor implements IKickSolver
public FlatKickSolverNonLin3Factor(final Pose kickingBotPose, final Optional<FilteredVisionBall> ballStateAtKick)
{
initialSpin = ballStateAtKick.map(FilteredVisionBall::getSpin).orElse(
Vector2.fromAngleLength(kickingBotPose.getOrientation() + Math.PI, 8000.0 / Geometry.getBallRadius()));
initialSpin = ballStateAtKick.map(FilteredVisionBall::getSpin).orElse(Vector2f.ZERO_VECTOR);
kickBotOrient = kickingBotPose.getOrientation();
}
......
/*
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.vision.kick.validators;
......@@ -46,7 +46,7 @@ public class VelocityValidator implements IKickValidator
for (List<MergedBall> group : groupedBalls.values())
{
boolean valid = true;
int validSamples = 0;
for (int i = 1; i < group.size(); i++)
{
CamBall bPrev = group.get(i - 1).getLatestCamBall().orElseThrow(IllegalStateException::new);
......@@ -63,13 +63,13 @@ public class VelocityValidator implements IKickValidator
double vel = prev.distanceTo(now) / ((tNow - tPrev) * 1e-9);
if (vel < minVelocity)
if (vel > minVelocity)
{
valid = false;
validSamples++;
}
}
if (valid)
if (validSamples >= 2)
{
return true;
}
......
Markdown is supported
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