Commit 85f3fee4 authored by NicolaiO's avatar NicolaiO 🐼 Committed by TIGERs GitLab
Browse files

Added estimator for redirected balls with spin.

Closes #1636

See merge request main/Sumatra!1382

sumatra-commit: 05b5e598be93d0fe899b542f50dc403bf50fc7e6
parent 175c60bc
Pipeline #16687 passed with stage
in 3 minutes and 45 seconds
......@@ -4,6 +4,7 @@
package edu.tigers.sumatra.ball.trajectory;
import edu.tigers.sumatra.ball.BallParameters;
import edu.tigers.sumatra.ball.BallState;
import edu.tigers.sumatra.math.IMirrorable;
import edu.tigers.sumatra.math.line.v2.IHalfLine;
......@@ -20,6 +21,13 @@ import java.util.List;
*/
public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCurveProvider
{
/**
* Get ball parameters for this trajectory.
*
* @return
*/
BallParameters getParameters();
/**
* Get initial position in [mm].
*
......@@ -228,4 +236,12 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu
* @return
*/
IBallTrajectory withAdjustedInitialPos(final IVector3 posNow, final double time);
/**
* Return a new trajectory with new ball parameters.
*
* @param ballParameters
* @return
*/
IBallTrajectory withBallParameters(final BallParameters ballParameters);
}
......@@ -125,6 +125,13 @@ public class ChipBallTrajectory extends ABallTrajectory
}
@Override
public IBallTrajectory withBallParameters(BallParameters ballParameters)
{
return new ChipBallTrajectory(ballParameters, initialPos, initialVel, initialSpin, tInAir, kickPos, kickVel);
}
@Override
public BallState getMilliStateAtTime(final double time)
{
......
......@@ -142,6 +142,13 @@ public class FlatBallTrajectory extends ABallTrajectory
}
@Override
public IBallTrajectory withBallParameters(BallParameters ballParameters)
{
return new FlatBallTrajectory(ballParameters, initialPos.getXYVector(), initialVel.getXYVector(), initialSpin);
}
@Override
public ABallTrajectory mirrored()
{
......
......@@ -64,6 +64,11 @@ public class DrawablePlanarCurve extends ADrawableWithStroke
points.add(pos);
vLast = vel;
}
if (t > stepSize * 1000)
{
break;
}
}
points.add(curve.getState(curve.getTEnd()).getPos());
......
......@@ -46,6 +46,13 @@ public class BallParameters
)
private double inertiaDistribution = 0.5;
@Configurable(
comment = "Amount of spin transferred during a redirect.",
defValue = "0.75",
spezis = { "SUMATRA", "LAB", "TISCH", "ROBOCUP", "ANDRE", "NICOLAI", "SIMULATOR" }
)
private double redirectSpinFactor = 0.75;
@Configurable(
comment = "Fixed velocity where the ball starts to roll [mm/s]",
defValue = "2000.0",
......
......@@ -95,7 +95,7 @@ public class BallFilterPreprocessor
MergedBall optMergedBall = ballTrackerMerger.process(ballTrackers, timestamp, lastFilteredBall);
KickEvent optKickEvent = kickDetectors.process(optMergedBall, mergedRobots);
KickFitResult optBestKickFitResult = kickEstimators.process(optKickEvent,
optMergedBall, mergedRobots, robotInfos, timestamp);
optMergedBall, mergedRobots, robotInfos, timestamp, lastFilteredBall);
return new BallFilterPreprocessorOutput(optMergedBall, optKickEvent, optBestKickFitResult);
}
......@@ -157,6 +157,7 @@ public class BallFilterPreprocessor
private IKickEstimator lastBestEstimator;
private long lastKickTimestamp = 0;
private CircularFifoQueue<KickEvent> kickEventHistory = new CircularFifoQueue<>(10);
private CircularFifoQueue<FilteredVisionBall> filteredBallHistory = new CircularFifoQueue<>(20);
private KickEstimators()
......@@ -174,8 +175,10 @@ public class BallFilterPreprocessor
private KickFitResult process(final KickEvent kickEvent, final MergedBall ball,
final List<FilteredVisionBot> mergedRobots, final Map<BotID, RobotInfo> robotInfos,
final long timestamp)
final long timestamp, final FilteredVisionBall lastFilteredBall)
{
filteredBallHistory.add(lastFilteredBall);
if ((ball != null) && ball.getLatestCamBall().isPresent())
{
// add cam ball to all estimators
......@@ -188,7 +191,7 @@ public class BallFilterPreprocessor
estimators.stream()
.filter(e -> e.isDone(mergedRobots, timestamp))
.map(IKickEstimator::getModelIdentResult)
.flatMap(Optional::stream)
.flatMap(List::stream)
.forEach(this::notifyBallModelIdentificationResult);
}
if (lastBestEstimator != null && lastBestEstimator.isDone(mergedRobots, timestamp))
......@@ -269,7 +272,7 @@ public class BallFilterPreprocessor
if (flatEstimator == null)
{
flatEstimator = new StraightKickEstimator(kickEvent);
flatEstimator = new StraightKickEstimator(kickEvent, filteredBallHistory);
log.debug("Spawned flat estimator");
}
......@@ -284,7 +287,7 @@ 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);
flatEstimator = new StraightKickEstimator(kickEvent, filteredBallHistory);
log.debug("Spawned flat estimator due to angle/pos deviation");
} else
{
......@@ -335,7 +338,7 @@ public class BallFilterPreprocessor
.removeIf(k -> k.getFitResult()
.orElse(new KickFitResult(null, 0,
Geometry.getBallFactory()
.createTrajectoryFromBallAtRest(Vector2f.ZERO_VECTOR), timestamp))
.createTrajectoryFromBallAtRest(Vector2f.ZERO_VECTOR), timestamp, ""))
.getAvgDistance() > bestKickFitResult.getAvgDistance());
}
......@@ -380,6 +383,9 @@ public class BallFilterPreprocessor
private void reset()
{
estimators.clear();
kickEventHistory.clear();
filteredBallHistory.clear();
lastBestEstimator = null;
}
}
......
/*
* Copyright (c) 2009 - 2018, DHBW Mannheim - Tigers Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.vision.data;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3;
import lombok.AllArgsConstructor;
import lombok.Value;
import java.util.Optional;
/**
* Result of a straight or chip kick solver.
*
*
* @author AndreR <andre@ryll.cc>
*/
@Value
@AllArgsConstructor
public class KickSolverResult
{
private final IVector2 kickPosition;
private final IVector3 kickVelocity;
private final long kickTimestamp;
IVector2 kickPosition;
IVector3 kickVelocity;
long kickTimestamp;
IVector2 kickSpin;
String solverName;
/**
* @param kickPosition
* @param kickVelocity
* @param kickTimestamp
*/
public KickSolverResult(final IVector2 kickPosition, final IVector3 kickVelocity, final long kickTimestamp)
public KickSolverResult(final IVector2 kickPosition, final IVector3 kickVelocity, final long kickTimestamp,
String solverName)
{
this.kickPosition = kickPosition;
this.kickVelocity = kickVelocity;
this.kickTimestamp = kickTimestamp;
this.kickSpin = null;
this.solverName = solverName;
}
/**
* @return the kickPosition
*/
public IVector2 getKickPosition()
{
return kickPosition;
}
/**
* @return the kickVelocity
*/
public IVector3 getKickVelocity()
{
return kickVelocity;
}
/**
* @return the kickTimestamp
*/
public long getKickTimestamp()
public Optional<IVector2> getKickSpin()
{
return kickTimestamp;
return Optional.ofNullable(kickSpin);
}
}
......@@ -143,7 +143,7 @@ public class ChipKickEstimator implements IKickEstimator
currentTraj = Geometry.getBallFactory()
.createTrajectoryFromKickedBallWithoutSpin(event.getPosition(), kickVel);
kickTimestamp = event.getTimestamp();
fitResult = new KickFitResult(new ArrayList<>(), 0, currentTraj, kickTimestamp);
fitResult = new KickFitResult(new ArrayList<>(), 0, currentTraj, kickTimestamp, "");
usesPriorKnowledge = true;
}
......@@ -168,7 +168,7 @@ public class ChipKickEstimator implements IKickEstimator
{
if (usesPriorKnowledge)
{
computeFitResult();
computeFitResult("prior");
}
return;
......@@ -205,11 +205,11 @@ public class ChipKickEstimator implements IKickEstimator
currentTraj = Geometry.getBallFactory()
.createTrajectoryFromKickedBallWithoutSpin(kickPos, kickVel);
computeFitResult();
computeFitResult(optSolverResult.get().getSolverName());
}
private void computeFitResult()
private void computeFitResult(String solverName)
{
List<IVector2> modelPoints = records.stream()
.map(r -> currentTraj.getMilliStateAtTime((r.gettCapture() - kickTimestamp) * 1e-9).getPos()
......@@ -228,7 +228,7 @@ public class ChipKickEstimator implements IKickEstimator
}
// save the fit result
fitResult = new KickFitResult(modelPoints, avgDist, currentTraj, kickTimestamp);
fitResult = new KickFitResult(modelPoints, avgDist, currentTraj, kickTimestamp, solverName);
}
......@@ -356,16 +356,18 @@ public class ChipKickEstimator implements IKickEstimator
@Override
public Optional<IBallModelIdentResult> getModelIdentResult()
public List<IBallModelIdentResult> getModelIdentResult()
{
List<IBallModelIdentResult> result = new ArrayList<>();
if (allRecords.size() < 20)
{
return Optional.empty();
return result;
}
if (solverNonLin == null)
{
return Optional.empty();
return result;
}
long timeAfterKick = allRecords.get(0).gettCapture() + 500_000_000;
......@@ -380,11 +382,13 @@ public class ChipKickEstimator implements IKickEstimator
ChipKickSolverNonLinIdentDirect identSolver = new ChipKickSolverNonLinIdentDirect(
solverNonLin.getKickPosition(), solverNonLin.getKickTimestamp(), camCalib, fitResult.getKickVel());
Optional<IBallModelIdentResult> result = identSolver.identModel(usedRecords);
if (result.isPresent())
Optional<IBallModelIdentResult> chipResult = identSolver.identModel(usedRecords);
if (chipResult.isPresent())
{
result.add(chipResult.get());
final String lineSeparator = System.lineSeparator();
log.info("Chip Model:{}{}", lineSeparator, result.get());
log.info("Chip Model:{}{}", lineSeparator, chipResult.get());
} else
{
log.info("Chip model identification failed.");
......
/*
* Copyright (c) 2009 - 2018, DHBW Mannheim - Tigers Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.vision.kick.estimators;
import edu.tigers.sumatra.vision.kick.estimators.chip.ChipKickSolverNonLinIdentDirect.ChipModelIdentResult;
import edu.tigers.sumatra.vision.kick.estimators.straight.FlatKickSolverNonLin3Factor;
import edu.tigers.sumatra.vision.kick.estimators.straight.StraightKickSolverNonLinIdentDirect.StraightModelIdentResult;
......@@ -13,17 +14,19 @@ import edu.tigers.sumatra.vision.kick.estimators.straight.StraightKickSolverNonL
public enum EBallModelIdentType
{
STRAIGHT_TWO_PHASE(StraightModelIdentResult.getParameterNames()),
CHIP_FIXED_LOSS_PLUS_ROLLING(ChipModelIdentResult.getParameterNames());
CHIP_FIXED_LOSS_PLUS_ROLLING(ChipModelIdentResult.getParameterNames()),
REDIRECT(FlatKickSolverNonLin3Factor.RedirectModelIdentResult.getParameterNames()),
;
private final String[] parameterNames;
private EBallModelIdentType(final String[] parameterNames)
EBallModelIdentType(final String[] parameterNames)
{
this.parameterNames = parameterNames;
}
/**
* @return the parameterNames
*/
......
/*
* Copyright (c) 2009 - 2016, DHBW Mannheim - Tigers Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.vision.kick.estimators;
import java.util.List;
import java.util.Optional;
import edu.tigers.sumatra.cam.data.CamBall;
import edu.tigers.sumatra.drawable.IDrawableShape;
import edu.tigers.sumatra.vision.data.FilteredVisionBot;
import java.util.List;
import java.util.Optional;
/**
* @author AndreR <andre@ryll.cc>
......@@ -56,12 +56,12 @@ public interface IKickEstimator
* @return
*/
EKickEstimatorType getType();
/**
* Do model identification.
*
*
* @return
*/
Optional<IBallModelIdentResult> getModelIdentResult();
List<IBallModelIdentResult> getModelIdentResult();
}
......@@ -7,6 +7,8 @@ import edu.tigers.sumatra.ball.BallState;
import edu.tigers.sumatra.ball.trajectory.IBallTrajectory;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3;
import lombok.AllArgsConstructor;
import lombok.Builder;
import lombok.Value;
import java.util.List;
......@@ -16,12 +18,15 @@ import java.util.List;
* Result of a fitted kick.
*/
@Value
@Builder(setterPrefix = "with", toBuilder = true)
@AllArgsConstructor
public class KickFitResult
{
List<IVector2> groundProjection;
double avgDistance;
IBallTrajectory trajectory;
long kickTimestamp;
String solverName;
/**
......
......@@ -14,12 +14,16 @@ import edu.tigers.sumatra.drawable.IDrawableShape;
import edu.tigers.sumatra.geometry.Geometry;
import edu.tigers.sumatra.math.AngleMath;
import edu.tigers.sumatra.math.line.Line;
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.FilteredVisionBot;
import edu.tigers.sumatra.vision.data.KickEvent;
import edu.tigers.sumatra.vision.data.KickSolverResult;
import edu.tigers.sumatra.vision.kick.estimators.straight.FlatKickSolverNonLin3Factor;
import edu.tigers.sumatra.vision.kick.estimators.straight.StraightKickSolverLin3;
import edu.tigers.sumatra.vision.kick.estimators.straight.StraightKickSolverNonLin3Direct;
import edu.tigers.sumatra.vision.kick.estimators.straight.StraightKickSolverNonLinIdentDirect;
......@@ -35,6 +39,7 @@ 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;
......@@ -54,11 +59,14 @@ public class StraightKickEstimator implements IKickEstimator
private final StraightKickSolverLin3 solverSliding;
private final StraightKickSolverNonLin3Direct solverFull;
private final FlatKickSolverNonLin3Factor solverFlatFull;
private Optional<Line> fitLastLine = Optional.empty();
private KickFitResult fitResult = null;
private List<KickFitResult> activeSolvers = new ArrayList<>();
@Configurable(comment = "Max fitting error until this estimator is dropped [mm]", defValue = "100.0")
private static double maxFittingError = 100.0;
......@@ -79,7 +87,7 @@ public class StraightKickEstimator implements IKickEstimator
*
* @param event Initial kick event from detector.
*/
public StraightKickEstimator(final KickEvent event)
public StraightKickEstimator(final KickEvent event, Collection<FilteredVisionBall> filteredBalls)
{
List<CamBall> camBalls = event.getRecordsSinceKick().stream()
.map(r -> r.getLatestCamBall().get())
......@@ -92,6 +100,9 @@ public class StraightKickEstimator implements IKickEstimator
camBalls.remove(0);
}
var ballStateAtKick = filteredBalls.stream()
.min(Comparator.comparingLong(b -> Math.abs(b.getTimestamp() - event.getTimestamp())));
records.addAll(camBalls);
allRecords.addAll(camBalls);
......@@ -99,6 +110,8 @@ public class StraightKickEstimator implements IKickEstimator
solverSliding = new StraightKickSolverLin3();
solverFull = new StraightKickSolverNonLin3Direct(event.getPosition(), avgKickVel);
solverFlatFull = new FlatKickSolverNonLin3Factor(
Pose.from(event.getKickingBotPosition(), event.getBotDirection()), ballStateAtKick);
runSolvers();
}
......@@ -148,6 +161,13 @@ public class StraightKickEstimator implements IKickEstimator
results.add(generateFitResult(solverSliding.solve(records)));
results.add(generateFitResult(solverFull.solve(records)));
results.add(generateFitResult(solverFlatFull.solve(records))
.map(k -> k.toBuilder().withAvgDistance(k.getAvgDistance() * 0.25).build()));
activeSolvers = results.stream()
.filter(Optional::isPresent)
.map(Optional::get)
.collect(Collectors.toList());
fitResult = results.stream()
.filter(Optional::isPresent)
......@@ -170,7 +190,9 @@ public class StraightKickEstimator implements IKickEstimator
IVector3 kickVel = result.get().getKickVelocity();
long tZero = result.get().getKickTimestamp();
var traj = Geometry.getBallFactory().createTrajectoryFromKickedBallWithoutSpin(kickPos, kickVel);
var traj = Geometry.getBallFactory()
.createTrajectoryFromKickedBall(kickPos, kickVel, result.get().getKickSpin().orElse(
Vector2f.ZERO_VECTOR));
double error = 0;
for (CamBall ball : records)
......@@ -183,7 +205,7 @@ public class StraightKickEstimator implements IKickEstimator
error /= records.size();
return Optional.of(new KickFitResult(ground, error, traj, tZero));
return Optional.of(new KickFitResult(ground, error, traj, tZero, result.get().getSolverName()));
}
......@@ -233,11 +255,13 @@ public class StraightKickEstimator implements IKickEstimator
@Override
public Optional<IBallModelIdentResult> getModelIdentResult()
public List<IBallModelIdentResult> getModelIdentResult()
{
List<IBallModelIdentResult> result = new ArrayList<>();
if (allRecords.size() < 20)
{
return Optional.empty();
return result;
}
long timeAfterKick = allRecords.get(0).gettCapture() + 500_000_000;
......@@ -251,16 +275,29 @@ public class StraightKickEstimator implements IKickEstimator
// solve to estimate all parameters
StraightKickSolverNonLinIdentDirect identSolver = new StraightKickSolverNonLinIdentDirect();
Optional<IBallModelIdentResult> result = identSolver.identModel(usedRecords);
if (result.isPresent())
Optional<IBallModelIdentResult> straightResult = identSolver.identModel(usedRecords);
if (straightResult.isPresent())
{
result.add(straightResult.get());
final String lineSeparator = System.lineSeparator();
log.info("Straight Model:{}{}", lineSeparator, result.get());
log.info("Straight Model:{}{}", lineSeparator, straightResult.get());
} else
{
log.info("Straight model identification failed.");
}
// check redirect identification
var redirectResult = solverFlatFull.identModel(usedRecords);
if (redirectResult.isPresent())
{
result.add(redirectResult.get());
final String lineSeparator = System.lineSeparator();
log.info("Redirect Model:{}{}", lineSeparator, redirectResult.get());
}
return result;
}