WIP: Observer improvements

.github/workflows/tests-action.yml

@@ -28,7 +28,7 @@ jobs:
         run: |
               sudo apt-get update
               sudo apt-get upgrade -y
-              sudo apt-get install -y cmake g++ qtbase5-dev qtchooser qt5-qmake qtbase5-dev-tools libqt5charts5-dev libsdl2-dev libzmq3-dev libeigen3-dev libgtest-dev ninja-build ccache
+              sudo apt-get install -y cmake g++ qtbase5-dev qtchooser qt5-qmake qtbase5-dev-tools libqt5charts5-dev libsdl2-dev libzmq3-dev libeigen3-dev libgtest-dev ninja-build ccache libdlib-dev libblas-dev liblapack-dev
 
       - name: Build Protobuf
         if: steps.cache-build-restore.outputs.cache-hit != 'true'

.gitmodules

@@ -1,9 +1,3 @@
-[submodule "external/framework"]
-	path = external/framework
-	url = https://github.com/robotics-erlangen/framework.git
-[submodule "external/autoref"]
-	path = external/autoref
-	url = https://github.com/robotics-erlangen/autoref.git
 [submodule "roboteam_autoref"]
 	path = roboteam_autoref
 	url = https://github.com/RoboTeamTwente/roboteam_autoref.git

build.sh

@@ -69,29 +69,6 @@ then
             pushd roboteam_interface
                 yarn install
             popd
-            if [ $SKIP == 0 ];
-            then
-                pushd external
-                    echo -e "${GREEN}Building external${RESET}"
-                    pushd framework
-                        echo -e "${GREEN}Building external/framework${RESET}"
-                        mkdir -p build
-                        pushd build
-                        cmake ..
-                        make simulator-cli -j$(nproc)
-                        popd
-                    popd
-                    pushd autoref
-                        echo -e "${GREEN}Building external/autoref${RESET}"
-                        mkdir -p build
-                        pushd build
-                        cmake ..
-                        make autoref-cli -j$(nproc)
-                        popd
-                    popd
-                popd
-            fi
-            echo -e "${GREEN}Done, exiting builder..${RESET}"
         else
             echo -e "${GREEN}Checking submodules${RESET}"
             git submodule update --init --recursive

docker/Dockerfile

@@ -36,6 +36,17 @@ RUN apk add --no-cache bash build-base cmake make musl-dev libtool \
     qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev
 RUN npm install -g yarn
 
+WORKDIR /tmp
+RUN wget http://dlib.net/files/dlib-19.21.tar.bz2 \
+    && tar xvf dlib-19.21.tar.bz2 \
+    && cd dlib-19.21 \
+    && mkdir build \
+    && cd build \
+    && cmake .. \
+    && make \
+    && make install \
+    && rm -rf /tmp/dlib-19.21 /tmp/dlib-19.21.tar.bz2
+
 WORKDIR /root/protobuf
 RUN wget https://github.com/protocolbuffers/protobuf/releases/download/v3.20.3/protobuf-cpp-3.20.3.zip -O protobuf.zip && unzip protobuf.zip && rm protobuf.zip
 WORKDIR /root/protobuf/protobuf-3.20.3
@@ -70,6 +81,17 @@ RUN apk add --no-cache \
     sudo nodejs npm
 RUN npm install -g yarn
 
+WORKDIR /tmp
+RUN wget http://dlib.net/files/dlib-19.21.tar.bz2 \
+    && tar xvf dlib-19.21.tar.bz2 \
+    && cd dlib-19.21 \
+    && mkdir build \
+    && cd build \
+    && cmake .. \
+    && make \
+    && make install \
+    && rm -rf /tmp/dlib-19.21 /tmp/dlib-19.21.tar.bz2
+
 COPY --from=development --chown=root:root /usr/local/lib/libprotobuf.so.31 /usr/local/lib/libprotobuf.so.31
 COPY --from=development --chown=root:root /usr/local/bin/protoc /usr/local/bin/protoc
 

docker/builder/docker-compose.yml

@@ -1,5 +1,3 @@
-version: '3'
-
 services:
 
   roboteam_builder:

docker/runner/docker-compose.yml

@@ -1,5 +1,3 @@
-version: '3'
-
 services:
 
   roboteam_primary_ai:
@@ -167,10 +165,9 @@ services:
     profiles: ["simulator","diff","game","autoref"]
 
   erforce_autoref_sim:
-    image: roboteamtwente/roboteam:development
+    image:  roboticserlangen/autoref
     container_name: RTT_erforce_autoref_sim
     restart: always
