Lucas Kanade Optical Flow Implementation

edit

I was working on my own optical flow script using lucas kanade method on python and numpy. But I get really different flow results with the opencv implementation of that algorithm (This is testing video), than with my own.
This is the full code:

import cv2
import numpy as np

def drawlines (image, pts0, pts1, color):
    for pt0, pt1 in zip(pts0, pts1):
        cv2.line(image,(pt0[0],pt0[1]),(pt1[0],pt1[1]),color,thickness=2);

def calcFlowLib (prev, curr, points):
    #USING OPENCV IMPLEMENTATION
    currpts = np.reshape(points,(-1,1,2));
    lk_params = dict(winSize=(41,41), maxLevel=0, criteria=(cv2.TERM_CRITERIA_EPS|cv2.TERM_CRITERIA_COUNT,10,0.03));
    nextpts, stat, err = cv2.calcOpticalFlowPyrLK(prev,curr,currpts,None,**lk_params);
    nextpts = np.reshape(nextpts,(-1,2));
    return nextpts;

def calcFlow (im1, im2, points, halfwin=(20,20)):
    #MY IMPLEMENTATION
    pts = np.copy(points)
    npts = points.shape[0]

    i_x = np.zeros(im1.shape)
    i_y = np.zeros(im1.shape)
    i_t = np.zeros(im1.shape)
    i_x[1:-1, 1:-1] = (im1[1:-1, 2:] - im1[1:-1, :-2]) / 2
    i_y[1:-1, 1:-1] = (im1[2:, 1:-1] - im1[:-2, 1:-1]) / 2
    i_t = im1 - im2


    for i in xrange(0,npts,1):
        p = pts[i]

        ix = i_x[p[1]-halfwin[1]:p[1]+halfwin[1]+1,p[0]-halfwin[0]:p[0]+halfwin[0]+1]
        iy = i_y[p[1]-halfwin[1]:p[1]+halfwin[1]+1,p[0]-halfwin[0]:p[0]+halfwin[0]+1]            
        it = i_t[p[1]-halfwin[1]:p[1]+halfwin[1]+1,p[0]-halfwin[0]:p[0]+halfwin[0]+1]

        A = np.matrix([np.ravel(ix),np.ravel(iy)],dtype=np.float32)
        B = np.matrix([np.ravel(it)],dtype=np.float32)
        v = np.linalg.lstsq(A*A.T, A*B.T)[0]
        pts[i] = p + np.ravel(v)
    return pts


cap = cv2.VideoCapture("OF.mp4")    
prevgray = None
currgray = None
prevpts0 = np.array([[320,180],[100,100]],dtype=np.float32)
prevpts1 = np.array([[320,180],[100,100]],dtype=np.float32)
while(cap.isOpened()):
    ret, frame = cap.read()
    if not ret:
        break
    frame = cv2.resize(frame,(640,360))
    currgray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)

    if(prevgray is not None):
        currpts0 = calcFlow(prevgray,currgray,prevpts0)
        drawlines(frame,prevpts0,currpts0,(0,255,0)) 
        currpts1 = calcFlowLib(prevgray,currgray,prevpts1)
        drawlines(frame,prevpts1,currpts1,(0,0,255))
        prevpts0 = np.copy(currpts0) 
        prevpts1 = np.copy(currpts1)

    prevgray = np.copy(currgray)
    cv2.imshow("Frame",frame)
    if cv2.waitKey(1) & 0xFF == ord('1'):
        break
cap.release()
cv2.destroyAllWindows()

Where is the wrong part of my implementation?