OpenCV 应用（1）卡尔曼滤波跟踪

0 卡尔曼OPENCV 预测鼠标位置

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。

因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。

卡尔曼滤波器会对含有噪声的输入数据流（比如计算机视觉中的视频输入）进行递归操作，并产生底层系统状态（比如视频中的位置）在统计意义上的最优估计。

卡尔曼滤波算法分为两个阶段： 

预测阶段：卡尔曼滤波器使用由当前点计算的协方差来估计目标的新位置； 
更新阶段：卡尔曼滤波器记录目标的位置，并为下一次循环计算修正协方差。

 

第一版

#include <cv.h>  
#include <cxcore.h>  
#include <highgui.h>  
  
#include <cmath>  
#include <vector>  
#include <iostream>  
using namespace std;  
  
const int winHeight=600;  
const int winWidth=800;  
  
  
CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);  
  
//mouse event callback  
void mouseEvent(int event, int x, int y, int flags, void *param )  
{  
    if (event==CV_EVENT_MOUSEMOVE) {  
        mousePosition=cvPoint(x,y);  
    }  
}  
  
int main (void)  
{  
    //1.kalman filter setup  
    const int stateNum=4;  
    const int measureNum=2;  
    CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)  
    CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );  
    CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y)  
    CvRNG rng = cvRNG(-1);  
    float A[stateNum][stateNum] ={//transition matrix  
        1,0,1,0,  
        0,1,0,1,  
        0,0,1,0,  
        0,0,0,1  
    };  
  
    memcpy( kalman->transition_matrix->data.fl,A,sizeof(A));  
    cvSetIdentity(kalman->measurement_matrix,cvRealScalar(1) );  
    cvSetIdentity(kalman->process_noise_cov,cvRealScalar(1e-5));  
    cvSetIdentity(kalman->measurement_noise_cov,cvRealScalar(1e-1));  
    cvSetIdentity(kalman->error_cov_post,cvRealScalar(1));  
    //initialize post state of kalman filter at random  
    cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight));  
  
    CvFont font;  
    cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);  
  
    cvNamedWindow("kalman");  
    cvSetMouseCallback("kalman",mouseEvent);  
    IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);  
    while (1){  
        //2.kalman prediction  
        const CvMat* prediction=cvKalmanPredict(kalman,0);  
        CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);  
  
        //3.update measurement  
        measurement->data.fl[0]=(float)mousePosition.x;  
        measurement->data.fl[1]=(float)mousePosition.y;  
  
        //4.update  
        cvKalmanCorrect( kalman, measurement );       
  
        //draw   
        cvSet(img,cvScalar(255,255,255,0));  
        cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green  
        cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red  
        char buf[256];  
        sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);  
        cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));  
        sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);  
        cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));  
          
        cvShowImage("kalman", img);  
        int key=cvWaitKey(3);  
        if (key==27){//esc     
            break;     
        }  
    }        
  
    cvReleaseImage(&img);  
    cvReleaseKalman(&kalman);  
    return 0;  
}  
 

　　

第二版程序

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

const int winHeight = 600;
const int winWidth = 800;


Point mousePosition = Point(winWidth >> 1, winHeight >> 1);

//mouse event callback  
void mouseEvent(int event, int x, int y, int flags, void *param)
{
	if (event == CV_EVENT_MOUSEMOVE) {
		mousePosition = Point(x, y);
	}
}

