CV_IMPL void
cvRandInit( CvRandState* state, double lower, double upper, int seed )
{
CV_FUNCNAME( "cvRandInit" );
__BEGIN__;
if( !state )
CV_ERROR_FROM_CODE( CV_StsNullPtr );
state->state = (uint64)(seed ? seed : UINT_MAX);
CV_CALL( cvRandSetRange( state, lower, upper ));
__END__;
}
void cvRandInit( CvRandState* state, double param1, double param2,
int seed, int disttype)
{
if( !state )
{
cvError( CV_StsNullPtr, "cvRandInit", "Null pointer to RNG state", "cvcompat.h", 0 );
return;
}
if( disttype != CV_RAND_UNI && disttype != CV_RAND_NORMAL )
{
cvError( CV_StsBadFlag, "cvRandInit", "Unknown distribution type", "cvcompat.h", 0 );
return;
}
state->state = (uint64)(seed ? seed : -1);
state->disttype = disttype;
cvRandSetRange( state, param1, param2, -1 );
}
KalmanFilter::KalmanFilter()
{
int dynam_params = 8; // x,y,width,height,dx,dy,dw,dh
int measure_params = 4;//x,y,width,height
m_pKalmanFilter = cvCreateKalman(dynam_params, measure_params,0);
cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
cvRandSetRange( &rng, 0, 1, 0 );
rng.disttype = CV_RAND_NORMAL;
measurement = cvCreateMat( measure_params, 1, CV_32FC1 );
cvZero(measurement);
// F matrix data
// F is transition matrix. It relates how the states interact
const float F[] = {
1, 0, 0, 0, 1, 0, 0, 0, //x + dx
0, 1, 0, 0, 0, 1, 0, 0,//y + dy
0, 0, 1, 0, 0, 0, 1, 0,//width + dw
0, 0, 0, 1, 0, 0, 0, 1,//height + dh
0, 0, 0, 0, 1, 0, 0, 0,//dx
0, 0, 0, 0, 0, 1, 0, 0,//dy
0, 0, 0, 0, 0, 0, 1, 0,//dw
0, 0, 0, 0, 0, 0, 0, 1 //dh
};
memcpy( m_pKalmanFilter->transition_matrix->data.fl, F, sizeof(F));
cvSetIdentity( m_pKalmanFilter->measurement_matrix, cvRealScalar(1) ); // (H)
cvSetIdentity( m_pKalmanFilter->process_noise_cov, cvRealScalar(1e-5) ); // (Q)
cvSetIdentity( m_pKalmanFilter->measurement_noise_cov, cvRealScalar(1e-1) ); //(R)
cvSetIdentity( m_pKalmanFilter->error_cov_post, cvRealScalar(1));
// choose random initial state
cvRand( &rng, m_pKalmanFilter->state_post );
}
IplImage* cvTestSeqQueryFrame(CvTestSeq* pTestSeq)
{
CvTestSeq_* pTS = (CvTestSeq_*)pTestSeq;
CvTestSeqElem* p = pTS->pElemList;
IplImage* pImg = pTS->pImg;
IplImage* pImgAdd = cvCloneImage(pTS->pImg);
IplImage* pImgAddG = cvCreateImage(cvSize(pImgAdd->width,pImgAdd->height),IPL_DEPTH_8U,1);
IplImage* pImgMask = pTS->pImgMask;
IplImage* pImgMaskAdd = cvCloneImage(pTS->pImgMask);
CvMat* pT = cvCreateMat(2,3,CV_32F);
if(pTS->CurFrame >= pTS->FrameNum) return NULL;
cvZero(pImg);
cvZero(pImgMask);
for(p=pTS->pElemList; p; p=p->next)
{
int DirectCopy = FALSE;
int frame = pTS->CurFrame - p->FrameBegin;
//float t = p->FrameNum>1?((float)frame/(p->FrameNum-1)):0;
CvTSTrans* pTrans = p->pTrans + frame%p->TransNum;
assert(pTrans);
if( p->FrameNum > 0 && (frame < 0 || frame >= p->FrameNum) )
{ /* Current frame is out of range: */
//if(p->pAVI)cvReleaseCapture(&p->pAVI);
p->pAVI = NULL;
continue;
}
cvZero(pImgAdd);
cvZero(pImgAddG);
cvZero(pImgMaskAdd);
if(p->noise_type == CV_NOISE_NONE)
{ /* For not noise: */
/* Get next frame: */
icvTestSeqQureyFrameElem(p, frame);
if(p->pImg == NULL) continue;
#if 1 /* transform using T filed in Trans */
{ /* Calculate transform matrix: */
float W = (float)(pImgAdd->width-1);
float H = (float)(pImgAdd->height-1);
float W0 = (float)(p->pImg->width-1);
float H0 = (float)(p->pImg->height-1);
cvZero(pT);
{ /* Calcualte inverse matrix: */
CvMat mat = cvMat(2,3,CV_32F, pTrans->T);
mat.width--;
pT->width--;
cvInvert(&mat, pT);
pT->width++;
}
CV_MAT_ELEM(pT[0], float, 0, 2) =
CV_MAT_ELEM(pT[0], float, 0, 0)*(W0/2-pTrans->T[2])+
CV_MAT_ELEM(pT[0], float, 0, 1)*(H0/2-pTrans->T[5]);
CV_MAT_ELEM(pT[0], float, 1, 2) =
CV_MAT_ELEM(pT[0], float, 1, 0)*(W0/2-pTrans->T[2])+
CV_MAT_ELEM(pT[0], float, 1, 1)*(H0/2-pTrans->T[5]);
CV_MAT_ELEM(pT[0], float, 0, 0) *= W0/W;
CV_MAT_ELEM(pT[0], float, 0, 1) *= H0/H;
CV_MAT_ELEM(pT[0], float, 1, 0) *= W0/W;
CV_MAT_ELEM(pT[0], float, 1, 1) *= H0/H;
} /* Calculate transform matrix. */
#else
{ /* Calculate transform matrix: */
float SX = (float)(p->pImg->width-1)/((pImgAdd->width-1)*pTrans->Scale.x);
float SY = (float)(p->pImg->height-1)/((pImgAdd->height-1)*pTrans->Scale.y);
float DX = pTrans->Shift.x;
float DY = pTrans->Shift.y;;
cvZero(pT);
((float*)(pT->data.ptr+pT->step*0))[0]=SX;
((float*)(pT->data.ptr+pT->step*1))[1]=SY;
((float*)(pT->data.ptr+pT->step*0))[2]=SX*(pImgAdd->width-1)*(0.5f-DX);
((float*)(pT->data.ptr+pT->step*1))[2]=SY*(pImgAdd->height-1)*(0.5f-DY);
} /* Calculate transform matrix. */
#endif
{ /* Check for direct copy: */
DirectCopy = TRUE;
if( fabs(CV_MAT_ELEM(pT[0],float,0,0)-1) > 0.00001) DirectCopy = FALSE;
if( fabs(CV_MAT_ELEM(pT[0],float,1,0)) > 0.00001) DirectCopy = FALSE;
if( fabs(CV_MAT_ELEM(pT[0],float,0,1)) > 0.00001) DirectCopy = FALSE;
if( fabs(CV_MAT_ELEM(pT[0],float,0,1)) > 0.00001) DirectCopy = FALSE;
if( fabs(CV_MAT_ELEM(pT[0],float,0,2)-(pImg->width-1)*0.5) > 0.5) DirectCopy = FALSE;
if( fabs(CV_MAT_ELEM(pT[0],float,1,2)-(pImg->height-1)*0.5) > 0.5) DirectCopy = FALSE;
}
/* Extract image and mask: */
if(p->pImg->nChannels == 1)
{
if(DirectCopy)
{
cvCvtColor( p->pImg,pImgAdd,CV_GRAY2BGR);
}
else
{
cvGetQuadrangleSubPix( p->pImg, pImgAddG, pT);
cvCvtColor( pImgAddG,pImgAdd,CV_GRAY2BGR);
}
}
if(p->pImg->nChannels == 3)
{
if(DirectCopy)
cvCopy(p->pImg, pImgAdd);
else
cvGetQuadrangleSubPix( p->pImg, pImgAdd, pT);
}
if(p->pImgMask)
{
if(DirectCopy)
cvCopy(p->pImgMask, pImgMaskAdd);
else
cvGetQuadrangleSubPix( p->pImgMask, pImgMaskAdd, pT);
cvThreshold(pImgMaskAdd,pImgMaskAdd,128,255,CV_THRESH_BINARY);
}
if(pTrans->C != 1 || pTrans->I != 0)
{ /* Intensity transformation: */
cvScale(pImgAdd, pImgAdd, pTrans->C,pTrans->I);
} /* Intensity transformation: */
if(pTrans->GN > 0)
{ /* Add noise: */
IplImage* pImgN = cvCloneImage(pImgAdd);
cvRandSetRange( &p->rnd_state, pTrans->GN, 0, -1 );
cvRand(&p->rnd_state, pImgN);
cvAdd(pImgN,pImgAdd,pImgAdd);
cvReleaseImage(&pImgN);
} /* Add noise. */
if(p->Mask)
{ /* Update only mask: */
cvOr(pImgMaskAdd, pImgMask, pImgMask);
}
else
{ /* Add image and mask to exist main image and mask: */
if(p->BG)
{ /* If image is background: */
cvCopy( pImgAdd, pImg, NULL);
}
else
{ /* If image is foreground: */
cvCopy( pImgAdd, pImg, pImgMaskAdd);
if(p->ObjID>=0)
cvOr(pImgMaskAdd, pImgMask, pImgMask);
}
} /* Not mask. */
} /* For not noise. */
else
{ /* Process noise video: */
if( p->noise_type == CV_NOISE_GAUSSIAN ||
int main(int argc, char **argv)
{
// Initialize, create Kalman Filter object, window, random number
// generator etc.
//
cvNamedWindow("Kalman", 1);
CvRandState rng;
cvRandInit(&rng, 0, 1, -1, CV_RAND_UNI);
IplImage *img = cvCreateImage(cvSize(500, 500), 8, 3);
CvKalman *kalman = cvCreateKalman(2, 1, 0);
// state is (phi, delta_phi) - angle and angular velocity
// Initialize with random guess.
//
CvMat *x_k = cvCreateMat(2, 1, CV_32FC1);
cvRandSetRange(&rng, 0, 0.1, 0);
rng.disttype = CV_RAND_NORMAL;
cvRand(&rng, x_k);
// process noise
//
CvMat *w_k = cvCreateMat(2, 1, CV_32FC1);
// measurements, only one parameter for angle
//
CvMat *z_k = cvCreateMat(1, 1, CV_32FC1);
cvZero(z_k);
// Transition matrix 'F' describes relationship between
// model parameters at step k and at step k+1 (this is
// the "dynamics" in our model.
//
const float F[] = { 1, 1, 0, 1 };
memcpy(kalman->transition_matrix->data.fl, F, sizeof(F));
// Initialize other Kalman filter parameters.
//
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));
// choose random initial state
//
cvRand(&rng, kalman->state_post);
while (1) {
// predict point position
const CvMat *y_k = cvKalmanPredict(kalman, 0);
// generate measurement (z_k)
//
cvRandSetRange(&rng,
0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0);
cvRand(&rng, z_k);
cvMatMulAdd(kalman->measurement_matrix, x_k, z_k, z_k);
// plot points (eg convert to planar co-ordinates and draw)
//
cvZero(img);
cvCircle(img, phi2xy(z_k), 4, CVX_YELLOW); // observed state
cvCircle(img, phi2xy(y_k), 4, CVX_WHITE, 2); // "predicted" state
cvCircle(img, phi2xy(x_k), 4, CVX_RED); // real state
cvShowImage("Kalman", img);
// adjust Kalman filter state
//
cvKalmanCorrect(kalman, z_k);
// Apply the transition matrix 'F' (eg, step time forward)
// and also apply the "process" noise w_k.
//
cvRandSetRange(&rng, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0);
cvRand(&rng, w_k);
cvMatMulAdd(kalman->transition_matrix, x_k, w_k, x_k);
// exit if user hits 'Esc'
if (cvWaitKey(100) == 27)
break;
}
return 0;
}
static int arithm_test( void* arg )
{
double success_error_level = 0;
int param = (int)arg;
int func = param / 256;
int depth = (param % 256) % 8;
int channels = (param % 256) / 8;
int mattype;
int seed = -1;//atsGetSeed();
int btpix, max_img_bytes;
int merr_i = 0, i;
double max_err = 0.;
uchar *src1data, *src2data, *dstdata, *dstdbdata, *maskdata;
CvRandState rng_state;
AtsBinArithmMaskFunc bin_func = 0;
AtsUnArithmMaskFunc un_func = 0;
AtsBinArithmFunc mul_func = 0;
CvScalar alpha, beta, gamma;
CvMat gammaarr;
alpha = beta = gamma = cvScalarAll(0);
read_arithm_params();
if( !(ATS_RANGE( depth, dt_l, dt_h+1 ) &&
ATS_RANGE( channels, ch_l, ch_h+1 ))) return TRS_UNDEF;
cvInitMatHeader( &gammaarr, 1, 1, CV_64FC4, gamma.val );
switch( func )
{
case 0:
bin_func = cvAdd;
alpha = beta = cvScalarAll(1);
break;
case 1:
bin_func = cvSub;
alpha = cvScalarAll(1);
beta = cvScalarAll(-1);
break;
case 2:
mul_func = cvMul;
break;
case 3:
un_func = cvAddS;
alpha = cvScalarAll(1);
break;
case 4:
un_func = cvSubRS;
alpha = cvScalarAll(-1);
break;
default:
assert(0);
return TRS_FAIL;
}
mattype = depth + channels*8;
depth = depth == 0 ? IPL_DEPTH_8U : depth == 1 ? IPL_DEPTH_8S :
depth == 2 ? IPL_DEPTH_16S : depth == 3 ? IPL_DEPTH_32S :
depth == 4 ? IPL_DEPTH_32F : IPL_DEPTH_64F;
channels = channels + 1;
cvRandInit( &rng_state, 0, 1, seed );
max_img_bytes = (max_img_size + 32) * (max_img_size + 2) * cvPixSize(mattype);
src1data = (uchar*)cvAlloc( max_img_bytes );
src2data = (uchar*)cvAlloc( max_img_bytes );
dstdata = (uchar*)cvAlloc( max_img_bytes );
dstdbdata = (uchar*)cvAlloc( max_img_bytes );
maskdata = (uchar*)cvAlloc( max_img_bytes / cvPixSize(mattype));
btpix = ((depth & 255)/8)*channels;
if( depth == IPL_DEPTH_32F )
success_error_level = FLT_EPSILON * img32f_range * (mul_func ? img32f_range : 2.f);
else if( depth == IPL_DEPTH_64F )
success_error_level = DBL_EPSILON * img32f_range * (mul_func ? img32f_range : 2.f);
for( i = 0; i < base_iters; i++ )
{
int continuous = (cvRandNext( &rng_state ) % 3) == 0;
int is_mask_op = mul_func ? 0 : ((cvRandNext( &rng_state ) % 3) == 0);
int step1, step2, step, mstep;
CvMat src1, src2, dst1, dst2, mask, dst;
double err;
int w, h;
w = cvRandNext( &rng_state ) % (max_img_size - min_img_size) + min_img_size;
h = cvRandNext( &rng_state ) % (max_img_size - min_img_size) + min_img_size;
step1 = step2 = step = w*btpix;
mstep = w;
if( !continuous )
{
step1 += (cvRandNext( &rng_state ) % 4)*(btpix/channels);
step2 += (cvRandNext( &rng_state ) % 4)*(btpix/channels);
step += (cvRandNext( &rng_state ) % 4)*(btpix/channels);
mstep += (cvRandNext( &rng_state ) % 4);
}
switch( depth )
{
case IPL_DEPTH_8U:
cvRandSetRange( &rng_state, 0, img8u_range );
break;
case IPL_DEPTH_8S:
cvRandSetRange( &rng_state, -img8s_range, img8s_range );
break;
case IPL_DEPTH_16S:
cvRandSetRange( &rng_state, -img16s_range, img16s_range );
break;
case IPL_DEPTH_32S:
cvRandSetRange( &rng_state, -img32s_range, img32s_range );
break;
case IPL_DEPTH_32F:
case IPL_DEPTH_64F:
cvRandSetRange( &rng_state, -img32f_range, img32f_range );
break;
}
cvInitMatHeader( &src1, h, w, mattype, src1data, step1 );
cvInitMatHeader( &src2, h, w, mattype, src2data, step2 );
cvInitMatHeader( &dst1, h, w, mattype, dstdata, step );
cvInitMatHeader( &dst2, h, w, mattype, dstdbdata, step );
cvInitMatHeader( &mask, h, w, CV_8UC1, maskdata, mstep );
cvRand( &rng_state, &src1 );
switch( cvRandNext(&rng_state) % 3 )
{
case 0:
memcpy( &dst, &src1, sizeof(dst));
break;
case 1:
if( un_func )
memcpy( &dst, &src1, sizeof(dst));
else
memcpy( &dst, &src2, sizeof(dst));
break;
default:
memcpy( &dst, &dst1, sizeof(dst));
break;
}
if( un_func )
{
if( depth == IPL_DEPTH_8U )
cvRandSetRange( &rng_state, -img8u_range, img8u_range );
cvRand( &rng_state, &gammaarr );
}
else
{
cvRand( &rng_state, &src2 );
}
if( is_mask_op )
{
const int upper = 4;
if( dst.data.ptr == dst1.data.ptr )
cvRand( &rng_state, &dst );
cvRandSetRange( &rng_state, 0, upper );
cvRand( &rng_state, &mask );
atsLinearFunc( &mask, cvScalarAll(1), 0, cvScalarAll(0),
cvScalarAll(2-upper), &mask );
}
if( !mul_func )
{
atsLinearFunc( &src1, alpha, un_func ? 0 : &src2, beta, gamma, &dst2 );
if( is_mask_op )
{
cvXorS( &mask, cvScalarAll(1), &mask );
cvCopy( &dst, &dst2, &mask );
cvXorS( &mask, cvScalarAll(1), &mask );
}
if( un_func )
un_func( &src1, gamma, &dst, is_mask_op ? &mask : 0 );
else
bin_func( &src1, &src2, &dst, is_mask_op ? &mask : 0 );
}
else
{
atsMul( &src1, &src2, &dst2 );
mul_func( &src1, &src2, &dst );
}
/*if( i == 9 )
{
putchar('.');
}*/
//cvXor( &dst2, &dst, &dst2 );
err = cvNorm( &dst2, &dst, CV_C );
if( err > max_err )
{
max_err = err;
merr_i = i;
if( max_err > success_error_level )
goto test_exit;
}
}
test_exit:
cvFree( (void**)&src1data );
cvFree( (void**)&src2data );
cvFree( (void**)&dstdata );
cvFree( (void**)&dstdbdata );
cvFree( (void**)&maskdata );
trsWrite( ATS_LST, "Max err is %g at iter = %d, seed = %08x",
max_err, merr_i, seed );
return max_err <= success_error_level ?
trsResult( TRS_OK, "No errors" ) :
trsResult( TRS_FAIL, "Bad accuracy" );
}
int main(int argc, char** argv)
{
/* A matrix data */
const float A[] = { 1, 1, 0, 1 };
IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );
CvKalman* kalman = cvCreateKalman( 2, 1, 0 );
/* state is (phi, delta_phi) - angle and angle increment */
CvMat* state = cvCreateMat( 2, 1, CV_32FC1 );
CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 );
/* only phi (angle) is measured */
CvMat* measurement = cvCreateMat( 1, 1, CV_32FC1 );
CvRandState rng;
int code = -1;
cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
cvZero( measurement );
cvNamedWindow( "Kalman", 1 );
for(;;)
{
cvRandSetRange( &rng, 0, 0.1, 0 );
rng.disttype = CV_RAND_NORMAL;
cvRand( &rng, state );
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));
/* choose random initial state */
cvRand( &rng, kalman->state_post );
rng.disttype = CV_RAND_NORMAL;
for(;;)
{
#define calc_point(angle) \
cvPoint( cvRound(img->width/2 + img->width/3*cos(angle)), \
cvRound(img->height/2 - img->width/3*sin(angle)))
float state_angle = state->data.fl[0];
CvPoint state_pt = calc_point(state_angle);
/* predict point position */
const CvMat* prediction = cvKalmanPredict( kalman, 0 );
float predict_angle = prediction->data.fl[0];
CvPoint predict_pt = calc_point(predict_angle);
float measurement_angle;
CvPoint measurement_pt;
cvRandSetRange( &rng,
0,
sqrt(kalman->measurement_noise_cov->data.fl[0]),
0 );
cvRand( &rng, measurement );
/* generate measurement */
cvMatMulAdd( kalman->measurement_matrix, state, measurement, measurement );
measurement_angle = measurement->data.fl[0];
measurement_pt = calc_point(measurement_angle);
/* plot points */
#define draw_cross( center, color, d ) \
cvLine( img, cvPoint( center.x - d, center.y - d ), \
cvPoint( center.x + d, center.y + d ), \
color, 1, 0 ); \
cvLine( img, cvPoint( center.x + d, center.y - d ), \
cvPoint( center.x - d, center.y + d ), \
color, 1, 0 )
cvZero( img );
draw_cross( state_pt, CV_RGB(255,255,255), 3 );
draw_cross( measurement_pt, CV_RGB(255,0,0), 3 );
draw_cross( predict_pt, CV_RGB(0,255,0), 3 );
cvLine( img, state_pt, predict_pt, CV_RGB(255,255,0), 3, 0 );
/* adjust Kalman filter state */
cvKalmanCorrect( kalman, measurement );
cvRandSetRange( &rng,
0,
sqrt(kalman->process_noise_cov->data.fl[0]),
0 );
cvRand( &rng, process_noise );
cvMatMulAdd( kalman->transition_matrix,
state,
process_noise,
state );
cvShowImage( "Kalman", img );
code = cvWaitKey( 100 );
if( code > 0 ) /* break current simulation by pressing a key */
break;
}
if( code == 27 ) /* exit by ESCAPE */
break;
}
return 0;
}
