def back_project(camera, shot, pixel, depth):
K = multiview.K_from_camera(camera)
R = cv2.Rodrigues(np.array(shot['rotation'], dtype=float))[0]
t = shot['translation']
A = K.dot(R)
b = depth * np.array([pixel[0], pixel[1], 1]) - K.dot(t)
return np.linalg.solve(A, b)
def triangulate_track(track,
graph,
reconstruction,
P_by_id,
KR1_by_id,
Kinv_by_id,
reproj_threshold,
min_ray_angle=2.0):
''' Triangulate a track
'''
Ps, Ps_initial, KR1_initial, Kinv_initial = [], [], [], []
xs, xs_initial = [], []
for shot in graph[track]:
if shot in reconstruction['shots']:
if shot not in P_by_id:
s = reconstruction['shots'][shot]
c = reconstruction['cameras'][s['camera']]
P = projection_matrix(c, s)
P_by_id[shot] = P
KR1_by_id[shot] = np.linalg.inv(P[:, :3])
Kinv_by_id[shot] = np.linalg.inv(multiview.K_from_camera(c))
Ps_initial.append(P_by_id[shot])
xs_initial.append(graph[track][shot]['feature'])
KR1_initial.append(KR1_by_id[shot])
Kinv_initial.append(Kinv_by_id[shot])
valid_set = []
if len(Ps_initial) >= 2:
max_angle = 0
for i, j in combinations(range(len(Ps_initial)), 2):
angle = angle_between_rays(KR1_initial[i], xs_initial[i],
KR1_initial[j], xs_initial[j])
if 1:
if i not in valid_set:
valid_set.append(i)
if j not in valid_set:
valid_set.append(j)
max_angle = max(angle, max_angle)
if max_angle > np.radians(min_ray_angle):
for k in valid_set:
Ps.append(np.dot(Kinv_initial[k], Ps_initial[k]))
xx = np.dot(Kinv_initial[k][:2, :],
multiview.homogeneous(np.array(xs_initial[k])))
xs.append(xx[0:2])
X = multiview.triangulate(Ps, xs)
error = 0
Xh = multiview.homogeneous(X)
for P, x in zip(Ps, xs):
xx, yy, zz = P.dot(Xh)
if zz <= 0:
error = 999999999.0
reprojected_x = np.array([xx / zz, yy / zz])
error = max(error, (reprojected_x - x).max())
if error < reproj_threshold:
reconstruction['points'][track] = {
"coordinates": list(X),
}
def resect(data, graph, reconstruction, shot_id):
'''Add a shot to the reconstruction.
'''
xs = []
Xs = []
for track in graph[shot_id]:
if track in reconstruction['points']:
xs.append(graph[shot_id][track]['feature'])
Xs.append(reconstruction['points'][track]['coordinates'])
x = np.array(xs)
X = np.array(Xs)
if len(x) < 5:
return False
exif = data.load_exif(shot_id)
camera_model = exif['camera']
K = multiview.K_from_camera(reconstruction['cameras'][camera_model])
dist = np.array([0, 0, 0, 0.])
# Prior on focal length
R, t, inliers = cv2.solvePnPRansac(X.astype(np.float32),
x.astype(np.float32),
K,
dist,
reprojectionError=data.config.get(
'resection_threshold', 0.004))
if inliers is None:
print 'Resection', shot_id, 'no inliers'
return False
print 'Resection', shot_id, 'inliers:', len(inliers), '/', len(x)
if len(inliers) >= data.config.get('resection_min_inliers', 15):
reconstruction['shots'][shot_id] = {
"camera": camera_model,
"rotation": list(R.flat),
"translation": list(t.flat),
}
add_gps_position(data, reconstruction, shot_id)
return True
else:
return False
def projection_matrix(camera, shot):
K = multiview.K_from_camera(camera)
Rt = Rt_from_shot(shot)
return np.dot(K, Rt)
