From f610c8810389226aeddf9860c4ddf6c9bb61707c Mon Sep 17 00:00:00 2001 From: Juan Carlos Niebles Date: Wed, 17 Sep 2014 18:45:48 -0500 Subject: [PATCH 1/3] extended python interface for KalmanFilter --- .../video/include/opencv2/video/tracking.hpp | 17 +++ samples/python2/kalman.py | 103 ++++++++++++++++++ 2 files changed, 120 insertions(+) create mode 100755 samples/python2/kalman.py diff --git a/modules/video/include/opencv2/video/tracking.hpp b/modules/video/include/opencv2/video/tracking.hpp index 18a3088aef..1c52f11507 100644 --- a/modules/video/include/opencv2/video/tracking.hpp +++ b/modules/video/include/opencv2/video/tracking.hpp @@ -129,6 +129,23 @@ public: //! updates the predicted state from the measurement CV_WRAP const Mat& correct( const Mat& measurement ); + //! sets predicted state + CV_WRAP void setStatePre( const Mat& state ) { statePre = state; } + //! sets corrected state + CV_WRAP void setStatePost( const Mat& state ) { statePost = state; } + //! sets transition matrix + CV_WRAP void setTransitionMatrix( const Mat& transition ) { transitionMatrix = transition; } + //! sets control matrix + CV_WRAP void setControlMatrix( const Mat& control ) { controlMatrix = control; } + //! sets measurement matrix + CV_WRAP void setMeasurementMatrix( const Mat& measurement ) { measurementMatrix = measurement; } + //! sets process noise covariance matrix + CV_WRAP void setProcessNoiseCov( const Mat& noise ) { processNoiseCov = noise; } + //! sets measurement noise covariance matrix + CV_WRAP void setMeasurementNoiseCov( const Mat& noise ) { measurementNoiseCov = noise; } + //! sets posteriori error covariance + CV_WRAP void setErrorCovPost( const Mat& error ) { errorCovPost = error; } + 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)) Mat transitionMatrix; //!< state transition matrix (A) diff --git a/samples/python2/kalman.py b/samples/python2/kalman.py new file mode 100755 index 0000000000..fcb78478ee --- /dev/null +++ b/samples/python2/kalman.py @@ -0,0 +1,103 @@ +#!/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 urllib2 +import cv2 +from math import cos, sin, sqrt +import sys +import numpy as np + +if __name__ == "__main__": + + img_height = 500 + img_width = 500 + img = np.array((img_height, img_width, 3), np.uint8) + kalman = cv2.KalmanFilter(2, 1, 0) + state = np.zeros((2, 1)) # (phi, delta_phi) + process_noise = np.zeros((2, 1)) + measurement = np.zeros((1, 1)) + + code = -1L + + cv2.namedWindow("Kalman") + + while True: + state = 0.1 * np.random.randn(2, 1) + + transition_matrix = np.array([[1., 1.], [0., 1.]]) + kalman.setTransitionMatrix(transition_matrix) + measurement_matrix = 1. * np.ones((1, 2)) + kalman.setMeasurementMatrix(measurement_matrix) + + process_noise_cov = 1e-5 + kalman.setProcessNoiseCov(process_noise_cov * np.eye(2)) + + measurement_noise_cov = 1e-1 + kalman.setMeasurementNoiseCov(measurement_noise_cov * np.ones((1, 1))) + + kalman.setErrorCovPost(1. * np.ones((2, 2))) + + kalman.setStatePost(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 = measurement_noise_cov * np.random.randn(1, 1) + + # generate measurement + measurement = np.dot(measurement_matrix, 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 = process_noise_cov * np.random.randn(2, 1) + + state = np.dot(transition_matrix, 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") From 1162f0ed63b243ca75ed23a0a8bd34567ed0ca5a Mon Sep 17 00:00:00 2001 From: Juan Carlos Niebles Date: Wed, 17 Sep 2014 19:02:12 -0500 Subject: [PATCH 2/3] fixed whitespaces --- samples/python2/kalman.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/samples/python2/kalman.py b/samples/python2/kalman.py index fcb78478ee..1b82e0d865 100755 --- a/samples/python2/kalman.py +++ b/samples/python2/kalman.py @@ -62,7 +62,7 @@ if __name__ == "__main__": predict_pt = calc_point(predict_angle) - measurement = measurement_noise_cov * np.random.randn(1, 1) + measurement = measurement_noise_cov * np.random.randn(1, 1) # generate measurement measurement = np.dot(measurement_matrix, state) + measurement @@ -88,7 +88,6 @@ if __name__ == "__main__": kalman.correct(measurement) process_noise = process_noise_cov * np.random.randn(2, 1) - state = np.dot(transition_matrix, state) + process_noise cv2.imshow("Kalman", img) From dc49e115274103d643568bf74b8e9b33ad6a8bef Mon Sep 17 00:00:00 2001 From: Juan Carlos Niebles Date: Thu, 18 Sep 2014 09:39:35 -0500 Subject: [PATCH 3/3] removed setter methods, replaced by CV_PROP_RW macro --- .../video/include/opencv2/video/tracking.hpp | 37 ++++---------- samples/python2/kalman.py | 49 +++++++------------ 2 files changed, 28 insertions(+), 58 deletions(-) diff --git a/modules/video/include/opencv2/video/tracking.hpp b/modules/video/include/opencv2/video/tracking.hpp index 1c52f11507..643b65a26f 100644 --- a/modules/video/include/opencv2/video/tracking.hpp +++ b/modules/video/include/opencv2/video/tracking.hpp @@ -129,33 +129,16 @@ public: //! updates the predicted state from the measurement CV_WRAP const Mat& correct( const Mat& measurement ); - //! sets predicted state - CV_WRAP void setStatePre( const Mat& state ) { statePre = state; } - //! sets corrected state - CV_WRAP void setStatePost( const Mat& state ) { statePost = state; } - //! sets transition matrix - CV_WRAP void setTransitionMatrix( const Mat& transition ) { transitionMatrix = transition; } - //! sets control matrix - CV_WRAP void setControlMatrix( const Mat& control ) { controlMatrix = control; } - //! sets measurement matrix - CV_WRAP void setMeasurementMatrix( const Mat& measurement ) { measurementMatrix = measurement; } - //! sets process noise covariance matrix - CV_WRAP void setProcessNoiseCov( const Mat& noise ) { processNoiseCov = noise; } - //! sets measurement noise covariance matrix - CV_WRAP void setMeasurementNoiseCov( const Mat& noise ) { measurementNoiseCov = noise; } - //! sets posteriori error covariance - CV_WRAP void setErrorCovPost( const Mat& error ) { errorCovPost = error; } - - 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)) - Mat transitionMatrix; //!< state transition matrix (A) - Mat controlMatrix; //!< control matrix (B) (not used if there is no control) - Mat measurementMatrix; //!< measurement matrix (H) - Mat processNoiseCov; //!< process noise covariance matrix (Q) - Mat measurementNoiseCov;//!< measurement noise covariance matrix (R) - 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) - Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k) + CV_PROP_RW Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k) + CV_PROP_RW Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) + CV_PROP_RW Mat transitionMatrix; //!< state transition matrix (A) + CV_PROP_RW Mat controlMatrix; //!< control matrix (B) (not used if there is no control) + CV_PROP_RW Mat measurementMatrix; //!< measurement matrix (H) + CV_PROP_RW Mat processNoiseCov; //!< process noise covariance matrix (Q) + CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R) + CV_PROP_RW Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/ + CV_PROP_RW Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R) + CV_PROP_RW Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k) // temporary matrices Mat temp1; diff --git a/samples/python2/kalman.py b/samples/python2/kalman.py index 1b82e0d865..101f1ea56a 100755 --- a/samples/python2/kalman.py +++ b/samples/python2/kalman.py @@ -11,21 +11,15 @@ Pressing any key (except ESC) will reset the tracking with a different speed. Pressing ESC will stop the program. """ -import urllib2 import cv2 -from math import cos, sin, sqrt -import sys +from math import cos, sin import numpy as np if __name__ == "__main__": img_height = 500 img_width = 500 - img = np.array((img_height, img_width, 3), np.uint8) kalman = cv2.KalmanFilter(2, 1, 0) - state = np.zeros((2, 1)) # (phi, delta_phi) - process_noise = np.zeros((2, 1)) - measurement = np.zeros((1, 1)) code = -1L @@ -34,25 +28,17 @@ if __name__ == "__main__": while True: state = 0.1 * np.random.randn(2, 1) - transition_matrix = np.array([[1., 1.], [0., 1.]]) - kalman.setTransitionMatrix(transition_matrix) - measurement_matrix = 1. * np.ones((1, 2)) - kalman.setMeasurementMatrix(measurement_matrix) - - process_noise_cov = 1e-5 - kalman.setProcessNoiseCov(process_noise_cov * np.eye(2)) - - measurement_noise_cov = 1e-1 - kalman.setMeasurementNoiseCov(measurement_noise_cov * np.ones((1, 1))) - - kalman.setErrorCovPost(1. * np.ones((2, 2))) - - kalman.setStatePost(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)) + np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int)) state_angle = state[0, 0] state_pt = calc_point(state_angle) @@ -61,21 +47,22 @@ if __name__ == "__main__": predict_angle = prediction[0, 0] predict_pt = calc_point(predict_angle) - - measurement = measurement_noise_cov * np.random.randn(1, 1) + measurement = kalman.measurementNoiseCov * np.random.randn(1, 1) # generate measurement - measurement = np.dot(measurement_matrix, state) + 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) + 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) @@ -87,8 +74,8 @@ if __name__ == "__main__": kalman.correct(measurement) - process_noise = process_noise_cov * np.random.randn(2, 1) - state = np.dot(transition_matrix, state) + process_noise + process_noise = kalman.processNoiseCov * np.random.randn(2, 1) + state = np.dot(kalman.transitionMatrix, state) + process_noise cv2.imshow("Kalman", img)