<|start_filename|>main.cpp<|end_filename|>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include "KalmanFilter.h"
#include "Tracker.h"
using namespace std;
using namespace cv;

bool mouseDown = false;

void CallBackFunc(int event, int x, int y, int flags, void* userdata)
{
     if ( event == EVENT_LBUTTONDOWN )  { mouseDown = true; }
     if ( event == EVENT_LBUTTONUP ) { mouseDown = false; }

     if ( event == EVENT_MOUSEMOVE && mouseDown )
     {
        *((int*) userdata) = x;
        *( ( (int*) userdata ) + 1) = y;
     }

     if ( !mouseDown )
     {
         *((int*) userdata) = -1;
         *( ( (int*) userdata ) + 1) = -1;
     }
}

int main()
{
    std::cout << "###############################" << std::endl;
    std::cout << "press q to quit" << std::endl;
    std::cout << "left click and hold mouse on image to create additional measurement" << std::endl;

    // 4-dimensional state, 2-dimensional measurements
    typedef Tracker<4, 2> GNNTracker;
    GNNTracker tracker;

    typedef GNNTracker::MeasurementSpaceVector Measurement;
    typedef GNNTracker::Measurements Measurements;

    namedWindow( "Kalman Demo", WINDOW_AUTOSIZE );

    int * mouse = new int[2]();
    mouse[0] = mouse[1] = -1;

    setMouseCallback("Kalman Demo",CallBackFunc, mouse);

    int k = 0;
    size_t step = 0;

    while ( k != 1048689 ) //'q'
    {
        Mat img(480, 640, CV_8UC3, Scalar(255, 255, 255));

        Measurements m;

        // build some virtual measurements
        m.emplace_back(Measurement(320 + 120 * std::sin(step * .05), 240 + 100 * std::cos(step * .05)));
        m.emplace_back(Measurement(240 - 120 * std::sin(step * .05), 240 + 100 * std::cos(step * .05)));
        m.emplace_back(Measurement(320 - 120 * std::sin(step * .05), 240 - 200 * std::cos(step * .05)));

        // get measurement from mouse coordinates
        if (mouse[0] != -1 && mouse[1] != -1)
            m.emplace_back(Measurement(mouse[0], mouse[1]));

        step++;

        for ( const auto & x : m )
        {
            const int r = 5;
            // draw measurement
            circle(img, Point(x(0), x(1)), 2*r, Scalar(0, 0, 0));
            line(img, Point(x(0) - r, x(1) - r), Point(x(0) + r, x(1) + r), Scalar(0, 0, 0) );
            line(img, Point(x(0) - r, x(1) + r), Point(x(0) + r, x(1) - r), Scalar(0, 0, 0) );
        }

        tracker.track(m);

        for ( const auto & filter : tracker.filters() )
        {

            const GNNTracker::StateSpaceVector s = filter.state();
            const GNNTracker::StateSpaceVector p = filter.prediction();

            // draw filter position
            circle(img, Point(s(0), s(1)), 5, Scalar(0, 0, 255));

            // draw filter prediction
            circle(img, Point(p(0), p(1)), 5, Scalar(255, 0, 0));

            // draw filter velocity (scaled)
            line(img, Point(s(0), s(1)), Point(s(0) + 5 * s(2), s(1) + 5 * s(3)), Scalar(0, 255, 0));

            stringstream id;

            id << filter.id();

            putText(img, id.str(), cv::Point2i(s(0) + 10, s(1)), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0, 0));

        }

        imshow( "Kalman Demo", img );

        k = cv::waitKey(30);
    }
    return 0;
}



<|start_filename|>Tracker.h<|end_filename|>
#ifndef TRACKER_H
#define TRACKER_H

#include "KalmanFilter.h"
#include "AuctionAlgorithm.h"

template<size_t StateDim, size_t MeasurementDim>
class Tracker
{
public:

    typedef KalmanFilter<StateDim, MeasurementDim> Filter;

    typedef typename Filter::StateSpaceVector StateSpaceVector;
    typedef typename Filter::MeasurementSpaceVector MeasurementSpaceVector;
    typedef typename Filter::MeasurementMatrix MeasurementMatrix;
    typedef typename Filter::MeasurementStateConversionMatrix MeasurementStateConversionMatrix;
    typedef typename Filter::StateMatrix StateMatrix;

    typedef std::vector<MeasurementSpaceVector> Measurements;

    typedef std::vector<Filter> Filters;


    Tracker()
    {

        // transition matrix
        _F  <<
               1, 0, 1, 0,
               0, 1, 0, 1,
               0, 0, 1, 0,
               0, 0, 0, 1;

        // sensor model
        _H << 1, 0, 0, 0,
              0, 1, 0, 0;

        // process noise covariance
        _Q = 4. *
                (StateMatrix() <<
                1, .5, 0, 0,
                .5, 1, 0, 0,
                0, 0, 1, .5,
                0, 0, .5, 1).finished();

        // measurement noise covariance
        _R = 10. *
                (MeasurementMatrix() <<
                1., .5,
                .5, 1.).finished();

    }

    void track( const Measurements & measurements )
    {
        const size_t m = measurements.size();
        const size_t f = _filters.size();

        // create matrix for calculating distances between measurements and predictions
        // additional rows for initializing filters (weightet by 1 / (640 * 480))
        Eigen::MatrixXd w_ij(m, f + m);

        w_ij = Eigen::MatrixXd::Zero(m, f + m);

        // get likelihoods of measurements within track pdfs
        for ( size_t i = 0; i < m; ++i )
        {
            for ( size_t j = 0; j < f; ++j )
                w_ij(i, j) = _filters[j].likelihood(measurements[i]);
        }

        // weights for initializing new filters
        for ( size_t j = f; j < m + f; ++j )
            w_ij(j - f, j) = 1. / (640. * 480. );

        // solve the maximum-sum-of-weights problem (i.e. assignment problem)
        // in this case it is global nearest neighbour by minimizing the distances
        // over all measurement-filter-associations
        Auction<double>::Edges assignments = Auction<double>::solve(w_ij);

        Filters newFilters;

        // for all found assignments
        for ( const auto & e : assignments )
        {
            // is assignment an assignment from an already existing filter to a measurement?
            if ( e.y < f )
            {
                // update filter and keep it
                _filters[e.y].update(measurements[e.x]);
                newFilters.emplace_back(_filters[e.y]);
            }
            else // is this assignment a measurement that is considered new?
            {
                // create filter with measurment and keep it
                Filter newFilter(measurements[e.x], _F, _Q, _R, _H);
                newFilters.emplace_back(newFilter);
            }
        }

        // current filters are now the kept filters
        _filters = newFilters;

    }

    const Filters & filters() const { return _filters; }

private:

    StateMatrix _F, _Q;

    MeasurementStateConversionMatrix _H;

    MeasurementMatrix _R;

    Filters _filters;
};

#endif // TRACKER_H


<|start_filename|>Dockerfile<|end_filename|>
FROM ubuntu:20.04

RUN apt update && apt upgrade -y

# do it like this, otherwise a prompt for timezone will be there
RUN DEBIAN_FRONTEND=noninteractive apt-get install -y tzdata

# install build tools, etc.
RUN apt install -y cmake build-essential

# finally install opencv (opencv-dev seems to be needed for cmake detection of the lib)
RUN apt install -y libopencv-core4.2 libopencv-dev