# 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.

Ã—