Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

WIP: Observer improvements #131

Open
wants to merge 29 commits into
base: main
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
29 commits
Select commit Hold shift + click to select a range
e4a4204
Only output balls that have 3+ observations
JornJorn Jun 26, 2024
202cb4f
Avoid copying balls
JornJorn Jun 27, 2024
6bf3738
Implement ball-robot collision for ball estimation for the Kalman. Th…
JornJorn Jun 28, 2024
7d726ac
remove reset covariance
JornJorn Jun 30, 2024
bf57def
Add comparison operators to TeamRobotID for set inclusion
JornJorn Jul 2, 2024
0f30585
Use CameraMap with data from GeoemtryFilter
JornJorn Jul 2, 2024
ce8c12a
Update BallFilter to contain more information
JornJorn Jul 2, 2024
37ee6f6
Implemented kick detection, result not used yet
JornJorn Jul 2, 2024
00a8e11
Add kickEvent
JornJorn Jul 2, 2024
2a41997
Use ball model data from vision
JornJorn Jul 3, 2024
ab26d7d
Implement Chip/Kick Trajectories
JornJorn Jul 5, 2024
7be6395
Add chip estimator
JornJorn Jul 5, 2024
fc5ec52
add package needed for optimization
JornJorn Jul 9, 2024
8509157
Merge branch 'main' into ObserverImprovements
JornJorn Jul 9, 2024
185f774
Fix docker
Jul 10, 2024
e1ea04f
Fix docker and tests perhaps
Jul 10, 2024
619a88d
Add dlib to release as well
Jul 10, 2024
fa73ae8
Docker fix for real
Jul 10, 2024
a919d2a
remove external autoref/framework as submodule, use them as image ins…
Jul 10, 2024
6b2832d
Oeps
Jul 10, 2024
6b2bb9b
Oeps
Jul 10, 2024
0c09dca
Finetune iteration count nonlinear kick estimator
rolfvdhulst Jul 11, 2024
97b1585
Merge branch 'main' into ObserverImprovements
JornJorn Jul 15, 2024
5dd52fa
Move find_package before add_library
JornJorn Jul 15, 2024
c1c3112
Move dlib to cpp
JornJorn Jul 15, 2024
8fb6b62
Merge branch 'main' into ObserverImprovements
JornJorn Jul 15, 2024
5eebae1
Detect NaN/infinities and reset filters when necessary
rolfvdhulst Jul 17, 2024
382256f
Change time extrapolation and add a cap to prevent long extrapolations
rolfvdhulst Jul 17, 2024
e71ae8b
Check for negative time differences in extrapolation
rolfvdhulst Jul 17, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Add chip estimator
Everything _seems_ to be working, now all that is left is to actual use the results
  • Loading branch information
JornJorn committed Jul 5, 2024
commit 7be63957d6fab38e98868de895d13cb73dcf0855
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
#ifndef CHIPESTIMATOR_H
#define CHIPESTIMATOR_H

#include <dlib/optimization.h>
JornJorn marked this conversation as resolved.
Show resolved Hide resolved

#include <Eigen/Dense>
#include <iostream>
#include <tuple>
#include <vector>

#include "ShotEvent.h"
#include "observer/filters/shot/ChipTrajectory.h"
#include "observer/filters/vision/ball/BallParameters.h"
#include "observer/filters/vision/ball/FilteredBall.h"

class ChipEstimator {
private:
std::vector<BallObservation> ballsSinceShot;
ShotEvent shotEvent;
Eigen::Vector3d bestShotPos;
Eigen::Vector3d bestShotVel;
BallParameters ballParameters;
CameraMap cameraMap;
int pruneIndex = 1;
bool doFirstHop = true;
double bestTOff = 0.0;

public:
ChipEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters, const CameraMap& cameraMap);
double getAverageDistance();
double getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3d shotVel, Eigen::Vector2d shotSpin);
void addFilteredBall(const BallObservation& newBall);
std::pair<Eigen::Vector3d, double> noSpinBall(double tOff);
Eigen::Vector3d getBestShotVel() { return bestShotVel; }
Eigen::Vector3d getBestShotPos() { return bestShotPos; }
Time getFirstBallTime() { return ballsSinceShot[0].timeCaptured; }
};

