卡尔曼滤波器2d opencv

Question

我正在尝试将卡尔曼滤波器示例从 OpenCV (https://docs.opencv.org/4.4.0/de/d70/samples_2cpp_2kalman_8cpp-example.html) 转换为 2D 实现，假设点正在移动平分线。但运行代码时，您可以看到视频输出远非最佳。你能告诉我哪里错了吗？

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui.hpp"
#include <stdio.h>
#include <iostream>
using namespace cv;
using namespace std;

static inline Point my_calcPoint(Point2f center, double x, double y)
{
    return center + Point2f(x, y);
}

static void help()
{
    printf("\n Test kalman predict future point \n");
}

int main(int, char **)
{
    // my version for point moving in 2d dimension
    Mat my_img(500, 500, CV_8UC3);
    KalmanFilter my_KF(4, 2, 0);
    Mat my_state(4, 1, CV_32F); /* (x, delta_x, y, delta_y) */
    Mat my_processNoise(4, 1, CV_32F);
    Mat my_measurement = Mat::zeros(2, 1, CV_32F);
    
    char code = (char)-1;
    for (;;)
    {
        my_state.at<float>(0) = 0; 
        my_state.at<float>(1) = 0.2;  
        my_state.at<float>(2) = 0;        
        my_state.at<float>(3) = 0.2f;        

        my_KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1);
        my_KF.measurementMatrix = (Mat_<float>(2, 4) << 1, 0, 0, 0, 0, 0, 1, 0);
        setIdentity(my_KF.processNoiseCov, Scalar::all(1e-5));
        setIdentity(my_KF.measurementNoiseCov, Scalar::all(1e-1));
        setIdentity(my_KF.errorCovPost, Scalar::all(1));
        randn(my_KF.statePost, Scalar::all(0), Scalar::all(0.1));
        double x = 0;
        double y = 0;

        for (;;)
        {
            Point2f my_center(my_img.cols * 0.5f, my_img.rows * 0.5f);
            double my_stateAngle = my_state.at<float>(0);
            x = x + 0.5;
            y = y + 0.5;
            Point my_statePt = my_calcPoint(my_center, x, y);
            Mat my_prediction = my_KF.predict();
            double my_predict_x = my_prediction.at<float>(0);
            double my_predict_y = my_prediction.at<float>(1);
            Point my_predictPt = my_calcPoint(my_center, my_predict_x, my_predict_y);

            randn(my_measurement, Scalar::all(0), Scalar::all(my_KF.measurementNoiseCov.at<float>(0))); // Dubbio: Questo va aggiunto solo qua ?
            // generate measurement
            my_measurement += my_KF.measurementMatrix * my_state; 
            double my_meas_x = my_measurement.at<float>(0);
            double my_meas_y = my_measurement.at<float>(1);
            Point my_measPt = my_calcPoint(my_center, my_meas_x, my_meas_y);

// plot points
#define drawCross(my_center, my_color, my_d)                                   \
    line(my_img, Point(my_center.x - my_d, my_center.y - my_d),                   \
         Point(my_center.x + my_d, my_center.y + my_d), my_color, 1, LINE_AA, 0); \
    line(my_img, Point(my_center.x + my_d, my_center.y - my_d),                   \
         Point(my_center.x - my_d, my_center.y + my_d), my_color, 1, LINE_AA, 0)

            my_img = Scalar::all(0);
            drawCross(my_statePt, Scalar(255, 255, 255), 3);
            drawCross(my_measPt, Scalar(0, 0, 255), 3);
            drawCross(my_predictPt, Scalar(0, 255, 0), 3);
            line(my_img, my_statePt, my_measPt, Scalar(0, 0, 255), 3, LINE_AA, 0);
            line(my_img, my_statePt, my_predictPt, Scalar(0, 255, 255), 3, LINE_AA, 0);
            if (theRNG().uniform(0, 4) != 0)
            my_KF.correct(my_measurement);
            randn(my_processNoise, Scalar(0), Scalar::all(sqrt(my_KF.processNoiseCov.at<float>(0, 0))));
            my_state = my_KF.transitionMatrix * my_state + my_processNoise;
            imshow("my_Kalman", my_img);


            code = (char)waitKey(100);
            if (code > 0)
                break;
        }
        if (code == 27 || code == 'q' || code == 'Q')
            break;
    }
    return 0;
}

