def test_optimize_fast():
b_true, b_init = create_test_problem(noise=0)
ba = BundleAdjuster(b_init)
ba.optimize(max_steps=50)
report_bundle(b_init, 'Initial')
report_bundle(ba.bundle, 'Estimated')
report_bundle(b_true, 'True')
#print '\nError in ts:'
#print abs(np.array(b_true.ts) - bcur.ts)
#print 'Error in Rs:'
#print abs(asarray(b_true.Rs) - bcur.Rs)
#print 'Error in points:'
#print sum(square(abs(array(b_true.pts) - bcur.pts)), axis=1)
#print '\nOffset in Rs:'
#for Rtrue,Rinit in zip(b_true.Rs, b_init.Rs):
# print dots(Rtrue, Rinit.T)
print '\nCost (initial -> estimated -> true)'
print ' %f -> %f -> %f' % (b_init.complete_cost(), ba.bundle.complete_cost(), b_true.complete_cost())
draw_bundle.output_views(b_init, 'out/init.pdf')
draw_bundle.output_views(b_true, 'out/true.pdf')
draw_bundle.output_views(ba.bundle, 'out/estimated.pdf')
def test_optimize_fast():
b_true, b_init = create_test_problem(noise=0)
ba = BundleAdjuster(b_init)
ba.optimize(max_steps=50)
report_bundle(b_init, 'Initial')
report_bundle(ba.bundle, 'Estimated')
report_bundle(b_true, 'True')
#print '\nError in ts:'
#print abs(np.array(b_true.ts) - bcur.ts)
#print 'Error in Rs:'
#print abs(asarray(b_true.Rs) - bcur.Rs)
#print 'Error in points:'
#print sum(square(abs(array(b_true.pts) - bcur.pts)), axis=1)
#print '\nOffset in Rs:'
#for Rtrue,Rinit in zip(b_true.Rs, b_init.Rs):
# print dots(Rtrue, Rinit.T)
print '\nCost (initial -> estimated -> true)'
print ' %f -> %f -> %f' % (b_init.complete_cost(),
ba.bundle.complete_cost(),
b_true.complete_cost())
draw_bundle.output_views(b_init, 'out/init.pdf')
draw_bundle.output_views(b_true, 'out/true.pdf')
draw_bundle.output_views(ba.bundle, 'out/estimated.pdf')
def run(complete_bundle, window_size, num_to_freeze=2, pdf_pattern=None):
NUM_TRACKS = 100
# Determine the projection so that we always draw the bundle on a fixed subspace
visualization_subspace = compute_subspace(complete_bundle)
# Create binary mask for frozen cameras
camera_mask = arange(window_size) < num_to_freeze
track_ids = range(NUM_TRACKS)
# Start optimizing
cur_bundle = complete_bundle
for i in range(0, len(complete_bundle.cameras)-window_size+1):
print '\n\n==============\nWINDOW: [%d..%d]\n' % (i, i+window_size)
prev_bundle = deepcopy(cur_bundle)
# Adjust this window
camera_ids = range(i, i+window_size)
ba = BundleAdjuster()
ba.set_bundle(cur_bundle, camera_ids=camera_ids, track_ids=track_ids)
ba.optimize()
cur_bundle = ba.bundle
# Propagate the update to the next camera
next_camera_id = i + window_size
if next_camera_id < len(cur_bundle.cameras):
geometry.propagate_pose_update_inplace(prev_bundle.cameras[i],
cur_bundle.cameras[i],
cur_bundle.cameras[i])
# Draw this window
print 'Drawing bundle...'
if pdf_pattern is not None:
pose_colors = ['b'] * len(ba.bundle.cameras)
for j in range(i):
pose_colors[j] = 'g'
for j in range(i,i+window_size):
pose_colors[j] = 'r'
pdf_path = pdf_pattern % i
print 'Writing to ',pdf_path
draw_bundle(ba.bundle, visualization_subspace, pose_colors)
if next_camera_id < len(prev_bundle.cameras):
draw_pose(prev_bundle.cameras[next_camera_id].R,
prev_bundle.cameras[next_camera_id].t,
visualization_subspace,
'c')
plt.savefig(pdf_path)
def run(complete_bundle, window_size, num_to_freeze=2, pdf_pattern=None):
NUM_TRACKS = 100
# Determine the projection so that we always draw the bundle on a fixed subspace
visualization_subspace = compute_subspace(complete_bundle)
# Create binary mask for frozen cameras
camera_mask = arange(window_size) < num_to_freeze
track_ids = range(NUM_TRACKS)
# Start optimizing
cur_bundle = complete_bundle
for i in range(0, len(complete_bundle.cameras) - window_size + 1):
print '\n\n==============\nWINDOW: [%d..%d]\n' % (i, i + window_size)
prev_bundle = deepcopy(cur_bundle)
# Adjust this window
camera_ids = range(i, i + window_size)
ba = BundleAdjuster()
ba.set_bundle(cur_bundle, camera_ids=camera_ids, track_ids=track_ids)
ba.optimize()
cur_bundle = ba.bundle
# Propagate the update to the next camera
next_camera_id = i + window_size
if next_camera_id < len(cur_bundle.cameras):
geometry.propagate_pose_update_inplace(prev_bundle.cameras[i],
cur_bundle.cameras[i],
cur_bundle.cameras[i])
# Draw this window
print 'Drawing bundle...'
if pdf_pattern is not None:
pose_colors = ['b'] * len(ba.bundle.cameras)
for j in range(i):
pose_colors[j] = 'g'
for j in range(i, i + window_size):
pose_colors[j] = 'r'
pdf_path = pdf_pattern % i
print 'Writing to ', pdf_path
draw_bundle(ba.bundle, visualization_subspace, pose_colors)
if next_camera_id < len(prev_bundle.cameras):
draw_pose(prev_bundle.cameras[next_camera_id].R,
prev_bundle.cameras[next_camera_id].t,
visualization_subspace, 'c')
plt.savefig(pdf_path)
for track in bundle.tracks:
track.reconstruction = bundle.triangulate(track)
NUM_CAMERAS = 100
NUM_TRACKS = 100
bundle.cameras = bundle.cameras[:NUM_CAMERAS]
bundle.tracks = bundle.tracks[:NUM_TRACKS]
for j,track in enumerate(bundle.tracks):
track.measurements = { i : track.get_measurement(i) for i in range(NUM_CAMERAS) }
param_mask = np.ones(bundle.num_params(), bool)
param_mask[:6] = False
param_mask[9] = False
b_init = deepcopy(bundle)
ba = BundleAdjuster(bundle)
ba.optimize(param_mask, max_steps=10)
# Write results
with open('out/adjusted_poses.txt', 'w') as fd:
for camera in ba.bundle.cameras:
P = camera.projection_matrix()
fd.write(''.join(['%f '%v for v in P.flatten()]))
fd.write('\n')
#draw_bundle.output_views(b_init, 'out/init.pdf')
#draw_bundle.output_views(ba.bundle, 'out/estimated.pdf')
track.reconstruction = bundle.triangulate(track)
NUM_CAMERAS = 100
NUM_TRACKS = 100
bundle.cameras = bundle.cameras[:NUM_CAMERAS]
bundle.tracks = bundle.tracks[:NUM_TRACKS]
for j, track in enumerate(bundle.tracks):
track.measurements = {
i: track.get_measurement(i)
for i in range(NUM_CAMERAS)
}
param_mask = np.ones(bundle.num_params(), bool)
param_mask[:6] = False
param_mask[9] = False
b_init = deepcopy(bundle)
ba = BundleAdjuster(bundle)
ba.optimize(param_mask, max_steps=10)
# Write results
with open('out/adjusted_poses.txt', 'w') as fd:
for camera in ba.bundle.cameras:
P = camera.projection_matrix()
fd.write(''.join(['%f ' % v for v in P.flatten()]))
fd.write('\n')
#draw_bundle.output_views(b_init, 'out/init.pdf')
#draw_bundle.output_views(ba.bundle, 'out/estimated.pdf')
