# Import library

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


    def degeneracyCheckPass(first_points, second_points, rot, trans):
        rot_inv = rot
        for first, second in zip(first_points, second_points):
            first_z = np.dot(rot[0, :] - second[0]*rot[2, :], trans) / np.dot(rot[0, :] - second[0]*rot[2, :], second)
            first_3d_point = np.array([first[0] * first_z, second[0] * first_z, first_z])
            second_3d_point = np.dot(rot.T, first_3d_point) - np.dot(rot.T, trans)

            if first_3d_point[2] < 0 or second_3d_point[2] < 0:
                return False

        return True

    start = time.time();
    # Load a pair of images
    img1 = cv2.imread('1.jpg',1)
    img2 = cv2.imread('2.jpg',1)

    #camera_matrix:

    #cmat is K
    zero = np.float32([[0.0], [0.0], [0.0]])
    cmat = np.float32([[2610.9791576918024, 0.0, 1035.3907882371566], [0.0, 2611.5091416583214, 479.1873404639389], [0.0, 0.0, 1.0]])
    cmat_inv = np.linalg.inv(cmat)
    #dist_coefs:
    dist = np.matrix('0.10783691621695163 0.0979247249215728 -0.010931168141218273 0.009561950912198006 -1.745839718546563')


    # Initiate ORB detector
    orb = cv2.ORB_create()

    # Find the keypoints and descriptors with ORB
    kp1, des1 = orb.detectAndCompute(img1,None)
    kp2, des2 = orb.detectAndCompute(img2,None)

    # Create BFMatcher object
    bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)

    # Match descriptors.
    matches = bf.match(des1,des2)


    #Ratio test as per Lowe's paper
    good = []
    pts1 = []
    pts2 = []

    dist = [m.distance for m in matches]
    thres_dist = (sum(dist) / len(dist)) * 0.8
    for m in matches:
    	if m.distance < thres_dist:
    		good.append(m)
    		pts2.append(kp2[m.trainIdx].pt)
    		pts1.append(kp1[m.queryIdx].pt)

    # Draw  matches.
    img3 = cv2.drawMatches(img1,kp1,img2,kp2,good, None, flags=2)
    plt.imshow(img3),plt.show()

    # Get the list of best matches from both the images
    pts1 = np.int32(pts1)
    pts2 = np.int32(pts2)

    # Find fundamental Matrix
    F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS)

    # Select inlier points
    pts1 = pts1[mask.ravel()==1]
    pts2 = pts2[mask.ravel()==1]

    # Find essential matrix
    #E = K'^T . F . K
    E = cmat.T.dot(F).dot(cmat)


    # Compute camera pose

    U, S, Vt = np.linalg.svd(E)
    W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3)
    first_inliers = []
    second_inliers = []


    for i in range(len(pts1)):
        first_inliers.append(cmat_inv.dot([pts1[i][0], pts1[i][1], 1.0]))
        second_inliers.append(cmat_inv.dot([pts2[i][0], pts2[i][1], 1.0]))


    # First choice: R = U * W * Vt, T = u_3
    R = U.dot(W).dot(Vt)
    T = U[:, 2]

    # Start degeneracy checks
    if not degeneracyCheckPass(first_inliers, second_inliers, R, T):
        # Second choice: R = U * W * Vt, T = -u_3
        T = - U[:, 2]
        if not degeneracyCheckPass(first_inliers, second_inliers, R, T):
            # Third choice: R = U * Wt * Vt, T = u_3
            R = U.dot(W.T).dot(Vt)
            T = U[:, 2]
            if not degeneracyCheckPass(first_inliers, second_inliers, R, T):
                # Fourth choice: R = U * Wt * Vt, T = -u_3
                T = - U[:, 2]

    T_r = T.reshape((T.shape[0],-1))

    # Reconstruct 3D location of matched points
    # Get camera projection matrix


    P1 = np.hstack((cmat, zero))
    P2 = np.dot(cmat, np.hstack((R,T_r)))


    pts1t = pts1.transpose()
    pts2t = pts2.transpose()


    floatpst1t = np.array([pts1t[0,:].astype(np.float) / 1944.0, pts1t[1,:].astype(np.float) / 2592.0])


    floatpst2t = np.array([pts2t[0,:].astype(np.float) / 1944.0, pts2t[1,:].astype(np.float) / 2592.0])


    #Demo projection matrix
    #P1 = np.eye(4)
    #P2 = np.array([[ 0.878, -0.01 ,  0.479, -1.995],
    #            [ 0.01 ,  1.   ,  0.002, -0.226],
    #            [-0.479,  0.002,  0.878,  0.615],
    #            [ 0.   ,  0.   ,  0.   ,  1.   ]])
    X = cv2.triangulatePoints(P1[:4], P2[:4], floatpst1t, floatpst2t)
    print X
    #here I will divide the first three parameters with the fourth one to get the 3d coordinate

    finish = time.time();
    print(finish - start)