-
Notifications
You must be signed in to change notification settings - Fork 9
/
Copy pathbundle_adjuster_test.py
105 lines (83 loc) · 3.96 KB
/
bundle_adjuster_test.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
import pytheia as pt
import numpy as np
from random_recon_gen import RandomReconGenerator
def test_BundleAdjustView(gen, ba_options):
for vid in gen.recon.ViewIds():
orig_pos = gen.recon.View(vid).Camera().GetPosition()
gen.add_noise_to_views(noise_pos=1e-3, noise_angle=1e-1)
result = pt.sfm.BundleAdjustView(gen.recon, ba_options, vid)
dist_pos = np.linalg.norm(
orig_pos - gen.recon.View(vid).Camera().Position)
assert dist_pos < 1e-4
assert result.success
def test_BundleAdjustTrack(gen, ba_options):
gen.add_noise_to_tracks(noise_track=1e-3)
for t_id in gen.recon.TrackIds:
result = pt.sfm.BundleAdjustTrack(gen.recon, ba_options, t_id)
assert result.success
def test_BundleAdjustWithPositionPrior(gen, ba_options):
ba_options.use_position_priors = True
for vid in gen.recon.ViewIds:
# get the original camera pose as a prior
pos_prior = gen.recon.View(vid).Camera().Position
# now put noise on the position
gen.add_noise_to_views(noise_pos=1e-2, noise_angle=1e-1)
position_prior_sqrt_information = np.eye(3, dtype=np.float64)
gen.recon.View(vid).SetPositionPrior(
pos_prior, position_prior_sqrt_information)
# check if setting the prior worked
assert np.all(gen.recon.View(vid).GetPositionPrior() == pos_prior)
result = pt.sfm.BundleAdjustView(gen.recon, ba_options, vid)
dist_pos = np.linalg.norm(
pos_prior - gen.recon.View(vid).Camera().Position)
assert dist_pos < 1e-4
assert result.success
def test_BundleAdjustWithPositionPriorAndInformation(gen, ba_options):
ba_options.use_position_priors = True
# set a low standard dev for the position prior
# the sqrt_information will then be 1./std * eye(3)
pos_priors_std_dev = 0.001
for vid in gen.recon.ViewIds:
view = gen.recon.View(vid)
original_position = view.Camera().Position
# get the original camera pose as a prior
pos_prior = original_position + pos_priors_std_dev * np.random.randn(3)
# now put a large noise on the position and see if the prior helps to recover the original position
gen.add_noise_to_views(noise_pos=5.0, noise_angle=1e-1)
position_prior_sqrt_information = 1. / \
pos_priors_std_dev * np.eye(3, dtype=np.float64)
view.SetPositionPrior(pos_prior, position_prior_sqrt_information)
# check if setting the prior worked
assert np.all(view.GetPositionPrior() == pos_prior)
assert view.HasPositionPrior()
result = pt.sfm.BundleAdjustView(gen.recon, ba_options, vid)
dist_pos = np.linalg.norm(
original_position - gen.recon.View(vid).Camera().Position)
assert dist_pos < 3 * pos_priors_std_dev
assert result.success
if __name__ == "__main__":
gen = RandomReconGenerator()
# test 1
gen.generate_random_recon(nr_views=1, nr_tracks=20)
ba_options = pt.sfm.BundleAdjustmentOptions()
ba_options.constant_camera_orientation = False
ba_options.constant_camera_position = False
ba_options.loss_function_type = pt.sfm.LossFunctionType(0)
ba_options.robust_loss_width = 1.345
ba_options.intrinsics_to_optimize = pt.sfm.OptimizeIntrinsicsType.NONE
ba_options.verbose = True
ba_options.use_position_priors = False
test_BundleAdjustView(gen, ba_options)
# test 2
gen = RandomReconGenerator()
gen.generate_random_recon(nr_views=1, nr_tracks=20)
test_BundleAdjustWithPositionPrior(gen, ba_options)
# test 2
gen = RandomReconGenerator()
gen.generate_random_recon(nr_views=1, nr_tracks=20, pixel_noise=1.0)
test_BundleAdjustWithPositionPriorAndInformation(gen, ba_options)
gen = RandomReconGenerator()
ba_options.constant_camera_orientation = True
ba_options.constant_camera_position = True
gen.generate_random_recon(nr_views=5, nr_tracks=20, pixel_noise=0.5)
test_BundleAdjustTrack(gen, ba_options)