def two_view_reconstruction(p1, p2, f1, f2, threshold):
assert len(p1) == len(p2)
assert len(p1) >= 5
npoints = len(p1)
# Run 5-point algorithm.
R, t, inliers = csfm.two_view_reconstruction(p1, p2, f1, f2, threshold)
K1 = np.array([[f1, 0, 0], [0, f1, 0], [0, 0, 1.]])
K2 = np.array([[f2, 0, 0], [0, f2, 0], [0, 0, 1.]])
old_inliers = np.zeros(npoints)
for it in range(10):
# Triangulate Features.
P1 = P_from_KRt(K1, np.eye(3), np.zeros(3))
P2 = P_from_KRt(K2, R, t)
Ps = [P1, P2]
Xs = []
inliers = []
for x1, x2 in zip(p1, p2):
X = triangulate(Ps, [x1, x2])
Xs.append(X)
P1X = P1.dot(homogeneous(X))
P2X = P2.dot(homogeneous(X))
e1 = np.linalg.norm(x1 - euclidean(P1X))
e2 = np.linalg.norm(x2 - euclidean(P2X))
inliers.append(e1 < threshold and e2 < threshold and P1X[2] > 0 and P2X[2] > 0)
inliers = np.array(inliers)
Xs = np.array(Xs)
inlier_changes = np.count_nonzero(inliers - old_inliers)
old_inliers = inliers.copy()
if inlier_changes < npoints * 0.05:
break
# Refine R, t
ba = csfm.BundleAdjuster()
ba.add_camera('c1', f1, 0, 0, f1, True)
ba.add_camera('c2', f2, 0, 0, f2, True)
ba.add_shot('s1', 'c1', 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, True)
r = cv2.Rodrigues(R)[0].ravel()
ba.add_shot('s2', 'c2', r[0], r[1], r[2], t[0], t[1], t[2], 0, 0, 0, 1, False)
for i in xrange(npoints):
if inliers[i]:
X = Xs[i]
ba.add_point(str(i), X[0], X[1], X[2], False)
ba.add_observation('s1', str(i), p1[i][0], p1[i][1])
ba.add_observation('s2', str(i), p2[i][0], p2[i][1])
ba.set_loss_function('TruncatedLoss', threshold);
ba.run()
s = ba.get_shot('s2')
R = cv2.Rodrigues((s.rx, s.ry, s.rz))[0]
t = np.array((s.tx, s.ty, s.tz))
t /= np.linalg.norm(t)
return R, t, Xs, np.nonzero(inliers)[0]
def bootstrap_reconstruction(data, graph, im1, im2):
'''Starts a reconstruction using two shots.
'''
print 'Initial reconstruction with', im1, 'and', im2
d1 = data.load_exif(im1)
d2 = data.load_exif(im2)
cameras = data.load_camera_models()
tracks, p1, p2 = dataset.common_tracks(graph, im1, im2)
print 'Number of common tracks', len(tracks)
f1 = d1['focal_prior']
f2 = d2['focal_prior']
threshold = data.config.get('five_point_algo_threshold', 0.006)
ret = csfm.two_view_reconstruction(p1, p2, f1, f2, threshold)
if ret is not None:
R, t, cov, inliers = ret
else:
return None
if len(inliers):
print 'Number of inliers', len(inliers)
reconstruction = {
"cameras": cameras,
"shots" : {
im1: {
"camera": str(d1['camera']),
"rotation": [0.0, 0.0, 0.0],
"translation": [0.0, 0.0, 0.0],
},
im2: {
"camera": str(d2['camera']),
"rotation": list(R),
"translation": list(t),
},
},
"points" : {
},
}
add_gps_position(data, reconstruction['shots'][im1], im1)
add_gps_position(data, reconstruction['shots'][im2], im2)
triangulate_shot_features(
graph, reconstruction, im1,
data.config.get('triangulation_threshold', 0.004),
data.config.get('triangulation_min_ray_angle', 2.0))
print 'Number of reconstructed 3D points :{}'.format(len(reconstruction['points']))
if len(reconstruction['points']) > data.config.get('five_point_algo_min_inliers', 50):
print 'Found initialize good pair', im1 , 'and', im2
return reconstruction
print 'Pair', im1, ':', im2, 'fails'
return None
def two_view_reconstruction_run_csfm(p1, p2, f1, f2, threshold):
return csfm.two_view_reconstruction(p1, p2, f1, f2, threshold)
def two_view_reconstruction_run_csfm(p1, p2, f1, f2, threshold):
return csfm.two_view_reconstruction(p1, p2, f1, f2, threshold)