CODEWITHSUNDEEP
pythonopencvopencv3.0kalman-filter
SinLok
SinLok

Reputation: 609

KalmanFilter always predict 0,0 in first time

The following code use to scan image from bottom to top. However, the prediction of Kalman filter always show 0,0 in first time. So that, it will draw line from bottom to 0,0. How to make path(Kalman filter) more similar to actual path?

The following code and image was updated.

import cv2
import matplotlib.pyplot as plt
import numpy as np

img = cv2.imread('IMG_4614.jpg',1)
img = cv2.resize(img, (600, 800))
hsv_image = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
low_yellow = np.array([18, 94, 140])
up_yellow = np.array([48, 255, 255])
hsv_mask = cv2.inRange(hsv_image, low_yellow, up_yellow)
hls_image = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
low_yellow = np.array([0, 170, 24])
up_yellow = np.array([54, 255, 255])
hls_mask = cv2.inRange(hls_image, low_yellow, up_yellow)
mask = np.logical_or(hsv_mask,hls_mask)

offset = 100
height, width, _ = img.shape
previousPos = h
currentPos = h - offset
finalImg = img.copy()
is_first = True

initState = np.array([[np.float32(int(width/2))], [np.float32(h)]], np.float32)
last_measurement = current_measurement = initState
last_prediction = current_prediction = np.array((2, 1), np.float32)
kalman = cv2.KalmanFilter(4, 2)
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
kalman.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)


while currentPos >= 0:
    histogram = np.sum(mask[currentPos:previousPos,:], axis=0)
    areas = np.where(histogram > 40)
    if areas[0].size >= 2:
        bottomLeft = areas[0][0]
        topRight = areas[0][-1]

        x = int((topRight-bottomLeft) / 2 + bottomLeft)
        y = int((previousPos - currentPos) / 2 + currentPos)
        last_prediction = current_prediction
        last_measurement = current_measurement 
        current_measurement = np.array([[np.float32(x)], [np.float32(y)]], np.float32)
        lmx, lmy = last_measurement[0], last_measurement[1]
        cmx, cmy = current_measurement[0], current_measurement[1]

        cv2.rectangle(finalImg, (bottomLeft,previousPos), (topRight,currentPos), (0,255,0), 5)
        cv2.circle(finalImg,(x,y), 5, (0,0,255), -1)
        cv2.line(finalImg, (lmx, lmy), (cmx, cmy), (255, 0, 0),5) #actual path




        kalman.correct(current_measurement-initState)
        current_prediction = kalman.predict()

        lpx, lpy = last_prediction[0] + initState[0], last_prediction[1] + initState[1]
        cpx, cpy = current_prediction[0] + initState[0], current_prediction[1] + initState[1]
        cv2.line(finalImg, (lpx, lpy), (cpx, cpy), (255, 0, 255),5) # predict path  



        plt.figure(figsize=(10,10))  
        plt.imshow(cv2.cvtColor(finalImg, cv2.COLOR_BGR2RGB))
        plt.show()


    previousPos = currentPos
    currentPos = currentPos - offset

enter image description here enter image description here

Upvotes: 4

Views: 1521

Answers (2)

lahmania
lahmania

Reputation: 411

I managed to change the initial state by changing statePost and statePre. In init:

self.KF = cv2.KalmanFilter(nmbStateVars, nmbMeasts, nmbControlInputs)

A = self.KF.statePost
A[0:4] = self.measurement.reshape((4, 1))
# A[4:8] = 0.0
self.KF.statePost = A
self.KF.statePre = A

Then update as usual

self.updatedMeasts = self.KF.correct(self.measurement)

Upvotes: 2

Fred Guth
Fred Guth

Reputation: 1667

This has already been answered here: Kalman filter always predicting origin

OpenCV Kalman filter implementation does not let you set the an initial state. You have to save your initial state and then when you call kalman.correct you have to subtract the initial state. And when you call kalman.predict you have to add your initial state.

Something like this pseudo-code:

initialState = (y,x)
....

kalman.correct(current_measurement - initialState)
...
prediction = kalman.predict()
prediction[0] = prediction[0] + initState[0]
prediction[1] = prediction[1] + initState[1]

Upvotes: 6

Related Questions