SfM.py

sfm.py

import numpy as np
import cv2
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import axes3d

FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks = 50)
flann = cv2.FlannBasedMatcher(index_params, search_params)

def flann_match(kp1,des1, kp2,des2, ratio=0.8, returnKP = True):
    ''' return a list of matches '''    
    matches12 = flann.knnMatch(des1,des2,k=2)
    good12 = [m for (m,n) in matches12 if m.distance<ratio*n.distance]
    matches21 = flann.knnMatch(des2,des1,k=2)
    good21 = [m for (m,n) in matches21 if m.distance<ratio*n.distance]
    
    good = [m12 for m12 in good12 for m21 in good21 if 
            (kp1[m12.queryIdx]==kp1[m21.trainIdx] and 
             kp2[m12.trainIdx]==kp2[m21.queryIdx])]
    
    if returnKP == True:
        kps1 = [kp1[m.queryIdx].pt for m in good]
        kps2 = [kp2[m.trainIdx].pt for m in good]
        pts1 = np.int32(kps1)
        pts2 = np.int32(kps2)
    return good, pts1, pts2

def show3d(X,Y,Z):
    fig = plt.figure()
    ax = fig.gca(projection="3d")
    ax.plot(X.flatten(),Y.flatten(),Z.flatten(),'k.')

imgl = cv2.imread('house.000.pgm',0)
imgr = cv2.imread('house.001.pgm',0)

sift = cv2.SIFT()
kp1, des1 = sift.detectAndCompute(imgl, None)
kp2, des2 = sift.detectAndCompute(imgr, None)

# ########################################### step 1 : Find the matching points
good, pts1, pts2 = flann_match(kp1,des1,kp2,des2)

# ########################################### step 2: find fundamental matrix, required is E, but we don't have K
F, mask = cv2.findFundamentalMat(pts1,pts2)

# Now decompose F to R and t using SVD
W = np.array([  [0,-1,0],
                [1,0,0],
                [0,0,1] ])
w, u, vt = cv2.SVDecomp(F)
R = u*W*vt
t = u[:,2,np.newaxis]

P1 = np.hstack((R,t))       # Projection matrix of second cam is ready

P0 = np.array([ [1,0,0,0],
                [0,1,0,0],
                [0,0,1,0]   ]) # Projection matrix of first cam at origin
                
########################################## Step 3 : Triangulation (using OpenCV function )
#pts1 = pts1.reshape(1,-1,2)
#pts2 = pts2.reshape(1,-1,2)            
res = cv2.triangulatePoints(P0, P1, pts1[0:2].T, pts2[0:2].T)

print res.shape