Commit 34e0f90e authored by Tadej Borovšak's avatar Tadej Borovšak
Browse files

finish

parent 8534cd60
......@@ -4,7 +4,8 @@ LDFLAGS := $(shell pkg-config --libs opencv) -Wall -Wextra -ggdb
APPS := \
kalman \
track
track \
track-model
all: $(APPS)
......
#include <opencv2/video/tracking.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
......@@ -29,6 +30,87 @@ static Scalar RED (0 , 0 , 255);
static Scalar GREEN (0 , 255, 0 );
static Scalar BLUE (255, 0 , 0 );
static inline Mat
p2m (Point const &p)
{
return (Mat_<float> (2, 1) << p.x, p.y);
}
static inline Point
m2p (Mat const &m)
{
return Point (m.at<float> (0), m.at<float> (1));
}
static void
init_kalman_rw (KalmanFilter &kf,
Point const &start)
{
kf.init (4, 2, 0);
setIdentity (kf.transitionMatrix);
kf.processNoiseCov = (Mat_<float> (4, 4) <<
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);
setIdentity (kf.measurementMatrix);
setIdentity (kf.measurementNoiseCov, Scalar::all (10));
setIdentity (kf.errorCovPost, Scalar::all (1));
kf.statePost = (Mat_<float> (4, 1) << start.x, start.y, 0, 0);
}
static void
init_kalman_ncv (KalmanFilter &kf,
Point const &start)
{
kf.init (4, 2, 0);
kf.transitionMatrix = (Mat_<float> (4, 4) <<
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1);
kf.processNoiseCov = (Mat_<float> (4, 4) <<
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) * 5;
setIdentity (kf.measurementMatrix);
setIdentity (kf.measurementNoiseCov, Scalar::all (10));
setIdentity (kf.errorCovPost, Scalar::all (1));
kf.statePost = (Mat_<float> (4, 1) << start.x, start.y, 0, 0);
}
static void
init_kalman_nca (KalmanFilter &kf,
Point const &start)
{
kf.init (6, 2, 0);
kf.transitionMatrix = (Mat_<float> (6, 6) <<
1.0, 0.0, 1.0, 0.0, 0.5, 0.0,
0.0, 1.0, 0.0, 1.0, 0.0, 0.5,
0.0, 0.0, 1.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 1.0,
0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
kf.processNoiseCov = (Mat_<float> (6, 6) <<
0.050, 0.000, 0.125, 0.000, 0.167, 0.000,
0.000, 0.050, 0.000, 0.125, 0.000, 0.167,
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;
setIdentity (kf.measurementMatrix);
setIdentity (kf.measurementNoiseCov, Scalar::all (10));
setIdentity (kf.errorCovPost, Scalar::all (1));
kf.statePost = (Mat_<float> (6, 1) << start.x, start.y, 0, 0, 0, 0);
}
static inline void
draw_cross (Mat &img,
Point const &point,
......@@ -140,6 +222,25 @@ detect_object (Mat const &stats,
return candidates[best];
}
static Point
detect_in_image (Mat const &img,
Mat const &avg,
Point const &prev)
{
/* Process image */
Mat diff = cv::abs (avg - img);
cvtColor (diff, diff, CV_BGR2GRAY);
threshold (diff, diff, 0.3, 1.0, THRESH_BINARY);
diff.convertTo (diff, CV_8U, 255);
morphologyEx (diff, diff, MORPH_CLOSE,
getStructuringElement (MORPH_RECT, Size (5, 5)));
/* Extract information and do detection */
Mat labels, stats, centroids;
connectedComponentsWithStats (diff, labels, stats, centroids);
return detect_object (stats, centroids, prev);
}
int
main (int ,
char **argv)
......@@ -150,29 +251,50 @@ main (int ,
imshow (WIN, avg);
waitKey ();
Point p (-1, -1);
for (unsigned int i = 0; i < seq.size (); i++)
KalmanFilter kf;
/* First loop runs until Kalman filter is not initialized */
Point detected (-1, -1);
unsigned int i = 0;
for (; i < seq.size (); i++)
{
/* Detection part */
Mat img = load_image (seq[i]);
Mat diff = cv::abs (avg - img);
cvtColor (diff, diff, CV_BGR2GRAY);
threshold (diff, diff, 0.3, 1.0, THRESH_BINARY);
diff.convertTo (diff, CV_8U, 255);
morphologyEx (diff, diff, MORPH_CLOSE,
getStructuringElement (MORPH_RECT, Size (5, 5)));
detected = detect_in_image (img, avg, detected);
if (detected.x < 0)
continue;
Mat labels, stats, centroids;
connectedComponentsWithStats (diff, labels, stats, centroids);
Point detected = detect_object (stats, centroids, p);
init_kalman_rw (kf, detected);
//init_kalman_ncv (kf, detected);
//init_kalman_nca (kf, detected);
break;
}
cvtColor (diff, diff, CV_GRAY2BGR);
/* Second loop processes remaining images using Kalman */
Mat predicted, estimated;
for (; i < seq.size (); i++)
{
/* Detection part */
Mat img = load_image (seq[i]);
predicted = kf.predict ();
detected = detect_in_image (img, avg, detected);
/* Display pred + det */
img.convertTo (img, CV_8UC3, 255);
draw_cross (img, m2p (predicted), BLUE);
if (detected.x >= 0)
draw_cross (img, detected, RED);
/* If detection fails, use prediction */
if (detected.x < 0)
{
p = detected;
draw_cross (diff, p, RED);
detected = m2p (predicted);
cout << "NO detection" << endl;
}
imshow (WIN, diff);
/* If we have detection, use it, else use prediction */
estimated = kf.correct (p2m (detected));
draw_cross (img, m2p (estimated), GREEN);
imshow (WIN, img);
waitKey ();
}
......
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