opencv 物体跟踪轨迹后处理

[Diaves]

/*======================= KALMAN FILTER =========================*/
/* State vector is (x,y,w,h,dx,dy,dw,dh). */
/* Measurement is (x,y,w,h). */

/* Dynamic matrix A: */
const float A8[] = { 1, 0, 0, 0, 1, 0, 0, 0,
	0, 1, 0, 0, 0, 1, 0, 0,
	0, 0, 1, 0, 0, 0, 1, 0,
	0, 0, 0, 1, 0, 0, 0, 1,
	0, 0, 0, 0, 1, 0, 0, 0,
	0, 0, 0, 0, 0, 1, 0, 0,
	0, 0, 0, 0, 0, 0, 1, 0,
	0, 0, 0, 0, 0, 0, 0, 1};

/* Measurement matrix H: */
const float H8[] = { 1, 0, 0, 0, 0, 0, 0, 0,
	0, 1, 0, 0, 0, 0, 0, 0,
	0, 0, 1, 0, 0, 0, 0, 0,
	0, 0, 0, 1, 0, 0, 0, 0};

/* Matrices for zero size velocity: */
/* Dinamic matrix A: */
const float A6[] = { 1, 0, 0, 0, 1, 0,
	0, 1, 0, 0, 0, 1,
	0, 0, 1, 0, 0, 0,
	0, 0, 0, 1, 0, 0,
	0, 0, 0, 0, 1, 0,
	0, 0, 0, 0, 0, 1};

/* Measurement matrix H: */
const float H6[] = { 1, 0, 0, 0, 0, 0,
	0, 1, 0, 0, 0, 0,
	0, 0, 1, 0, 0, 0,
	0, 0, 0, 1, 0, 0};

#define STATE_NUM 6
#define A A6
#define H H6


CvBlobTrackPostProcKalman::CvBlobTrackPostProcKalman()
{
	m_ModelNoise = 1e-6f;
	m_DataNoisePos = 1e-6f;
	m_DataNoiseSize = 1e-1f;

#if STATE_NUM>6
	m_DataNoiseSize *= (float)pow(20.,2.);
#else
	m_DataNoiseSize /= (float)pow(20.,2.);
#endif

	AddParam("ModelNoise",&m_ModelNoise);
	AddParam("DataNoisePos",&m_DataNoisePos);
	AddParam("DataNoiseSize",&m_DataNoiseSize);

	m_Frame = 0;
	m_pKalman = cvCreateKalman(STATE_NUM,4);
	memcpy( m_pKalman->transition_matrix->data.fl, A, sizeof(A));
	memcpy( m_pKalman->measurement_matrix->data.fl, H, sizeof(H));

	cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
	cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
	CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
	CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
	cvSetIdentity( m_pKalman->error_cov_post, cvRealScalar(1));
	cvZero(m_pKalman->state_post);
	cvZero(m_pKalman->state_pre);

	SetModuleName("Kalman");
}

CvBlobTrackPostProcKalman::~CvBlobTrackPostProcKalman()
{
	cvReleaseKalman(&m_pKalman);
}

void CvBlobTrackPostProcKalman::ParamUpdate()
{
	cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
	cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
	CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
	CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
}

CvBlob* CvBlobTrackPostProcKalman::Process(CvBlob* pBlob)
{
	CvBlob* pBlobRes = &m_Blob;
	float   Z[4];
	CvMat   Zmat = cvMat(4,1,CV_32F,Z);
	m_Blob = pBlob[0];

	if(m_Frame < 2)
	{   /* First call: */
		m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
		m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
		if(m_pKalman->DP>6)
		{
			m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
			m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
		}
		m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
		m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
		m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
		m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
	}
	else
	{   /* Nonfirst call: */
		cvKalmanPredict(m_pKalman,0);
		Z[0] = CV_BLOB_X(pBlob);
		Z[1] = CV_BLOB_Y(pBlob);
		Z[2] = CV_BLOB_WX(pBlob);
		Z[3] = CV_BLOB_WY(pBlob);
		cvKalmanCorrect(m_pKalman,&Zmat);
		cvMatMulAdd(m_pKalman->measurement_matrix, m_pKalman->state_post, NULL, &Zmat);
		CV_BLOB_X(pBlobRes) = Z[0];
		CV_BLOB_Y(pBlobRes) = Z[1];
		//        CV_BLOB_WX(pBlobRes) = Z[2];
		//        CV_BLOB_WY(pBlobRes) = Z[3];
	}
	m_Frame++;
	return pBlobRes;
}

void CvBlobTrackPostProcKalman::Release()
{
	delete this;
}

static CvBlobTrackPostProcOne* cvCreateModuleBlobTrackPostProcKalmanOne()
{
	return (CvBlobTrackPostProcOne*) new CvBlobTrackPostProcKalman;
}

CvBlobTrackPostProc* cvCreateModuleBlobTrackPostProcKalman()
{
	return cvCreateBlobTrackPostProcList(cvCreateModuleBlobTrackPostProcKalmanOne);
}
/*======================= KALMAN FILTER =========================*/

这是opencv里一个轨迹后处理的Kalman滤波，请问如何使用这个类，刚接触，请指点一下

[图灵狗]

先弄清楚它的原理是必要的，参考http://en.wikipedia.org/wiki/Kalman_filter

[cattpon]

learning~

[blueink_200451]

卡尔曼滤波。