int main(void)
{
	RNG rng;
	//1.kalman filter setup  
	const int stateNum = 4;                                      //状态值4×1向量(x,y,△x,△y)  
	const int measureNum = 2;                                    //测量值2×1向量(x,y)    
	KalmanFilter KF(stateNum, measureNum, 0);

	KF.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);  //转移矩阵A  
	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));                                  //后验错误估计协方差矩阵P  
	rng.fill(KF.statePost, RNG::UNIFORM, 0, winHeight>winWidth ? winWidth : winHeight);   //初始状态值x(0)  
	Mat measurement = Mat::zeros(measureNum, 1, CV_32F);                           //初始测量值x'(0)，因为后面要更新这个值，所以必须先定义  

	namedWindow("kalman");
	setMouseCallback("kalman", mouseEvent);

	Mat image(winHeight, winWidth, CV_8UC3, Scalar(0));

	while (1)
	{
		//2.kalman prediction  
		Mat prediction = KF.predict();
		Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1));   //预测值(x',y')  

		//3.update measurement  
		measurement.at<float>(0) = (float)mousePosition.x;
		measurement.at<float>(1) = (float)mousePosition.y;

		//4.update  
		KF.correct(measurement);

		//draw   
		image.setTo(Scalar(255, 255, 255, 0));
		circle(image, predict_pt, 5, Scalar(0, 255, 0), 3);    //predicted point with green  
		circle(image, mousePosition, 5, Scalar(255, 0, 0), 3); //current position with red          

		char buf[256];
		sprintf_s(buf, 256, "predicted position:(%3d,%3d)", predict_pt.x, predict_pt.y);
		putText(image, buf, Point(10, 30), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8);
		sprintf_s(buf, 256, "current position :(%3d,%3d)", mousePosition.x, mousePosition.y);
		putText(image, buf, cvPoint(10, 60), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8);

		imshow("kalman", image);
		int key = waitKey(3);
		if (key == 27){//esc     
			break;
		}
	}
}

　　

1 OPENCV自带样例

 //状态坐标白色
drawCross(statePt, Scalar(255, 255, 255), 3);
//测量坐标蓝色
drawCross(measPt, Scalar(0, 0, 255), 3);
//预测坐标绿色
drawCross(predictPt, Scalar(0, 255, 0), 3);

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/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 main2(int, char**)
{
	/*
	使用kalma步骤一
	下面语句到for前都是kalman的初始化过程，一般在使用kalman这个类时需要初始化的值有：
	转移矩阵，测量矩阵，过程噪声协方差，测量噪声协方差，后验错误协方差矩阵，
	前一状态校正后的值，当前观察值
	*/


	Mat img(500, 500, CV_8UC3);
	KalmanFilter KF(2, 1, 0);
	Mat state(2, 1, CV_32F); /* (phi, delta_phi) */
	Mat processNoise(2, 1, CV_32F);
	Mat measurement = Mat::zeros(1, 1, CV_32F);
	char code = (char)-1;

	for (;;)
	{
		randn(state, Scalar::all(0), Scalar::all(0.1));//产生均值为0，标准差为0.1的二维高斯列向量
		KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);//转移矩阵为[1,1;0,1]

		//函数setIdentity是给参数矩阵对角线赋相同值，默认对角线值值为1
		setIdentity(KF.measurementMatrix);
		setIdentity(KF.processNoiseCov, Scalar::all(1e-5));//系统过程噪声方差矩阵
		setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));//测量过程噪声方差矩阵
		setIdentity(KF.errorCovPost, Scalar::all(1));//后验错误估计协方差矩阵

		//statePost为校正状态，其本质就是前一时刻的状态
		randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));

		for (;;)
		{
			Point2f center(img.cols*0.5f, img.rows*0.5f);
			float R = img.cols / 3.f;
			//state中存放起始角，state为初始状态
			double stateAngle = state.at<float>(0);
			Point statePt = calcPoint(center, R, stateAngle);


			/*
			使用kalma步骤二
			调用kalman这个类的predict方法得到状态的预测值矩阵
			*/


			Mat prediction = KF.predict();
			//用kalman预测的是角度
			double predictAngle = prediction.at<float>(0);
			Point predictPt = calcPoint(center, R, predictAngle);

			randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));

			// generate measurement
			//带噪声的测量
			measurement += KF.measurementMatrix*state;

			double measAngle = measurement.at<float>(0);
			Point measPt = calcPoint(center, R, measAngle);

			// plot points
			//这个define语句是画2条线段(线长很短)，其实就是画一个“X”叉符号

