opencv之kalman滤波（2）

1.kalman 对一个圆进行kalman追踪
kalman.proQT += 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
b297
)的随机值

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;
}

结果：



版权声明：本文为博主原创文章，未经博主允许不得转载。