float traparea (float a[], float cphi, float r)
{
float phi2xy (float phi, float r, float a[], float *x, float *y);
float x[5], y[5], area, delphi;
delphi = DELPHI / 180. * PI;
phi2xy (cphi - delphi/2., (r*(1+ASTEP) + r)/2., a, &x[1], &y[1]);
phi2xy (cphi + delphi/2., (r*(1+ASTEP) + r)/2., a, &x[2], &y[2]);
phi2xy (cphi + delphi/2., (r/(1+ASTEP) + r)/2., a, &x[3], &y[3]);
phi2xy (cphi - delphi/2., (r/(1+ASTEP) + r)/2., a, &x[4], &y[4]);
area = 0.5 * (fabs((x[1]-x[2]) * (y[3]-y[2]) - (y[1]-y[2]) * (x[3]-x[2])) +
fabs((x[1]-x[4]) * (y[3]-y[4]) - (y[1]-y[4]) * (x[3]-x[4])));
return (area);
}
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;
}
float egrid (float (*func)(float a[], float x, float y), float xmin,
float xmax, float ymin, float ymax, float a[])
{
void minmaxphi (float xmin, float xmax, float ymin, float ymax, float a[],
float *minphi, float *maxphi, int quad[]);
void minmaxrad (float xmin, float xmax, float ymin, float ymax, float a[],
float *minr, float *maxr);
float phi2xy (float phi, float r, float a[], float *x, float *y);
void swap (float *min, float *max);
float traparea (float a[], float cphi, float r);
float xf[33], yf[3], minphi, maxphi, minr, maxr, x, y,
theta, r, cphi, flux, dphi, dflux, temp;
int i, done = 0, centpix = 0, quad[5]={0, 0, 0, 0, 0};
a[9] = -(a[9] - PI/2.); /* A kludge, due to the definition of PA in
nuker.c */
if (xmin > xmax)
swap (&xmin, &xmax);
if (ymin > ymax)
swap (&ymin, &ymax);
minmaxphi (xmin, xmax, ymin, ymax, a, &minphi, &maxphi, quad);
minmaxrad (xmin, xmax, ymin, ymax, a, &minr, &maxr);
if ((xmin <= a[1] && xmax >= a[1]) && (ymin <= a[2] && ymax >= a[2]) ||
quad[0]==1) {
minr = 0.;
centpix = 1;
if ((xmin < a[1] && xmax > a[1]) && (ymin < a[2] && ymax > a[2])) {
minphi = 0.;
maxphi = 2. * PI;
};
}
maxr = maxr / a[8]; /* Make sure we integrate out */
minr = minr * a[8]; /* and in far enough. */
if (minr == 0.) centpix = 1; /* If the center is on the boundary */
flux = 0.;
dphi = DELPHI /180. * PI;
for (cphi = minphi; cphi <= maxphi; cphi += dphi) {
r = maxr;
done = 0;
while (!done && r >= minr) {
r = r / (1+ASTEP);
phi2xy (cphi, r, a, &x, &y);
if (x >= xmin && x <= xmax && y >= ymin && y <= ymax) {
dflux = func (a, x, y) * traparea (a, cphi, r);
flux += dflux;
if (centpix && (dflux/flux) < 1.e-5) done = 1;
} else if (r < minr)
done = 1;
};
};
a[9] = (-a[9] + PI/2.); /* Convert back to the regular def. of PA */
return (flux);
}
