This is an example of how to use various filters: FilterAverage, FilterMedian, FilterRunningAverage, FilterDoubleExponentialSmoothing and Kalman.
#include "cv.h" #include "highgui.h" #include <iostream> #include <cstdlib> #include <string> #include "Filter.h" #include "Kalman.h" #include "Platform.h" #include "AlvarException.h" using namespace alvar; using namespace std; const int res=320; void filter_none(double x, double y, double *fx, double *fy) { *fx = x; *fy = y; } void filter_average(double x, double y, double *fx, double *fy) { static FilterAverage ax(30); static FilterAverage ay(30); *fx = ax.next(x); *fy = ay.next(y); } void filter_median(double x, double y, double *fx, double *fy) { static FilterMedian ax(30); static FilterMedian ay(30); *fx = ax.next(x); *fy = ay.next(y); } void filter_running_average(double x, double y, double *fx, double *fy) { static FilterRunningAverage ax(0.03); static FilterRunningAverage ay(0.03); *fx = ax.next(x); *fy = ay.next(y); } void filter_des(double x, double y, double *fx, double *fy) { static FilterDoubleExponentialSmoothing ax(0.03,0.01); static FilterDoubleExponentialSmoothing ay(0.03,0.01); *fx = ax.next(x); *fy = ay.next(y); } void filter_kalman(double x, double y, double *fx, double *fy) { static bool init=true; static KalmanSensor sensor(4,2); static Kalman kalman(4); // x, y, dx, dy if (init) { init = false; // H cvZero(sensor.H); cvmSet(sensor.H, 0, 0, 1); cvmSet(sensor.H, 1, 1, 1); // R cvSetIdentity(sensor.R, cvScalar(10)); // F cvSetIdentity(kalman.F); cvmSet(kalman.F, 0, 2, 1); cvmSet(kalman.F, 1, 3, 1); // Q cvmSet(kalman.Q, 0, 0, 0.0001); cvmSet(kalman.Q, 1, 1, 0.0001); cvmSet(kalman.Q, 2, 2, 0.000001); cvmSet(kalman.Q, 3, 3, 0.000001); // P cvSetIdentity(kalman.P, cvScalar(100)); } cvmSet(sensor.z, 0, 0, x); cvmSet(sensor.z, 1, 0, y); kalman.predict_update(&sensor, (unsigned long)(cv::getTickCount() / cv::getTickFrequency() * 1000)); *fx = cvmGet(kalman.x, 0, 0); *fy = cvmGet(kalman.x, 1, 0); } void filter_array_average(double x, double y, double *fx, double *fy) { static bool init=true; static FilterArray<FilterAverage> fa(2); if (init) { init=false; for (int i=0; i<2; i++) { fa[i].setWindowSize(30); } } *fx = fa[0].next(x); *fy = fa[1].next(y); } class KalmanSensorOwn : public KalmanSensorEkf { virtual void h(CvMat *x_pred, CvMat *_z_pred) { double x = cvmGet(x_pred, 0, 0); double y = cvmGet(x_pred, 1, 0); double dx = cvmGet(x_pred, 2, 0); double dy = cvmGet(x_pred, 3, 0); cvmSet(_z_pred, 0, 0, x); cvmSet(_z_pred, 1, 0, y); } public: KalmanSensorOwn(int _n, int _m) : KalmanSensorEkf(_n, _m) {} }; class KalmanOwn : public KalmanEkf { virtual void f(CvMat *_x, CvMat *_x_pred, double dt) { double x = cvmGet(_x, 0, 0); double y = cvmGet(_x, 1, 0); double dx = cvmGet(_x, 2, 0); double dy = cvmGet(_x, 3, 0); cvmSet(_x_pred, 0, 0, x + dt*dx); cvmSet(_x_pred, 1, 0, y + dt*dy); cvmSet(_x_pred, 2, 0, dx); cvmSet(_x_pred, 3, 0, dy); } public: KalmanOwn(int _n) : KalmanEkf(_n) {} }; void filter_ekf(double x, double y, double *fx, double *fy) { static bool init=true; static KalmanSensorOwn sensor(4,2); static KalmanOwn kalman(4); // x, y, dx, dy if (init) { init = false; // R cvSetIdentity(sensor.R, cvScalar(100)); // Q cvmSet(kalman.Q, 0, 0, 0.001); cvmSet(kalman.Q, 1, 1, 0.001); cvmSet(kalman.Q, 2, 2, 0.01); cvmSet(kalman.Q, 3, 3, 0.01); // P cvSetIdentity(kalman.P, cvScalar(100)); } cvmSet(sensor.z, 0, 0, x); cvmSet(sensor.z, 1, 0, y); kalman.predict_update(&sensor, (unsigned long)(cv::getTickCount() / cv::getTickFrequency() * 1000)); *fx = cvmGet(kalman.x, 0, 0); *fy = cvmGet(kalman.x, 1, 0); } //Make list of filters const int nof_filters = 8; void (*(filters[nof_filters]))(double x, double y, double *fx, double *fy) = { filter_none, filter_average, filter_median, filter_running_average, filter_des, filter_kalman, filter_ekf, filter_array_average, }; char filter_names[nof_filters][64]={ "No filter - Press any key to change", "Average", "Median", "Running Average", "Double Exponential Smoothing", "Kalman", "Extended Kalman", "Array (average)" }; // Just generate some random data that can be used as sensor input void get_measurement(double *x, double *y) { static double xx=0; static double yy=0; static double dxx = 0.3; static double dyy = 0.7; xx += dxx; yy += dyy; if ((xx > res) || (xx < 0)) dxx = -dxx; if ((yy > res) || (yy < 0)) dyy = -dyy; double rx = (rand()*20.0/RAND_MAX)-10.0; double ry = (rand()*20.0/RAND_MAX)-10.0; // Add some outliers if(fabs(rx*ry)>50) { rx *= 5; ry *= 5; } *x = xx + rx; *y = yy + ry; } int main(int argc, char *argv[]) { try { // Output usage message std::string filename(argv[0]); filename = filename.substr(filename.find_last_of('\\') + 1); std::cout << "SampleFilter" << std::endl; std::cout << "============" << std::endl; std::cout << std::endl; std::cout << "Description:" << std::endl; std::cout << " This is an example of how to use the 'FilterAverage', 'FilterMedian'," << std::endl; std::cout << " 'FilterRunningAverage', 'FilterDoubleExponentialSmoothing', 'Kalman'" << std::endl; std::cout << " 'KalmanEkf' and 'FilterArray' filtering classes. First the example" << std::endl; std::cout << " shows unfiltered test data with outliers. The data is then filtered" << std::endl; std::cout << " using the various filters. Press any key to cycle through the filters." << std::endl; std::cout << std::endl; std::cout << "Usage:" << std::endl; std::cout << " " << filename << std::endl; std::cout << std::endl; std::cout << "Keyboard Shortcuts:" << std::endl; std::cout << " any key: cycle through filters" << std::endl; std::cout << " q: quit" << std::endl; std::cout << std::endl; // Processing loop IplImage *img = cvCreateImage(cvSize(res, res), IPL_DEPTH_8U, 3); cvNamedWindow("SampleFilter"); CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0); for (int ii = 0; ii < nof_filters; ii++) { int key = 0; double x, y; double fx, fy; vector<CvPoint> tail; while (1) { get_measurement(&x, &y); filters[ii](x, y, &fx, &fy); cvZero(img); cvPutText(img, filter_names[ii], cvPoint(3, res - 10), &font, CV_RGB(255, 255, 255)); cvCircle(img, cvPoint(int(x), int(y)), 2, CV_RGB(0, 255, 255)); cvCircle(img, cvPoint(int(x), int(y)), 3, CV_RGB(255, 255, 255)); CvPoint fp; fp.x = int(fx); fp.y = int(fy); tail.push_back(fp); for (size_t iii = 0; iii < tail.size(); iii++) { cvCircle(img, tail[iii], 0, CV_RGB(255, 255, 0)); } cvCircle(img, fp, 2, CV_RGB(255, 0, 255)); cvShowImage("SampleFilter", img); key = cvWaitKey(10); if (key != -1) { break; } } if (key == 'q') { break; } } cvReleaseImage(&img); return 0; } catch (const std::exception &e) { std::cout << "Exception: " << e.what() << endl; } catch (...) { std::cout << "Exception: unknown" << std::endl; } }