00001 #include "cv.h"
00002 #include "highgui.h"
00003 #include <iostream>
00004 #include <cstdlib>
00005 #include <string>
00006 #include "Filter.h"
00007 #include "Kalman.h"
00008 #include "Platform.h"
00009 #include "AlvarException.h"
00010 using namespace alvar;
00011 using namespace std;
00012
00013 const int res=320;
00014
00015 void filter_none(double x, double y, double *fx, double *fy) {
00016 *fx = x; *fy = y;
00017 }
00018
00019 void filter_average(double x, double y, double *fx, double *fy) {
00020 static FilterAverage ax(30);
00021 static FilterAverage ay(30);
00022 *fx = ax.next(x);
00023 *fy = ay.next(y);
00024 }
00025
00026 void filter_median(double x, double y, double *fx, double *fy) {
00027 static FilterMedian ax(30);
00028 static FilterMedian ay(30);
00029 *fx = ax.next(x);
00030 *fy = ay.next(y);
00031 }
00032
00033 void filter_running_average(double x, double y, double *fx, double *fy) {
00034 static FilterRunningAverage ax(0.03);
00035 static FilterRunningAverage ay(0.03);
00036 *fx = ax.next(x);
00037 *fy = ay.next(y);
00038 }
00039
00040 void filter_des(double x, double y, double *fx, double *fy) {
00041 static FilterDoubleExponentialSmoothing ax(0.03,0.01);
00042 static FilterDoubleExponentialSmoothing ay(0.03,0.01);
00043 *fx = ax.next(x);
00044 *fy = ay.next(y);
00045 }
00046
00047 void filter_kalman(double x, double y, double *fx, double *fy) {
00048 static bool init=true;
00049 static KalmanSensor sensor(4,2);
00050 static Kalman kalman(4);
00051 if (init) {
00052 init = false;
00053
00054 cvZero(sensor.H);
00055 cvmSet(sensor.H, 0, 0, 1);
00056 cvmSet(sensor.H, 1, 1, 1);
00057
00058 cvSetIdentity(sensor.R, cvScalar(10));
00059
00060 cvSetIdentity(kalman.F);
00061 cvmSet(kalman.F, 0, 2, 1);
00062 cvmSet(kalman.F, 1, 3, 1);
00063
00064 cvmSet(kalman.Q, 0, 0, 0.0001);
00065 cvmSet(kalman.Q, 1, 1, 0.0001);
00066 cvmSet(kalman.Q, 2, 2, 0.000001);
00067 cvmSet(kalman.Q, 3, 3, 0.000001);
00068
00069 cvSetIdentity(kalman.P, cvScalar(100));
00070 }
00071 cvmSet(sensor.z, 0, 0, x);
00072 cvmSet(sensor.z, 1, 0, y);
00073 kalman.predict_update(&sensor, (unsigned long)(cv::getTickCount() / cv::getTickFrequency() * 1000));
00074 *fx = cvmGet(kalman.x, 0, 0);
00075 *fy = cvmGet(kalman.x, 1, 0);
00076 }
00077
00078 void filter_array_average(double x, double y, double *fx, double *fy) {
00079 static bool init=true;
00080 static FilterArray<FilterAverage> fa(2);
00081 if (init) {
00082 init=false;
00083 for (int i=0; i<2; i++) {
00084 fa[i].setWindowSize(30);
00085 }
00086 }
00087 *fx = fa[0].next(x);
00088 *fy = fa[1].next(y);
00089 }
00090
00091 class KalmanSensorOwn : public KalmanSensorEkf {
00092 virtual void h(CvMat *x_pred, CvMat *_z_pred) {
00093 double x = cvmGet(x_pred, 0, 0);
00094 double y = cvmGet(x_pred, 1, 0);
00095 double dx = cvmGet(x_pred, 2, 0);
00096 double dy = cvmGet(x_pred, 3, 0);
00097 cvmSet(_z_pred, 0, 0, x);
00098 cvmSet(_z_pred, 1, 0, y);
00099 }
00100 public:
00101 KalmanSensorOwn(int _n, int _m) : KalmanSensorEkf(_n, _m) {}
00102 };
00103
00104 class KalmanOwn : public KalmanEkf {
00105 virtual void f(CvMat *_x, CvMat *_x_pred, double dt) {
00106 double x = cvmGet(_x, 0, 0);
00107 double y = cvmGet(_x, 1, 0);
00108 double dx = cvmGet(_x, 2, 0);
00109 double dy = cvmGet(_x, 3, 0);
00110 cvmSet(_x_pred, 0, 0, x + dt*dx);
00111 cvmSet(_x_pred, 1, 0, y + dt*dy);
00112 cvmSet(_x_pred, 2, 0, dx);
00113 cvmSet(_x_pred, 3, 0, dy);
00114 }
00115 public:
00116 KalmanOwn(int _n) : KalmanEkf(_n) {}
00117 };
00118
00119 void filter_ekf(double x, double y, double *fx, double *fy) {
00120 static bool init=true;
00121 static KalmanSensorOwn sensor(4,2);
00122 static KalmanOwn kalman(4);
00123 if (init) {
00124 init = false;
00125
00126 cvSetIdentity(sensor.R, cvScalar(100));
00127
00128 cvmSet(kalman.Q, 0, 0, 0.001);
00129 cvmSet(kalman.Q, 1, 1, 0.001);
00130 cvmSet(kalman.Q, 2, 2, 0.01);
00131 cvmSet(kalman.Q, 3, 3, 0.01);
00132
00133 cvSetIdentity(kalman.P, cvScalar(100));
00134 }
00135 cvmSet(sensor.z, 0, 0, x);
00136 cvmSet(sensor.z, 1, 0, y);
00137 kalman.predict_update(&sensor, (unsigned long)(cv::getTickCount() / cv::getTickFrequency() * 1000));
00138 *fx = cvmGet(kalman.x, 0, 0);
00139 *fy = cvmGet(kalman.x, 1, 0);
00140 }
00141
00142
00143 const int nof_filters = 8;
00144 void (*(filters[nof_filters]))(double x, double y, double *fx, double *fy) = {
00145 filter_none,
00146 filter_average,
00147 filter_median,
00148 filter_running_average,
00149 filter_des,
00150 filter_kalman,
00151 filter_ekf,
00152 filter_array_average,
00153 };
00154 char filter_names[nof_filters][64]={
00155 "No filter - Press any key to change",
00156 "Average",
00157 "Median",
00158 "Running Average",
00159 "Double Exponential Smoothing",
00160 "Kalman",
00161 "Extended Kalman",
00162 "Array (average)"
00163 };
00164
00165
00166 void get_measurement(double *x, double *y) {
00167 static double xx=0;
00168 static double yy=0;
00169 static double dxx = 0.3;
00170 static double dyy = 0.7;
00171 xx += dxx; yy += dyy;
00172 if ((xx > res) || (xx < 0)) dxx = -dxx;
00173 if ((yy > res) || (yy < 0)) dyy = -dyy;
00174 double rx = (rand()*20.0/RAND_MAX)-10.0;
00175 double ry = (rand()*20.0/RAND_MAX)-10.0;
00176
00177
00178 if(fabs(rx*ry)>50)
00179 {
00180 rx *= 5;
00181 ry *= 5;
00182 }
00183
00184 *x = xx + rx; *y = yy + ry;
00185 }
00186
00187 int main(int argc, char *argv[])
00188 {
00189 try {
00190
00191 std::string filename(argv[0]);
00192 filename = filename.substr(filename.find_last_of('\\') + 1);
00193 std::cout << "SampleFilter" << std::endl;
00194 std::cout << "============" << std::endl;
00195 std::cout << std::endl;
00196 std::cout << "Description:" << std::endl;
00197 std::cout << " This is an example of how to use the 'FilterAverage', 'FilterMedian'," << std::endl;
00198 std::cout << " 'FilterRunningAverage', 'FilterDoubleExponentialSmoothing', 'Kalman'" << std::endl;
00199 std::cout << " 'KalmanEkf' and 'FilterArray' filtering classes. First the example" << std::endl;
00200 std::cout << " shows unfiltered test data with outliers. The data is then filtered" << std::endl;
00201 std::cout << " using the various filters. Press any key to cycle through the filters." << std::endl;
00202 std::cout << std::endl;
00203 std::cout << "Usage:" << std::endl;
00204 std::cout << " " << filename << std::endl;
00205 std::cout << std::endl;
00206 std::cout << "Keyboard Shortcuts:" << std::endl;
00207 std::cout << " any key: cycle through filters" << std::endl;
00208 std::cout << " q: quit" << std::endl;
00209 std::cout << std::endl;
00210
00211
00212 IplImage *img = cvCreateImage(cvSize(res, res), IPL_DEPTH_8U, 3);
00213 cvNamedWindow("SampleFilter");
00214 CvFont font;
00215 cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0);
00216 for (int ii = 0; ii < nof_filters; ii++) {
00217 int key = 0;
00218 double x, y;
00219 double fx, fy;
00220 vector<CvPoint> tail;
00221 while (1) {
00222 get_measurement(&x, &y);
00223 filters[ii](x, y, &fx, &fy);
00224 cvZero(img);
00225 cvPutText(img, filter_names[ii], cvPoint(3, res - 10), &font, CV_RGB(255, 255, 255));
00226 cvCircle(img, cvPoint(int(x), int(y)), 2, CV_RGB(0, 255, 255));
00227 cvCircle(img, cvPoint(int(x), int(y)), 3, CV_RGB(255, 255, 255));
00228 CvPoint fp;
00229 fp.x = int(fx);
00230 fp.y = int(fy);
00231 tail.push_back(fp);
00232 for (size_t iii = 0; iii < tail.size(); iii++) {
00233 cvCircle(img, tail[iii], 0, CV_RGB(255, 255, 0));
00234 }
00235 cvCircle(img, fp, 2, CV_RGB(255, 0, 255));
00236 cvShowImage("SampleFilter", img);
00237 key = cvWaitKey(10);
00238 if (key != -1) {
00239 break;
00240 }
00241 }
00242 if (key == 'q') {
00243 break;
00244 }
00245 }
00246 cvReleaseImage(&img);
00247 return 0;
00248 }
00249 catch (const std::exception &e) {
00250 std::cout << "Exception: " << e.what() << endl;
00251 }
00252 catch (...) {
00253 std::cout << "Exception: unknown" << std::endl;
00254 }
00255 }