Commit 401732a9 authored by Tadej Borovšak's avatar Tadej Borovšak
Browse files

Make kalman user friendly

parent 34e0f90e
CXX := g++
CXXFLAGS := $(shell pkg-config --cflags opencv) -Wall -Wextra -ggdb
LDFLAGS := $(shell pkg-config --libs opencv) -Wall -Wextra -ggdb
CXXFLAGS := $(shell pkg-config --cflags "opencv >= 3") -Wall -Wextra -ggdb
LDFLAGS := $(shell pkg-config --libs "opencv >= 3") -Wall -Wextra -ggdb
APPS := \
kalman \
......@@ -9,7 +9,6 @@ APPS := \
all: $(APPS)
.PHONY: clean
clean:
rm -f *.o $(APPS)
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <stdio.h>
#include <math.h>
using namespace std;
......@@ -13,11 +12,8 @@ using namespace cv;
#define PATH_LEN 50
typedef enum
{
PATH_DEFAULT
}
PathType;
/* Path generator function type */
typedef Point (* PathGenFunc) (float);
typedef struct
{
......@@ -26,6 +22,7 @@ typedef struct
Point point;
Mat img;
KalmanFilter kf;
PathGenFunc path_gen;
}
Data;
......@@ -47,17 +44,32 @@ m2p (Mat const &m)
}
static Point
gen_point (float pos)
gen_point_spiral (float pos)
{
Point off (WIN_SIZE / 2, WIN_SIZE / 2);
return off + Point ((int)(cos (pos * 5 * M_PI) * WIN_SIZE / 2 * pos),
(int)(sin (pos * 5 * M_PI) * WIN_SIZE / 2 * pos));
}
static Point
gen_point_line_c (float pos)
{
Point off (WIN_SIZE / 10, WIN_SIZE / 10);
return off + Point (4 * WIN_SIZE * pos, 4 * WIN_SIZE / 2 * pos);
}
static Point
gen_point_line_a (float pos)
{
Point off (WIN_SIZE / 10, WIN_SIZE / 10);
return off + Point (2 * WIN_SIZE * pos, 6 * WIN_SIZE * pos * pos);
}
/* Various settings for Kalman */
static void
init_kalman_rw (KalmanFilter &kf,
Point const &start)
Point const &start,
float qc)
{
kf.init (4, 2, 0);
......@@ -66,7 +78,7 @@ init_kalman_rw (KalmanFilter &kf,
1.000, 0.000, 0.000, 0.000,
0.000, 1.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000);
0.000, 0.000, 0.000, 0.000) * qc;
setIdentity (kf.measurementMatrix);
setIdentity (kf.measurementNoiseCov, Scalar::all (10));
......@@ -76,7 +88,8 @@ init_kalman_rw (KalmanFilter &kf,
static void
init_kalman_ncv (KalmanFilter &kf,
Point const &start)
Point const &start,
float qc)
{
kf.init (4, 2, 0);
......@@ -89,7 +102,7 @@ init_kalman_ncv (KalmanFilter &kf,
0.333, 0.000, 0.500, 0.000,
0.000, 0.333, 0.000, 0.500,
0.500, 0.000, 1.000, 0.000,
0.000, 0.500, 0.000, 1.000);
0.000, 0.500, 0.000, 1.000) * qc;
setIdentity (kf.measurementMatrix);
setIdentity (kf.measurementNoiseCov, Scalar::all (10));
......@@ -99,7 +112,8 @@ init_kalman_ncv (KalmanFilter &kf,
static void
init_kalman_nca (KalmanFilter &kf,
Point const &start)
Point const &start,
float qc)
{
kf.init (6, 2, 0);
......@@ -116,7 +130,7 @@ init_kalman_nca (KalmanFilter &kf,
0.125, 0.000, 0.333, 0.000, 0.500, 0.000,
0.000, 0.125, 0.000, 0.333, 0.000, 0.500,
0.167, 0.000, 0.500, 0.000, 1.000, 0.000,
0.000, 0.167, 0.000, 0.500, 0.000, 1.000) * 10;
0.000, 0.167, 0.000, 0.500, 0.000, 1.000) * qc;
setIdentity (kf.measurementMatrix);
setIdentity (kf.measurementNoiseCov, Scalar::all (10));
......@@ -161,12 +175,14 @@ mouse_cb (int e,
/* Generate next path point */
data->pos += data->step;
p = gen_point (data->pos);
p = data->path_gen (data->pos);
draw_cross (data->img, p, GREEN);
/* Sync display */
imshow (WIN, data->img);
}
else if (EVENT_RBUTTONUP == e)
imwrite ("sample.png", data->img);
}
static void
......@@ -174,27 +190,79 @@ init_window (Data *data)
{
namedWindow (WIN);
setMouseCallback (WIN, mouse_cb, (void *)data);
draw_cross (data->img, gen_point (0 ), Scalar (0, 255, 0));
draw_cross (data->img, gen_point (data->step), Scalar (0, 255, 0));
draw_cross (data->img, data->path_gen (0 ), Scalar (0, 255, 0));
draw_cross (data->img, data->path_gen (data->step), Scalar (0, 255, 0));
imshow (WIN, data->img);
}
static void
usage (char const *name)
{
printf ("USAGE:\n"
" %s model qc path\n"
"\n"
"Available models: RW - random walk\n"
" NCV - nearly constant velocity\n"
" NCA - nearly constant accelleration\n"
"\n"
"Available paths: spiral - default provided path\n"
" const_line - move with constant speed on line\n"
" acc_line - move with constant accel on line\n"
"\n"
"Example call:\n"
" %s NCV 10 spiral\n",
name, name);
}
int
main (int ,
char ** )
main (int argc,
char **argv)
{
Data data = {
.025, .025, Point (400, 400),
Mat (800, 800, CV_8UC3, Scalar (255, 255, 255)),
Mat (WIN_SIZE, WIN_SIZE, CV_8UC3, Scalar (255, 255, 255)),
KalmanFilter ()
};
init_window (&data);
//init_kalman_rw (data.kf, gen_point (0));
//init_kalman_ncv (data.kf, gen_point (0));
init_kalman_nca (data.kf, gen_point (0));
if (argc < 4)
{
usage (argv[0]);
return 1;
}
/* Parse line */
if (strcmp ("spiral", argv[3]) == 0)
data.path_gen = gen_point_spiral;
else if (strcmp ("const_line", argv[3]) == 0)
data.path_gen = gen_point_line_c;
else if (strcmp ("acc_line", argv[3]) == 0)
data.path_gen = gen_point_line_a;
else
{
printf ("Invalid path: %s\n\n", argv[3]);
usage (argv[0]);
return 1;
}
/* Parse spectral density */
float qc = strtod (argv[2], NULL);
/* Parse model */
if (strcmp ("RW", argv[1]) == 0)
init_kalman_rw (data.kf, data.path_gen (0), qc);
else if (strcmp ("NCV", argv[1]) == 0)
init_kalman_ncv (data.kf, data.path_gen (0), qc);
else if (strcmp ("NCA", argv[1]) == 0)
init_kalman_nca (data.kf, data.path_gen (0), qc);
else
{
printf ("Invalid model: %s\n\n", argv[1]);
usage (argv[0]);
return 1;
}
/* Wait for ESC key */
/* Start the program and wait for ESC key */
init_window (&data);
while ((char)waitKey () != 27)
;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment