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_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():
"""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_pair(bundle_adjuster):
"""Simple two camera test"""
sa = bundle_adjuster
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)
