#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;
}