-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.py
187 lines (146 loc) · 8.2 KB
/
main.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
import glob
import sys
import time
import haversine as hs
from haversine import Unit
import cv2
import numpy as np
import Matcher.match_image
from Matcher import match_image
import math
import os
import csv
distCoeffs = np.array([-0.11143032409169303,
0.0874541163945344,
0,
-8.486542026836708e-06,
0.00014540126479700144]
, dtype = "double")
# distCoeffs = np.zeros((5, 1))
intrisicMat = np.array([[1174.6973334747497,0,1001.7220922936314],
[0,1186.5886209150665,765.2708767726933],
[0,0,1]]
, dtype = "double")
def solve_image_pairs():
if method == 'sift':
H_found, correct_matched_map, correct_matched_img = match_image.sift_match(map, img)
elif method == 'spp':
H_found, correct_matched_map, correct_matched_img = match_image.spp_match(map_path, img_path)
elif method == 'd2net':
H_found, correct_matched_map, correct_matched_img = match_image.d2net_match(map_path, img_path)
else:
print("Wrong method instruct")
return H_found, correct_matched_map, correct_matched_img
def direct_match_solve():
h, w, _ = (cv2.imread(img_path)).shape
gt_map_lon_res = (float(map_path.split("@")[4]) - float(map_path.split("@")[2])) / map.shape[1]
gt_map_lat_res = (float(map_path.split("@")[5]) - float(map_path.split("@")[3])) / map.shape[0]
LT_lon = 120.42114259488751
LT_lat = 36.604504047017464
RB_lon = 120.4568481612987
RB_lat = 36.586863027841225
gt_img_lon, gt_img_lat = (float(img_path.split("@")[2]) - LT_lon) / gt_map_lon_res, (
float(img_path.split("@")[3]) - LT_lat) / gt_map_lat_res
gt_map_lon, gt_map_lat = (float(map_path.split("@")[2]) - LT_lon) / gt_map_lon_res, (
float(map_path.split("@")[3]) - LT_lat) / gt_map_lat_res
H_found, map_points_3D, img_points_2D = solve_image_pairs()
H_found_inv = np.linalg.inv(H_found)
H_found_inv = H_found_inv / H_found_inv[-1, -1]
# H_found_inv = H_found
# mind the index of the first dimension and the second dimension
[img_cx, img_cy, _] = np.dot(H_found_inv, np.array([w/2.0, h/2.0, 1]))
img_c = np.array([img_cy, img_cx])
(es_img_lat, es_img_lon) = img_c + np.array([gt_map_lat, gt_map_lon])
dist_error = math.sqrt((gt_img_lat - es_img_lat) ** 2 + (gt_img_lon - es_img_lon) ** 2) * 0.381
dir_sub_path = img_path.replace(img_path.split('\\')[-1], '')
if not os.path.exists(dir_sub_path + '\\match_DM_{}.jpg'.format(dist_error)):
os.rename(dir_sub_path + '\\match.jpg', dir_sub_path + '\\match_DM_{}.jpg'.format(dist_error))
print("gt_img_lat: ", gt_img_lat, "es_img_lat: ", es_img_lat)
print("gt_img_lon: ", gt_img_lon, "es_img_lon: ", es_img_lon)
print("localization error is : {} m".format(dist_error))
return dist_error
def dlk_match_solve():
h, w, _ = (cv2.imread(img_path)).shape
gt_map_lon_res = (float(map_path.split("@")[4]) - float(map_path.split("@")[2])) / map.shape[1]
gt_map_lat_res = (float(map_path.split("@")[5]) - float(map_path.split("@")[3])) / map.shape[0]
LT_lon = 120.42114259488751
LT_lat = 36.604504047017464
RB_lon = 120.4568481612987
RB_lat = 36.586863027841225
gt_img_lon, gt_img_lat = (float(img_path.split("@")[2]) - LT_lon) / gt_map_lon_res, (
float(img_path.split("@")[3]) - LT_lat) / gt_map_lat_res
gt_map_lon, gt_map_lat = (float(map_path.split("@")[2]) - LT_lon) / gt_map_lon_res, (
float(map_path.split("@")[3]) - LT_lat) / gt_map_lat_res
H_found = match_image.dlk_match(map_path, img_path)
H_found_inv = np.linalg.inv(H_found)
H_found_inv = H_found_inv / H_found_inv[-1, -1]
# H_found_inv = H_found
# mind the index of the first dimension and the second dimension
[img_cx, img_cy, _] = np.dot(H_found_inv, np.array([w / 2.0, h / 2.0, 1]))
img_c = np.array([img_cy, img_cx])
(es_img_lat, es_img_lon) = img_c + np.array([gt_map_lat, gt_map_lon])
dist_error = math.sqrt((gt_img_lat - es_img_lat) ** 2 + (gt_img_lon - es_img_lon) ** 2) * 0.381
dir_sub_path = img_path.replace(img_path.split('\\')[-1], '')
if not os.path.exists(dir_sub_path + '\\match_DL_{}.jpg'.format(dist_error)):
os.rename(dir_sub_path + '\\match.jpg', dir_sub_path + '\\match_DLs_{}.jpg'.format(dist_error))
print("gt_img_lat: ", gt_img_lat, "es_img_lat: ", es_img_lat)
print("gt_img_lon: ", gt_img_lon, "es_img_lon: ", es_img_lon)
print("localization error is : {} m".format(dist_error))
return dist_error
def PnP_solve():
gt_map_lon_res = (float(map_path.split("@")[4])-float(map_path.split("@")[2])) / map.shape[1]
gt_map_lat_res = (float(map_path.split("@")[5])-float(map_path.split("@")[3])) / map.shape[0]
# longitude and latitude for the whole map
LT_lon = 120.42114259488751
LT_lat = 36.604504047017464
RB_lon = 120.4568481612987
RB_lat = 36.586863027841225
gt_img_lon, gt_img_lat = (float(img_path.split("@")[2])-LT_lon)/gt_map_lon_res, (float(img_path.split("@")[3])-LT_lat)/gt_map_lat_res
gt_map_lon, gt_map_lat = (float(map_path.split("@")[2])-LT_lon)/gt_map_lon_res, (float(map_path.split("@")[3])-LT_lat)/gt_map_lat_res
_, map_points_3D, img_points_2D = solve_image_pairs()
img_points_2D = np.array(img_points_2D, dtype = "double").squeeze()
map_points_3D = np.array(map_points_3D, dtype = "double").squeeze()
map_points_3D = map_points_3D + np.array([gt_map_lat, gt_map_lon])
map_points_3D = np.concatenate((map_points_3D, np.zeros((map_points_3D.shape[0], 1))), axis=1)
# flag=cv2.SOLVEPNP_EPNP might be wrong
# https://blog.csdn.net/cocoaqin/article/details/77848588
# success, vector_rotation, vector_translation = cv2.solvePnP(map_points_3D, img_points_2D, intrisicMat, distCoeffs, flags=cv2.SOLVEPNP_EPNP)
success, vector_rotation, vector_translation, _ = cv2.solvePnPRansac(map_points_3D, img_points_2D, intrisicMat, distCoeffs, flags=cv2.SOLVEPNP_EPNP, confidence=0.9999, iterationsCount=10000, reprojectionError=13)
R, _ = cv2.Rodrigues(vector_rotation)
inv_vector_translation = np.dot(-np.linalg.inv(R), vector_translation)
es_img_lon, es_img_lat = inv_vector_translation[1], inv_vector_translation[0]
dist_error = math.sqrt((gt_img_lat - es_img_lat[0]) ** 2 + (gt_img_lon - es_img_lon[0]) ** 2) * 0.381
dir_sub_path = img_path.replace(img_path.split('\\')[-1], '')
if not os.path.exists(dir_sub_path + '\\match_PnP_{}.jpg'.format(dist_error)):
os.rename(dir_sub_path + '\\match.jpg', dir_sub_path + '\\match_PnP_{}.jpg'.format(dist_error))
print("gt_img_lat: ", gt_img_lat, "es_img_lat: ", es_img_lat[0])
print("gt_img_lon: ", gt_img_lon, "es_img_lon: ", es_img_lon[0])
print("localization error is : {} m".format(dist_error))
return dist_error
if __name__ == '__main__':
dir_path = '.\\data\\sample\\*'
if os.path.exists('./data/result.csv'):
os.remove('./data/result.csv')
for dir_sub_path in glob.glob(dir_path):
img_path, map_path = glob.glob(dir_sub_path+'\\*.png')
img = cv2.imread(img_path)
map = cv2.imread(map_path)
method = 'd2net'
start_time = time.time()
print("\n=====PnP method=====")
PnP_dist_error = PnP_solve()
PnP_runtime = time.time() - start_time
print("PnP running time is : {} s".format(PnP_runtime))
print("\n=====direct align method=====")
DrM_dist_error = direct_match_solve()
DrM_runtime = time.time() - start_time - PnP_runtime
print("direct align running time is : {} s".format(DrM_runtime))
# print("\n=====direct align method=====")
# DlM_dist_error = dlk_match_solve()
# DlM_runtime = time.time() - start_time - PnP_runtime
# print("direct align running time is : {} s".format(DlM_runtime))
with open('./data/result.csv', 'a', encoding = 'UTF8', newline = '') as f:
writer = csv.writer(f)
# line = [img_path, PnP_runtime, PnP_dist_error, DrM_runtime, DrM_dist_error, DlM_runtime, DlM_dist_error]
line = [img_path, PnP_runtime, PnP_dist_error, DrM_runtime, DrM_dist_error]
writer.writerow(line)