From f45dd535a05536782e8fae9dcc2832729309a544 Mon Sep 17 00:00:00 2001
From: neingeist <neingeist@bl0rg.net>
Date: Sat, 28 Jan 2017 02:57:31 +0100
Subject: [PATCH] Add Kalman mouse example

---
 CMakeLists.txt  |  3 ++
 KalmanMouse.cpp | 82 +++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 85 insertions(+)
 create mode 100644 KalmanMouse.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 52af387..226a2c6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,3 +10,6 @@ target_link_libraries( DisplayImage ${OpenCV_LIBS} )
 
 add_executable( SharpenImage SharpenImage.cpp )
 target_link_libraries( SharpenImage ${OpenCV_LIBS} )
+
+add_executable( KalmanMouse KalmanMouse.cpp )
+target_link_libraries( KalmanMouse ${OpenCV_LIBS} )
diff --git a/KalmanMouse.cpp b/KalmanMouse.cpp
new file mode 100644
index 0000000..69f3138
--- /dev/null
+++ b/KalmanMouse.cpp
@@ -0,0 +1,82 @@
+// http://opencvexamples.blogspot.com/2014/01/kalman-filter-implementation-tracking.html
+// (slightly cleaned up and patched to use opencv's gui functions)
+
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/video/tracking.hpp"
+
+#define drawCross( center, color, d )                                 \
+line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
+line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )
+
+using namespace cv;
+using namespace std;
+
+Point mousePos;
+
+void mouseCallback(int event, int x, int y, int flags, void* userdata) {
+     if ( event == EVENT_MOUSEMOVE ) {
+         mousePos.x = x;
+         mousePos.y = y;
+     }
+}
+
+int main() {
+
+    KalmanFilter KF(4, 2, 0);
+
+    // intialization of KF...
+    KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
+    Mat_<float> measurement(2,1);
+
+    KF.statePre.at<float>(0) = mousePos.x;
+    KF.statePre.at<float>(1) = mousePos.y;
+    KF.statePre.at<float>(2) = 0;
+    KF.statePre.at<float>(3) = 0;
+    setIdentity(KF.measurementMatrix);
+    setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
+    setIdentity(KF.measurementNoiseCov, Scalar::all(10));
+    setIdentity(KF.errorCovPost, Scalar::all(.1));
+    // Image to show mouse tracking
+    Mat img(600, 800, CV_8UC3);
+    vector<Point> mousev,kalmanv;
+    mousev.clear();
+    kalmanv.clear();
+
+    namedWindow("mouse kalman", 1);
+    setMouseCallback("mouse kalman", mouseCallback, NULL);
+
+    while(1) {
+        // First predict, to update the internal statePre variable
+        Mat prediction = KF.predict();
+
+
+        // The update phase
+        measurement(0) = mousePos.x;
+        measurement(1) = mousePos.y;
+        Mat estimated = KF.correct(measurement);
+
+
+        // Plot
+        img = Scalar::all(0);
+
+        Point statePt(estimated.at<float>(0),estimated.at<float>(1));
+        Point measPt(measurement(0),measurement(1));
+        drawCross(statePt, Scalar(255,255,255), 5);
+        drawCross(measPt, Scalar(0,0,255), 5);
+
+        mousev.push_back(measPt);
+        kalmanv.push_back(statePt);
+
+        for (int i = 0; i < mousev.size()-1; i++)
+            line(img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);
+
+        for (int i = 0; i < kalmanv.size()-1; i++)
+            line(img, kalmanv[i], kalmanv[i+1], Scalar(0,155,255), 1);
+
+        imshow("mouse kalman", img);
+
+        waitKey(10);
+    }
+
+    return 0;
+}