← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/samples/python/kalman.py

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/samples/python/kalman.py
 
1
#!/usr/bin/python
 
2
"""
 
3
   Tracking of rotating point.
 
4
   Rotation speed is constant.
 
5
   Both state and measurements vectors are 1D (a point angle),
 
6
   Measurement is the real point angle + gaussian noise.
 
7
   The real and the estimated points are connected with yellow line segment,
 
8
   the real and the measured points are connected with red line segment.
 
9
   (if Kalman filter works correctly,
 
10
    the yellow segment should be shorter than the red one).
 
11
   Pressing any key (except ESC) will reset the tracking with a different speed.
 
12
   Pressing ESC will stop the program.
 
13
"""
 
14
# Python 2/3 compatibility
 
15
import sys
 
16
PY3 = sys.version_info[0] == 3
 
17
 
 
18
if PY3:
 
19
    long = int
 
20
 
 
21
import cv2
 
22
from math import cos, sin
 
23
import numpy as np
 
24
 
 
25
if __name__ == "__main__":
 
26
 
 
27
    img_height = 500
 
28
    img_width = 500
 
29
    kalman = cv2.KalmanFilter(2, 1, 0)
 
30
 
 
31
    code = long(-1)
 
32
 
 
33
    cv2.namedWindow("Kalman")
 
34
 
 
35
    while True:
 
36
        state = 0.1 * np.random.randn(2, 1)
 
37
 
 
38
        kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]])
 
39
        kalman.measurementMatrix = 1. * np.ones((1, 2))
 
40
        kalman.processNoiseCov = 1e-5 * np.eye(2)
 
41
        kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1))
 
42
        kalman.errorCovPost = 1. * np.ones((2, 2))
 
43
        kalman.statePost = 0.1 * np.random.randn(2, 1)
 
44
 
 
45
        while True:
 
46
            def calc_point(angle):
 
47
                return (np.around(img_width/2 + img_width/3*cos(angle), 0).astype(int),
 
48
                        np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int))
 
49
 
 
50
            state_angle = state[0, 0]
 
51
            state_pt = calc_point(state_angle)
 
52
 
 
53
            prediction = kalman.predict()
 
54
            predict_angle = prediction[0, 0]
 
55
            predict_pt = calc_point(predict_angle)
 
56
 
 
57
            measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)
 
58
 
 
59
            # generate measurement
 
60
            measurement = np.dot(kalman.measurementMatrix, state) + measurement
 
61
 
 
62
            measurement_angle = measurement[0, 0]
 
63
            measurement_pt = calc_point(measurement_angle)
 
64
 
 
65
            # plot points
 
66
            def draw_cross(center, color, d):
 
67
                cv2.line(img,
 
68
                         (center[0] - d, center[1] - d), (center[0] + d, center[1] + d),
 
69
                         color, 1, cv2.LINE_AA, 0)
 
70
                cv2.line(img,
 
71
                         (center[0] + d, center[1] - d), (center[0] - d, center[1] + d),
 
72
                         color, 1, cv2.LINE_AA, 0)
 
73
 
 
74
            img = np.zeros((img_height, img_width, 3), np.uint8)
 
75
            draw_cross(np.int32(state_pt), (255, 255, 255), 3)
 
76
            draw_cross(np.int32(measurement_pt), (0, 0, 255), 3)
 
77
            draw_cross(np.int32(predict_pt), (0, 255, 0), 3)
 
78
 
 
79
            cv2.line(img, state_pt, measurement_pt, (0, 0, 255), 3, cv2.LINE_AA, 0)
 
80
            cv2.line(img, state_pt, predict_pt, (0, 255, 255), 3, cv2.LINE_AA, 0)
 
81
 
 
82
            kalman.correct(measurement)
 
83
 
 
84
            process_noise = kalman.processNoiseCov * np.random.randn(2, 1)
 
85
            state = np.dot(kalman.transitionMatrix, state) + process_noise
 
86
 
 
87
            cv2.imshow("Kalman", img)
 
88
 
 
89
            code = cv2.waitKey(100) % 0x100
 
90
            if code != -1:
 
91
                break
 
92
 
 
93
        if code in [27, ord('q'), ord('Q')]:
 
94
            break
 
95
 
 
96
    cv2.destroyWindow("Kalman")