opencv之kalman滤波（2）

缺乏、安全感 2022-05-28 11:06 238阅读 0赞

1.kalman 对一个圆进行kalman追踪

kalman.pro

QT += core
    QT -= gui
    
    CONFIG += c++11
    
    TARGET = kalman
    CONFIG += console
    CONFIG -= app_bundle
    
    TEMPLATE = app
    
    SOURCES += main.cpp
    
    # The following define makes your compiler emit warnings if you use
    # any feature of Qt which as been marked deprecated (the exact warnings
    # depend on your compiler). Please consult the documentation of the
    # deprecated API in order to know how to port your code away from it.
    DEFINES += QT_DEPRECATED_WARNINGS
    
    # You can also make your code fail to compile if you use deprecated APIs.
    # In order to do so, uncomment the following line.
    # You can also select to disable deprecated APIs only up to a certain version of Qt.
    #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
    
    INCLUDEPATH += E:\opencv_3.2\build\install\include
    LIBS += -LE:\opencv_3.2\build\install\x86\mingw\bin -lopencv_core320 -lopencv_highgui320 -lopencv_imgcodecs320 -lopencv_video320\
                -lopencv_videoio320 -lopencv_imgproc320

main.cpp

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

//状态坐标白色  测量坐标蓝色  预测坐标绿色  状态值与测量值之间用红色连接  状态值与估计值之间用黄色连接