-    working_dir: "/home/roboteamtwente/external/autoref/build/bin/"
     command: sh -c "./autoref-cli --vision-port 10020 --tracker-port 10010 --gc-port 10003"
     network_mode: "host" # Workaround to connect from interface on host to AI websocket, please fix
     expose:
@@ -182,10 +179,9 @@ services:
     profiles: ["simulator","diff"]
 
   erforce_autoref_game:
-    image: roboteamtwente/roboteam:development
+    image:  roboticserlangen/autoref
     container_name: RTT_erforce_autoref_game
     restart: always
-    working_dir: "/home/roboteamtwente/external/autoref/build/bin/"
     command: sh -c "./autoref-cli --vision-port 10006 --tracker-port 10010 --gc-port 10003"
     network_mode: "host" # Workaround to connect from interface on host to AI websocket, please fix
     expose:
@@ -197,10 +193,9 @@ services:
     profiles: ["game"]
 
   erforce_simulator:
-    image: roboteamtwente/roboteam:development
+    image: roboticserlangen/simulatorcli
     container_name: RTT_erforce_simulator
     restart: always
-    working_dir: "/home/roboteamtwente/external/framework/build/bin/"
     command: sh -c "./simulator-cli"
     network_mode: "host" # Workaround to connect from interface on host to AI websocket, please fix
     expose:

roboteam_ai/src/world/Robot.cpp

@@ -62,10 +62,7 @@ void Robot::setWorkingBallSensor(bool _workingBallSensor) noexcept { Robot::work
 
 void Robot::setBallSensorSeesBall(bool _seesBall) noexcept { ballSensorSeesBall = _seesBall; }
 
-void Robot::setDribblerSeesBall(bool _seesBall) noexcept {
-    if (_seesBall) RTT_INFO("Robot " + std::to_string(id) + " has ball")
-    dribblerSeesBall = _seesBall;
-}
+void Robot::setDribblerSeesBall(bool _seesBall) noexcept { dribblerSeesBall = _seesBall; }
 
 void Robot::setHasBall(bool _hasBall) noexcept { Robot::robotHasBall = _hasBall; }
 

roboteam_observer/observer/CMakeLists.txt

@@ -1,35 +1,15 @@
-add_library(observer STATIC
-        src/filters/Scaling.h
-        src/Observer.cpp
-        src/parameters/RobotParameterDatabase.cpp
-        src/filters/referee/RefereeFilter.cpp
-        src/filters/vision/CameraObjectFilter.cpp
-        src/filters/vision/robot/CameraRobotFilter.cpp
-        src/filters/vision/ball/CameraGroundBallFilter.cpp
-        src/filters/vision/WorldFilter.cpp
-        src/filters/vision/GeometryFilter.cpp
-        src/filters/vision/VisionFilter.cpp
-        src/filters/vision/robot/RobotFilter.cpp
-        src/filters/vision/ball/BallFilter.cpp
-        src/filters/vision/PosVelFilter2D.cpp
-        src/filters/vision/robot/RobotOrientationFilter.cpp
-        src/filters/vision/PosVelFilter1D.cpp
-        src/filters/vision/robot/RobotObservation.cpp
-        src/filters/vision/robot/RobotPos.cpp
-        src/filters/vision/robot/FilteredRobot.cpp
-        src/filters/vision/DetectionFrame.cpp
-        src/filters/vision/ball/BallObservation.cpp
-        include/observer/filters/vision/KalmanFilter.h
-        src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp
-        src/filters/vision/ball/FilteredBall.cpp
-        src/filters/vision/ball/BallAssigner.cpp
-        src/data/RobotParameters.cpp
-        src/filters/vision/Camera.cpp
-        src/filters/vision/CameraMap.cpp
-        src/filters/vision/RobotFeedbackFilter.cpp
-        )
+file(GLOB_RECURSE OBSERVER_SOURCES 
+        "${CMAKE_CURRENT_SOURCE_DIR}/src/filters/vision/*.cpp"
+        "${CMAKE_CURRENT_SOURCE_DIR}/src/filters/vision/*.h"
+        "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp"
+        "${CMAKE_CURRENT_SOURCE_DIR}/src/*.h"
+        "${CMAKE_CURRENT_SOURCE_DIR}/include/observer/filters/vision/*.h"
+)
 
+add_library(observer STATIC ${OBSERVER_SOURCES})
+find_package(dlib REQUIRED)
 target_link_libraries(observer
+        PUBLIC dlib::dlib
         PUBLIC roboteam_networking
         PRIVATE roboteam_utils
         PUBLIC Eigen3::Eigen

roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h

@@ -0,0 +1,39 @@
+#ifndef CHIPESTIMATOR_H
+#define CHIPESTIMATOR_H
+
+#include <dlib/optimization.h>
+
+#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

roboteam_observer/observer/include/observer/filters/shot/ChipTrajectory.h

@@ -0,0 +1,25 @@
+#ifndef CHIP_TRAJECTORY_H
+#define CHIP_TRAJECTORY_H
+
+#include <Eigen/Dense>
+#include <cmath>
+
+#include "observer/filters/shot/ShotEvent.h"
+#include "observer/filters/vision/ball/BallParameters.h"
+
+class ChipTrajectory {
+   private:
+    Eigen::Vector3d initialPos;
+    Eigen::Vector3d initialVel;
+    Eigen::Vector2d initialSpin;
+    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);
+
+    ShotState getPositionAtTime(double time);
+};
+
+#endif  // CHIP_TRAJECTORY_H

roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h

@@ -0,0 +1,31 @@
+#ifndef KICKESTIMATOR_H
+#define KICKESTIMATOR_H
+#include <dlib/optimization.h>
+
+#include <vector>
+
+#include "ShotEvent.h"
+#include "observer/filters/shot/KickTrajectory.h"
+#include "observer/filters/vision/ball/BallParameters.h"
+#include "observer/filters/vision/ball/FilteredBall.h"
+
+class KickEstimator {
+   private:
+    std::vector<BallObservation> ballsSinceShot;
+    ShotEvent shotEvent;
+    BallParameters ballParameters;
+    int pruneIndex = 1;
+
+   public:
+    Eigen::Vector3d bestShotPos;
+    Eigen::Vector3d bestShotVel;
+    KickEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters);
+    double getAverageDistance();
+    double getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3d shotVel, Eigen::Vector2d shotSpin);
+    std::pair<Eigen::Vector3d, Eigen::Vector3d> slidingBall();
+    Eigen::Vector2d getKickDir();
+    std::pair<Eigen::Vector3d, Eigen::Vector3d> noSpinBall();
+    void addFilteredBall(const BallObservation& newBall);
+};
+
+#endif  // KICKESTIMATOR_H

roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h

@@ -0,0 +1,32 @@
+#ifndef KICK_TRAJECTORY_H
+#define KICK_TRAJECTORY_H
+
+#include <Eigen/Dense>
+#include <cmath>
+
+#include "observer/filters/shot/ShotEvent.h"
+#include "observer/filters/vision/ball/BallParameters.h"
+
+class KickTrajectory {
+   private:
+    double tSwitch;
+    Eigen::Vector2d posSwitch;
+    Eigen::Vector2d velSwitch;
+    Eigen::Vector2d accSlide;
+    Eigen::Vector2d accSlideSpin;
+    Eigen::Vector2d accRoll;
+    Eigen::Vector2d initialPos;
+    Eigen::Vector2d initialVel;
+    Eigen::Vector2d initialSpin;
+    BallParameters parameters;
+
+   public:
+    KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector2d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters);
+    KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector2d& initialVel, const BallParameters& ballParameters);
+    KickTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters);
+
+    ShotState getPositionAtTime(double time);
+    double getTimeAtRest();
+};
+
+#endif  // KICK_TRAJECTORY_H

roboteam_observer/observer/include/observer/filters/shot/ShotEvent.h

@@ -0,0 +1,24 @@
+#ifndef SHOTEVENT_H
+#define SHOTEVENT_H
+
+#include "observer/filters/vision/CameraMap.h"
+#include "observer/filters/vision/DetectionFrame.h"
+#include "observer/filters/vision/GeometryFilter.h"
+#include "observer/filters/vision/RobotFeedbackFilter.h"
+#include "observer/filters/vision/ball/BallFilter.h"
+#include "observer/filters/vision/robot/RobotFilter.h"
+#include "observer/parameters/RobotParameterDatabase.h"
+
+struct ShotEvent {
+    TeamRobotID shootingBot;
+    RobotPos shottingBotPos;
+    Eigen::Vector2d ballPosition;
+    Time time;
+    std::vector<FilteredBall> ballsSinceShot;
+};
+struct ShotState {
+    Eigen::Vector3d pos;
+    Eigen::Vector3d vel;
+};
+
+#endif  // SHOTEVENT_H

roboteam_observer/observer/include/observer/filters/vision/GeometryFilter.h

@@ -21,6 +21,7 @@ class GeometryFilter {
      * @return The most recent filtered geometry
      */
     proto::SSL_GeometryData getGeometry() const;
+    const std::map<unsigned int, proto::SSL_GeometryCameraCalibration>& getCameras() const { return cameras; }
 
    private:
     std::string lastGeometryString;