55_Vision/detectedObject.cpp at master · benEnsta/55_Vision · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
#include "detectedObject.h"


detectedObject::detectedObject()
{
    KalmanFilter KF(4, 2, 0);
    Mat_<float> state(4, 1); /* (x, y, Vx, Vy) */
    Mat processNoise(4, 1, CV_32F);
    Mat_<float> measurement(2,1);
    measurement.setTo(Scalar(0));

    KFs = KF;
    measurements = measurement;
}

void detectedObject::setKalman(float x, float y)
{
    KFs.statePre.at<float>(0) = x;
    KFs.statePre.at<float>(1) = y;
    KFs.statePre.at<float>(2) = 0;
    KFs.statePre.at<float>(3) = 0;
    KFs.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,0,0,   0,1,0,0,  0,0,1,0,  0,0,0,1);

    setIdentity(KFs.measurementMatrix);
    setIdentity(KFs.processNoiseCov, Scalar::all(1e-2));
    setIdentity(KFs.measurementNoiseCov, Scalar::all(1e-3));
    setIdentity(KFs.errorCovPost, Scalar::all(1));


}

Point detectedObject::step1()
{
    Mat prediction = KFs.predict();
    Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
    return predictPt;
}

Point detectedObject::step2()
{
    Mat estimated = KFs.correct(measurements);
    Point statePt(estimated.at<float>(0),estimated.at<float>(1));
    return statePt;
}

void detectedObject::changeMeasure(int x, int y)
{
    measurements(0) = x;
    measurements(1) = y;
}