static inline Point calcPoint(Point2f center, double R, double angle)\{ return center + Point2f((float)cos(angle), (float)-sin(angle))\*(float)R;\}int main(int, char\*\*)\{ Mat img(500, 500, CV\_8UC3); KalmanFilter KF(2, 1, 0); //创建卡尔曼滤波器对象KF Mat state(2, 1, CV\_32F); /\* (phi, delta\_phi) \*/ //state(角度，△角度) 角度、角速度 Mat processNoise(2, 1, CV\_32F); Mat measurement = Mat::zeros(1, 1, CV\_32F); //定义测量值 char code = (char)-1; for(;;) \{ //1.初始化 randn( state, Scalar::all(0), Scalar::all(0.1) ); //随机生成一个矩阵，期望是0，标准差为0.1; KF.transitionMatrix = (Mat\_<float>(2, 2) << 1, 1, 0, 1); // 转移矩阵 //setIdentity: 缩放的单位对角矩阵; // !< measurement matrix (H) 观测模型 //setIdentity函数是给参数矩阵对角线赋相同值，默认是1 setIdentity(KF.measurementMatrix); //测量矩阵 H setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); //系统噪声方差矩阵 Q setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //测量噪声方差矩阵 R setIdentity(KF.errorCovPost, Scalar::all(1)); //预测估计协方差矩阵; //statePost为校正状态，其本质就是前一时刻的状态 randn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); //x(0)初始化，产生均值为0，标准差为0.1的二维高斯列向量 for(;;) \{ Point2f center(img.cols\*0.5f, img.rows\*0.5f); //center图像中心点 float R = img.cols/3.f; //半径 double stateAngle = state.at<float>(0); //跟踪点角度，state中存放起始角，state为初始状态 Point statePt = calcPoint(center, R, stateAngle); //跟踪点坐标statePt //2. 预测 Mat prediction = KF.predict(); //计算预测值，返回x' 得到状态的预测值矩阵 double predictAngle = prediction.at<float>(0); //预测点的角度 Point predictPt = calcPoint(center, R, predictAngle); //预测点坐标predictPt //3.更新 measurement是测量值 randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); //给measurement赋值N(0,R)的随机值 // generate measurement measurement += KF.measurementMatrix\*state; //z = z + H\*x; 带噪声的测量 double measAngle = measurement.at<float>(0); Point measPt = calcPoint(center, R, measAngle); // plot points //定义了画十字的方法，值得学习下 \#define drawCross( center, color, d ) line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 1, LINE\_AA, 0); \\ line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 1, LINE\_AA, 0 ) img = Scalar::all(0); drawCross( statePt, Scalar(255,255,255), 3 ); //状态坐标白色 drawCross( measPt, Scalar(0,0,255), 3 ); //测量坐标蓝色 drawCross( predictPt, Scalar(0,255,0), 3 ); //预测坐标绿色 line( img, statePt, measPt, Scalar(0,0,255), 3, LINE\_AA, 0 ); //真实值与测量值之间用红色连接 line( img, statePt, predictPt, Scalar(0,255,255), 3, LINE\_AA, 0 ); //真实值与估计值之间用黄色连接 //调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵 if(theRNG().uniform(0,4) != 0) KF.correct(measurement); //校正 //不加噪声的话就是匀速圆周运动，加了点噪声类似匀速圆周运动，因为噪声的原因，运动方向可能会改变 randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))); state = KF.transitionMatrix\*state + processNoise; imshow( "Kalman", img ); code = (char)waitKey(100); if( code > 0 ) break; \} if( code == 27 || code == 'q' || code == 'Q' ) break; \} return 0;\}

这样就能生成一个在圆上不断循环预测--更新的过程，实现了kalman的功能。

2. kalman对两个圆进行追踪

其中main.cpp

//对两个圆同时进行kalman追踪
    
    #include "opencv2/video/tracking.hpp"
    #include "opencv2/highgui.hpp"
    
    #include <stdio.h>
    
    using namespace cv;
    
    static inline Point calcPoint(Point2f center, double R, double angle)
    {
        return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
    }
    
    int main(int, char**)
    {
        Mat img(500, 500, CV_8UC3);
    
        KalmanFilter KF(2, 1, 0);      //创建卡尔曼滤波器对象KF  
        Mat state(2, 1, CV_32F); //(phi, delta_phi) state(角度，△角度)   角度、角速度
        Mat processNoise(2, 1, CV_32F);
        Mat measurement = Mat::zeros(1, 1, CV_32F);    //定义测量值
    
    
        KalmanFilter KF2(2, 1, 0);
        Mat state2(2, 1, CV_32F); // (phi, delta_phi) state(角度，△角度)   角度、角速度
        Mat processNoise2(2, 1, CV_32F);
        Mat measurement2 = Mat::zeros(1, 1, CV_32F);    //定义测量值
    
    
        char code = (char)-1;
    
        for(;;)
        {
            //1.初始化
    
            randn( state, Scalar::all(0), Scalar::all(0.1) );   //随机生成一个矩阵，期望是0，标准差为0.1;   
            KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);  //  转移矩阵
    
            randn( state2, Scalar::all(0), Scalar::all(0.1) );
            KF2.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);
    
    
            //setIdentity: 缩放的单位对角矩阵;
            // !< measurement matrix (H) 观测模型
            //setIdentity函数是给参数矩阵对角线赋相同值，默认是1
    
            setIdentity(KF.measurementMatrix);          //测量矩阵 H
            setIdentity(KF.processNoiseCov, Scalar::all(1e-5));   //系统噪声方差矩阵 Q
            setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));   //测量噪声方差矩阵 R
            setIdentity(KF.errorCovPost, Scalar::all(1));    //预测估计协方差矩阵;
    
    
            setIdentity(KF2.measurementMatrix);          //测量矩阵 H
            setIdentity(KF2.processNoiseCov, Scalar::all(1e-5));   //系统噪声方差矩阵 Q
            setIdentity(KF2.measurementNoiseCov, Scalar::all(1e-1));   //测量噪声方差矩阵 R
            setIdentity(KF2.errorCovPost, Scalar::all(1));    //预测估计协方差矩阵;
    
    
            //statePost为校正状态，其本质就是前一时刻的状态
            randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));   //x(0)初始化，产生均值为0，标准差为0.1的二维高斯列向量
            randn(KF2.statePost, Scalar::all(0), Scalar::all(0.1));
    
            for(;;)
            {
    
                Point2f center(img.cols*0.5f, img.rows*0.5f);        //center图像中心点
                float R = img.cols/3.f;                             //半径
                double stateAngle = state.at<float>(0);             //跟踪点角度，state中存放起始角，state为初始状态
                Point statePt = calcPoint(center, R, stateAngle);      //跟踪点坐标statePt
    
    
                Point2f center2(img.cols*0.2f, img.rows*0.2f);        //center图像中心点
                float R2 = img.cols/3.f;                             //半径
                double stateAngle2 = state2.at<float>(0);             //跟踪点角度，state中存放起始角，state为初始状态
                Point statePt2 = calcPoint(center2, R2, stateAngle2);      //跟踪点坐标statePt
    
    
                 //2. 预测
    
                Mat prediction = KF.predict();  //计算预测值，返回x' 得到状态的预测值矩阵
                double predictAngle = prediction.at<float>(0);  //预测点的角度
                Point predictPt = calcPoint(center, R, predictAngle);   //预测点坐标predictPt
    
    
                Mat prediction2 = KF2.predict();  //计算预测值，返回x' 得到状态的预测值矩阵
                double predictAngle2 = prediction2.at<float>(0);  //预测点的角度
                Point predictPt2 = calcPoint(center2, R2, predictAngle2);   //预测点坐标predictPt
    
    
                 //3.更新    measurement是测量值
                randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));   //给measurement赋值N(0,R)的随机值
    
                randn( measurement2, Scalar::all(0), Scalar::all(KF2.measurementNoiseCov.at<float>(0)));
    
                // generate measurement
                measurement += KF.measurementMatrix*state;       //z = z + H*x;  带噪声的测量
                measurement2 += KF2.measurementMatrix*state2;
    
    
                double measAngle = measurement.at<float>(0);
                Point measPt = calcPoint(center, R, measAngle);
    
    
                double measAngle2 = measurement2.at<float>(0);
                Point measPt2 = calcPoint(center2, R2, measAngle2);
    
    
                // plot points    //定义了画十字的方法，值得学习下
                #define drawCross( center, color, d ) line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0); \
                    line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 1, LINE_AA, 0 )
    
                img = Scalar::all(0);
    
                drawCross( statePt, Scalar(255,255,255), 3 );    //状态坐标白色
                drawCross( measPt, Scalar(0,0,255), 3 );          //测量坐标蓝色
                drawCross( predictPt, Scalar(0,255,0), 3 );        //预测坐标绿色
                line( img, statePt, measPt, Scalar(0,0,255), 3, LINE_AA, 0 );    //真实值与测量值之间用红色连接
                line( img, statePt, predictPt, Scalar(0,255,255), 3, LINE_AA, 0 );   //真实值与估计值之间用黄色连接
    
    
                drawCross( statePt2, Scalar(255,255,255), 3 );    //状态坐标白色
                drawCross( measPt2, Scalar(0,0,255), 3 );          //测量坐标蓝色
                drawCross( predictPt2, Scalar(0,255,0), 3 );        //预测坐标绿色
                line( img, statePt2, measPt2, Scalar(0,0,255), 3, LINE_AA, 0 );    //真实值与测量值之间用红色连接
                line( img, statePt2, predictPt2, Scalar(0,255,255), 3, LINE_AA, 0 );   //真实值与估计值之间用黄色连接
    
    
                 //调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵
                if(theRNG().uniform(0,4) != 0)
                    KF.correct(measurement);       //校正
                    KF2.correct(measurement2);
    
                 //不加噪声的话就是匀速圆周运动，加了点噪声类似匀速圆周运动，因为噪声的原因，运动方向可能会改变
                 ///*
                randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
                state = KF.transitionMatrix*state + processNoise;
                //*/
    
                randn( processNoise2, Scalar(0), Scalar::all(sqrt(KF2.processNoiseCov.at<float>(0, 0))));
                state2 = KF2.transitionMatrix*state2 + processNoise2;
    
    
                imshow( "Kalman", img );
                code = (char)waitKey(100);
    
                if( code > 0 )
                    break;
            }
            if( code == 27 || code == 'q' || code == 'Q' )
                break;
        }
        return 0;
    }

结果：

![70][]

[70]: /images/20220528/bee22c4772c54ae78c4418f71014619f.png