fundamental_matrix.py

def getFundamentalMatrix(pts1, pts2):
  vvec = []
  for p1, p2 in zip(pts1, pts2):
    vvec.append([p2[0] * p1[0], p2[0] * p1[1], p2[0], p2[1] * p1[0], p2[1] * p1[1], p2[1], p1[0], p1[1], 1]) 
  vvec = np.array(vvec)

  U, S, Vt = np.linalg.svd(vvec)
  Fvl = Vt[-1]
  
  om = Fvl.reshape(3,3)
  assert np.allclose(np.linalg.norm(om, 'fro'), 1)
  
  U, S, Vt = np.linalg.svd(om)
  D = np.diag(S)
  D[-1, -1] = 0   # makes determinant 0

  nm = np.dot(np.dot(U, D), Vt) 
  assert np.allclose(np.linalg.norm(nm, 'fro'), 1)  
  assert np.allclose(np.linalg.det(nm), 0)

  err = np.linalg.norm(np.dot(vvec, nm.reshape(9)))
  print "error:",err
  return nm

keeps failing with 'assertion error'