#include <stdio.h>
static inline Point calcPoint(
Point2f center,
double R,
double angle)
{
return center +
Point2f((
float)
cos(angle), (
float)-
sin(angle))*(float)R;
}
static void help()
{
printf( "\nExample of c calls to OpenCV's Kalman filter.\n"
" Tracking of rotating point.\n"
" Rotation speed is constant.\n"
" Both state and measurements vectors are 1D (a point angle),\n"
" Measurement is the real point angle + gaussian noise.\n"
" The real and the estimated points are connected with yellow line segment,\n"
" the real and the measured points are connected with red line segment.\n"
" (if Kalman filter works correctly,\n"
" the yellow segment should be shorter than the red one).\n"
"\n"
" Pressing any key (except ESC) will reset the tracking with a different speed.\n"
" Pressing ESC will stop the program.\n"
);
}
int main(int, char**)
{
help();
char code = (char)-1;
for(;;)
{
for(;;)
{
double stateAngle = state.
at<
float>(0);
Point statePt = calcPoint(center, R, stateAngle);
double predictAngle = prediction.
at<
float>(0);
Point predictPt = calcPoint(center, R, predictAngle);
double measAngle = measurement.
at<
float>(0);
Point measPt = calcPoint(center, R, measAngle);
#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 )
drawCross( statePt,
Scalar(255,255,255), 3 );
drawCross( measPt,
Scalar(0,0,255), 3 );
drawCross( predictPt,
Scalar(0,255,0), 3 );
if(
theRNG().uniform(0,4) != 0)
if( code > 0 )
break;
}
if( code == 27 || code == 'q' || code == 'Q' )
break;
}
return 0;
}