mirror of
https://github.com/opencv/opencv.git
synced 2025-06-13 13:13:26 +08:00
removed setter methods, replaced by CV_PROP_RW macro
This commit is contained in:
parent
1162f0ed63
commit
dc49e11527
@ -129,33 +129,16 @@ public:
|
|||||||
//! updates the predicted state from the measurement
|
//! updates the predicted state from the measurement
|
||||||
CV_WRAP const Mat& correct( const Mat& measurement );
|
CV_WRAP const Mat& correct( const Mat& measurement );
|
||||||
|
|
||||||
//! sets predicted state
|
CV_PROP_RW Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
|
||||||
CV_WRAP void setStatePre( const Mat& state ) { statePre = state; }
|
CV_PROP_RW Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
|
||||||
//! sets corrected state
|
CV_PROP_RW Mat transitionMatrix; //!< state transition matrix (A)
|
||||||
CV_WRAP void setStatePost( const Mat& state ) { statePost = state; }
|
CV_PROP_RW Mat controlMatrix; //!< control matrix (B) (not used if there is no control)
|
||||||
//! sets transition matrix
|
CV_PROP_RW Mat measurementMatrix; //!< measurement matrix (H)
|
||||||
CV_WRAP void setTransitionMatrix( const Mat& transition ) { transitionMatrix = transition; }
|
CV_PROP_RW Mat processNoiseCov; //!< process noise covariance matrix (Q)
|
||||||
//! sets control matrix
|
CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
|
||||||
CV_WRAP void setControlMatrix( const Mat& control ) { controlMatrix = control; }
|
CV_PROP_RW Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
|
||||||
//! sets measurement matrix
|
CV_PROP_RW Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
|
||||||
CV_WRAP void setMeasurementMatrix( const Mat& measurement ) { measurementMatrix = measurement; }
|
CV_PROP_RW Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
|
||||||
//! 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)
|
|
||||||
|
|
||||||
// temporary matrices
|
// temporary matrices
|
||||||
Mat temp1;
|
Mat temp1;
|
||||||
|
@ -11,21 +11,15 @@
|
|||||||
Pressing any key (except ESC) will reset the tracking with a different speed.
|
Pressing any key (except ESC) will reset the tracking with a different speed.
|
||||||
Pressing ESC will stop the program.
|
Pressing ESC will stop the program.
|
||||||
"""
|
"""
|
||||||
import urllib2
|
|
||||||
import cv2
|
import cv2
|
||||||
from math import cos, sin, sqrt
|
from math import cos, sin
|
||||||
import sys
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
|
|
||||||
img_height = 500
|
img_height = 500
|
||||||
img_width = 500
|
img_width = 500
|
||||||
img = np.array((img_height, img_width, 3), np.uint8)
|
|
||||||
kalman = cv2.KalmanFilter(2, 1, 0)
|
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
|
code = -1L
|
||||||
|
|
||||||
@ -34,25 +28,17 @@ if __name__ == "__main__":
|
|||||||
while True:
|
while True:
|
||||||
state = 0.1 * np.random.randn(2, 1)
|
state = 0.1 * np.random.randn(2, 1)
|
||||||
|
|
||||||
transition_matrix = np.array([[1., 1.], [0., 1.]])
|
kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]])
|
||||||
kalman.setTransitionMatrix(transition_matrix)
|
kalman.measurementMatrix = 1. * np.ones((1, 2))
|
||||||
measurement_matrix = 1. * np.ones((1, 2))
|
kalman.processNoiseCov = 1e-5 * np.eye(2)
|
||||||
kalman.setMeasurementMatrix(measurement_matrix)
|
kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1))
|
||||||
|
kalman.errorCovPost = 1. * np.ones((2, 2))
|
||||||
process_noise_cov = 1e-5
|
kalman.statePost = 0.1 * np.random.randn(2, 1)
|
||||||
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:
|
while True:
|
||||||
def calc_point(angle):
|
def calc_point(angle):
|
||||||
return (np.around(img_width/2 + img_width/3*cos(angle), 0).astype(int),
|
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_angle = state[0, 0]
|
||||||
state_pt = calc_point(state_angle)
|
state_pt = calc_point(state_angle)
|
||||||
@ -61,21 +47,22 @@ if __name__ == "__main__":
|
|||||||
predict_angle = prediction[0, 0]
|
predict_angle = prediction[0, 0]
|
||||||
predict_pt = calc_point(predict_angle)
|
predict_pt = calc_point(predict_angle)
|
||||||
|
|
||||||
|
measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)
|
||||||
measurement = measurement_noise_cov * np.random.randn(1, 1)
|
|
||||||
|
|
||||||
# generate measurement
|
# generate measurement
|
||||||
measurement = np.dot(measurement_matrix, state) + measurement
|
measurement = np.dot(kalman.measurementMatrix, state) + measurement
|
||||||
|
|
||||||
measurement_angle = measurement[0, 0]
|
measurement_angle = measurement[0, 0]
|
||||||
measurement_pt = calc_point(measurement_angle)
|
measurement_pt = calc_point(measurement_angle)
|
||||||
|
|
||||||
# plot points
|
# plot points
|
||||||
def draw_cross(center, color, d):
|
def draw_cross(center, color, d):
|
||||||
cv2.line(img, (center[0] - d, center[1] - d),
|
cv2.line(img,
|
||||||
(center[0] + d, center[1] + d), color, 1, cv2.LINE_AA, 0)
|
(center[0] - d, center[1] - d), (center[0] + d, center[1] + d),
|
||||||
cv2.line(img, (center[0] + d, center[1] - d),
|
color, 1, cv2.LINE_AA, 0)
|
||||||
(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)
|
img = np.zeros((img_height, img_width, 3), np.uint8)
|
||||||
draw_cross(np.int32(state_pt), (255, 255, 255), 3)
|
draw_cross(np.int32(state_pt), (255, 255, 255), 3)
|
||||||
@ -87,8 +74,8 @@ if __name__ == "__main__":
|
|||||||
|
|
||||||
kalman.correct(measurement)
|
kalman.correct(measurement)
|
||||||
|
|
||||||
process_noise = process_noise_cov * np.random.randn(2, 1)
|
process_noise = kalman.processNoiseCov * np.random.randn(2, 1)
|
||||||
state = np.dot(transition_matrix, state) + process_noise
|
state = np.dot(kalman.transitionMatrix, state) + process_noise
|
||||||
|
|
||||||
cv2.imshow("Kalman", img)
|
cv2.imshow("Kalman", img)
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user