pythonでmeanshiftによる物体追跡

前提・実現したいこと

pythonでmeanshiftを用いた物体追跡のプログラムを作っています。
opencvのcv2.meanshiftを使わずに作成したいと考え、http://clientver2.hatenablog.com/entry/2015/11/22/142248
を参考に作成してみたのですが、かなり精度が低く追跡がうまくできません。
原因の判断ができずに困っています。助けてください。

該当のソースコード

python
1import numpy as np
2import cv2
3import scipy.stats as st
4
5class CameraGui(object):
6    def __init__(self):
7        self.pos1, self.pos2 = [300,300], [400,400]
8        self.image = None
9        self.patch_height, self.patch_width = None, None
10        self.kernel = None
11
12    def start(self):
13        cap = cv2.VideoCapture(0)
14        cv2.namedWindow("frame")
15              
16        while(True):
17            ret, frame = cap.read()
18            self.image = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
19            self.hist = cv2.calcHist([self.image[self.pos1[0]:self.pos2[0], self.pos1[1]:self.pos2[1]]], [0], None, [256], [0, 256]).astype(np.float)
20            self.hist /= np.sum(self.hist)
21            self.patch_height = self.pos2[0] - self.pos1[0]
22            self.patch_width = self.pos2[1] - self.pos1[1]
23            self.kernel = self.calc_gkern(self.patch_height, self.patch_width)
24            if self.pos1 != None and self.pos2 != None:
25                center = self.calcWeight(self.image)
26                draw = cv2.cvtColor(np.copy(self.image), cv2.COLOR_GRAY2RGB)
27                pt1 = (center[1] - self.patch_height // 2, center[0] - self.patch_width // 2)
28                pt2 = (center[1] + self.patch_height // 2, center[0] + self.patch_width // 2)
29                draw2 = cv2.rectangle(frame, pt1, pt2, 255,2)
30                self.pos1 = pt1
31                self.pos2 = pt2
32            cv2.imshow("frame", draw2)
33            if cv2.waitKey(1) & 0xFF == 27:
34                break
35        self.cap.release()
36        cv2.destroyAllWindows()
37
38    def calcWeight(self, image):
39        patch = image[self.pos1[0]:self.pos1[0]+self.patch_height,self.pos1[1]:self.pos1[1]+self.patch_width]
40        height, width = patch.shape
41        weight = np.copy(self.kernel)
42
43        for i in range(height):
44            for j in range(width):
45                weight[i, j] *= self.hist[patch[i, j]]
46                
47        Y, X = np.mgrid[:height, :width]
48        Y, X = Y * weight, X * weight
49        W = np.sum(weight)
50        centerY, centerX = np.sum(Y) / W, np.sum(X) / W
51
52        return [int(self.pos1[0] + centerY), int(self.pos1[1] + centerX)]
53        
54    def calc_gkern(self, height, width):
55        import scipy.ndimage.filters as fi
56        inp = np.zeros((height, width))
57        inp[height//2, width//2] = 1
58        return fi.gaussian_filter(inp, 50)
59        
60def main():
61    gui = CameraGui()
62    gui.start()
63
64main()