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')