Merge pull request #3233 from niebles:master

modules/video/include/opencv2/video/tracking.hpp

@@ -129,16 +129,16 @@ public:
     //! updates the predicted state from the measurement
     CV_WRAP const Mat& correct( const Mat& measurement );
 
-    Mat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
+    CV_PROP_RW Mat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
-    Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
+    CV_PROP_RW Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
-    Mat transitionMatrix;   //!< state transition matrix (A)
+    CV_PROP_RW Mat transitionMatrix;   //!< state transition matrix (A)
-    Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
+    CV_PROP_RW Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
-    Mat measurementMatrix;  //!< measurement matrix (H)
+    CV_PROP_RW Mat measurementMatrix;  //!< measurement matrix (H)
-    Mat processNoiseCov;    //!< process noise covariance matrix (Q)
+    CV_PROP_RW Mat processNoiseCov;    //!< process noise covariance matrix (Q)
-    Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
+    CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
-    Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
+    CV_PROP_RW Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
-    Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
+    CV_PROP_RW Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
-    Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
+    CV_PROP_RW Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
 
     // temporary matrices
     Mat temp1;

samples/python2/kalman.py

@@ -0,0 +1,89 @@
+#!/usr/bin/python
+"""
+   Tracking of rotating point.
+   Rotation speed is constant.
+   Both state and measurements vectors are 1D (a point angle),
+   Measurement is the real point angle + gaussian noise.
+   The real and the estimated points are connected with yellow line segment,
+   the real and the measured points are connected with red line segment.
+   (if Kalman filter works correctly,
+    the yellow segment should be shorter than the red one).
+   Pressing any key (except ESC) will reset the tracking with a different speed.
+   Pressing ESC will stop the program.
+"""
+import cv2
+from math import cos, sin
+import numpy as np
+
+if __name__ == "__main__":
+
+    img_height = 500
+    img_width = 500
+    kalman = cv2.KalmanFilter(2, 1, 0)
+
+    code = -1L
+
+    cv2.namedWindow("Kalman")
+
+    while True:
+        state = 0.1 * np.random.randn(2, 1)
+
+        kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]])
+        kalman.measurementMatrix = 1. * np.ones((1, 2))
+        kalman.processNoiseCov = 1e-5 * np.eye(2)
+        kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1))
+        kalman.errorCovPost = 1. * np.ones((2, 2))
+        kalman.statePost = 0.1 * np.random.randn(2, 1)
+
+        while True:
+            def calc_point(angle):
+                return (np.around(img_width/2 + img_width/3*cos(angle), 0).astype(int),
+                        np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int))
+
+            state_angle = state[0, 0]
+            state_pt = calc_point(state_angle)
+
+            prediction = kalman.predict()
+            predict_angle = prediction[0, 0]
+            predict_pt = calc_point(predict_angle)
+
+            measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)
+
+            # generate measurement
+            measurement = np.dot(kalman.measurementMatrix, state) + measurement
+
+            measurement_angle = measurement[0, 0]
+            measurement_pt = calc_point(measurement_angle)
+
+            # plot points
+            def draw_cross(center, color, d):
+                cv2.line(img,
+                         (center[0] - d, center[1] - d), (center[0] + d, center[1] + d),
+                         color, 1, cv2.LINE_AA, 0)
+                cv2.line(img,
+                         (center[0] + d, center[1] - d), (center[0] - d, center[1] + d),
+                         color, 1, cv2.LINE_AA, 0)
+
+            img = np.zeros((img_height, img_width, 3), np.uint8)
+            draw_cross(np.int32(state_pt), (255, 255, 255), 3)
+            draw_cross(np.int32(measurement_pt), (0, 0, 255), 3)
+            draw_cross(np.int32(predict_pt), (0, 255, 0), 3)
+
+            cv2.line(img, state_pt, measurement_pt, (0, 0, 255), 3, cv2.LINE_AA, 0)
+            cv2.line(img, state_pt, predict_pt, (0, 255, 255), 3, cv2.LINE_AA, 0)
+
+            kalman.correct(measurement)
+
+            process_noise = kalman.processNoiseCov * np.random.randn(2, 1)
+            state = np.dot(kalman.transitionMatrix, state) + process_noise
+
+            cv2.imshow("Kalman", img)
+
+            code = cv2.waitKey(100) % 0x100
+            if code != -1:
+                break
+
+        if code in [27, ord('q'), ord('Q')]:
+            break
+
+    cv2.destroyWindow("Kalman")