ball_filter.h

#pragma once
 
#include <boost/circular_buffer.hpp>
#include <optional>
 
#include "software/geom/line.h"
#include "software/geom/point.h"
#include "software/geom/rectangle.h"
#include "software/sensor_fusion/filter/vision_detection.h"
#include "software/time/timestamp.h"
#include "software/world/ball.h"
 
class BallFilter
{
   public:
    // The min and max sizes of the ball detection buffer.
    // As the ball slows down, the buffer size will approach the MAX_BUFFER_SIZE.
    // As the ball speeds up, the buffer size will approach the MIN_BUFFER_SIZE.
    static constexpr unsigned int MIN_BUFFER_SIZE = 4;
    static constexpr unsigned int MAX_BUFFER_SIZE = 10;
    // If the estimated ball speed is less than this value, the largest possible buffer
    // will be used by the filter
    static constexpr double MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE = 0.5;
    // If the estimated ball speed is greater than this value, the smallest possible
    // buffer will be used by the filter
    static constexpr double MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE = 4.0;
    // The extra amount beyond the ball's max speed that we treat ball detections as valid
    static constexpr double MAX_ACCEPTABLE_BALL_SPEED_BUFFER = 2.0;
    // The maximum error threshold to considering using the generated linear regression
    // TODO (#2752): Investigate different values of error threshold
    static constexpr double LINEAR_REGRESSION_ERROR_THRESHOLD = 1000.0;
 
    explicit BallFilter();
 
    std::optional<Ball> estimateBallState(
        const std::vector<BallDetection>& new_ball_detections,
        const Rectangle& filter_area);
 
   private:
    struct BallVelocityEstimate
    {
        Vector average_velocity;
        double average_velocity_magnitude;
        // The average of the max velocity magnitude and min velocity magnitude
        double min_max_magnitude_average;
    };
 
    struct LinearRegressionResults
    {
        Line regression_line;
        double regression_error;
    };
 
    void addNewDetectionsToBuffer(std::vector<BallDetection> new_ball_detections,
                                  const Rectangle& filter_area);
 
    static std::optional<Ball> estimateBallStateFromBuffer(
        boost::circular_buffer<BallDetection> ball_detections);
 
    static std::optional<size_t> getAdjustedBufferSize(
        boost::circular_buffer<BallDetection> ball_detections);
 
    static LinearRegressionResults calculateLineOfBestFit(
        boost::circular_buffer<BallDetection> ball_detections);
 
    static LinearRegressionResults calculateLinearRegression(
        boost::circular_buffer<BallDetection> ball_detections);
 
    static Point estimateBallPosition(
        boost::circular_buffer<BallDetection> ball_detections,
        const Line& regression_line);
 
    static std::optional<BallVelocityEstimate> estimateBallVelocity(
        boost::circular_buffer<BallDetection> ball_detections,
        const std::optional<Line>& ball_regression_line = std::nullopt);
 
    boost::circular_buffer<BallDetection> ball_detection_buffer;
};