import numpy as np
import cv2
import math

cap = cv2.VideoCapture(0)

#fgbg = cv2.BackgroundSubtractorMOG()

while(1):
    ret, frame = cap.read()
    gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)

#    fgmask = fgbg.apply(frame)

    gray = np.float32(gray)
    dst = cv2.cornerHarris(gray,4,3,0.04)

    dst = cv2.dilate(dst,None)
    ret, dst = cv2.threshold(dst,0.01*dst.max(),255,0)
    print math.log(dst.max())
    frame[dst>0.001*dst.min()]=[0,0,255]

    dst = np.uint8(dst)
    ret, labels, stats, centroids = cv2.connectedComponentsWithStats(dst)
    # define the criteria to stop and refine the corners
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
    corners = cv2.cornerSubPix(gray,np.float32(centroids),(5,5),(-1,-1),criteria)
    # Now draw them
    res = np.hstack((centroids,corners))
    res = np.int0(res)
    frame[res[:,1],res[:,0]]=[0,0,255]
    frame[res[:,3],res[:,2]] = [0,255,0]

    cv2.imshow('esquinas frame',frame)
    cv2.imshow('grises',dst)
    k = cv2.waitKey(30) & 0xff
    if k == 27:
        break

cap.release()
cv2.destroyAllWindows()