感谢您的帮助！

Answer 1

我解决了

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui.hpp"
#include <stdio.h>
#include <iostream>
using namespace cv;
using namespace std;

static inline Point my_calcPoint(Point2f center, double x, double y)
{
    return center + Point2f(x, y);
}
static void help()
{
    printf("\n Test kalman predict future point \n");
}
int main(int, char **)
{

    // my version for point moving in 2d dimension
    Mat my_img(500, 500, CV_8UC3);
    KalmanFilter my_KF(4, 2, 0);
    Mat my_state(4, 1, CV_32F); /* (x, delta_x, y, delta_y) */
    Mat my_processNoise(4, 1, CV_32F);
    Mat my_measurement = Mat::zeros(2, 1, CV_32F);
    int k = 0;

    char code = (char)-1;
    for (;;)
    {

        my_state.at<float>(0) = 0;
        my_state.at<float>(1) = 1.0;
        my_state.at<float>(2) = 0;
        my_state.at<float>(3) = 1.7;

        my_KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1);
        my_KF.measurementMatrix = (Mat_<float>(2, 4) << 1, 0, 0, 0, 0, 0, 1, 0);
        setIdentity(my_KF.processNoiseCov, Scalar::all(1e-5));
        setIdentity(my_KF.measurementNoiseCov, Scalar::all(1e-1));
        setIdentity(my_KF.errorCovPost, Scalar::all(1));
        randn(my_KF.statePost, Scalar::all(0), Scalar::all(0.1));

        for (;;)
        {
            //k += 1;
            //cout << "\nk:" << k << ".";
            //if ((k < 50) | (k > 110))
            //{}
                Point2f my_center(my_img.cols * 0.5f, my_img.rows * 0.5f);

                Point my_statePt = my_calcPoint(my_center, my_state.at<float>(0), my_state.at<float>(2));
                // cout << "\n my_state \t" << my_state;
                Mat my_prediction = my_KF.predict();
                // cout << "\n my_prediction \t" << my_prediction;
                double my_predict_x = my_prediction.at<float>(0);
                double my_predict_y = my_prediction.at<float>(2);
                Point my_predictPt = my_calcPoint(my_center, my_predict_x, my_predict_y);

                randn(my_measurement, Scalar::all(0), Scalar::all(my_KF.measurementNoiseCov.at<float>(0)));
                // cout << "\n my_measurement error rndm \t" << my_measurement;

                // generate measurement
                my_measurement += my_KF.measurementMatrix * my_state;

                // cout << " \n my_KF.measurementMatrix \t" << my_KF.measurementMatrix;

                double my_meas_x = my_measurement.at<float>(0);
                double my_meas_y = my_measurement.at<float>(1);
                // cout << "\n my_measurement \t" << my_measurement;

                Point my_measPt = my_calcPoint(my_center, my_meas_x, my_meas_y);

// plot points
#define drawCross(my_center, my_color, my_d)                                      \
    line(my_img, Point(my_center.x - my_d, my_center.y - my_d),                   \
         Point(my_center.x + my_d, my_center.y + my_d), my_color, 1, LINE_AA, 0); \
    line(my_img, Point(my_center.x + my_d, my_center.y - my_d),                   \
         Point(my_center.x - my_d, my_center.y + my_d), my_color, 1, LINE_AA, 0)

                my_img = Scalar::all(0);
                drawCross(my_statePt, Scalar(255, 255, 255), 3);
                drawCross(my_measPt, Scalar(0, 0, 255), 3);
                drawCross(my_predictPt, Scalar(0, 255, 0), 3);
                line(my_img, my_statePt, my_measPt, Scalar(0, 0, 255), 3, LINE_AA, 0);
                line(my_img, my_statePt, my_predictPt, Scalar(0, 255, 255), 3, LINE_AA, 0);
                
                if (theRNG().uniform(0, 4) != 0)
                    my_KF.correct(my_measurement);
                randn(my_processNoise, Scalar(0), Scalar::all(sqrt(my_KF.processNoiseCov.at<float>(0, 0))));
                my_state = my_KF.transitionMatrix * my_state + my_processNoise;
            imshow("my_Kalman", my_img);

            code = (char)waitKey(100);
            if (code > 0)
                break;
        }
        if (code == 27 || code == 'q' || code == 'Q')
            break;
    }
    return 0;
}