def test_pair_with_shot_point():
"""Simple two camera test with a point constraint for anchoring"""
sa = csfm.BundleAdjuster()
sa.add_shot('1', 'cam1', [0, 0, 0], [1e-3, 1e-3, 1e-3], False)
sa.add_shot('2', 'cam1', [0, 0, 0], [1e-3, 1e-3, 1e-3], False)
sa.add_point('p1', [0, 0, 0], False)
sa.add_reconstruction('12', False)
sa.add_reconstruction_shot('12', 4, '1')
sa.add_reconstruction_shot('12', 4, '2')
sa.set_scale_sharing('12', True)
sa.add_relative_motion(
csfm.BARelativeMotion('12', '1', '12', '2', [0, 0, 0], [-1, 0, 0]))
sa.add_point_position_shot('p1', '1', '12', [1, 0, 0], 1, csfm.XYZ)
sa.add_point_position_shot('p1', '2', '12', [-1, 0, 0], 1, csfm.XYZ)
sa.add_point_bearing_shot('p1', '1', '12', [1, 0, 0], 2e-3)
sa.add_point_position_world('p1', [1, 0, 0], 1, csfm.XYZ)
sa.run()
s1 = sa.get_shot('1')
s2 = sa.get_shot('2')
r12 = sa.get_reconstruction('12')
p1 = sa.get_point('p1')
assert np.allclose(s1.t, [0.5, 0, 0], atol=1e-2)
assert np.allclose(s2.t, [-1.5, 0, 0], atol=1e-2)
assert np.allclose(p1.p, [1, 0, 0], atol=1e-6)
assert np.allclose(r12.get_scale('1'), 0.5)
assert np.allclose(r12.get_scale('2'), 0.5)
def test_four_cams_one_fixed():
"""Four cameras, one reconstruction"""
sa = csfm.BundleAdjuster()
sa.add_shot('1', 'cam1', [0, 0, 0], [0, 0, 0], True)
sa.add_shot('2', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_shot('3', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_shot('4', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_reconstruction('1234', False)
sa.add_reconstruction_shot('1234', 1, '1')
sa.add_reconstruction_shot('1234', 1, '2')
sa.add_reconstruction_shot('1234', 1, '3')
sa.add_reconstruction_shot('1234', 1, '4')
sa.set_scale_sharing('1234', True)
sa.add_relative_motion(
csfm.BARelativeMotion('1234', '1', '1234', '2', [0, 0, 0], [-1, 0, 0]))
sa.add_relative_motion(
csfm.BARelativeMotion('1234', '1', '1234', '3', [0, 0, 0], [0, -1, 0]))
sa.add_relative_motion(
csfm.BARelativeMotion('1234', '1', '1234', '4', [0, 0, 0], [0, 0, -1]))
sa.add_absolute_position('1', [100, 0, 0], 1, '1')
sa.add_absolute_position('2', [2, 0, 0], 1, '2')
sa.add_absolute_position('3', [0, 2, 0], 1, '3')
sa.run()
s1 = sa.get_shot('1')
s2 = sa.get_shot('2')
s3 = sa.get_shot('3')
s4 = sa.get_shot('4')
assert np.allclose(s1.t, [0, 0, 0], atol=1e-6)
assert np.allclose(s2.t, [-2, 0, 0], atol=1e-6)
assert np.allclose(s3.t, [0, -2, 0], atol=1e-6)
assert np.allclose(s4.t, [0, 0, -2], atol=1e-6)
def bundle(graph, reconstruction, config):
'''Bundle adjust a reconstruction.
'''
start = time.time()
ba = csfm.BundleAdjuster()
for k, v in reconstruction['cameras'].items():
ba.add_camera(str(k), v['focal'], v['k1'], v['k2'], v['exif_focal'],
False)
for k, v in reconstruction['shots'].items():
r = v['rotation']
t = v['translation']
g = v['gps_position']
ba.add_shot(str(k), str(v['camera']), r[0], r[1], r[2], t[0], t[1],
t[2], g[0], g[1], g[2], v['gps_dop'], False)
for k, v in reconstruction['points'].items():
x = v['coordinates']
ba.add_point(str(k), x[0], x[1], x[2], False)
for shot in reconstruction['shots']:
for track in graph[shot]:
if track in reconstruction['points']:
ba.add_observation(str(shot), str(track),
*graph[shot][track]['feature'])
ba.set_loss_function(config.get('loss_function', 'TruncatedLoss'),
config.get('loss_function_threshold', 0.004))
ba.set_reprojection_error_sd(config.get('reprojection_error_sd', 1))
ba.set_focal_prior_sd(config.get('exif_focal_sd', 999))
setup = time.time()
ba.run()
print ba.brief_report()
run = time.time()
for k, v in reconstruction['cameras'].items():
c = ba.get_camera(str(k))
v['focal'] = c.focal
v['k1'] = c.k1
v['k2'] = c.k2
for k, v in reconstruction['shots'].items():
s = ba.get_shot(str(k))
v['rotation'] = [s.rx, s.ry, s.rz]
v['translation'] = [s.tx, s.ty, s.tz]
for k, v in reconstruction['points'].items():
p = ba.get_point(str(k))
v['coordinates'] = [p.x, p.y, p.z]
teardown = time.time()
print 'setup/run/teardown {0}/{1}/{2}'.format(setup - start, run - setup,
teardown - run)
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 test_unicode_strings_in_bundle():
"""Test that byte and unicode strings can be used as camera ids."""
ba = csfm.BundleAdjuster()
unicode_id = u"A\xb2"
byte_id = b"A_2"
ba.add_equirectangular_camera(unicode_id)
ba.add_equirectangular_camera(byte_id)
def bundle_single_view(graph, reconstruction, shot_id, config):
"""Bundle adjust a single camera."""
ba = csfm.BundleAdjuster()
shot = reconstruction.shots[shot_id]
camera = shot.camera
if camera.projection_type == 'perspective':
ba.add_perspective_camera(str(camera.id), camera.focal, camera.k1,
camera.k2, camera.focal_prior,
camera.k1_prior, camera.k2_prior, True)
elif camera.projection_type == 'fisheye':
ba.add_fisheye_camera(str(camera.id), camera.focal, camera.k1,
camera.k2, camera.focal_prior, camera.k1_prior,
camera.k2_prior, True)
elif camera.projection_type in ['equirectangular', 'spherical']:
ba.add_equirectangular_camera(str(camera.id))
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(str(shot.id), str(camera.id), r[0], r[1], r[2], t[0], t[1],
t[2], False)
for track_id in graph[shot_id]:
if track_id in reconstruction.points:
track = reconstruction.points[track_id]
x = track.coordinates
ba.add_point(str(track_id), x[0], x[1], x[2], True)
ba.add_observation(str(shot_id), str(track_id),
*graph[shot_id][track_id]['feature'])
if config['bundle_use_gps']:
g = shot.metadata.gps_position
ba.add_position_prior(str(shot.id), g[0], g[1], g[2],
shot.metadata.gps_dop)
ba.set_loss_function(config.get('loss_function', 'SoftLOneLoss'),
config.get('loss_function_threshold', 1))
ba.set_reprojection_error_sd(config.get('reprojection_error_sd', 0.004))
ba.set_internal_parameters_prior_sd(
config.get('exif_focal_sd', 0.01),
config.get('radial_distorsion_k1_sd', 0.01),
config.get('radial_distorsion_k2_sd', 0.01))
setup = time.time()
ba.set_num_threads(config['processes'])
ba.run()
run = time.time()
s = ba.get_shot(str(shot_id))
shot.pose.rotation = [s.rx, s.ry, s.rz]
shot.pose.translation = [s.tx, s.ty, s.tz]
logger.debug('Single-Bundle run {0}'.format(run - setup))
def test_sigleton():
"""Single camera test"""
sa = csfm.BundleAdjuster()
sa.add_shot('1', 'cam1', [0.5, 0, 0], [0, 0, 0], False)
sa.add_absolute_position('1', [1, 0, 0], 1, '1')
sa.add_absolute_up_vector('1', [0, -1, 0], 1)
sa.add_absolute_pan('1', np.radians(180), 1)
sa.run()
s1 = sa.get_shot('1')
assert np.allclose(s1.t, [1, 0, 0], atol=1e-6)
def bundle_single_view(graph, reconstruction, shot_id, config):
"""Bundle adjust a single camera."""
ba = csfm.BundleAdjuster()
shot = reconstruction.shots[shot_id]
camera = shot.camera
_add_camera_to_bundle(ba, camera, constant=True)
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(
shot.id, camera.id,
r[0], r[1], r[2],
t[0], t[1], t[2],
False
)
for track_id in graph[shot_id]:
if track_id in reconstruction.points:
track = reconstruction.points[track_id]
x = track.coordinates
ba.add_point(track_id, x[0], x[1], x[2], True)
ba.add_observation(shot_id, track_id,
*graph[shot_id][track_id]['feature'])
if config['bundle_use_gps']:
g = shot.metadata.gps_position
ba.add_position_prior(shot.id, g[0], g[1], g[2],
shot.metadata.gps_dop)
ba.set_loss_function(config['loss_function'],
config['loss_function_threshold'])
ba.set_reprojection_error_sd(config['reprojection_error_sd'])
ba.set_internal_parameters_prior_sd(
config['exif_focal_sd'],
config['principal_point_sd'],
config['radial_distorsion_k1_sd'],
config['radial_distorsion_k2_sd'],
config['radial_distorsion_p1_sd'],
config['radial_distorsion_p2_sd'],
config['radial_distorsion_k3_sd'])
ba.set_num_threads(config['processes'])
ba.set_max_num_iterations(10)
ba.set_linear_solver_type("DENSE_QR")
ba.run()
logger.debug(ba.brief_report())
s = ba.get_shot(shot_id)
shot.pose.rotation = [s.rx, s.ry, s.rz]
shot.pose.translation = [s.tx, s.ty, s.tz]
def bundle_single_view(graph, reconstruction, shot_id, config):
'''Bundle adjust a single camera
'''
ba = csfm.BundleAdjuster()
shot = reconstruction['shots'][shot_id]
camera_id = shot['camera']
camera = reconstruction['cameras'][camera_id]
pt = camera.get('projection_type', 'perspective')
if pt == 'perspective':
ba.add_perspective_camera(str(camera_id), camera['focal'],
camera['k1'], camera['k2'],
camera['focal_prior'],
camera.get('k1_prior', 0.0),
camera.get('k2_prior', 0.0), True)
elif pt in ['equirectangular', 'spherical']:
ba.add_equirectangular_camera(str(camera_id))
r = shot['rotation']
t = shot['translation']
g = shot['gps_position']
ba.add_shot(str(shot_id), str(camera_id), r[0], r[1], r[2], t[0], t[1],
t[2], g[0], g[1], g[2], shot['gps_dop'], False)
for track_id in graph[shot_id]:
if track_id in reconstruction['points']:
track = reconstruction['points'][track_id]
x = track['coordinates']
ba.add_point(str(track_id), x[0], x[1], x[2], True)
ba.add_observation(str(shot_id), str(track_id),
*graph[shot_id][track_id]['feature'])
ba.set_loss_function(config.get('loss_function', 'SoftLOneLoss'),
config.get('loss_function_threshold', 1))
ba.set_reprojection_error_sd(config.get('reprojection_error_sd', 0.004))
ba.set_internal_parameters_prior_sd(
config.get('exif_focal_sd', 0.01),
config.get('radial_distorsion_k1_sd', 0.01),
config.get('radial_distorsion_k2_sd', 0.01))
ba.set_num_threads(config['processes'])
ba.run()
s = ba.get_shot(str(shot_id))
shot['rotation'] = [s.rx, s.ry, s.rz]
shot['translation'] = [s.tx, s.ty, s.tz]
def test_linear_motion_prior_rotation():
"""Three cameras, middle has no gps or orientation info"""
sa = csfm.BundleAdjuster()
sa.add_shot('1', 'cam1', [0, 0, 0], [0, 0, 0], True)
sa.add_shot('2', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_shot('3', 'cam1', [0, 1, 0], [0, 0, 0], True)
sa.add_reconstruction('123', False)
sa.add_reconstruction_shot('123', 1, '1')
sa.add_reconstruction_shot('123', 1, '2')
sa.add_reconstruction_shot('123', 1, '3')
sa.set_scale_sharing('123', True)
sa.add_linear_motion('1', '2', '3', 0.3, 0.1, 0.1)
sa.run()
s2 = sa.get_shot('2')
assert np.allclose(s2.r, [0, 0.3, 0], atol=1e-6)
def test_sigleton_pan_tilt_roll():
"""Single camera test with pan, tilt, roll priors."""
pan, tilt, roll = 1, 0.3, 0.2
sa = csfm.BundleAdjuster()
sa.add_shot('1', 'cam1', [0.5, 0, 0], [0, 0, 0], False)
sa.add_absolute_position('1', [1, 0, 0], 1, '1')
sa.add_absolute_pan('1', pan, 1)
sa.add_absolute_tilt('1', tilt, 1)
sa.add_absolute_roll('1', roll, 1)
sa.run()
s1 = sa.get_shot('1')
pose = types.Pose(s1.r, s1.t)
assert np.allclose(pose.get_origin(), [1, 0, 0], atol=1e-6)
ptr = geometry.ptr_from_rotation(pose.get_rotation_matrix())
assert np.allclose(ptr, (pan, tilt, roll))
def test_four_cams_single_reconstruction_non_rigid():
"""Four cameras, one reconstruction"""
sa = csfm.BundleAdjuster()
sa.add_shot('1', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_shot('2', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_shot('3', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_shot('4', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_reconstruction('1234', False)
sa.add_reconstruction_shot('1234', 1, '1')
sa.add_reconstruction_shot('1234', 1, '2')
sa.add_reconstruction_shot('1234', 1, '3')
sa.add_reconstruction_shot('1234', 1, '4')
sa.set_scale_sharing('1234', False)
sa.add_relative_similarity(
csfm.BARelativeSimilarity('1234', '1', '1234', '2', [0, 0, 0],
[-1, 0, 0], 1))
sa.add_relative_similarity(
csfm.BARelativeSimilarity('1234', '2', '1234', '3', [0, 0, 0],
[-1, -1, 0], 1))
sa.add_relative_similarity(
csfm.BARelativeSimilarity('1234', '3', '1234', '4', [0, 0, 0],
[0, -1, 0], 1))
sa.add_absolute_position('1', [0, 0, 0], 1, '1')
sa.add_absolute_position('2', [2, 0, 0], 1, '2')
sa.add_absolute_position('3', [4, 2, 0], 1, '3')
sa.add_absolute_position('4', [4, 4, 0], 1, '4')
sa.run()
s1 = sa.get_shot('1')
s2 = sa.get_shot('2')
s3 = sa.get_shot('3')
s4 = sa.get_shot('4')
r1234 = sa.get_reconstruction('1234')
assert np.allclose(s1.t, [0, 0, 0], atol=1e-6)
assert np.allclose(s2.t, [-2, 0, 0], atol=1e-6)
assert np.allclose(s3.t, [-4, -2, 0], atol=1e-6)
assert np.allclose(s4.t, [-4, -4, 0], atol=1e-6)
assert np.allclose(r1234.get_scale('1'), 0.5)
assert np.allclose(r1234.get_scale('2'), 0.5)
assert np.allclose(r1234.get_scale('3'), 0.5)
assert np.allclose(r1234.get_scale('4'), 0.5)
def test_pair():
"""Simple two camera test"""
sa = csfm.BundleAdjuster()
sa.add_shot('1', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_shot('2', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_reconstruction('12', False)
sa.add_reconstruction_shot('12', 4, '1')
sa.add_reconstruction_shot('12', 4, '2')
sa.set_scale_sharing('12', True)
sa.add_relative_motion(
csfm.BARelativeMotion('12', '1', '12', '2', [0, 0, 0], [-1, 0, 0]))
sa.add_absolute_position('1', [0, 0, 0], 1, '1')
sa.add_absolute_position('2', [2, 0, 0], 1, '2')
sa.run()
s1 = sa.get_shot('1')
s2 = sa.get_shot('2')
r12 = sa.get_reconstruction('12')
assert np.allclose(s1.t, [0, 0, 0], atol=1e-6)
assert np.allclose(s2.t, [-2, 0, 0], atol=1e-6)
assert np.allclose(r12.get_scale('1'), 0.5)
assert np.allclose(r12.get_scale('2'), 0.5)
def test_linear_motion_prior_position():
"""Three cameras, middle has no gps info. Translation only"""
sa = csfm.BundleAdjuster()
sa.add_shot('1', 'cam1', [0, 0, 0], [0, 0, 0], True)
sa.add_shot('2', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_shot('3', 'cam1', [0, 0, 0], [0, 0, 0], False)
sa.add_reconstruction('123', False)
sa.add_reconstruction_shot('123', 1, '1')
sa.add_reconstruction_shot('123', 1, '2')
sa.add_reconstruction_shot('123', 1, '3')
sa.set_scale_sharing('123', True)
sa.add_absolute_position('1', [0, 0, 0], 1, '1')
sa.add_absolute_position('3', [2, 0, 0], 1, '3')
sa.add_linear_motion('1', '2', '3', 0.5, 0.1, 0.1)
sa.run()
s1 = sa.get_shot('1')
s2 = sa.get_shot('2')
s3 = sa.get_shot('3')
assert np.allclose(s1.t, [0, 0, 0], atol=1e-6)
assert np.allclose(s2.t, [-1, 0, 0], atol=1e-6)
assert np.allclose(s3.t, [-2, 0, 0], atol=1e-6)
def bundle(graph, reconstruction, gcp, config):
"""Bundle adjust a reconstruction."""
fix_cameras = not config['optimize_camera_parameters']
chrono = Chronometer()
ba = csfm.BundleAdjuster()
for camera in reconstruction.cameras.values():
_add_camera_to_bundle(ba, camera, fix_cameras)
for shot in reconstruction.shots.values():
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(
shot.id, shot.camera.id,
r[0], r[1], r[2],
t[0], t[1], t[2],
False
)
for point in reconstruction.points.values():
x = point.coordinates
ba.add_point(point.id, x[0], x[1], x[2], False)
for shot_id in reconstruction.shots:
if shot_id in graph:
for track in graph[shot_id]:
if track in reconstruction.points:
ba.add_observation(shot_id, track,
*graph[shot_id][track]['feature'])
if config['bundle_use_gps']:
for shot in reconstruction.shots.values():
g = shot.metadata.gps_position
ba.add_position_prior(shot.id, g[0], g[1], g[2],
shot.metadata.gps_dop)
if config['bundle_use_gcp'] and gcp:
_add_gcp_to_bundle(ba, gcp, reconstruction)
ba.set_loss_function(config['loss_function'],
config['loss_function_threshold'])
ba.set_reprojection_error_sd(config['reprojection_error_sd'])
ba.set_internal_parameters_prior_sd(
config['exif_focal_sd'],
config['principal_point_sd'],
config['radial_distorsion_k1_sd'],
config['radial_distorsion_k2_sd'],
config['radial_distorsion_p1_sd'],
config['radial_distorsion_p2_sd'],
config['radial_distorsion_k3_sd'])
ba.set_num_threads(config['processes'])
ba.set_max_num_iterations(50)
ba.set_linear_solver_type("SPARSE_SCHUR")
chrono.lap('setup')
ba.run()
chrono.lap('run')
for camera in reconstruction.cameras.values():
_get_camera_from_bundle(ba, camera)
for shot in reconstruction.shots.values():
s = ba.get_shot(shot.id)
shot.pose.rotation = [s.rx, s.ry, s.rz]
shot.pose.translation = [s.tx, s.ty, s.tz]
for point in reconstruction.points.values():
p = ba.get_point(point.id)
point.coordinates = [p.x, p.y, p.z]
point.reprojection_error = p.reprojection_error
chrono.lap('teardown')
logger.debug(ba.brief_report())
report = {
'wall_times': dict(chrono.lap_times()),
'brief_report': ba.brief_report(),
}
return report
def bundle(graph, reconstruction, gcp, config):
"""Bundle adjust a reconstruction."""
fix_cameras = not config['optimize_camera_parameters']
chrono = Chronometer()
ba = csfm.BundleAdjuster()
for camera in reconstruction.cameras.values():
_add_camera_to_bundle(ba, camera, fix_cameras)
for shot in reconstruction.shots.values():
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(shot.id, shot.camera.id, r, t, False)
for point in reconstruction.points.values():
ba.add_point(point.id, point.coordinates, False)
for shot_id in reconstruction.shots:
if shot_id in graph:
for track in graph[shot_id]:
if track in reconstruction.points:
point = graph[shot_id][track]['feature']
scale = graph[shot_id][track]['feature_scale']
ba.add_point_projection_observation(
shot_id, track, point[0], point[1], scale)
if config['bundle_use_gps']:
for shot in reconstruction.shots.values():
g = shot.metadata.gps_position
ba.add_position_prior(shot.id, g[0], g[1], g[2],
shot.metadata.gps_dop)
if config['bundle_use_gcp'] and gcp:
_add_gcp_to_bundle(ba, gcp, reconstruction.shots)
align_method = config['align_method']
if align_method == 'auto':
align_method = align.detect_alignment_constraints(config, reconstruction, gcp)
if align_method == 'orientation_prior':
if config['align_orientation_prior'] == 'vertical':
for shot_id in reconstruction.shots:
ba.add_absolute_up_vector(shot_id, [0, 0, -1], 1e-3)
if config['align_orientation_prior'] == 'horizontal':
for shot_id in reconstruction.shots:
ba.add_absolute_up_vector(shot_id, [0, 1, 0], 1e-3)
ba.set_point_projection_loss_function(config['loss_function'],
config['loss_function_threshold'])
ba.set_internal_parameters_prior_sd(
config['exif_focal_sd'],
config['principal_point_sd'],
config['radial_distorsion_k1_sd'],
config['radial_distorsion_k2_sd'],
config['radial_distorsion_p1_sd'],
config['radial_distorsion_p2_sd'],
config['radial_distorsion_k3_sd'])
ba.set_num_threads(config['processes'])
ba.set_max_num_iterations(50)
ba.set_linear_solver_type("SPARSE_SCHUR")
chrono.lap('setup')
ba.run()
chrono.lap('run')
for camera in reconstruction.cameras.values():
_get_camera_from_bundle(ba, camera)
for shot in reconstruction.shots.values():
s = ba.get_shot(shot.id)
shot.pose.rotation = [s.r[0], s.r[1], s.r[2]]
shot.pose.translation = [s.t[0], s.t[1], s.t[2]]
for point in reconstruction.points.values():
p = ba.get_point(point.id)
point.coordinates = [p.p[0], p.p[1], p.p[2]]
point.reprojection_errors = p.reprojection_errors
chrono.lap('teardown')
logger.debug(ba.brief_report())
report = {
'wall_times': dict(chrono.lap_times()),
'brief_report': ba.brief_report(),
}
return report
def bundle(graph, reconstruction, gcp, config):
"""Bundle adjust a reconstruction."""
fix_cameras = not config['optimize_camera_parameters']
start = time.time()
ba = csfm.BundleAdjuster()
for camera in reconstruction.cameras.values():
if camera.projection_type == 'perspective':
ba.add_perspective_camera(str(camera.id), camera.focal, camera.k1,
camera.k2, camera.focal_prior,
camera.k1_prior, camera.k2_prior,
fix_cameras)
elif camera.projection_type == 'fisheye':
ba.add_fisheye_camera(str(camera.id), camera.focal, camera.k1,
camera.k2, camera.focal_prior,
camera.k1_prior, camera.k2_prior,
fix_cameras)
elif camera.projection_type in ['equirectangular', 'spherical']:
ba.add_equirectangular_camera(str(camera.id))
for shot in reconstruction.shots.values():
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(str(shot.id), str(shot.camera.id), r[0], r[1], r[2], t[0],
t[1], t[2], False)
for point in reconstruction.points.values():
x = point.coordinates
ba.add_point(str(point.id), x[0], x[1], x[2], False)
for shot_id in reconstruction.shots:
if shot_id in graph:
for track in graph[shot_id]:
if track in reconstruction.points:
ba.add_observation(str(shot_id), str(track),
*graph[shot_id][track]['feature'])
if config['bundle_use_gps']:
for shot in reconstruction.shots.values():
g = shot.metadata.gps_position
ba.add_position_prior(str(shot.id), g[0], g[1], g[2],
shot.metadata.gps_dop)
if config['bundle_use_gcp'] and gcp:
for observation in gcp:
if observation.shot_id in reconstruction.shots:
ba.add_ground_control_point_observation(
str(observation.shot_id), observation.coordinates[0],
observation.coordinates[1], observation.coordinates[2],
observation.shot_coordinates[0],
observation.shot_coordinates[1])
ba.set_loss_function(config.get('loss_function', 'SoftLOneLoss'),
config.get('loss_function_threshold', 1))
ba.set_reprojection_error_sd(config.get('reprojection_error_sd', 0.004))
ba.set_internal_parameters_prior_sd(
config.get('exif_focal_sd', 0.01),
config.get('radial_distorsion_k1_sd', 0.01),
config.get('radial_distorsion_k2_sd', 0.01))
setup = time.time()
ba.set_num_threads(config['processes'])
ba.run()
run = time.time()
logger.debug(ba.brief_report())
for camera in reconstruction.cameras.values():
if camera.projection_type == 'perspective':
c = ba.get_perspective_camera(str(camera.id))
camera.focal = c.focal
camera.k1 = c.k1
camera.k2 = c.k2
elif camera.projection_type == 'fisheye':
c = ba.get_fisheye_camera(str(camera.id))
camera.focal = c.focal
camera.k1 = c.k1
camera.k2 = c.k2
for shot in reconstruction.shots.values():
s = ba.get_shot(str(shot.id))
shot.pose.rotation = [s.rx, s.ry, s.rz]
shot.pose.translation = [s.tx, s.ty, s.tz]
for point in reconstruction.points.values():
p = ba.get_point(str(point.id))
point.coordinates = [p.x, p.y, p.z]
point.reprojection_error = p.reprojection_error
teardown = time.time()
logger.debug('Bundle setup/run/teardown {0}/{1}/{2}'.format(
setup - start, run - setup, teardown - run))
def bundle_local(graph, reconstruction, gcp, central_shot_id, config):
"""Bundle adjust the local neighborhood of a shot."""
start = time.time()
interior, boundary = shot_neighborhood(graph, reconstruction,
central_shot_id,
config['local_bundle_radius'])
logger.debug(
'Local bundle sets: interior {} boundary {} other {}'.format(
len(interior), len(boundary),
len(reconstruction.shots) - len(interior) - len(boundary)))
point_ids = set()
for shot_id in interior:
if shot_id in graph:
for track in graph[shot_id]:
if track in reconstruction.points:
point_ids.add(track)
ba = csfm.BundleAdjuster()
for camera in reconstruction.cameras.values():
if camera.projection_type == 'perspective':
ba.add_perspective_camera(str(camera.id), camera.focal, camera.k1,
camera.k2, camera.focal_prior,
camera.k1_prior, camera.k2_prior, True)
elif camera.projection_type in ['equirectangular', 'spherical']:
ba.add_equirectangular_camera(str(camera.id))
for shot_id in interior | boundary:
shot = reconstruction.shots[shot_id]
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(str(shot.id), str(shot.camera.id), r[0], r[1], r[2], t[0],
t[1], t[2], shot.id in boundary)
for point_id in point_ids:
point = reconstruction.points[point_id]
x = point.coordinates
ba.add_point(str(point.id), x[0], x[1], x[2], False)
for shot_id in interior | boundary:
if shot_id in graph:
for track in graph[shot_id]:
if track in reconstruction.points:
ba.add_observation(str(shot_id), str(track),
*graph[shot_id][track]['feature'])
if config['bundle_use_gps']:
for shot_id in interior:
shot = reconstruction.shots[shot_id]
g = shot.metadata.gps_position
ba.add_position_prior(str(shot.id), g[0], g[1], g[2],
shot.metadata.gps_dop)
if config['bundle_use_gcp'] and gcp:
for observation in gcp:
if observation.shot_id in interior:
ba.add_ground_control_point_observation(
observation.shot_id, observation.coordinates[0],
observation.coordinates[1], observation.coordinates[2],
observation.shot_coordinates[0],
observation.shot_coordinates[1])
ba.set_loss_function(config.get('loss_function', 'SoftLOneLoss'),
config.get('loss_function_threshold', 1))
ba.set_reprojection_error_sd(config.get('reprojection_error_sd', 0.004))
ba.set_internal_parameters_prior_sd(
config.get('exif_focal_sd', 0.01),
config.get('radial_distorsion_k1_sd', 0.01),
config.get('radial_distorsion_k2_sd', 0.01))
setup = time.time()
ba.set_num_threads(config['processes'])
ba.run()
run = time.time()
logger.debug(ba.brief_report())
for camera in reconstruction.cameras.values():
if camera.projection_type == 'perspective':
c = ba.get_perspective_camera(str(camera.id))
camera.focal = c.focal
camera.k1 = c.k1
camera.k2 = c.k2
for shot_id in interior:
shot = reconstruction.shots[shot_id]
s = ba.get_shot(str(shot.id))
shot.pose.rotation = [s.rx, s.ry, s.rz]
shot.pose.translation = [s.tx, s.ty, s.tz]
for point in point_ids:
point = reconstruction.points[point]
p = ba.get_point(str(point.id))
point.coordinates = [p.x, p.y, p.z]
point.reprojection_error = p.reprojection_error
teardown = time.time()
logger.debug('Local bundle setup/run/teardown {0}/{1}/{2}'.format(
setup - start, run - setup, teardown - run))
def bundle(graph, reconstruction, config, fix_cameras=False):
'''Bundle adjust a reconstruction.
'''
start = time.time()
ba = csfm.BundleAdjuster()
for k, v in reconstruction['cameras'].items():
pt = v.get('projection_type', 'perspective')
if pt == 'perspective':
ba.add_perspective_camera(str(k), v['focal'], v['k1'], v['k2'],
v['focal_prior'], v.get('k1_prior', 0.0),
v.get('k2_prior', 0.0), fix_cameras)
elif pt in ['equirectangular', 'spherical']:
ba.add_equirectangular_camera(str(k))
for k, v in reconstruction['shots'].items():
r = v['rotation']
t = v['translation']
g = v['gps_position']
ba.add_shot(str(k), str(v['camera']), r[0], r[1], r[2], t[0], t[1],
t[2], g[0], g[1], g[2], v['gps_dop'], False)
for k, v in reconstruction['points'].items():
x = v['coordinates']
ba.add_point(str(k), x[0], x[1], x[2], False)
for shot in reconstruction['shots']:
if shot in graph:
for track in graph[shot]:
if track in reconstruction['points']:
ba.add_observation(str(shot), str(track),
*graph[shot][track]['feature'])
ba.set_loss_function(config.get('loss_function', 'SoftLOneLoss'),
config.get('loss_function_threshold', 1))
ba.set_reprojection_error_sd(config.get('reprojection_error_sd', 0.004))
ba.set_internal_parameters_prior_sd(
config.get('exif_focal_sd', 0.01),
config.get('radial_distorsion_k1_sd', 0.01),
config.get('radial_distorsion_k2_sd', 0.01))
setup = time.time()
ba.set_num_threads(config['processes'])
ba.run()
run = time.time()
print ba.brief_report()
for k, v in reconstruction['cameras'].items():
if v.get('projection_type', 'perspective') == 'perspective':
c = ba.get_perspective_camera(str(k))
v['focal'] = c.focal
v['k1'] = c.k1
v['k2'] = c.k2
for k, v in reconstruction['shots'].items():
s = ba.get_shot(str(k))
v['rotation'] = [s.rx, s.ry, s.rz]
v['translation'] = [s.tx, s.ty, s.tz]
for k, v in reconstruction['points'].items():
p = ba.get_point(str(k))
v['coordinates'] = [p.x, p.y, p.z]
v['reprojection_error'] = p.reprojection_error
teardown = time.time()
print 'setup/run/teardown {0}/{1}/{2}'.format(setup - start, run - setup,
teardown - run)
def bundle_local(graph, reconstruction, config, n_neighbour, gcp, shot_id):
"""Bundle adjust a reconstruction."""
start = time.time()
ba = csfm.BundleAdjuster()
# figure out which cameras to add
# each value in reconstruction.cameras.values() is a camera
all_images = sorted([shot.id for shot in reconstruction.shots.values()])
loc = bisect.bisect(all_images, shot_id)
# get the local images
local_ids = all_images[max(0, loc - n_neighbour): min(len(all_images), loc + n_neighbour)]
print("using bundle local, with shot=%s, and local being:" % shot_id)
print(local_ids)
local_ids = set(local_ids)
# add all cameras
for camera in reconstruction.cameras.values():
in_recon = camera.id in local_ids
if camera.projection_type == 'perspective':
ba.add_perspective_camera(
str(camera.id), camera.focal, camera.k1, camera.k2,
camera.focal_prior, camera.k1_prior, camera.k2_prior,
not in_recon)
elif camera.projection_type in ['equirectangular', 'spherical']:
ba.add_equirectangular_camera(str(camera.id))
# add all the shots with their initial values
for shot in reconstruction.shots.values():
in_recon = shot.id in local_ids
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(
str(shot.id), str(shot.camera.id),
r[0], r[1], r[2],
t[0], t[1], t[2],
not in_recon
)
var_points = set()
for shot_id in local_ids:
for track_id in graph[shot_id]:
if track_id in reconstruction.points:
if track_id not in var_points:
var_points.add(track_id)
track = reconstruction.points[track_id]
x = track.coordinates
ba.add_point(str(track_id), x[0], x[1], x[2], False)
ba.add_observation(str(shot_id), str(track_id),
*graph[shot_id][track_id]['feature'])
for shot_id in reconstruction.shots:
if (shot_id in graph) and (shot_id not in local_ids):
for track in graph[shot_id]:
if track in var_points:
ba.add_observation(str(shot_id), str(track),
*graph[shot_id][track]['feature'])
if config['bundle_use_gps']:
for shot in reconstruction.shots.values():
g = shot.metadata.gps_position
ba.add_position_prior(str(shot.id), g[0], g[1], g[2],
shot.metadata.gps_dop)
if config['bundle_use_gcp'] and gcp:
for observation in gcp:
if observation.shot_id in reconstruction.shots:
ba.add_ground_control_point_observation(
str(observation.shot_id),
observation.coordinates[0],
observation.coordinates[1],
observation.coordinates[2],
observation.shot_coordinates[0],
observation.shot_coordinates[1])
# set loss function, reprojection error, camera internal parameter sd
ba.set_loss_function(config.get('loss_function', 'SoftLOneLoss'),
config.get('loss_function_threshold', 1))
ba.set_reprojection_error_sd(config.get('reprojection_error_sd', 0.004))
ba.set_internal_parameters_prior_sd(
config.get('exif_focal_sd', 0.01),
config.get('radial_distorsion_k1_sd', 0.01),
config.get('radial_distorsion_k2_sd', 0.01))
setup = time.time()
ba.set_num_threads(config['processes'])
ba.run()
run = time.time()
logger.debug(ba.brief_report())
# extract all values from the bundler and assign them to the reconstruction
for camera in reconstruction.cameras.values():
if camera.id in local_ids:
if camera.projection_type == 'perspective':
c = ba.get_perspective_camera(str(camera.id))
camera.focal = c.focal
camera.k1 = c.k1
camera.k2 = c.k2
for shot in reconstruction.shots.values():
if shot.id in local_ids:
s = ba.get_shot(str(shot.id))
shot.pose.rotation = [s.rx, s.ry, s.rz]
shot.pose.translation = [s.tx, s.ty, s.tz]
for point in reconstruction.points.values():
if point.id in var_points:
p = ba.get_point(str(point.id))
point.coordinates = [p.x, p.y, p.z]
point.reprojection_error = p.reprojection_error
teardown = time.time()
logger.debug('Bundle setup/run/teardown {0}/{1}/{2}'.format(
setup - start, run - setup, teardown - run))
def bundle_local(graph, reconstruction, gcp, central_shot_id, config):
"""Bundle adjust the local neighborhood of a shot."""
chrono = Chronometer()
interior, boundary = shot_neighborhood(
graph, reconstruction, central_shot_id,
config['local_bundle_radius'],
config['local_bundle_min_common_points'],
config['local_bundle_max_shots'])
logger.debug(
'Local bundle sets: interior {} boundary {} other {}'.format(
len(interior), len(boundary),
len(reconstruction.shots) - len(interior) - len(boundary)))
point_ids = set()
for shot_id in interior:
if shot_id in graph:
for track in graph[shot_id]:
if track in reconstruction.points:
point_ids.add(track)
ba = csfm.BundleAdjuster()
for camera in reconstruction.cameras.values():
_add_camera_to_bundle(ba, camera, constant=True)
for shot_id in interior | boundary:
shot = reconstruction.shots[shot_id]
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(
shot.id, shot.camera.id,
r[0], r[1], r[2],
t[0], t[1], t[2],
shot.id in boundary
)
for point_id in point_ids:
point = reconstruction.points[point_id]
x = point.coordinates
ba.add_point(point.id, x[0], x[1], x[2], False)
for shot_id in interior | boundary:
if shot_id in graph:
for track in graph[shot_id]:
if track in point_ids:
ba.add_observation(shot_id, track,
*graph[shot_id][track]['feature'])
if config['bundle_use_gps']:
for shot_id in interior:
shot = reconstruction.shots[shot_id]
g = shot.metadata.gps_position
ba.add_position_prior(shot.id, g[0], g[1], g[2],
shot.metadata.gps_dop)
if config['bundle_use_gcp'] and gcp:
_add_gcp_to_bundle(ba, gcp, reconstruction)
ba.set_loss_function(config['loss_function'],
config['loss_function_threshold'])
ba.set_reprojection_error_sd(config['reprojection_error_sd'])
ba.set_internal_parameters_prior_sd(
config['exif_focal_sd'],
config['principal_point_sd'],
config['radial_distorsion_k1_sd'],
config['radial_distorsion_k2_sd'],
config['radial_distorsion_p1_sd'],
config['radial_distorsion_p2_sd'],
config['radial_distorsion_k3_sd'])
ba.set_num_threads(config['processes'])
ba.set_max_num_iterations(10)
ba.set_linear_solver_type("DENSE_SCHUR")
chrono.lap('setup')
ba.run()
chrono.lap('run')
for shot_id in interior:
shot = reconstruction.shots[shot_id]
s = ba.get_shot(shot.id)
shot.pose.rotation = [s.rx, s.ry, s.rz]
shot.pose.translation = [s.tx, s.ty, s.tz]
for point in point_ids:
point = reconstruction.points[point]
p = ba.get_point(point.id)
point.coordinates = [p.x, p.y, p.z]
point.reprojection_error = p.reprojection_error
chrono.lap('teardown')
logger.debug(ba.brief_report())
report = {
'wall_times': dict(chrono.lap_times()),
'brief_report': ba.brief_report(),
'num_interior_images': len(interior),
'num_boundary_images': len(boundary),
'num_other_images': (len(reconstruction.shots)
- len(interior) - len(boundary)),
}
return report
def two_view_reconstruction(p1, p2, f1, f2, threshold, bundle=True):
assert len(p1) == len(p2)
assert len(p1) >= 5
npoints = len(p1)
# Run 5-point algorithm.
res = two_view_reconstruction_run_csfm(p1, p2, f1, f2, threshold)
num_iter = 10
if not bundle:
num_iter = 1
if res:
R, t, inliers = res
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(num_iter):
# 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 = csfm.triangulate(Ps, [x1, x2], threshold, -1.0)
if X is not None:
Xs.append(X)
inliers.append(True)
else:
Xs.append([0, 0, 0])
inliers.append(False)
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
if bundle:
# 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)
two_view_reconstruction_run_bundle(ba)
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)
else:
return None
return R, t, Xs, np.nonzero(inliers)[0]