def add_camera_constraints_soft(ra, reconstruction_shots, reconstruction_name):
added_shots = set()
for key in reconstruction_shots:
shots = reconstruction_shots[key]
rec_name = reconstruction_name(key)
ra.add_reconstruction(rec_name, 0, 0, 0, 0, 0, 0, 1, False)
for shot_id in shots:
shot = shots[shot_id]
shot_name = str(shot_id)
R = shot.pose.rotation
t = shot.pose.translation
if shot_id not in added_shots:
ra.add_shot(shot_name, R[0], R[1], R[2], t[0], t[1], t[2],
False)
gps = shot.metadata.gps_position
gps_sd = shot.metadata.gps_dop
ra.add_absolute_position_constraint(shot_name, gps[0], gps[1],
gps[2], gps_sd)
added_shots.add(shot_id)
covariance = np.diag([1e-5, 1e-5, 1e-5, 1e-2, 1e-2, 1e-2])
sm = scale_matrix(covariance)
rmc = csfm.RARelativeMotionConstraint(rec_name, shot_name, R[0],
R[1], R[2], t[0], t[1], t[2])
for i in range(6):
for j in range(6):
rmc.set_scale_matrix(i, j, sm[i, j])
ra.add_relative_motion_constraint(rmc)
def test_pair():
"""Simple single reconstruction two shots test."""
ra = csfm.ReconstructionAlignment()
ra.add_shot('1', 0, 0, 0, 0, 0, 0, False)
ra.add_shot('2', 0, 0, 0, 0, 0, 0, False)
ra.add_reconstruction('a', 0, 0, 0, 0, 0, 0, 4, False)
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('a', '1', 0, 0, 0, 0, 0, 0))
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('a', '2', 0, 0, 0, -1, 0, 0))
ra.add_absolute_position_constraint('1', 1, 0, 0, 1)
ra.add_absolute_position_constraint('2', 3, 0, 0, 1)
ra.run()
s1 = ra.get_shot('1')
s2 = ra.get_shot('2')
rec_a = ra.get_reconstruction('a')
assert np.allclose(get_shot_origin(s1), [1, 0, 0], atol=1e-6)
assert np.allclose(get_shot_origin(s2), [3, 0, 0], atol=1e-6)
assert np.allclose(get_reconstruction_origin(rec_a), [1, 0, 0], atol=1e-6)
assert np.allclose(rec_a.scale, 0.5)
def test_singleton_reconstruction():
"""Single shot in a single reconstruction."""
ra = csfm.ReconstructionAlignment()
ra.add_shot('1', 0, 0, 0, 0, 0, 0, False)
ra.add_reconstruction('a', 0, 0, 0, 0, 0, 0, 4, False)
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('a', '1', 0, 0, 0, -1, 0, 0))
ra.add_absolute_position_constraint('1', 1, 0, 0, 1)
ra.run()
s1 = ra.get_shot('1')
assert np.allclose(get_shot_origin(s1), [1, 0, 0], atol=1e-6)
def test_two_shots_one_fixed():
"""Two shot, one reconstruction. One shot is fixed"""
ra = csfm.ReconstructionAlignment()
ra.add_shot('1', 0, 0, 0, -1, 0, 0, True)
ra.add_shot('2', 0, 0, 0, 0, 0, 0, False)
ra.add_reconstruction('a', 0, 0, 0, 0, 0, 0, 1, False)
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('a', '1', 0, 0, 0, 0, 0, 0))
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('a', '2', 0, 0, 0, -1, 0, 0))
# Next line should be ignored because shot 1 is fixed
ra.add_absolute_position_constraint('1', 100, 0, 0, 1)
ra.add_absolute_position_constraint('2', 3, 0, 0, 1)
ra.run()
s1 = ra.get_shot('1')
s2 = ra.get_shot('2')
rec_a = ra.get_reconstruction('a')
assert np.allclose(get_shot_origin(s1), [1, 0, 0], atol=1e-6)
assert np.allclose(get_shot_origin(s2), [3, 0, 0], atol=1e-6)
assert np.allclose(get_reconstruction_origin(rec_a), [1, 0, 0], atol=1e-6)
assert np.allclose(rec_a.scale, 0.5)
def test_two_reconstructions():
"""Two reconstructions"""
ra = csfm.ReconstructionAlignment()
ra.add_shot('1', 0, 0, 0, 0, 0, 0, False)
ra.add_shot('2', 0, 0, 0, 0, 0, 0, False)
ra.add_shot('3', 0, 0, 0, 0, 0, 0, False)
ra.add_shot('4', 0, 0, 0, 0, 0, 0, False)
ra.add_reconstruction('a', 0, 0, 0, 0, 0, 0, 1, False)
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('a', '1', 0, 0, 0, 0, 0, 0))
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('a', '2', 0, 0, 0, -1, 0, 0))
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('a', '3', 0, 0, 0, -2, 0, 0))
ra.add_reconstruction('b', 0, 0, 0, 0, 0, 0, 1, False)
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('b', '2', 0, 0, 0, 0, 0, 0))
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('b', '3', 0, 0, 0, -1, 0, 0))
ra.add_relative_motion_constraint(
csfm.RARelativeMotionConstraint('b', '4', 0, 0, 0, -2, 0, 0))
ra.add_absolute_position_constraint('1', 1, 0, 0, 1)
ra.add_absolute_position_constraint('2', 2, 0, 0, 1)
ra.run()
s1 = ra.get_shot('1')
s2 = ra.get_shot('2')
s3 = ra.get_shot('3')
s4 = ra.get_shot('4')
rec_a = ra.get_reconstruction('a')
rec_b = ra.get_reconstruction('b')
assert np.allclose(get_shot_origin(s1), [1, 0, 0], atol=1e-6)
assert np.allclose(get_shot_origin(s2), [2, 0, 0], atol=1e-6)
assert np.allclose(get_shot_origin(s3), [3, 0, 0], atol=1e-6)
assert np.allclose(get_shot_origin(s4), [4, 0, 0], atol=1e-6)
assert np.allclose(get_reconstruction_origin(rec_a), [1, 0, 0], atol=1e-6)
assert np.allclose(get_reconstruction_origin(rec_b), [2, 0, 0], atol=1e-6)
assert np.allclose(rec_a.scale, 1)
assert np.allclose(rec_b.scale, 1)
