head balance

import cv2
import numpy as np
cap = cv2.VideoCapture('/home/sm/Desktop/VIRB0034.MP4')
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out = cv2.VideoWriter('/home/sm/Desktop/balance.avi',fourcc, 30.0, (1920,1080))

color = (123,123,12)
def dst(f,c):
    distance = math.sqrt((f[0]-c[0])**2+(f[1]-c[1])**2)
    return distance

# params for ShiTomasi corner detection
feature_params = dict( maxCorners = 50,
                       qualityLevel = 0.2,
                       minDistance = 7,
                       blockSize = 7 )

# Parameters for lucas kanade optical flow
lk_params = dict( winSize  = (13,13),
                  maxLevel = 3,
                  criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 100, 0.5))



f, img = cap.read()
img = cv2.resize(img,(img.shape[1]/2, img.shape[0]/2), interpolation = cv2.INTER_CUBIC)
cv2.imshow('img', img)

cv2.waitKey(0)

# Take first frame and find corners in it
ret, img = cap.read()
#old_frame = cv2.resize(img,(img.shape[1]/2, img.shape[0]/2), interpolation = cv2.INTER_CUBIC)
old_frame = img
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)


p0 = np.array([[[518*2,273*2]]], np.float32)
d0 = np.array([[[519*2,220*2]]], np.float32)

# Create a mask image for drawing purposes
mask = np.zeros_like(old_frame)
#for i in range(300): ret, frame = cap.read()

while(1):
    ret,frame = cap.read()
    #frame = cv2.resize(frame,(frame.shape[1]/2, frame.shape[0]/2), interpolation = cv2.INTER_CUBIC)
    frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # calculate optical flow
    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)
    d1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, d0, None, **lk_params)

    # Select good points
    good_new = p1
    good_old = p0
    xy = int(p1[0][0][0]), int(p1[0][0][1])
    
    good_new_d = d1
    good_old_d = d0
    xy1 = int(d1[0][0][0]), int(d1[0][0][1])

    slope = xy[0] - xy1[0]
    slope = abs(slope*17)
    color = (255-slope,0,slope)
    print color, slope
    cv2.line(frame, xy, xy1, color, 7)
    

    cv2.imshow('frame',frame)
    out.write(frame)
    k = cv2.waitKey(1) & 0xff
    if k == 27:
        break

    # Now update the previous frame and previous points
    old_gray = frame_gray.copy()
    p0 = good_new.reshape(-1,1,2)
    d0 = good_new_d.reshape(-1,1,2)

cv2.destroyAllWindows()