#endif // CHIPESTIMATOR_H
Original file line number Diff line number Diff line change
@@ -3,8 +3,9 @@

#include <Eigen/Dense>
#include <cmath>
#include "observer/filters/vision/ball/BallParameters.h"

#include "observer/filters/shot/ShotEvent.h"
#include "observer/filters/vision/ball/BallParameters.h"

class ChipTrajectory {
private:
@@ -14,7 +15,6 @@ class ChipTrajectory {
BallParameters parameters;

public:

ChipTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters);
ChipTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const BallParameters& ballParameters);
ChipTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector3d& initialVel, const BallParameters& ballParameters);
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#ifndef KICKESTIMATOR_H
#define KICKESTIMATOR_H
#include <dlib/optimization.h>
JornJorn marked this conversation as resolved.
Show resolved Hide resolved

#include <vector>

#include "ShotEvent.h"
Original file line number Diff line number Diff line change
@@ -3,8 +3,9 @@

#include <Eigen/Dense>
#include <cmath>
#include "observer/filters/vision/ball/BallParameters.h"

#include "observer/filters/shot/ShotEvent.h"
#include "observer/filters/vision/ball/BallParameters.h"

class KickTrajectory {
private:
Original file line number Diff line number Diff line change
@@ -10,8 +10,9 @@
#include "DetectionFrame.h"
#include "GeometryFilter.h"
#include "RobotFeedbackFilter.h"
#include "observer/filters/shot/ShotEvent.h"
#include "observer/filters/shot/ChipEstimator.h"
#include "observer/filters/shot/KickEstimator.h"
#include "observer/filters/shot/ShotEvent.h"
#include "observer/filters/vision/CameraMap.h"
#include "observer/filters/vision/ball/BallFilter.h"
#include "observer/filters/vision/ball/BallParameters.h"
@@ -47,6 +48,7 @@ class WorldFilter {
CameraMap cameraMap;
Time lastKickTime = Time::now();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't like this, Time::Now() is a function call at compile time? Why not just put this in the constructor?

std::optional<KickEstimator> kickEstimator;
std::optional<ChipEstimator> chipEstimator;
struct RecentData {
std::vector<FilteredRobot> blue;
std::vector<FilteredRobot> yellow;
20 changes: 17 additions & 3 deletions roboteam_observer/observer/src/filters/vision/WorldFilter.cpp
Original file line number Diff line number Diff line change
@@ -289,7 +289,7 @@ void WorldFilter::kickDetector(FilteredBall bestBall, Time time, const BallParam
lastKickTime = frameHistory.front().filteredBall->time;
mostRecentShot = ShotEvent{id, filteredRobots[0].position, allBalls[0].positionCamera, lastKickTime, allBalls};
kickEstimator = KickEstimator(*mostRecentShot, ballParameters);
// chipEstimator = ChipEstimator(*mostRecentShot, ballParameters, cameraMap);
chipEstimator = ChipEstimator(*mostRecentShot, ballParameters, cameraMap);
std::cout << "Kick detected by robot " << id.robot_id.robotID << " from team " << (id.team == TeamColor::BLUE ? "blue" : "yellow") << std::endl;
break;
}
@@ -365,11 +365,25 @@ void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time, co
if (kickEstimator->getAverageDistance() > 0.1) {
std::cout << "\033[31mRemoved straight kick estimator\033[0m" << std::endl;
kickEstimator = std::nullopt;
} else {
std::cout << "[KICK] Average distance: " << kickEstimator->getAverageDistance() << std::endl;
}
}

if (chipEstimator.has_value()) {
chipEstimator->addFilteredBall(bestBall.currentObservation.value());
if (chipEstimator->getAverageDistance() > 0.1) {
std::cout << "\033[31mRemoved chip estimator\033[0m" << std::endl;
chipEstimator = std::nullopt;
}
}
}

// if (kickEstimator.has_value() && !chipEstimator.has_value()) {
// std::cout << "\033[32mStraight kick detected\033[0m" << std::endl;
// // kickEstimator = std::nullopt;
// } else if (chipEstimator.has_value() && !kickEstimator.has_value()) {
// std::cout << "\033[32mChip detected\033[0m" << std::endl;
// // chipEstimator = std::nullopt;
// }

world.mutable_ball()->CopyFrom(bestBall.asWorldBall());
}
114 changes: 114 additions & 0 deletions roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
#include "filters/shot/ChipEstimator.h"

