Kalman filter always predicting origin

Question

I am learning kalman filters for the purpose of trajectory prediction. Right now, I am able to track the ball. For my first practical attempt with prediction and kalman filter, I used an example of drawing lines as given here:

Is there any example of cv2.KalmanFilter implementation?

Here is the complete code:

import cv2
import numpy as np
import math
cap = cv2.VideoCapture('videoplayback (1).mp4')
loHue = 0
loSaturation = 50
loValue = 50
high_hue = 0
high_saturation = 255
high_value = 255
flag_for_center = 1
def low_hue(x):
    global loHue
    loHue = x 

#def low_saturation(x):
    #global loSaturation
    #loSaturation = x

#def low_value(x):
    #global loValue
    #loValue = x

def upper_hue (x):
    global high_hue
    high_hue = x

#def upper_saturation(x):
    #global high_saturation
    #high_saturation= x

#def upper_value(x):
    #global high_value
    #high_value = x

cv2.namedWindow('Trackbars', flags=cv2.WINDOW_OPENGL)
cv2.resizeWindow('Trackbars', 500, 30)
cv2.moveWindow('Trackbars', 500, 600)
cv2.createTrackbar('loHue', 'Trackbars', 0, 180, low_hue)
#cv2.createTrackbar('loSaturation', 'Trackbars', 0, 255, low_saturation)
#cv2.createTrackbar('lowValue', 'Trackbars', 0, 255, low_value)
cv2.createTrackbar('upperHue', 'Trackbars', 0, 180, upper_hue)
#cv2.createTrackbar('upperSat', 'Trackbars', 0, 255, upper_saturation)
#cv2.createTrackbar('upperValue', 'Trackbars', 0, 255, upper_value)
cv2.setTrackbarPos('loHue', 'Trackbars', 5)
cv2.setTrackbarPos('upperHue', 'Trackbars', 30)

frame_count = 0
measure = []
predicted = []

while(True):




_, image = cap.read()
frame_count = frame_count + 1
image = cv2.GaussianBlur(image, (3, 3), 2)
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_limit = np.array([loHue,loSaturation,loValue])
upper_limit = np.array([high_hue,high_saturation,high_value])
mask = cv2.inRange(hsv, lower_limit, upper_limit)
res = cv2.bitwise_and(image, image, mask = mask)
#b,g,r = cv2.split(res)
#b = cv2.adaptiveThreshold(b,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\
#   cv2.THRESH_BINARY,11,20)
#g = cv2.adaptiveThreshold(g,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\
#   cv2.THRESH_BINARY, 11,20)
#r = cv2.adaptiveThreshold(r,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\
#   cv2.THRESH_BINARY,11,20)    
#res = cv2.merge((b,g,r))           
erode_element = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
dilate_element = cv2.getStructuringElement(cv2.MORPH_RECT, (7, 7))
erosion = cv2.erode(mask, erode_element, iterations = 1)
erosion = cv2.erode(erosion, erode_element, iterations = 1)
dilation = cv2.dilate(erosion, dilate_element, iterations = 1)
dilation = cv2.dilate(dilation, dilate_element, iterations = 1)
copy_dilation = dilation.copy()

_, contours, hierarchy = cv2.findContours(copy_dilation, cv2.RETR_CCOMP, 
 cv2.CHAIN_APPROX_SIMPLE)
center = None

if len(contours) > 0:
    c = max(contours, key = cv2.contourArea)
    ((x, y), radius) = cv2.minEnclosingCircle(c)
    M = cv2.moments(c)
    center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
    x,y = center

    measure = np.array([[np.float32(x)],[np.float32(y)]])
    #print(measure)
    #if (radius>10):
    #   cv2.circle(image, (int(x), int(y)), int(radius), (0, 255, 255), -2)
    #   cv2.circle(image, center, 3, (0,0,255),-1)
kalman = cv2.KalmanFilter(4,2)

kalman.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
kalman.processNoiseCov = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]],np.float32) * 0.03

#while(True):
kalman.correct(measure)
update_predicted_state = kalman.predict()
predicted.append((int(update_predicted_state[0]), int(update_predicted_state[1])))
for i in range(len(predicted)-1):
    cv2.imshow('tracking', image)
    cv2.moveWindow('tracking', 150, 150)
    cv2.imshow('mask', mask)
    cv2.moveWindow('mask', 700, 150)
    cv2.circle(image, (predicted[i][0], predicted[i + 1][1]), int(radius), (0, 255, 255), -2)
k = cv2.waitKey(20) & 0xFF
if k ==27:
    break
cap.release()
cv2.destroyAllWindows()

The issue is that the predicted values are all zero. As a result, I am getting a quarter circle at the top left corner. Any explanation ? BTW, the video on which I am running this thing is here: https://www.youtube.com/watch?v=CcFVOzQ1Oqc

The tracking part is working good and I am able to track the ball. However the problem starts with this line:

kalman.correct(measure)

When I tried to print it, it was all zero

[[0.]
[0.]
[0.]
[0.]]

Is it because I have not considered control matrix here ? Or is it just because of the odd bouncing of the ball ?

As you might have guessed it, the frame rates are awfully low.

Thank you.

Answer 1

There is a better explanation here: KalmanFilter always predict 0,0 in first time

The Kalman filter implementation of opencv doesn't let you set an initial state. This is not intuitive and the lack of documentation makes things even worse.

The way to circumvent this problem is to override the kalman.correct and kalman.predict methods. You set an initial value of the variable you want to work with and every time you call correct, you first subtract the initial value. When you call predict, you have to add the initial value.

I have an implementation example here where the Kalman filter is used in an visual tracking problem (tracking car chased by police): https://github.com/fredguth/unb-cv-3183/blob/master/p6/r4.py

Answer 2

I tried a loop to get rid of the offset. It may affect the estimator though. Probably, not a good way to handle the initial offset.

Mat_<float> measurement(2, 1);

KF.statePre.at<float> (0) = curMeasurement.x;
KF.statePre.at<float> (1) = curMeasurement.y;
KF.statePre.at<float> (2) = 0;
KF.statePre.at<float> (3) = 0;

measurement(0) = curMeasurement.x;
measurement(1) = curMeasurement.y;

for (int i = 0; i < 100; i++) {
  KF.predict();
  KF.correct(measurement);
}