卡尔曼滤波器总是预测起源

时间:2018-06-14 18:02:13

标签: python opencv computer-vision prediction kalman-filter

我正在学习卡尔曼滤波器以达到轨迹预测的目的。现在,我能够跟踪球。对于我对预测和卡尔曼滤波器的第一次实际尝试,我使用了这里给出的绘制线的示例:

Is there any example of cv2.KalmanFilter implementation?

以下是完整的代码:

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()

问题是预测值都是零。结果,我在左上角得到一个四分之一圆。任何解释? 顺便说一下,我运行这个东西的视频在这里: https://www.youtube.com/watch?v=CcFVOzQ1Oqc

跟踪部分运作良好,我能够跟踪球。但问题始于这一行:

kalman.correct(measure)

当我尝试打印时,它全部为零

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

是不是因为我在这里没有考虑控制矩阵?或者只是因为球的奇怪弹跳?

正如您可能已经猜到的那样,帧速率非常低。

谢谢。

2 个答案:

答案 0 :(得分:2)

opencv的卡尔曼滤波器实现不允许您设置初始状态。这不直观,缺乏文档会使事情变得更糟。

解决此问题的方法是覆盖<integer>0</integer>kalman.correct方法。您可以设置要使用的变量的初始值,并且每次调用正确时,首先减去初始值。当您调用predict时,您必须添加初始值。

我在这里有一个实现示例,其中卡尔曼滤波器用于视觉跟踪问题(跟踪由警察追踪的车辆): https://github.com/fredguth/unb-cv-3183/blob/master/p6/r4.py

答案 1 :(得分:0)

我尝试了循环以消除偏移量。 它可能会影响估计量。 可能不是处理初始偏移的好方法。

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);
}