#include <Eigen/Dense>

ChipEstimator::ChipEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters, const CameraMap& cameraMap)
: shotEvent(shotEvent), ballParameters(ballParameters), cameraMap(cameraMap) {
for (const auto& ball : shotEvent.ballsSinceShot) {
if (ball.currentObservation.has_value()) {
if ((ball.currentObservation.value().position - shotEvent.ballPosition).norm() < 0.05) {
continue;
}
ballsSinceShot.push_back(ball.currentObservation.value());
}
}
bestShotPos = Eigen::Vector3d(shotEvent.ballPosition.x(), shotEvent.ballPosition.y(), 0);
}
Comment on lines +9 to +18
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Check that these are set; other behavior relies on these being set.


void ChipEstimator::addFilteredBall(const BallObservation& newBall) {
ballsSinceShot.push_back(newBall);
if (ballsSinceShot.size() > 50) {
ballsSinceShot.erase(ballsSinceShot.begin() + pruneIndex);
pruneIndex++;
if (pruneIndex > 40) {
pruneIndex = 1;
}
}
Comment on lines +21 to +28
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This seems hacky, and I do not understand it well...

// only predict based on the first hop, bit janky but it _might_ work
if (doFirstHop && ballsSinceShot.size() > 8) {
auto tOff = 0.05;
auto inc = tOff / 2;
while (inc > 1e-3) {
auto resNeg = noSpinBall(tOff - 1e-5);
auto resPos = noSpinBall(tOff + 1e-5);
if (resNeg.second < resPos.second) {
tOff -= inc;
} else {
tOff += inc;
}
inc /= 2;
}
auto res = noSpinBall(tOff);
bestShotVel = res.first;
bestTOff = tOff;

auto tFly = 2 * bestShotVel.z() / 9.81;
if ((newBall.timeCaptured - ballsSinceShot[0].timeCaptured).asSeconds() > tFly) {
doFirstHop = false;
std::cout << "\033[33mFirst hop done\033[0m" << std::endl;
}
}
}

std::pair<Eigen::Vector3d, double> ChipEstimator::noSpinBall(double tOff) {
int numRecords = ballsSinceShot.size();
auto tZero = ballsSinceShot[0].timeCaptured;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

possible error if vector is empty.

double pz = 0.02;

Eigen::MatrixXd matA(numRecords * 2, 3);
Eigen::VectorXd b(numRecords * 2);

for (int i = 0; i < numRecords; i++) {
const auto& ballRecord = ballsSinceShot[i];

auto g = ballRecord.position;
double t = (ballRecord.timeCaptured - tZero).asSeconds() + tOff;
auto f = cameraMap[ballRecord.cameraID].position();

matA.row(i * 2) << f.z() * t, 0, (g.x() - f.x()) * t;
matA.row((i * 2) + 1) << 0, f.z() * t, (g.y() - f.y()) * t;

b(i * 2) = ((0.5 * 9.81 * t * t * (g.x() - f.x())) + (g.x() * f.z())) - (bestShotPos.x() * f.z()) - ((g.x() - f.x()) * pz);
b((i * 2) + 1) = ((0.5 * 9.81 * t * t * (g.y() - f.y())) + (g.y() * f.z())) - (bestShotPos.y() * f.z()) - ((g.y() - f.y()) * pz);
}

Eigen::VectorXd x;
try {
x = matA.colPivHouseholderQr().solve(b);
} catch (const std::exception& e) {
}
auto bestVel = Eigen::Vector3d(x(0), x(1), x(2));
auto l1Error = (matA * x - b).lpNorm<1>();
return std::make_pair(bestVel, l1Error);
}

