#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"
" Point moves in a circle and is characterized by a 1D state.\n"
" state_k+1 = state_k + speed + process_noise N(0, 1e-5)\n"
" The speed is constant.\n"
" Both state and measurements vectors are 1D (a point angle),\n"
" Measurement is the real state + gaussian noise N(0, 1e-1).\n"
" The real and the measured points are connected with red line segment,\n"
" the real and the estimated points are connected with yellow line segment,\n"
" the real and the corrected estimated points are connected with green line segment.\n"
" (if Kalman filter works correctly,\n"
" the yellow segment should be shorter than the red one and\n"
" the green segment should be shorter than the yellow one)."
"\n"
" Pressing any key (except ESC) will reset the tracking.\n"
" Pressing ESC will stop the program.\n"
);
}
int main(int, char**)
{
help();
char code = (char)-1;
for(;;)
{
state.
at<
float>(0) = 0.0f;
state.
at<
float>(1) = 2.f * (
float)
CV_PI / 6;
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);
Point improvedPt = calcPoint(center, R, improvedAngle);
img = img * 0.2;
if( code > 0 )
break;
}
if( code == 27 || code == 'q' || code == 'Q' )
break;
}
return 0;
}