Advertisement
Guest User

EssentialMatrix

a guest
Jul 28th, 2015
1,171
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 4.84 KB | None | 0 0
  1. import cv2
  2. import numpy as np
  3.  
  4. from matplotlib import pyplot as plt
  5.  
  6. # K1 = np.float32([[1357.3, 0, 441.413], [0, 1355.9, 259.393], [0, 0, 1]]).reshape(3,3)
  7. # K2 = np.float32([[1345.8, 0, 394.9141], [0, 1342.9, 291.6181], [0, 0, 1]]).reshape(3,3)
  8.  
  9. # K1_inv = np.linalg.inv(K1)
  10. # K2_inv = np.linalg.inv(K2)
  11.  
  12. # Camera matrix from chessboard calibration
  13. K = np.float32([[3541.5, 0, 2088.8], [0, 3546.9, 1161.4], [0, 0, 1]])
  14. K_inv = np.linalg.inv(K)
  15.  
  16. def degeneracyCheckPass(first_points, second_points, rot, trans):
  17.     rot_inv = rot
  18.     for first, second in zip(first_points, second_points):
  19.         first_z = np.dot(rot[0, :] - second[0]*rot[2, :], trans) / np.dot(rot[0, :] - second[0]*rot[2, :], second)
  20.         first_3d_point = np.array([first[0] * first_z, second[0] * first_z, first_z])
  21.         second_3d_point = np.dot(rot.T, first_3d_point) - np.dot(rot.T, trans)
  22.  
  23.         if first_3d_point[2] < 0 or second_3d_point[2] < 0:
  24.             return False
  25.  
  26.     return True
  27.  
  28. def drawlines(img1,img2,lines,pts1,pts2):
  29.     ''' img1 - image on which we draw the epilines for the points in img1
  30.        lines - corresponding epilines '''
  31.     pts1 = np.int32(pts1)
  32.     pts2 = np.int32(pts2)
  33.     r,c = img1.shape
  34.     img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR)
  35.     img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR)
  36.     for r,pt1,pt2 in zip(lines,pts1,pts2):
  37.         color = tuple(np.random.randint(0,255,3).tolist())
  38.         x0,y0 = map(int, [0, -r[2]/r[1] ])
  39.         x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ])
  40.         cv2.line(img1, (x0,y0), (x1,y1), color,1)
  41.         cv2.circle(img1, tuple(pt1), 10, color, -1)
  42.         cv2.circle(img2, tuple(pt2), 10, color, -1)
  43.     return img1,img2
  44.  
  45. # Read the images
  46.  
  47. img1 = cv2.imread('sam1.jpg',0)   # Query image
  48. img2 = cv2.imread('sam1.jpg',0)  # Train image
  49. img1 = cv2.resize(img1, (0,0), fx = 0.5, fy = 0.5)
  50. img2 = cv2.resize(img2, (0,0), fx = 0.5, fy = 0.5)
  51.  
  52. sift = cv2.SIFT()
  53.  
  54. # find the keypoints and descriptors with SIFT
  55. kp1, des1 = sift.detectAndCompute(img1, None)
  56. kp2, des2 = sift.detectAndCompute(img2, None)
  57.  
  58. # FLANN parameters
  59. FLANN_INDEX_KDTREE = 0
  60. index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
  61. search_params = dict(checks = 50)   # or pass empty dictionary
  62.  
  63. flann = cv2.FlannBasedMatcher(index_params,search_params)
  64.  
  65. matches = flann.knnMatch(des1, des2, k=2)
  66.  
  67. good = []
  68. pts1 = []
  69. pts2 = []
  70.  
  71. # ratio test as per Lowe's paper
  72. for i,(m,n) in enumerate(matches):
  73.     if m.distance < 0.7*n.distance:
  74.         pts2.append(kp2[m.trainIdx].pt)
  75.         pts1.append(kp1[m.queryIdx].pt)
  76.  
  77. pts2 = np.float32(pts2)
  78. pts1 = np.float32(pts1)
  79. F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC, 0.1, 0.99)
  80.  
  81. # Selecting only the inliers
  82. pts1 = pts1[mask.ravel()==1]
  83. pts2 = pts2[mask.ravel()==1]
  84.  
  85. # drawing lines on left image
  86. lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2, F)
  87. lines1 = lines1.reshape(-1,3)
  88. img5,img6 = drawlines(img1,img2,lines1,pts1,pts2)
  89.  
  90. # drawing lines on right image
  91. lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1, F)
  92. lines2 = lines2.reshape(-1,3)
  93. img3,img4 = drawlines(img2,img1,lines2,pts2,pts1)
  94.  
  95. pt1 = np.array([[pts1[0][0]], [pts1[0][1]], [1]])
  96. pt2 = np.array([[pts2[0][0], pts2[0][1], 1]])
  97.  
  98. print pt1
  99. print pt2
  100.  
  101. print
  102. print "The fundamental matrix is"
  103. print F
  104. print
  105.  
  106. # Should be close to 0
  107. print "Fundamental matrix error check: %f"%np.dot(np.dot(pt2,F),pt1)
  108. print
  109.  
  110. E = K.T.dot(F).dot(K)
  111.  
  112. print "The essential matrix is"
  113. print E
  114. print
  115.  
  116. U, S, Vt = np.linalg.svd(E)
  117. W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3)
  118. first_inliers = []
  119. second_inliers = []
  120. for i in range(len(pts1)):
  121.     first_inliers.append(K_inv.dot([pts1[i][0], pts1[i][1], 1.0]))
  122.     second_inliers.append(K_inv.dot([pts2[i][0], pts2[i][1], 1.0]))
  123.  
  124. # First choice: R = U * W * Vt, T = u_3
  125. R = U.dot(W).dot(Vt)
  126. T = U[:, 2]
  127.  
  128. # Start degeneracy checks
  129. if not degeneracyCheckPass(first_inliers, second_inliers, R, T):
  130.     # Second choice: R = U * W * Vt, T = -u_3
  131.     T = - U[:, 2]
  132.     if not degeneracyCheckPass(first_inliers, second_inliers, R, T):
  133.         # Third choice: R = U * Wt * Vt, T = u_3
  134.         R = U.dot(W.T).dot(Vt)
  135.         T = U[:, 2]
  136.         if not degeneracyCheckPass(first_inliers, second_inliers, R, T):
  137.             # Fourth choice: R = U * Wt * Vt, T = -u_3
  138.             T = - U[:, 2]
  139.  
  140. print "Translation matrix is"
  141. print T
  142. print "Modulus is %f" % np.sqrt((T[0]*T[0] + T[1]*T[1] + T[2]*T[2]))
  143. print "Rotation matrix is"
  144. print R
  145.  
  146. # Decomposing rotation matrix
  147. pitch = np.arctan2(R[1][2], R[2][2]) * 180/3.1415
  148. yaw = np.arctan2(-R[2][0], np.sqrt(R[2][1]*R[2][1] + R[2][2]*R[2][2])) * 180/3.1415
  149. roll = np.arctan2(R[1][0],  R[0][0]) * 180/3.1415
  150.  
  151. print "Roll: %f, Pitch: %f, Yaw: %f" %(roll , pitch , yaw)
  152.  
  153. plt.subplot(121),plt.imshow(img5)
  154. plt.subplot(122),plt.imshow(img3)
  155. plt.show()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement