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 ...@@ -4,7 +4,8 @@ LDFLAGS := $(shell pkg-config --libs opencv) -Wall -Wextra -ggdb
APPS := \ APPS := \
kalman \ kalman \
track track \
track-model
all: $(APPS) all: $(APPS)
......
#include <opencv2/video/tracking.hpp>
#include <opencv2/imgproc/imgproc.hpp> #include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp> #include <opencv2/highgui/highgui.hpp>
...@@ -29,6 +30,87 @@ static Scalar RED (0 , 0 , 255); ...@@ -29,6 +30,87 @@ static Scalar RED (0 , 0 , 255);
static Scalar GREEN (0 , 255, 0 ); static Scalar GREEN (0 , 255, 0 );
static Scalar BLUE (255, 0 , 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 static inline void
draw_cross (Mat &img, draw_cross (Mat &img,
Point const &point, Point const &point,
...@@ -140,6 +222,25 @@ detect_object (Mat const &stats, ...@@ -140,6 +222,25 @@ detect_object (Mat const &stats,
return candidates[best]; 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 int
main (int , main (int ,
char **argv) char **argv)
...@@ -150,29 +251,50 @@ main (int , ...@@ -150,29 +251,50 @@ main (int ,
imshow (WIN, avg); imshow (WIN, avg);
waitKey (); waitKey ();
Point p (-1, -1); KalmanFilter kf;
for (unsigned int i = 0; i < seq.size (); i++)
/* 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]);
detected = detect_in_image (img, avg, detected);
if (detected.x < 0)
continue;
init_kalman_rw (kf, detected);
//init_kalman_ncv (kf, detected);
//init_kalman_nca (kf, detected);
break;
}
/* Second loop processes remaining images using Kalman */
Mat predicted, estimated;
for (; i < seq.size (); i++)
{ {
/* Detection part */
Mat img = load_image (seq[i]); Mat img = load_image (seq[i]);
Mat diff = cv::abs (avg - img); predicted = kf.predict ();
cvtColor (diff, diff, CV_BGR2GRAY); detected = detect_in_image (img, avg, detected);
threshold (diff, diff, 0.3, 1.0, THRESH_BINARY); /* Display pred + det */
diff.convertTo (diff, CV_8U, 255); img.convertTo (img, CV_8UC3, 255);
morphologyEx (diff, diff, MORPH_CLOSE, draw_cross (img, m2p (predicted), BLUE);
getStructuringElement (MORPH_RECT, Size (5, 5)));
Mat labels, stats, centroids;
connectedComponentsWithStats (diff, labels, stats, centroids);
Point detected = detect_object (stats, centroids, p);
cvtColor (diff, diff, CV_GRAY2BGR);
if (detected.x >= 0) if (detected.x >= 0)
draw_cross (img, detected, RED);
/* If detection fails, use prediction */
if (detected.x < 0)
{ {
p = detected; detected = m2p (predicted);
draw_cross (diff, p, RED); 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 (); 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