import numpy as np
import cv2
import time
from matplotlib import pyplot as plt

imagefn=["drawing"]
MIN_MATCH_COUNT = 10

def getComponentsHM(hm):
    theta=np.arctan(hm[0,1]/hm[0,0])*(180.0/np.pi)
    scale=np.abs(hm[0,0]/np.cos(theta*(np.pi/180.0)))
    #homography matrix to translation
    #px=0
    #py=0
    translation =(px,py)
    return (translation,theta,scale)

for imn in imagefn:
    print imn
    t = time.time()
    # read images
    img1 = cv2.imread(imn+'_'+str(0)+'.png',0)
    img2 = cv2.imread(imn+'_'+str(1)+'.png',0)
    # Feature detection and description with SIFT
    sift = cv2.SIFT()
    kp1, des1 = sift.detectAndCompute(img1,None)
    kp2, des2 = sift.detectAndCompute(img2,None)
    # match detected features
    bf = cv2.BFMatcher()
    matches = bf.match(des1,des2)
    # find homography matrix
    if len(matches)>MIN_MATCH_COUNT:
        src_pts = np.float32([ kp1[m.queryIdx].pt for m in matches]).reshape(-1,1,2)
        dst_pts = np.float32([ kp2[m.trainIdx].pt for m in matches]).reshape(-1,1,2)
        M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
        print "Matrix"
        print M
        T,th,sc= getComponentsHM(M)
        print "(x,y):",T
        print "Angle:",th
        print "Scale:",sc

    else:
        print "Not enough matches - %d/%d" % (len(matches),MIN_MATCH_COUNT)