double ChipEstimator::getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3d shotVel, Eigen::Vector2d shotSpin) {
ChipTrajectory chipTrajectory = ChipTrajectory(shotPos, shotVel, shotSpin, ballParameters);

auto tZero = ballsSinceShot[0].timeCaptured;
double totalDistance = 0;

for (std::size_t i = 0; i < ballsSinceShot.size(); i++) {
double time = (ballsSinceShot[i].timeCaptured - tZero).asSeconds() + bestTOff;
auto realPos = chipTrajectory.getPositionAtTime(time).pos;
auto camera = cameraMap[ballsSinceShot[i].cameraID].position();

auto scale = camera.z() / (camera.z() - realPos.z());
auto xPos = (realPos.x() - camera.x()) * scale + camera.x();
auto yPos = (realPos.y() - camera.y()) * scale + camera.y();
auto projectedPos = Eigen::Vector2d(xPos, yPos);
double distance = (projectedPos - ballsSinceShot[i].position).norm();
totalDistance += distance;
}

return totalDistance / ballsSinceShot.size();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

possible division by zero.

}

double ChipEstimator::getAverageDistance() {
if (ballsSinceShot.size() < 9) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this seems strange... why 9?

// TODO: CHECK HOW MANY WE NEED
// std::cout << "Not enough balls to estimate chip" << std::endl;
return 0;
}
return getAverageDistance(bestShotPos, bestShotVel, Eigen::Vector2d(0, 0));
}
Original file line number Diff line number Diff line change
@@ -1,17 +1,15 @@
#include "observer/filters/shot/ChipTrajectory.h"

#include <Eigen/Dense>

ChipTrajectory::ChipTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters)
: initialPos(initialPos), initialVel(initialVel), initialSpin(initialSpin), parameters(ballParameters) {
}
: initialPos(initialPos), initialVel(initialVel), initialSpin(initialSpin), parameters(ballParameters) {}

ChipTrajectory::ChipTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const BallParameters& ballParameters)
: initialPos(initialPos), initialVel(initialVel), initialSpin(Eigen::Vector2d::Zero()), parameters(ballParameters) {
}
: initialPos(initialPos), initialVel(initialVel), initialSpin(Eigen::Vector2d::Zero()), parameters(ballParameters) {}

ChipTrajectory::ChipTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector3d& initialVel, const BallParameters& ballParameters)
: initialPos(initialPos.x(), initialPos.y(), 0), initialVel(initialVel), initialSpin(Eigen::Vector2d::Zero()), parameters(ballParameters) {
}
: initialPos(initialPos.x(), initialPos.y(), 0), initialVel(initialVel), initialSpin(Eigen::Vector2d::Zero()), parameters(ballParameters) {}

