def predicted_improvement(d, e, J, sqnorm_e, JTJ, JTe): d = col(d) e = col(e) aa = .5 * sqnorm_e bb = JTe.T.dot(d) c1 = .5 * d.T c2 = JTJ c3 = d cc = c1.dot(c2.dot(c3)) result = 2. * (aa - bb + cc)[0,0] return sqnorm_e - result
def compute_dr_wrt(self, wrt): if wrt is self.locations: locations = self.locations.r.copy() for i in range(3): locations[:,i] = np.clip(locations[:,i], 0, self.image.shape[i]-1) locations = locations.astype(np.uint32) xc = col(self.gx[locations[:,0], locations[:,1], locations[:,2]]) yc = col(self.gy[locations[:,0], locations[:,1], locations[:,2]]) zc = col(self.gz[locations[:,0], locations[:,1], locations[:,2]]) data = np.vstack([xc.ravel(), yc.ravel(), zc.ravel()]).T.copy() JS = np.arange(locations.size) IS = JS // 3 return sp.csc_matrix((data.ravel(), (IS, JS)))
def compute_d1(self): # To stay consistent with numpy, we must upgrade 1D arrays to 2D ar = row(self.a.r) if len(self.a.r.shape)<2 else self.a.r.reshape((-1, self.a.r.shape[-1])) br = col(self.b.r) if len(self.b.r.shape)<2 else self.b.r.reshape((self.b.r.shape[0], -1)) if ar.ndim <= 2: return sp.kron(sp.eye(ar.shape[0], ar.shape[0]),br.T) else: raise NotImplementedError
def compute_dr_wrt(self, wrt): if wrt is self.locations: locations = self.locations.r.copy() for i in range(3): locations[:, i] = np.clip(locations[:, i], 0, self.image.shape[i] - 1) locations = locations.astype(np.uint32) xc = col(self.gx[locations[:, 0], locations[:, 1], locations[:, 2]]) yc = col(self.gy[locations[:, 0], locations[:, 1], locations[:, 2]]) zc = col(self.gz[locations[:, 0], locations[:, 1], locations[:, 2]]) data = np.vstack([xc.ravel(), yc.ravel(), zc.ravel()]).T.copy() JS = np.arange(locations.size) IS = JS // 3 return sp.csc_matrix((data.ravel(), (IS, JS)))
def compute_d1(self): # To stay consistent with numpy, we must upgrade 1D arrays to 2D ar = row(self.a.r) if len(self.a.r.shape) < 2 else self.a.r.reshape( (-1, self.a.r.shape[-1])) br = col(self.b.r) if len(self.b.r.shape) < 2 else self.b.r.reshape( (self.b.r.shape[0], -1)) if ar.ndim <= 2: return sp.kron(sp.eye(ar.shape[0], ar.shape[0]), br.T) else: raise NotImplementedError
def compute_d2(self): # To stay consistent with numpy, we must upgrade 1D arrays to 2D ar = row(self.a.r) if len(self.a.r.shape)<2 else self.a.r br = col(self.b.r) if len(self.b.r.shape)<2 else self.b.r if br.ndim <= 1: return self.ar elif br.ndim <= 2: return sp.kron(ar, sp.eye(br.shape[1],br.shape[1])) else: raise NotImplementedError
def test_maximum(self): from utils import row, col from ch import maximum # Make sure that when we compare the max of two *identical* numbers, # we get the right derivatives wrt both the_max = maximum(ch.Ch(1), ch.Ch(1)) self.assertTrue(the_max.r.ravel()[0] == 1.) self.assertTrue(the_max.dr_wrt(the_max.a)[0, 0] == 1.) self.assertTrue(the_max.dr_wrt(the_max.b)[0, 0] == 1.) # Now test given that all numbers are different, by allocating from # a pool of randomly permuted numbers. # We test combinations of scalars and 2d arrays. rnd = np.asarray(np.random.permutation(np.arange(20)), np.float64) c1 = ch.Ch(rnd[:6].reshape((2, 3))) c2 = ch.Ch(rnd[6:12].reshape((2, 3))) s1 = ch.Ch(rnd[12]) s2 = ch.Ch(rnd[13]) eps = .1 for first in [c1, s1]: for second in [c2, s2]: the_max = maximum(first, second) for which_to_change in [first, second]: max_r0 = the_max.r.copy() max_r_diff = np.max( np.abs(max_r0 - np.maximum(first.r, second.r))) self.assertTrue(max_r_diff == 0) max_dr = the_max.dr_wrt(which_to_change).copy() which_to_change.x = which_to_change.x + eps max_r1 = the_max.r.copy() emp_diff = (the_max.r - max_r0).ravel() pred_diff = max_dr.dot(col( eps * np.ones(max_dr.shape[1]))).ravel() #print 'comparing the following numbers/vectors:' #print first.r #print second.r #print 'empirical vs predicted difference:' #print emp_diff #print pred_diff #print '-----' max_dr_diff = np.max(np.abs(emp_diff - pred_diff)) #print 'max dr diff: %.2e' % (max_dr_diff,) self.assertTrue(max_dr_diff < 1e-14)
def test_maximum(self): from utils import row, col from ch import maximum # Make sure that when we compare the max of two *identical* numbers, # we get the right derivatives wrt both the_max = maximum(ch.Ch(1), ch.Ch(1)) self.assertTrue(the_max.r.ravel()[0] == 1.) self.assertTrue(the_max.dr_wrt(the_max.a)[0,0] == 1.) self.assertTrue(the_max.dr_wrt(the_max.b)[0,0] == 1.) # Now test given that all numbers are different, by allocating from # a pool of randomly permuted numbers. # We test combinations of scalars and 2d arrays. rnd = np.asarray(np.random.permutation(np.arange(20)), np.float64) c1 = ch.Ch(rnd[:6].reshape((2,3))) c2 = ch.Ch(rnd[6:12].reshape((2,3))) s1 = ch.Ch(rnd[12]) s2 = ch.Ch(rnd[13]) eps = .1 for first in [c1, s1]: for second in [c2, s2]: the_max = maximum(first, second) for which_to_change in [first, second]: max_r0 = the_max.r.copy() max_r_diff = np.max(np.abs(max_r0 - np.maximum(first.r, second.r))) self.assertTrue(max_r_diff == 0) max_dr = the_max.dr_wrt(which_to_change).copy() which_to_change.x = which_to_change.x + eps max_r1 = the_max.r.copy() emp_diff = (the_max.r - max_r0).ravel() pred_diff = max_dr.dot(col(eps*np.ones(max_dr.shape[1]))).ravel() #print 'comparing the following numbers/vectors:' #print first.r #print second.r #print 'empirical vs predicted difference:' #print emp_diff #print pred_diff #print '-----' max_dr_diff = np.max(np.abs(emp_diff-pred_diff)) #print 'max dr diff: %.2e' % (max_dr_diff,) self.assertTrue(max_dr_diff < 1e-14)
def test_transpose(self): from utils import row, col from copy import deepcopy for which in ('C', 'F'): # test in fortran and contiguous mode a = ch.Ch(np.require(np.zeros(8).reshape((4,2)), requirements=which)) b = a.T b1 = b.r.copy() #dr = b.dr_wrt(a).copy() dr = deepcopy(b.dr_wrt(a)) diff = np.arange(a.size).reshape(a.shape) a.x = np.require(a.r + diff, requirements=which) b2 = b.r.copy() diff_pred = dr.dot(col(diff)).ravel() diff_emp = (b2 - b1).ravel() np.testing.assert_array_equal(diff_pred, diff_emp)
def _minimize_dogleg(obj, free_variables, on_step=None, maxiter=200, max_fevals=np.inf, sparse_solver='spsolve', disp=False, show_residuals=None, e_1=1e-15, e_2=1e-15, e_3=0., delta_0=None): """"Nonlinear optimization using Powell's dogleg method. See Lourakis et al, 2005, ICCV '05, "Is Levenberg-Marquardt the Most Efficient Optimization for Implementing Bundle Adjustment?": http://www.ics.forth.gr/cvrl/publications/conferences/0201-P0401-lourakis-levenberg.pdf """ import warnings if show_residuals is not None: import warnings warnings.warn('minimize_dogleg: show_residuals parm is deprecaed, pass a dict instead.') labels = {} if isinstance(obj, list) or isinstance(obj, tuple): obj = ch.concatenate([f.ravel() for f in obj]) elif isinstance(obj, dict): labels = obj obj = ch.concatenate([f.ravel() for f in obj.values()]) niters = maxiter verbose = disp num_unique_ids = len(np.unique(np.array([id(freevar) for freevar in free_variables]))) if num_unique_ids != len(free_variables): raise Exception('The "free_variables" param contains duplicate variables.') obj = ChInputsStacked(obj=obj, free_variables=free_variables, x=np.concatenate([freevar.r.ravel() for freevar in free_variables])) def call_cb(): if on_step is not None: on_step(obj) report_line = "" if len(labels) > 0: report_line += '%.2e | ' % (np.sum(obj.r**2),) for label in sorted(labels.keys()): objective = labels[label] report_line += '%s: %.2e | ' % (label, np.sum(objective.r**2)) if len(labels) > 0: report_line += '\n' sys.stderr.write(report_line) call_cb() # pif = print-if-verbose. # can't use "print" because it's a statement, not a fn pif = lambda x: sys.stdout.write(x + '\n') if verbose else 0 if callable(sparse_solver): solve = sparse_solver elif isinstance(sparse_solver, str) and sparse_solver in _solver_fns.keys(): solve = _solver_fns[sparse_solver] else: raise Exception('sparse_solver argument must be either a string in the set (%s) or have the api of scipy.sparse.linalg.spsolve.' % ''.join(_solver_fns.keys(), ' ')) # optimization parms k_max = niters fevals = 0 k = 0 delta = delta_0 p = col(obj.x.r) fevals += 1 tm = time.time() pif('computing Jacobian...') J = obj.J if sp.issparse(J): assert(J.nnz > 0) pif('Jacobian (%dx%d) computed in %.2fs' % (J.shape[0], J.shape[1], time.time() - tm)) if J.shape[1] != p.size: import pdb; pdb.set_trace() assert(J.shape[1] == p.size) tm = time.time() pif('updating A and g...') A = J.T.dot(J) r = col(obj.r.copy()) g = col(J.T.dot(-r)) pif('A and g updated in %.2fs' % (time.time() - tm)) stop = norm(g, np.inf) < e_1 while (not stop) and (k < k_max) and (fevals < max_fevals): k += 1 pif('beginning iteration %d' % (k,)) d_sd = col((sqnorm(g)) / (sqnorm (J.dot(g))) * g) GNcomputed = False while True: # if the Cauchy point is outside the trust region, # take that direction but only to the edge of the trust region if delta is not None and norm(d_sd) >= delta: pif('PROGRESS: Using stunted cauchy') d_dl = np.array(col(delta/norm(d_sd) * d_sd)) else: if not GNcomputed: tm = time.time() if scipy.sparse.issparse(A): A.eliminate_zeros() pif('sparse solve...sparsity infill is %.3f%% (hessian %dx%d), J infill %.3f%%' % ( 100. * A.nnz / (A.shape[0] * A.shape[1]), A.shape[0], A.shape[1], 100. * J.nnz / (J.shape[0] * J.shape[1]))) if g.size > 1: d_gn = col(solve(A, g)) if np.any(np.isnan(d_gn)) or np.any(np.isinf(d_gn)): from scipy.sparse.linalg import lsqr d_gn = col(lsqr(A, g)[0]) else: d_gn = np.atleast_1d(g.ravel()[0]/A[0,0]) pif('sparse solve...done in %.2fs' % (time.time() - tm)) else: pif('dense solve...') try: d_gn = col(np.linalg.solve(A, g)) except Exception: d_gn = col(np.linalg.lstsq(A, g)[0]) pif('dense solve...done in %.2fs' % (time.time() - tm)) GNcomputed = True # if the gauss-newton solution is within the trust region, use it if delta is None or norm(d_gn) <= delta: pif('PROGRESS: Using gauss-newton solution') d_dl = np.array(d_gn) if delta is None: delta = norm(d_gn) else: # between cauchy step and gauss-newton step pif('PROGRESS: between cauchy and gauss-newton') # compute beta multiplier delta_sq = delta**2 pnow = ( (d_gn-d_sd).T.dot(d_gn-d_sd)*delta_sq + d_gn.T.dot(d_sd)**2 - sqnorm(d_gn) * (sqnorm(d_sd))) B = delta_sq - sqnorm(d_sd) B /= ((d_gn-d_sd).T.dot(d_sd) + math.sqrt(pnow)) # apply step d_dl = np.array(d_sd + float(B) * (d_gn - d_sd)) #assert(math.fabs(norm(d_dl) - delta) < 1e-12) if norm(d_dl) <= e_2*norm(p): pif('stopping because of small step size (norm_dl < %.2e)' % (e_2*norm(p))) stop = True else: p_new = p + d_dl tm_residuals = time.time() obj.x = p_new fevals += 1 r_trial = obj.r.copy() tm_residuals = time.time() - tm # rho is the ratio of... # (improvement in SSE) / (predicted improvement in SSE) # slower #rho = norm(e_p)**2 - norm(e_p_trial)**2 #rho = rho / (L(d_dl*0, e_p, J) - L(d_dl, e_p, J)) # faster sqnorm_ep = sqnorm(r) rho = sqnorm_ep - norm(r_trial)**2 with warnings.catch_warnings(): warnings.filterwarnings('ignore',category=RuntimeWarning) if rho > 0: rho /= predicted_improvement(d_dl, -r, J, sqnorm_ep, A, g) improvement_occurred = rho > 0 # if the objective function improved, update input parameter estimate. # Note that the obj.x already has the new parms, # and we should not set them again to the same (or we'll bust the cache) if improvement_occurred: p = col(p_new) call_cb() if (sqnorm_ep - norm(r_trial)**2) / sqnorm_ep < e_3: stop = True pif('stopping because improvement < %.1e%%' % (100*e_3,)) else: # Put the old parms back obj.x = ch.Ch(p) obj.on_changed('x') # copies from flat vector to free variables # if the objective function improved and we're not done, # get ready for the next iteration if improvement_occurred and not stop: tm_jac = time.time() pif('computing Jacobian...') J = obj.J.copy() tm_jac = time.time() - tm_jac pif('Jacobian (%dx%d) computed in %.2fs' % (J.shape[0], J.shape[1], tm_jac)) pif('Residuals+Jac computed in %.2fs' % (tm_jac + tm_residuals,)) tm = time.time() pif('updating A and g...') A = J.T.dot(J) r = col(r_trial) g = col(J.T.dot(-r)) pif('A and g updated in %.2fs' % (time.time() - tm)) if norm(g, np.inf) < e_1: stop = True pif('stopping because norm(g, np.inf) < %.2e' % (e_1)) # update our trust region delta = updateRadius(rho, delta, d_dl) if delta <= e_2*norm(p): stop = True pif('stopping because trust region is too small') # the following "collect" is very expensive. # please contact matt if you find situations where it actually helps things. #import gc; gc.collect() if stop or improvement_occurred or (fevals >= max_fevals): break if k >= k_max: pif('stopping because max number of user-specified iterations (%d) has been met' % (k_max,)) elif fevals >= max_fevals: pif('stopping because max number of user-specified func evals (%d) has been met' % (max_fevals,)) return obj.free_variables
def _minimize_dogleg(obj, free_variables, on_step=None, maxiter=200, max_fevals=np.inf, sparse_solver='spsolve', disp=False, show_residuals=None, e_1=1e-15, e_2=1e-15, e_3=0., delta_0=None): """"Nonlinear optimization using Powell's dogleg method. See Lourakis et al, 2005, ICCV '05, "Is Levenberg-Marquardt the Most Efficient Optimization for Implementing Bundle Adjustment?": http://www.ics.forth.gr/cvrl/publications/conferences/0201-P0401-lourakis-levenberg.pdf """ if show_residuals is not None: import warnings warnings.warn('minimize_dogleg: show_residuals parm is deprecaed, pass a dict instead.') labels = {} if isinstance(obj, list) or isinstance(obj, tuple): obj = ch.concatenate([f.ravel() for f in obj]) elif isinstance(obj, dict): labels = obj obj = ch.concatenate([f.ravel() for f in obj.values()]) niters = maxiter verbose = disp num_unique_ids = len(np.unique(np.array([id(freevar) for freevar in free_variables]))) if num_unique_ids != len(free_variables): raise Exception('The "free_variables" param contains duplicate variables.') obj = ChInputsStacked(obj=obj, free_variables=free_variables, x=np.concatenate([freevar.r.ravel() for freevar in free_variables])) def call_cb(): if on_step is not None: on_step(obj) report_line = "" if len(labels) > 0: report_line += '%.2e | ' % (np.sum(obj.r**2),) for label in sorted(labels.keys()): objective = labels[label] report_line += '%s: %.2e | ' % (label, np.sum(objective.r**2)) if len(labels) > 0: report_line += '\n' sys.stderr.write(report_line) call_cb() # pif = print-if-verbose. # can't use "print" because it's a statement, not a fn pif = lambda x: sys.stdout.write(x + '\n') if verbose else 0 if callable(sparse_solver): solve = sparse_solver elif isinstance(sparse_solver, str) and sparse_solver in _solver_fns.keys(): solve = _solver_fns[sparse_solver] else: raise Exception('sparse_solver argument must be either a string in the set (%s) or have the api of scipy.sparse.linalg.spsolve.' % ''.join(_solver_fns.keys(), ' ')) # optimization parms k_max = niters fevals = 0 k = 0 delta = delta_0 p = col(obj.x.r) fevals += 1 tm = time.time() pif('computing Jacobian...') J = obj.J if sp.issparse(J): assert(J.nnz > 0) pif('Jacobian (%dx%d) computed in %.2fs' % (J.shape[0], J.shape[1], time.time() - tm)) if J.shape[1] != p.size: import pdb; pdb.set_trace() assert(J.shape[1] == p.size) tm = time.time() pif('updating A and g...') A = J.T.dot(J) r = col(obj.r.copy()) g = col(J.T.dot(-r)) pif('A and g updated in %.2fs' % (time.time() - tm)) stop = norm(g, np.inf) < e_1 while (not stop) and (k < k_max) and (fevals < max_fevals): k += 1 pif('beginning iteration %d' % (k,)) d_sd = col((sqnorm(g)) / (sqnorm (J.dot(g))) * g) GNcomputed = False while True: # if the Cauchy point is outside the trust region, # take that direction but only to the edge of the trust region if delta is not None and norm(d_sd) >= delta: pif('PROGRESS: Using stunted cauchy') d_dl = np.array(col(delta/norm(d_sd) * d_sd)) else: if not GNcomputed: tm = time.time() if scipy.sparse.issparse(A): A.eliminate_zeros() pif('sparse solve...sparsity infill is %.3f%% (hessian %dx%d), J infill %.3f%%' % ( 100. * A.nnz / (A.shape[0] * A.shape[1]), A.shape[0], A.shape[1], 100. * J.nnz / (J.shape[0] * J.shape[1]))) d_gn = col(solve(A, g)) if g.size>1 else np.atleast_1d(g.ravel()[0]/A[0,0]) pif('sparse solve...done in %.2fs' % (time.time() - tm)) else: pif('dense solve...') try: d_gn = col(np.linalg.solve(A, g)) except Exception: d_gn = col(np.linalg.lstsq(A, g)[0]) pif('dense solve...done in %.2fs' % (time.time() - tm)) GNcomputed = True # if the gauss-newton solution is within the trust region, use it if delta is None or norm(d_gn) <= delta: pif('PROGRESS: Using gauss-newton solution') d_dl = np.array(d_gn) if delta is None: delta = norm(d_gn) else: # between cauchy step and gauss-newton step pif('PROGRESS: between cauchy and gauss-newton') # compute beta multiplier delta_sq = delta**2 pnow = ( (d_gn-d_sd).T.dot(d_gn-d_sd)*delta_sq + d_gn.T.dot(d_sd)**2 - sqnorm(d_gn) * (sqnorm(d_sd))) B = delta_sq - sqnorm(d_sd) B /= ((d_gn-d_sd).T.dot(d_sd) + math.sqrt(pnow)) # apply step d_dl = np.array(d_sd + float(B) * (d_gn - d_sd)) #assert(math.fabs(norm(d_dl) - delta) < 1e-12) if norm(d_dl) <= e_2*norm(p): pif('stopping because of small step size (norm_dl < %.2e)' % (e_2*norm(p))) stop = True else: p_new = p + d_dl tm_residuals = time.time() obj.x = p_new fevals += 1 r_trial = obj.r.copy() tm_residuals = time.time() - tm # rho is the ratio of... # (improvement in SSE) / (predicted improvement in SSE) # slower #rho = norm(e_p)**2 - norm(e_p_trial)**2 #rho = rho / (L(d_dl*0, e_p, J) - L(d_dl, e_p, J)) # faster sqnorm_ep = sqnorm(r) rho = sqnorm_ep - norm(r_trial)**2 if rho > 0: rho /= predicted_improvement(d_dl, -r, J, sqnorm_ep, A, g) improvement_occurred = rho > 0 # if the objective function improved, update input parameter estimate. # Note that the obj.x already has the new parms, # and we should not set them again to the same (or we'll bust the cache) if improvement_occurred: p = col(p_new) call_cb() if (sqnorm_ep - norm(r_trial)**2) / sqnorm_ep < e_3: stop = True pif('stopping because improvement < %.1e%%' % (100*e_3,)) else: # Put the old parms back obj.x = ch.Ch(p) obj.on_changed('x') # copies from flat vector to free variables # if the objective function improved and we're not done, # get ready for the next iteration if improvement_occurred and not stop: tm_jac = time.time() pif('computing Jacobian...') J = obj.J.copy() tm_jac = time.time() - tm_jac pif('Jacobian (%dx%d) computed in %.2fs' % (J.shape[0], J.shape[1], tm_jac)) pif('Residuals+Jac computed in %.2fs' % (tm_jac + tm_residuals,)) tm = time.time() pif('updating A and g...') A = J.T.dot(J) r = col(r_trial) g = col(J.T.dot(-r)) pif('A and g updated in %.2fs' % (time.time() - tm)) if norm(g, np.inf) < e_1: stop = True pif('stopping because norm(g, np.inf) < %.2e' % (e_1)) # update our trust region delta = updateRadius(rho, delta, d_dl) if delta <= e_2*norm(p): stop = True pif('stopping because trust region is too small') # the following "collect" is very expensive. # please contact matt if you find situations where it actually helps things. #import gc; gc.collect() if stop or improvement_occurred or (fevals >= max_fevals): break if k >= k_max: pif('stopping because max number of user-specified iterations (%d) has been met' % (k_max,)) elif fevals >= max_fevals: pif('stopping because max number of user-specified func evals (%d) has been met' % (max_fevals,)) return obj.free_variables
def set_and_get_r(self, x_in): self.x = Ch(x_in) return col(self.r)
def compute_r(self): result = self.mtx.dot(col(self.vec.r.ravel())).ravel() if len(self.vec.r.shape) > 1 and self.vec.r.shape[1] > 1: result = result.reshape((-1,self.vec.r.shape[1])) return result