SampleFilter.cpp

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;
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);
}
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) = {
};
"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;
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;
}
}

ar_track_alvar
Author(s): Scott Niekum
autogenerated on Mon Jun 10 2019 12:47:03