Guest User

Posestimation

a guest
Jul 30th, 2015
338
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

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×