ShotState ChipTrajectory::getPositionAtTime(double time) {
Eigen::Vector3d posNow = initialPos;
@@ -24,7 +22,7 @@ ShotState ChipTrajectory::getPositionAtTime(double time) {
if ((tNow + tFly) > time) {
double t = time - tNow;
posNow += velNow * t;
posNow.z() += - 0.5 * 9.81 * t * t;
posNow.z() -= 0.5 * 9.81 * t * t;
velNow.z() -= 9.81 * t;
return ShotState{posNow, velNow};
}
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include "filters/shot/KickEstimator.h"

#include <Eigen/Dense>

KickEstimator::KickEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters) : shotEvent(shotEvent), ballParameters(ballParameters) {
@@ -12,7 +13,7 @@ KickEstimator::KickEstimator(const ShotEvent& shotEvent, const BallParameters& b
void KickEstimator::addFilteredBall(const BallObservation& newBall) {
ballsSinceShot.push_back(newBall);
if (ballsSinceShot.size() > 50) {
ballsSinceShot.erase(ballsSinceShot.begin(), ballsSinceShot.begin() + pruneIndex);
ballsSinceShot.erase(ballsSinceShot.begin() + pruneIndex);
pruneIndex++;
if (pruneIndex > 40) {
pruneIndex = 1;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Shouldn't this be 0?

@@ -35,17 +36,15 @@ void KickEstimator::addFilteredBall(const BallObservation& newBall) {
bestShotPos = nonLinPosVel.first;
bestShotVel = nonLinPosVel.second;
}
std::cout << std::endl;
}


std::pair<Eigen::Vector3d, Eigen::Vector3d> KickEstimator::noSpinBall() {
auto dir = getKickDir();
auto objectiveFunction = [&](const dlib::matrix<double, 0, 1>& params) -> double {
Eigen::Vector3d shotPos(params(0), params(1), 0);
auto shotVelInt = dir * params(2);
Eigen::Vector3d shotVel(shotVelInt.x(), shotVelInt.y(), 0);
Eigen::Vector2d shotSpin(0, 0);
Eigen::Vector2d shotSpin(0, 0);
return getAverageDistance(shotPos, shotVel, shotSpin);
};

@@ -71,10 +70,7 @@ std::pair<Eigen::Vector3d, Eigen::Vector3d> KickEstimator::noSpinBall() {
}
}
try {
dlib::find_min_bobyqa(objectiveFunction, initialParams, 7,
x_lower, x_upper, 0.2,
1e-3,
50);
dlib::find_min_bobyqa(objectiveFunction, initialParams, 7, x_lower, x_upper, 0.2, 1e-3, 50);
} catch (const dlib::bobyqa_failure& e) {
}

@@ -138,7 +134,7 @@ std::pair<Eigen::Vector3d, Eigen::Vector3d> KickEstimator::slidingBall() {
return std::make_pair(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0));
}
dir *= x(2) / dir.norm();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Possible division by zero.

auto shotPos = Eigen::Vector3d(x(0), x(1), 0);
auto shotPos = Eigen::Vector3d(x(0), x(1), 0);
auto shotVel = Eigen::Vector3d(dir.x(), dir.y(), 0);
return std::make_pair(shotPos, shotVel);
}
@@ -147,7 +143,7 @@ double KickEstimator::getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3
KickTrajectory kickTrajectory = KickTrajectory(shotPos, shotVel, shotSpin, ballParameters);
auto tZero = ballsSinceShot[0].timeCaptured;
double totalDistance = 0;
for (int i = 0; i < ballsSinceShot.size(); i++) {
for (std::size_t i = 0; i < ballsSinceShot.size(); i++) {
double time = (ballsSinceShot[i].timeCaptured - tZero).asSeconds();
auto trajState = kickTrajectory.getPositionAtTime(time);
auto pos2d = trajState.pos.head<2>();
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "observer/filters/shot/KickTrajectory.h"
#include <cmath>

#include <Eigen/Dense>
#include <cmath>

KickTrajectory::KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector2d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters)
: initialPos(initialPos), initialVel(initialVel), initialSpin(initialSpin), parameters(ballParameters) {
@@ -10,7 +11,7 @@ KickTrajectory::KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::V
accSlide = initialVel.normalized() * parameters.getAccRoll();
accSlideSpin = accSlide / parameters.getBallRadius();
tSwitch = 0.0;
// Ball is sliding
// Ball is sliding
} else {
accSlide = contactVelocity.normalized() * parameters.getAccSlide();
accSlideSpin = accSlide / (parameters.getBallRadius() * parameters.getInertiaDistribution());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

assert no division by 0 can happen here.

@@ -32,12 +33,10 @@ KickTrajectory::KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::V
}

KickTrajectory::KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector2d& initialVel, const BallParameters& ballParameters)
: KickTrajectory(initialPos, initialVel, Eigen::Vector2d::Zero(), ballParameters) {
}
: KickTrajectory(initialPos, initialVel, Eigen::Vector2d::Zero(), ballParameters) {}

KickTrajectory::KickTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters)
: KickTrajectory(Eigen::Vector2d(initialPos.head<2>()), Eigen::Vector2d(initialVel.head<2>()), initialSpin, ballParameters) {
}
: KickTrajectory(Eigen::Vector2d(initialPos.head<2>()), Eigen::Vector2d(initialVel.head<2>()), initialSpin, ballParameters) {}

ShotState KickTrajectory::getPositionAtTime(double time) {
if (time < tSwitch) {
Loading