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 ...@@ -272,7 +272,8 @@ public class BallFilterPreprocessor
if (flatEstimator == null) if (flatEstimator == null)
{ {
flatEstimator = new StraightKickEstimator(kickEvent, filteredBallHistory); flatEstimator = new StraightKickEstimator(kickEvent,
filteredBallHistory.stream().collect(Collectors.toList()));
log.debug("Spawned flat estimator"); log.debug("Spawned flat estimator");
} }
...@@ -287,7 +288,8 @@ public class BallFilterPreprocessor ...@@ -287,7 +288,8 @@ public class BallFilterPreprocessor
(lastBestEstimator.getFitResult().get().getKickPos().distanceTo(kickEvent.getPosition()) > 500.0)) (lastBestEstimator.getFitResult().get().getKickPos().distanceTo(kickEvent.getPosition()) > 500.0))
{ {
// large angle deviation or some distance away from last kick, spawn new estimator // 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"); log.debug("Spawned flat estimator due to angle/pos deviation");
} else } else
{ {
......
...@@ -39,7 +39,6 @@ import org.apache.logging.log4j.Logger; ...@@ -39,7 +39,6 @@ import org.apache.logging.log4j.Logger;
import java.awt.Color; import java.awt.Color;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Collection;
import java.util.Comparator; import java.util.Comparator;
import java.util.List; import java.util.List;
import java.util.Optional; import java.util.Optional;
...@@ -87,7 +86,7 @@ public class StraightKickEstimator implements IKickEstimator ...@@ -87,7 +86,7 @@ public class StraightKickEstimator implements IKickEstimator
* *
* @param event Initial kick event from detector. * @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() List<CamBall> camBalls = event.getRecordsSinceKick().stream()
.map(r -> r.getLatestCamBall().get()) .map(r -> r.getLatestCamBall().get())
...@@ -100,8 +99,7 @@ public class StraightKickEstimator implements IKickEstimator ...@@ -100,8 +99,7 @@ public class StraightKickEstimator implements IKickEstimator
camBalls.remove(0); camBalls.remove(0);
} }
var ballStateAtKick = filteredBalls.stream() var ballStateAtKick = getBallStateAtKick(event, filteredBalls);
.min(Comparator.comparingLong(b -> Math.abs(b.getTimestamp() - event.getTimestamp())));
records.addAll(camBalls); records.addAll(camBalls);
allRecords.addAll(camBalls); allRecords.addAll(camBalls);
...@@ -126,6 +124,39 @@ public class StraightKickEstimator implements IKickEstimator ...@@ -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 @Override
public void addCamBall(final CamBall record) public void addCamBall(final CamBall record)
{ {
......
...@@ -12,6 +12,7 @@ import edu.tigers.sumatra.math.pose.Pose; ...@@ -12,6 +12,7 @@ import edu.tigers.sumatra.math.pose.Pose;
import edu.tigers.sumatra.math.vector.IVector2; import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3; import edu.tigers.sumatra.math.vector.IVector3;
import edu.tigers.sumatra.math.vector.Vector2; 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.FilteredVisionBall;
import edu.tigers.sumatra.vision.data.KickSolverResult; import edu.tigers.sumatra.vision.data.KickSolverResult;
import edu.tigers.sumatra.vision.kick.estimators.EBallModelIdentType; import edu.tigers.sumatra.vision.kick.estimators.EBallModelIdentType;
...@@ -51,8 +52,7 @@ public class FlatKickSolverNonLin3Factor implements IKickSolver ...@@ -51,8 +52,7 @@ public class FlatKickSolverNonLin3Factor implements IKickSolver
public FlatKickSolverNonLin3Factor(final Pose kickingBotPose, final Optional<FilteredVisionBall> ballStateAtKick) public FlatKickSolverNonLin3Factor(final Pose kickingBotPose, final Optional<FilteredVisionBall> ballStateAtKick)
{ {
initialSpin = ballStateAtKick.map(FilteredVisionBall::getSpin).orElse( initialSpin = ballStateAtKick.map(FilteredVisionBall::getSpin).orElse(Vector2f.ZERO_VECTOR);
Vector2.fromAngleLength(kickingBotPose.getOrientation() + Math.PI, 8000.0 / Geometry.getBallRadius()));
kickBotOrient = kickingBotPose.getOrientation(); 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; package edu.tigers.sumatra.vision.kick.validators;
...@@ -46,7 +46,7 @@ public class VelocityValidator implements IKickValidator ...@@ -46,7 +46,7 @@ public class VelocityValidator implements IKickValidator
for (List<MergedBall> group : groupedBalls.values()) for (List<MergedBall> group : groupedBalls.values())
{ {
boolean valid = true; int validSamples = 0;
for (int i = 1; i < group.size(); i++) for (int i = 1; i < group.size(); i++)
{ {
CamBall bPrev = group.get(i - 1).getLatestCamBall().orElseThrow(IllegalStateException::new); CamBall bPrev = group.get(i - 1).getLatestCamBall().orElseThrow(IllegalStateException::new);
...@@ -63,13 +63,13 @@ public class VelocityValidator implements IKickValidator ...@@ -63,13 +63,13 @@ public class VelocityValidator implements IKickValidator
double vel = prev.distanceTo(now) / ((tNow - tPrev) * 1e-9); 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; return true;
} }
......
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