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)