def test_four_cams_one_fixed():
"""Four cameras, one reconstruction"""
sa = pybundle.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(pybundle.BARelativeMotion('1234', '1', '1234', '2', [0, 0, 0], [-1, 0, 0], 1))
sa.add_relative_motion(pybundle.BARelativeMotion('1234', '1', '1234', '3', [0, 0, 0], [0, -1, 0], 1))
sa.add_relative_motion(pybundle.BARelativeMotion('1234', '1', '1234', '4', [0, 0, 0], [0, 0, -1], 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 test_pair_with_shot_point():
"""Simple two camera test with a point constraint for anchoring"""
sa = pybundle.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.add_rotation_prior("1", 0, 0, 0, 1)
sa.set_scale_sharing("12", True)
sa.add_relative_motion(
pybundle.BARelativeMotion("12", "1", "12", "2", [0, 0, 0], [-1, 0, 0], 1)
)
sa.add_point_position_shot("p1", "1", "12", [1, 0, 0], 1, pybundle.XYZ)
sa.add_point_position_shot("p1", "2", "12", [-1, 0, 0], 1, pybundle.XYZ)
sa.add_point_position_world("p1", [1, 0, 0], 1, pybundle.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_pair_with_shot_point():
"""Simple two camera test with a point constraint for anchoring"""
sa = pybundle.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(
pybundle.BARelativeMotion('12', '1', '12', '2', [0, 0, 0], [-1, 0, 0],
1))
sa.add_point_position_shot('p1', '1', '12', [1, 0, 0], 1, pybundle.XYZ)
sa.add_point_position_shot('p1', '2', '12', [-1, 0, 0], 1, pybundle.XYZ)
sa.add_point_bearing_shot('p1', '1', '12', [1, 0, 0], 2e-3)
sa.add_point_position_world('p1', [1, 0, 0], 1, pybundle.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_unicode_strings_in_bundle():
"""Test that byte and unicode strings can be used as camera ids."""
ba = pybundle.BundleAdjuster()
unicode_id = u"A\xb2"
byte_id = b"A_2"
ba.add_equirectangular_camera(unicode_id)
ba.add_equirectangular_camera(byte_id)
def test_sigleton():
"""Single camera test"""
sa = pybundle.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(reconstruction, shot_id, camera_priors, config):
"""Bundle adjust a single camera."""
ba = pybundle.BundleAdjuster()
ba.set_use_analytic_derivatives(config["bundle_analytic_derivatives"])
shot = reconstruction.shots[shot_id]
camera = shot.camera
camera_prior = camera_priors[camera.id]
ba.add_camera(camera.id, camera, camera_prior, True)
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(shot_id, camera.id, r, t, False)
for track in shot.get_valid_landmarks():
ba.add_point(track.id, track.coordinates, True)
obs = shot.get_landmark_observation(track)
point = obs.point
ba.add_point_projection_observation(
shot_id, track.id, point[0], point[1], obs.scale
)
if config["bundle_use_gps"]:
g = shot.metadata.gps_position.value
ba.add_position_prior(
shot_id, g[0], g[1], g[2], shot.metadata.gps_accuracy.value
)
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_distortion_k1_sd"],
config["radial_distortion_k2_sd"],
config["tangential_distortion_p1_sd"],
config["tangential_distortion_p2_sd"],
config["radial_distortion_k3_sd"],
config["radial_distortion_k4_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.r[0], s.r[1], s.r[2]]
shot.pose.translation = [s.t[0], s.t[1], s.t[2]]
def test_unicode_strings_in_bundle():
"""Test that byte and unicode strings can be used as camera ids."""
ba = pybundle.BundleAdjuster()
unicode_id = u"A\xb2"
byte_id = b"A_2"
camera = pygeometry.Camera.create_perspective(0.4, 0.1, -0.01)
camera.id = unicode_id
ba.add_camera(camera.id, camera, camera, True)
camera.id = byte_id
ba.add_camera(camera.id, camera, camera, True)
def test_four_cams_single_reconstruction_non_rigid():
"""Four cameras, one reconstruction"""
sa = pybundle.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(
pybundle.BARelativeSimilarity(
"1234", "1", "1234", "2", [0, 0, 0], [-1, 0, 0], 1, 1
)
)
sa.add_relative_similarity(
pybundle.BARelativeSimilarity(
"1234", "2", "1234", "3", [0, 0, 0], [-1, -1, 0], 1, 1
)
)
sa.add_relative_similarity(
pybundle.BARelativeSimilarity(
"1234", "3", "1234", "4", [0, 0, 0], [0, -1, 0], 1, 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_unicode_strings_in_bundle() -> None:
"""Test that byte and unicode strings can be used as camera ids."""
ba = pybundle.BundleAdjuster()
unicode_id = "A\xb2"
byte_id = b"A_2"
camera = pygeometry.Camera.create_perspective(0.4, 0.1, -0.01)
camera.id = unicode_id
ba.add_camera(camera.id, camera, camera, True)
# pyre-fixme[8]: Attribute has type `str`; used as `bytes`.
camera.id = byte_id
ba.add_camera(camera.id, camera, camera, True)
def bundle_single_view(graph, reconstruction, shot_id, camera_priors, config):
"""Bundle adjust a single camera."""
ba = pybundle.BundleAdjuster()
shot = reconstruction.shots[shot_id]
camera = shot.camera
camera_prior = camera_priors[camera.id]
ba.add_camera(camera.id, camera, camera_prior, True)
r = shot.pose.rotation
t = shot.pose.translation
ba.add_shot(shot.id, camera.id, r, t, False)
for track_id in graph[shot_id]:
track = reconstruction.points[track_id]
ba.add_point(track_id, track.coordinates, True)
point = graph[shot_id][track_id]['feature']
scale = graph[shot_id][track_id]['feature_scale']
ba.add_point_projection_observation(
shot_id, track_id, point[0], point[1], scale)
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_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(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.r[0], s.r[1], s.r[2]]
shot.pose.translation = [s.t[0], s.t[1], s.t[2]]
def test_linear_motion_prior_rotation():
"""Three cameras, middle has no gps or orientation info"""
sa = pybundle.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_linear_motion_prior_rotation():
"""Three cameras, middle has no gps or orientation info"""
sa = pybundle.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 = pybundle.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 = pygeometry.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 = pybundle.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(
pybundle.BARelativeSimilarity('1234', '1', '1234', '2', [0, 0, 0],
[-1, 0, 0], 1, 1))
sa.add_relative_similarity(
pybundle.BARelativeSimilarity('1234', '2', '1234', '3', [0, 0, 0],
[-1, -1, 0], 1, 1))
sa.add_relative_similarity(
pybundle.BARelativeSimilarity('1234', '3', '1234', '4', [0, 0, 0],
[0, -1, 0], 1, 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 = pybundle.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(pybundle.BARelativeMotion('12', '1', '12', '2', [0, 0, 0], [-1, 0, 0], 1))
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 = pybundle.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 test_four_cams_one_fixed():
"""Four cameras, one reconstruction"""
sa = pybundle.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(
pybundle.BARelativeMotion("1234", "1", "1234", "2", [0, 0, 0], [-1, 0, 0], 1)
)
sa.add_relative_motion(
pybundle.BARelativeMotion("1234", "1", "1234", "3", [0, 0, 0], [0, -1, 0], 1)
)
sa.add_relative_motion(
pybundle.BARelativeMotion("1234", "1", "1234", "4", [0, 0, 0], [0, 0, -1], 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 test_pair_non_rigid():
"""Simple two camera test"""
sa = pybundle.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", False)
sa.add_relative_similarity(
pybundle.BARelativeSimilarity("12", "1", "12", "2", [0, 0, 0], [-1, 0, 0], 1, 1)
)
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_heatmaps_position():
"""Three cameras. Same heatmap different offsets"""
sa = pybundle.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_reconstruction("123", True)
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)
def bell_heatmap(size, r, mu_x, mu_y):
sigma_x = r * 0.5
sigma_y = r * 0.5
x = np.linspace(-r, r, size)
y = np.linspace(r, -r, size)
x, y = np.meshgrid(x, y)
z = (1 / (2 * np.pi * sigma_x * sigma_y) *
np.exp(-((x - mu_x)**2 / (2 * sigma_x**2) + (y - mu_y)**2 /
(2 * sigma_y**2))))
z /= max(z.reshape(-1))
z = 1 - z
return z
hmap_x, hmap_y = 1, -1
hmap_size, hmap_r = 101, 10
res = 2 * hmap_r / (hmap_size - 1)
hmap = bell_heatmap(size=hmap_size, r=hmap_r, mu_x=hmap_x, mu_y=hmap_y)
sa.add_heatmap("hmap1", hmap.flatten(), hmap_size, res)
x1_offset, y1_offset = 2, 0
x2_offset, y2_offset = 0, 2
x3_offset, y3_offset = -2, 0
sa.add_absolute_position_heatmap(
"1",
"hmap1",
x1_offset,
y1_offset,
1.0,
)
sa.add_absolute_position_heatmap(
"2",
"hmap1",
x2_offset,
y2_offset,
1.0,
)
sa.add_absolute_position_heatmap(
"3",
"hmap1",
x3_offset,
y3_offset,
1.0,
)
sa.run()
s1 = sa.get_shot("1")
s2 = sa.get_shot("2")
s3 = sa.get_shot("3")
assert np.allclose(-s1.t, [x1_offset + hmap_x, y1_offset + hmap_y, 0],
atol=res)
assert np.allclose(-s2.t, [x2_offset + hmap_x, y2_offset + hmap_y, 0],
atol=res)
assert np.allclose(-s3.t, [x3_offset + hmap_x, y3_offset + hmap_y, 0],
atol=res)
def bundle_local(graph, reconstruction, camera_priors, 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 = pybundle.BundleAdjuster()
for camera in reconstruction.cameras.values():
camera_prior = camera_priors[camera.id]
ba.add_camera(camera.id, camera, camera_prior, 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, t, shot.id in boundary)
for point_id in point_ids:
point = reconstruction.points[point_id]
ba.add_point(point.id, point.coordinates, False)
for shot_id in interior | boundary:
if shot_id in graph:
for track in graph[shot_id]:
if track in point_ids:
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_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.shots)
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(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.r[0], s.r[1], s.r[2]]
shot.pose.translation = [s.t[0], s.t[1], s.t[2]]
for point in point_ids:
point = reconstruction.points[point]
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(),
'num_interior_images': len(interior),
'num_boundary_images': len(boundary),
'num_other_images': (len(reconstruction.shots)
- len(interior) - len(boundary)),
}
return point_ids, report
def bundle(graph, reconstruction, camera_priors, gcp, config):
"""Bundle adjust a reconstruction."""
fix_cameras = not config['optimize_camera_parameters']
chrono = Chronometer()
ba = pybundle.BundleAdjuster()
for camera in reconstruction.cameras.values():
camera_prior = camera_priors[camera.id]
ba.add_camera(camera.id, camera, camera_prior, 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(config['bundle_max_iterations'])
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_adjuster():
ba = pybundle.BundleAdjuster()
camera = pygeometry.Camera.create_spherical()
ba.add_camera("cam1", camera, camera, True)
return ba
def bundle_adjuster():
ba = pybundle.BundleAdjuster()
camera = pygeometry.Camera.create_perspective(1.0, 0.0, 0.0)
ba.add_camera("cam1", camera, camera, True)
ba.add_rig_camera("rig_cam1", pygeometry.Pose(), pygeometry.Pose(), True)
return ba
