/
test.py
190 lines (146 loc) · 5.64 KB
/
test.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
import http_handler as hh
import my_util as mt
import setting as st
import objects as obj
import file_handler as fh
import feature
import register
import cv2
import matplotlib.pyplot as plt
import numpy as np
import pyopengv as gv
def plot():
marker_style = dict(color='cornflowerblue', linestyle=':', marker='o',
markersize=8, markerfacecoloralt='gray')
fig, ax = plt.subplots()
x = []
y = []
for r in road:
x.append(r[1])
y.append(r[0])
ax.plot(x, y, fillstyle='full', **marker_style)
plt.show()
def drawMatches(img1, kp1, img2, kp2, matches):
"""
My own implementation of cv2.drawMatches as OpenCV 2.4.9
does not have this function available but it's supported in
OpenCV 3.0.0
This function takes in two images with their associated
keypoints, as well as a list of DMatch data structure (matches)
that contains which keypoints matched in which images.
An image will be produced where a montage is shown with
the first image followed by the second image beside it.
Keypoints are delineated with circles, while lines are connected
between matching keypoints.
img1,img2 - Grayscale images
kp1,kp2 - Detected list of keypoints through any of the OpenCV keypoint
detection algorithms
matches - A list of matches of corresponding keypoints through any
OpenCV keypoint matching algorithm
"""
# Create a new output image that concatenates the two images together
# (a.k.a) a montage
rows1 = img1.shape[0]
cols1 = img1.shape[1]
rows2 = img2.shape[0]
cols2 = img2.shape[1]
out = np.zeros((max([rows1,rows2]),cols1+cols2,3), dtype='uint8')
# Place the first image to the left
out[:rows1,:cols1] = np.dstack([img1, img1, img1])
# Place the next image to the right of it
out[:rows2,cols1:] = np.dstack([img2, img2, img2])
# For each pair of points we have between both images
# draw circles, then connect a line between them
for m in matches:
# Get the matching keypoints for each of the images
img1_idx = m.queryIdx
img2_idx = m.trainIdx
# x - columns
# y - rows
(x1,y1) = kp1[img1_idx].pt
(x2,y2) = kp2[img2_idx].pt
# Draw a small circle at both co-ordinates
# radius 4
# colour blue
# thickness = 1
cv2.circle(out, (int(x1),int(y1)), 4, (255, 0, 0), 1)
cv2.circle(out, (int(x2)+cols1,int(y2)), 4, (255, 0, 0), 1)
# Draw a line in between the two points
# thickness = 1
# colour blue
# cv2.line(out, (int(x1),int(y1)), (int(x2)+cols1,int(y2)), (255, 0, 0), 1)
# Show the image
cv2.imshow('Matched Features', out)
cv2.waitKey(0)
cv2.destroyWindow('Matched Features')
# Also return the image if you'd like a copy
return out
def ransacTest():
MIN_MATCH_COUNT = 10
img1 = cv2.imread(st.path + "/image/test1.jpg",0) # queryImage
img2 = cv2.imread(st.path + "/image/test2.jpg",0) # trainImage
# Initiate SIFT detector
sift = cv2.SIFT()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1,None)
kp2, des2 = sift.detectAndCompute(img2,None)
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks = 50)
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(des1,des2,k=2)
# store all the good matches as per Lowe's ratio test.
good = []
for m,n in matches:
if m.distance < 0.7*n.distance:
good.append(m)
if len(good)>MIN_MATCH_COUNT:
src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
matchesMask = mask.ravel().tolist()
h,w = img1.shape
pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
dst = cv2.perspectiveTransform(pts,M)
# img2 = cv2.polylines(img2,[np.int32(dst)],True,255,3, cv2.CV_AA)
else:
print "Not enough matches are found - %d/%d" % (len(good),MIN_MATCH_COUNT)
matchesMask = None
img3 = drawMatches(img1,kp1,img2,kp2,good)
plt.imshow(img3, 'gray'),plt.show()
def snapRoadTest():
fh.cleanDir(st.path + "image/sfm/", ".jpeg")
interval = 0.0001
path = [(40.694051, -73.983359), (40.690187, -73.981525)]
road = hh.parseJsonRoad(hh.snapToRoad(path))
road = mt.interpolate(road, interval)
mt.filterPath(road, interval)
hh.buildDataset(st.path + "image/sfm/images/", road, obj.CameraPara(size=(800, 800), fov=120, heading=[0], pitch=10))
print road
def fivePointMatchTest(point1, point2):
result = gv.relative_pose_ransac(point1, point2, "NISTER", 0.01, 1000)
return result
def triangulationTest(src, dst, position, rotation):
points1 = gv.triangulation_triangulate(src, dst, position, rotation)
# print points1
points2 = gv.triangulation_triangulate2(src, dst, position, rotation)
# print points2
if __name__ == '__main__':
# snapRoadTest()
# src, dst, result = fivePointMatchTest()
# triangulationTest(src, dst, result[:, :3], result[:, 3])
# img1 = st.path + "test_1.jpeg"
# img2 = st.path + "test_2.jpeg"
# R, t = register.relativePositionEstimation(img2, img1)
# x = np.arctan2(R[2][1], R[2][2]) * 180 / np.pi
# y = np.arctan2(-R[2][0], np.linalg.norm([R[2][1], R[2][2]], ord=2)) * 180 / np.pi
# z = np.arctan2(R[1][0], R[0][0]) * 180 / np.pi
# print (x, y, z)
# print t
# rp = np.array([40.69435, -73.98329, 0])
# rr = np.eye(3)
# position, rotation = register.positionEstimation(rp, rr, t, R)
# print position
# print rotation
# print (rp[0]/position[0], rp[1]/position[1])
ransacTest()