SHARE
TWEET

Posestimation

a guest Jul 30th, 2015 246 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import cv2
  2. import numpy as np
  3.  
  4. from matplotlib import pyplot as plt
  5.  
  6. # K2 = np.float32([[1357.3, 0, 441.413], [0, 1355.9, 259.393], [0, 0, 1]]).reshape(3,3)
  7. # K1 = 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. K = np.float32([3541.5, 0, 2088.8, 0, 3546.9, 1161.4, 0, 0, 1]).reshape(3,3)
  13. K_inv = np.linalg.inv(K)
  14.  
  15. def in_front_of_both_cameras(first_points, second_points, rot, trans):
  16.     # check if the point correspondences are in front of both images
  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.  
  46. img1 = cv2.imread('C:\\Users\\Sai\\Desktop\\room1.jpg', 0)  
  47. img2 = cv2.imread('C:\\Users\\Sai\\Desktop\\room0.jpg', 0)
  48. img1 = cv2.resize(img1, (0,0), fx=0.5, fy=0.5)
  49. img2 = cv2.resize(img2, (0,0), fx=0.5, fy=0.5)
  50.  
  51. sift = cv2.SIFT()
  52.  
  53. # find the keypoints and descriptors with SIFT
  54. kp1, des1 = sift.detectAndCompute(img1,None)
  55. kp2, des2 = sift.detectAndCompute(img2,None)
  56.  
  57. # FLANN parameters
  58. FLANN_INDEX_KDTREE = 0
  59. index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
  60. search_params = dict(checks=50)   # or pass empty dictionary
  61.  
  62. flann = cv2.FlannBasedMatcher(index_params,search_params)
  63.  
  64. matches = flann.knnMatch(des1,des2,k=2)
  65.  
  66. good = []
  67. pts1 = []
  68. pts2 = []
  69.  
  70. # ratio test as per Lowe's paper
  71. for i,(m,n) in enumerate(matches):
  72.     if m.distance < 0.7*n.distance:
  73.         good.append(m)
  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)
  80.  
  81. # Selecting only the inliers
  82. pts1 = pts1[mask.ravel()==1]
  83. pts2 = pts2[mask.ravel()==1]
  84.  
  85. F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_8POINT)
  86.  
  87. print "Fundamental matrix is"
  88. print
  89. print F
  90.  
  91. pt1 = np.array([[pts1[0][0]], [pts1[0][1]], [1]])
  92. pt2 = np.array([[pts2[0][0], pts2[0][1], 1]])
  93.  
  94. print "Fundamental matrix error check: %f"%np.dot(np.dot(pt2,F),pt1)
  95. print " "
  96.  
  97.  
  98. # drawing lines on left image
  99. lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F)
  100. lines1 = lines1.reshape(-1,3)
  101. img5,img6 = drawlines(img1,img2,lines1,pts1,pts2)
  102.  
  103. # drawing lines on right image
  104. lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F)
  105. lines2 = lines2.reshape(-1,3)
  106. img3,img4 = drawlines(img2,img1,lines2,pts2,pts1)
  107.  
  108. E = K.T.dot(F).dot(K)
  109.  
  110. print "The essential matrix is"
  111. print E
  112. print
  113.  
  114. U, S, Vt = np.linalg.svd(E)
  115. W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3)
  116.  
  117. first_inliers = []
  118. second_inliers = []
  119. for i in range(len(pts1)):
  120.     # normalize and homogenize the image coordinates
  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. # Determine the correct choice of second camera matrix
  125. # only in one of the four configurations will all the points be in front of both cameras
  126. # First choice: R = U * Wt * Vt, T = +u_3 (See Hartley Zisserman 9.19)
  127.  
  128. R = U.dot(W).dot(Vt)
  129. T = U[:, 2]
  130. if not in_front_of_both_cameras(first_inliers, second_inliers, R, T):
  131.  
  132.     # Second choice: R = U * W * Vt, T = -u_3
  133.     T = - U[:, 2]
  134.     if not in_front_of_both_cameras(first_inliers, second_inliers, R, T):
  135.  
  136.         # Third choice: R = U * Wt * Vt, T = u_3
  137.         R = U.dot(W.T).dot(Vt)
  138.         T = U[:, 2]
  139.  
  140.         if not in_front_of_both_cameras(first_inliers, second_inliers, R, T):
  141.  
  142.             # Fourth choice: R = U * Wt * Vt, T = -u_3
  143.             T = - U[:, 2]
  144.  
  145. # Computing Euler angles
  146.  
  147. thetaX = np.arctan2(R[1][2], R[2][2])
  148. c2 = np.sqrt((R[0][0]*R[0][0] + R[0][1]*R[0][1]))
  149.  
  150. thetaY = np.arctan2(-R[0][2], c2)
  151.  
  152. s1 = np.sin(thetaX)
  153. c1 = np.cos(thetaX)
  154.  
  155. thetaZ = np.arctan2((s1*R[2][0] - c1*R[1][0]), (c1*R[1][1] - s1*R[2][1]))
  156.  
  157. print "Pitch: %f, Yaw: %f, Roll: %f"%(thetaX*180/3.1415, thetaY*180/3.1415, thetaZ*180/3.1415)
  158.  
  159. print "Rotation matrix:"
  160. print R
  161. print
  162. print "Translation vector:"
  163. print T
  164.  
  165. plt.subplot(121),plt.imshow(img5)
  166. plt.subplot(122),plt.imshow(img3)
  167. plt.show()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top