#define drawCross( center, color, d )                                 
                line( img, Point( center.x - d, center.y - d ),                
                             Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); 
                line( img, Point( center.x + d, center.y - d ),                
                             Point( center.x - d, center.y + d ), color, 1, CV_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, CV_AA, 0);
			//真实值和估计值之间用黄色线连接起来
			line(img, statePt, predictPt, Scalar(0, 255, 255), 3, CV_AA, 0);


			/*
			使用kalma步骤三
			调用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;
}

　　2 

白色真实位置

蓝色观测位置

绿色实际位置

版本一

 

//#include <stdafx.h>
#include <cv.h>  
#include <highgui.h>  
#include <stdio.h>  

int main()
{
	cvNamedWindow("Kalman", 1);
	CvRandState random;//创建随机  
	cvRandInit(&random, 0, 1, -1, CV_RAND_NORMAL);
	IplImage * image = cvCreateImage(cvSize(600, 450), 8, 3);
	CvKalman * kalman = cvCreateKalman(4, 2, 0);//状态变量4维，x、y坐标和在x、y方向上的速度，测量变量2维，x、y坐标  

	CvMat * xK = cvCreateMat(4, 1, CV_32FC1);//初始化状态变量，坐标为（40,40），x、y方向初速度分别为10、10  
	xK->data.fl[0] = 40.;
	xK->data.fl[1] = 40;
	xK->data.fl[2] = 10;
	xK->data.fl[3] = 10;

	const float F[] = { 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1 };//初始化传递矩阵 [1  0  1  0]  
	//               [0  1  0  1]  
	//               [0  0  1  0]  
	//               [0  0  0  1]  
	memcpy(kalman->transition_matrix->data.fl, F, sizeof(F));



	CvMat * wK = cvCreateMat(4, 1, CV_32FC1);//过程噪声  
	cvZero(wK);

	CvMat * zK = cvCreateMat(2, 1, CV_32FC1);//测量矩阵2维，x、y坐标  
	cvZero(zK);

	CvMat * vK = cvCreateMat(2, 1, CV_32FC1);//测量噪声  
	cvZero(vK);

	cvSetIdentity(kalman->measurement_matrix, cvScalarAll(1));//初始化测量矩阵H=[1  0  0  0]  
	//                [0  1  0  0]  
	cvSetIdentity(kalman->process_noise_cov, cvScalarAll(1e-1));/*过程噪声____设置适当数值，
																增大目标运动的随机性，
																但若设置的很大，则系统不能收敛，
																即速度越来越快*/
	cvSetIdentity(kalman->measurement_noise_cov, cvScalarAll(10));/*观测噪声____故意将观测噪声设置得很大，
																  使之测量结果和预测结果同样存在误差*/
	cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));/*后验误差协方差*/
	cvRand(&random, kalman->state_post);

	CvMat * mK = cvCreateMat(1, 1, CV_32FC1);  //反弹时外加的随机化矩阵  


	while (1){
		cvZero(image);
		cvRectangle(image, cvPoint(30, 30), cvPoint(570, 420), CV_RGB(255, 255, 255), 2);//绘制目标弹球的“撞击壁”  
		const CvMat * yK = cvKalmanPredict(kalman, 0);//计算预测位置  
		cvRandSetRange(&random, 0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0);
		cvRand(&random, vK);//设置随机的测量误差  
		cvMatMulAdd(kalman->measurement_matrix, xK, vK, zK);//zK=H*xK+vK  
		cvCircle(image, cvPoint(cvRound(CV_MAT_ELEM(*xK, float, 0, 0)), cvRound(CV_MAT_ELEM(*xK, float, 1, 0))),
			4, CV_RGB(255, 255, 255), 2);//白圈，真实位置  
		cvCircle(image, cvPoint(cvRound(CV_MAT_ELEM(*yK, float, 0, 0)), cvRound(CV_MAT_ELEM(*yK, float, 1, 0))),
			4, CV_RGB(0, 255, 0), 2);//绿圈，预估位置  
		cvCircle(image, cvPoint(cvRound(CV_MAT_ELEM(*zK, float, 0, 0)), cvRound(CV_MAT_ELEM(*zK, float, 1, 0))),
			4, CV_RGB(0, 0, 255), 2);//蓝圈，观测位置  

		cvRandSetRange(&random, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0);
		cvRand(&random, wK);//设置随机的过程误差  
		cvMatMulAdd(kalman->transition_matrix, xK, wK, xK);//xK=F*xK+wK  

		if (cvRound(CV_MAT_ELEM(*xK, float, 0, 0))<30){  //当撞击到反弹壁时，对应轴方向取反外加随机化  
			cvRandSetRange(&random, 0, sqrt(1e-1), 0);
			cvRand(&random, mK);
			xK->data.fl[2] = 10 + CV_MAT_ELEM(*mK, float, 0, 0);
		}
		if (cvRound(CV_MAT_ELEM(*xK, float, 0, 0))>570){
			cvRandSetRange(&random, 0, sqrt(1e-2), 0);
			cvRand(&random, mK);
			xK->data.fl[2] = -(10 + CV_MAT_ELEM(*mK, float, 0, 0));
		}
		if (cvRound(CV_MAT_ELEM(*xK, float, 1, 0))<30){
			cvRandSetRange(&random, 0, sqrt(1e-1), 0);
			cvRand(&random, mK);
			xK->data.fl[3] = 10 + CV_MAT_ELEM(*mK, float, 0, 0);
		}
		if (cvRound(CV_MAT_ELEM(*xK, float, 1, 0))>420){
			cvRandSetRange(&random, 0, sqrt(1e-3), 0);
			cvRand(&random, mK);
			xK->data.fl[3] = -(10 + CV_MAT_ELEM(*mK, float, 0, 0));
		}

		printf("%f_____%f
", xK->data.fl[2], xK->data.fl[3]);


		cvShowImage("Kalman", image);

		cvKalmanCorrect(kalman, zK);


		if (cvWaitKey(100) == 'e'){
			break;
		}
	}


	cvReleaseImage(&image);/*释放图像*/
	cvDestroyAllWindows();
}

　　

本版二

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/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 main2(int, char**)
{
	/*
	使用kalma步骤一
	下面语句到for前都是kalman的初始化过程，一般在使用kalman这个类时需要初始化的值有：
	转移矩阵，测量矩阵，过程噪声协方差，测量噪声协方差，后验错误协方差矩阵，
	前一状态校正后的值，当前观察值
	*/


	Mat img(500, 500, CV_8UC3);
	KalmanFilter KF(2, 1, 0);
	Mat state(2, 1, CV_32F); /* (phi, delta_phi) */
	Mat processNoise(2, 1, CV_32F);
	Mat measurement = Mat::zeros(1, 1, CV_32F);
	char code = (char)-1;

	for (;;)
	{
		randn(state, Scalar::all(0), Scalar::all(0.1));//产生均值为0，标准差为0.1的二维高斯列向量
		KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);//转移矩阵为[1,1;0,1]

		//函数setIdentity是给参数矩阵对角线赋相同值，默认对角线值值为1
		setIdentity(KF.measurementMatrix);
		setIdentity(KF.processNoiseCov, Scalar::all(1e-5));//系统过程噪声方差矩阵
		setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));//测量过程噪声方差矩阵
		setIdentity(KF.errorCovPost, Scalar::all(1));//后验错误估计协方差矩阵

		//statePost为校正状态，其本质就是前一时刻的状态
		randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));

		for (;;)
		{
			Point2f center(img.cols*0.5f, img.rows*0.5f);
			float R = img.cols / 3.f;
			//state中存放起始角，state为初始状态
			double stateAngle = state.at<float>(0);
			Point statePt = calcPoint(center, R, stateAngle);


			/*
			使用kalma步骤二
			调用kalman这个类的predict方法得到状态的预测值矩阵
			*/


			Mat prediction = KF.predict();
			//用kalman预测的是角度
			double predictAngle = prediction.at<float>(0);
			Point predictPt = calcPoint(center, R, predictAngle);

			randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));

			// generate measurement
			//带噪声的测量
			measurement += KF.measurementMatrix*state;

			double measAngle = measurement.at<float>(0);
			Point measPt = calcPoint(center, R, measAngle);

			// plot points
			//这个define语句是画2条线段(线长很短)，其实就是画一个“X”叉符号

#define drawCross( center, color, d )                                 
                line( img, Point( center.x - d, center.y - d ),                
                             Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); 
                line( img, Point( center.x + d, center.y - d ),                
                             Point( center.x - d, center.y + d ), color, 1, CV_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, CV_AA, 0);
			//真实值和估计值之间用黄色线连接起来
			line(img, statePt, predictPt, Scalar(0, 255, 255), 3, CV_AA, 0);


			/*
			使用kalma步骤三
			调用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;
}