def test_schur_compliment(self): damping = 0. bundle = self.bundle r = bundle.residuals() # do not adjust the first camera J = bundle.Jresiduals()[:,6:] # do not adjust the first camera JTJ = dots(J.T, J) JTr = dots(J.T, r) n = 6*(len(bundle.cameras)-1) #print '\n'.join([''.join(row) for row in np.array([' ','x']).take(JTJ>1e-5)]) #print np.round(JTr,2) #print n optimize.apply_lm_damping_inplace(JTJ, damping) Aslow, bslow = schur.get_schur_complement(JTJ, JTr, n) ba = BundleAdjuster(bundle) ba.prepare_schur_complement() ba.apply_damping(damping) A,b = ba.compute_schur_complement() nc = len(ba.optim_camera_ids) #print 'nc=%d'%nc A = A.transpose((0,2,1,3)).reshape((6*nc, 6*nc)) b = b.flatten() self.assertArrayEqual(A, Aslow) self.assertArrayEqual(b, bslow)
def test_schur_compliment(self): damping = 0. bundle = self.bundle r = bundle.residuals() # do not adjust the first camera J = bundle.Jresiduals()[:, 6:] # do not adjust the first camera JTJ = dots(J.T, J) JTr = dots(J.T, r) n = 6 * (len(bundle.cameras) - 1) #print '\n'.join([''.join(row) for row in np.array([' ','x']).take(JTJ>1e-5)]) #print np.round(JTr,2) #print n optimize.apply_lm_damping_inplace(JTJ, damping) Aslow, bslow = schur.get_schur_complement(JTJ, JTr, n) ba = BundleAdjuster(bundle) ba.prepare_schur_complement() ba.apply_damping(damping) A, b = ba.compute_schur_complement() nc = len(ba.optim_camera_ids) #print 'nc=%d'%nc A = A.transpose((0, 2, 1, 3)).reshape((6 * nc, 6 * nc)) b = b.flatten() self.assertArrayEqual(A, Aslow) self.assertArrayEqual(b, bslow)
def test_subset_schur(self): bundle = self.bundle damping = 2. camera_ids = [3, 1] track_ids = [0, 1, 2] cam_mask = [False, True] track_mask = [False, True, False] cameras_to_optimize = list(np.array(camera_ids)[np.array(cam_mask)]) tracks_to_optimize = list(np.array(track_ids)[np.array(track_mask)]) # Compute normal equations ba = BundleAdjuster() ba.set_bundle(bundle, camera_ids, track_ids, cam_mask, track_mask) ba.prepare_schur_complement() ba.apply_damping(damping) A, b = ba.compute_schur_complement() # Flatten the arrays nc = np.sum(cam_mask) self.assertShape(A, (nc, nc, 6, 6)) self.assertShape(b, (nc, 6)) Aflat = A.transpose((0, 2, 1, 3)).reshape((nc * 6, nc * 6)) bflat = b.flatten() # Compute terms explicitly for reference # Note that for equivalence with BundleAdjuster, we must # compute residuals with *all* selected cameras and tracks # (i.e. ignoring cam_mask and track_mask), then compute the # Schur compliment, then eliminate rows and columns from the # normal equations *afterwards* r = bundle.residuals_partial(camera_ids, track_ids) J = bundle.Jresiduals_partial(camera_ids, track_ids) JTJ = dots(J.T, J) JTr = dots(J.T, r) optimize.apply_lm_damping_inplace(JTJ, damping) Aslow, bslow = schur.get_schur_complement(JTJ, JTr, len(camera_ids) * 6) # Apply masking now cam_param_mask = np.array( [cam_mask[i / 6] for i in range(len(camera_ids) * 6)]) track_param_mask = np.array( [track_mask[i / 3] for i in range(len(track_ids) * 3)]) Aslow = Aslow[cam_param_mask].T[cam_param_mask].T bslow = bslow[cam_param_mask] # Check the results self.assertArrayEqual(Aflat, Aslow) self.assertArrayEqual(bflat, bslow)
def test_subset_schur(self): bundle = self.bundle damping = 2. camera_ids = [ 3,1 ] track_ids = [ 0,1,2 ] cam_mask = [ False, True ] track_mask = [ False, True, False ] cameras_to_optimize = list(np.array(camera_ids)[np.array(cam_mask)]) tracks_to_optimize = list(np.array(track_ids)[np.array(track_mask)]) # Compute normal equations ba = BundleAdjuster() ba.set_bundle(bundle, camera_ids, track_ids, cam_mask, track_mask) ba.prepare_schur_complement() ba.apply_damping(damping) A,b = ba.compute_schur_complement() # Flatten the arrays nc = np.sum(cam_mask) self.assertShape(A, (nc,nc,6,6)) self.assertShape(b, (nc,6)) Aflat = A.transpose((0,2,1,3)).reshape((nc*6, nc*6)) bflat = b.flatten() # Compute terms explicitly for reference # Note that for equivalence with BundleAdjuster, we must # compute residuals with *all* selected cameras and tracks # (i.e. ignoring cam_mask and track_mask), then compute the # Schur compliment, then eliminate rows and columns from the # normal equations *afterwards* r = bundle.residuals_partial(camera_ids, track_ids) J = bundle.Jresiduals_partial(camera_ids, track_ids) JTJ = dots(J.T, J) JTr = dots(J.T, r) optimize.apply_lm_damping_inplace(JTJ, damping) Aslow, bslow = schur.get_schur_complement(JTJ, JTr, len(camera_ids)*6) # Apply masking now cam_param_mask = np.array([ cam_mask[i/6] for i in range(len(camera_ids)*6) ]) track_param_mask = np.array([ track_mask[i/3] for i in range(len(track_ids)*3) ]) Aslow = Aslow[cam_param_mask].T[cam_param_mask].T bslow = bslow[cam_param_mask] # Check the results self.assertArrayEqual(Aflat, Aslow) self.assertArrayEqual(bflat, bslow)