pythonopencvkalman-filterpykalman

OpenCV Kalman Filter python


Can anyone provide me a sample code or some sort of example of Kalman filter implementation in python 2.7 and openCV 2.4.13

I want to implement it in a video to track a person but, I don't have any reference to learn and I couldn't find any python examples.

I know Kalman Filter exists in openCV as cv2.KalmanFilter but I have no idea how to use it. Any guidance would be appreciated


Solution

  • The kalman.py code below is the example included in OpenCV 3.2 source in github. It should be easy to change the syntax back to 2.4 if needed.

    #!/usr/bin/env 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.
    """
    # Python 2/3 compatibility
    import sys
    PY3 = sys.version_info[0] == 3
    
    if PY3:
        long = int
    
    import cv2
    from math import cos, sin, sqrt
    import numpy as np
    
    if __name__ == "__main__":
    
        img_height = 500
        img_width = 500
        kalman = cv2.KalmanFilter(2, 1, 0)
    
        code = long(-1)
    
        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 = sqrt(kalman.processNoiseCov[0,0]) * np.random.randn(2, 1)
                state = np.dot(kalman.transitionMatrix, state) + process_noise
    
                cv2.imshow("Kalman", img)
    
                code = cv2.waitKey(100)
                if code != -1:
                    break
    
            if code in [27, ord('q'), ord('Q')]:
                break
    
        cv2.destroyWindow("Kalman")
    

    Here is the OpenCV 2.4 Doc on Kalman Filter. Hope this help.