def optimize(self): num_points = len(self.graph.free_points) point_dim = self.graph.point_dim num_landmarks = len(self.graph.landmarks) landmark_dim = self.graph.landmark_dim Am, Ap, d, sigma_d = self.graph.observation_system() Bp, t, sigma_t = self.graph.odometry_system() S_d, S_t = sp.sparse.diags( 1 / _sanitized_noise_array(sigma_d)), sp.sparse.diags( 1 / _sanitized_noise_array(sigma_t)) if (num_points != 0) and (num_landmarks != 0): M = cp.Variable((landmark_dim, num_landmarks)) P = cp.Variable((point_dim, num_points)) objective = cp.Minimize( sum_squares(S_d * ((Am * vec(M)) + (Ap * vec(P)) - d)) + sum_squares(S_t * ((Bp * vec(P)) - t))) problem = cp.Problem(objective) problem.solve(verbose=self._verbosity, solver=self._solver) self.M = M.value self.P = P.value m = self.M.ravel(order='F') p = self.P.ravel(order='F') self.res_d = Am.dot(m) + Ap.dot(p) - d self.res_t = Bp.dot(p) - t elif (num_points != 0) and (num_landmarks == 0): P = cp.Variable((point_dim, num_points)) objective = cp.Minimize( sum_squares(sum_squares(S_t * ((Bp * vec(P)) - t)))) problem = cp.Problem(objective) problem.solve(verbose=self._verbosity, solver=self._solver) self.P = P.value p = self.P.ravel(order='F') self.res_t = Bp.dot(p) - t else: return
def get_special_slice(expr, key): """Indexing using logical indexing or a list of indices. Parameters ---------- expr : Expression The expression being indexed/sliced into. key : tuple ndarrays or lists. Returns ------- Expression An expression representing the index/slice. """ expr = index.cast_to_const(expr) # Order the entries of expr and select them using key. idx_mat = np.arange(expr.size[0]*expr.size[1]) idx_mat = np.reshape(idx_mat, expr.size, order='F') select_mat = idx_mat[key] if select_mat.ndim == 2: final_size = select_mat.shape else: # Always cast 1d arrays as column vectors. final_size = (select_mat.size, 1) select_vec = np.reshape(select_mat, select_mat.size, order='F') # Select the chosen entries from expr. identity = sp.eye(expr.size[0]*expr.size[1]).tocsc() return reshape(identity[select_vec]*vec(expr), *final_size)
def optimize(self): num_points = len(self.graph.free_points) point_dim = self.graph.point_dim num_landmarks = len(self.graph.landmarks) landmark_dim = self.graph.landmark_dim Am, Ap, d, _ = self.graph.observation_system() Bp, t, _ = self.graph.odometry_system() if (num_points != 0) and (num_landmarks != 0): M = cp.Variable((landmark_dim, num_landmarks)) P = cp.Variable((point_dim, num_points)) objective = cp.Minimize( sum_squares(Am * vec(M) + Ap * vec(P) - d) + sum_squares(Bp * vec(P) - t)) problem = cp.Problem(objective) problem.solve(verbose=self._verbosity, solver=self._solver) self.M = M.value self.P = P.value m = self.M.ravel(order='F') p = self.P.ravel(order='F') self.res_d = Am.dot(m) + Ap.dot(p) - d self.res_t = Bp.dot(p) - t elif (num_points != 0) and (num_landmarks == 0): P = cp.Variable((point_dim, num_points)) objective = cp.Minimize(sum_squares(Bp * vec(P) - t)) problem = cp.Problem(objective) problem.solve(verbose=self._verbosity, solver=self._solver) self.P = P.value p = self.P.ravel(order='F') self.res_t = Bp.dot(p) - t else: return
def special_index_canon(expr, args): select_mat = expr._select_mat final_shape = expr._select_mat.shape select_vec = np.reshape(select_mat, select_mat.size, order='F') # Select the chosen entries from expr. arg = args[0] identity = sp.eye(arg.size).tocsc() lowered = reshape(identity[select_vec] * vec(arg), final_shape) return lowered, []
def pnorm_canon(expr, args): x = args[0] p = expr.p axis = expr.axis shape = expr.shape t = Variable(shape) if p == 2: if axis is None: assert shape == tuple() return t, [SOC(t, vec(x))] else: return t, [SOC(vec(t), x, axis)] # we need an absolute value constraint for the symmetric convex branches # (p > 1) constraints = [] if p > 1: # TODO(akshayka): Express this more naturally (recursively), in terms # of the other atoms abs_expr = abs(x) abs_x, abs_constraints = abs_canon(abs_expr, abs_expr.args) x = abs_x constraints += abs_constraints # now, we take care of the remaining convex and concave branches # to create the rational powers, we need a new variable, r, and # the constraint sum(r) == t r = Variable(x.shape) constraints += [sum(r) == t] # todo: no need to run gm_constr to form the tree each time. # we only need to form the tree once promoted_t = Constant(np.ones(x.shape)) * t p = Fraction(p) if p < 0: constraints += gm_constrs(promoted_t, [x, r], (-p / (1 - p), 1 / (1 - p))) if 0 < p < 1: constraints += gm_constrs(r, [x, promoted_t], (p, 1 - p)) if p > 1: constraints += gm_constrs(x, [r, promoted_t], (1 / p, 1 - 1 / p)) return t, constraints
def sum_canon(expr, args): X = args[0] if expr.axis is None: x = vec(X) summation = sum([xi for xi in x]) canon, _ = add_canon(summation, summation.args) return reshape(canon, expr.shape), [] if expr.axis == 0: X = X.T rows = [] for i in range(X.shape[0]): x = vec(X[i]) summation = sum([xi for xi in x]) canon, _ = add_canon(summation, summation.args) rows.append(canon) canon = hstack(rows) return reshape(canon, expr.shape), []
def _m_step(self, W, Am, Ap, d, sigma_d): num_points = len(self.graph.free_points) point_dim = self.graph.point_dim num_landmarks = len(self.graph.landmarks) landmark_dim = self.graph.landmark_dim Bp, t, sigma_t = self.graph.odometry_system() S_t = sp.sparse.diags(1 / _sanitized_noise_array(sigma_t)) sigma_d = np.tile(_sanitized_noise_array(sigma_d), num_landmarks) S_d = sp.sparse.diags(1 / sigma_d) W = sp.sparse.diags(W.flatten('F')) Am = [ np.zeros((Am.shape[0], Am.shape[1])) for _ in range(num_landmarks) ] for j in range(num_landmarks): Am[j][:, j] = 1 Am = sp.sparse.csr_matrix(np.concatenate(Am, axis=0)) Ap = sp.sparse.vstack([Ap for _ in range(num_landmarks)]) d = np.tile(d, num_landmarks) M = cp.Variable((landmark_dim, num_landmarks)) P = cp.Variable((point_dim, num_points)) objective = cp.Minimize( sum_squares(W * S_d * ((Am * vec(M)) + (Ap * vec(P)) - d)) + sum_squares(S_t * ((Bp * vec(P)) - t))) problem = cp.Problem(objective) problem.solve(verbose=self._verbosity, solver=self._solver) self.M = M.value self.P = P.value m = self.M.ravel(order='F') p = self.P.ravel(order='F') self.res_d = Am.dot(m) + Ap.dot(p) - d self.res_t = Bp.dot(p) - t
def grad(self): """Gives the (sub/super)gradient of the expression w.r.t. each variable. Matrix expressions are vectorized, so the gradient is a matrix. None indicates variable values unknown or outside domain. Returns: A map of variable to SciPy CSC sparse matrix or None. """ select_vec = np.reshape(self._select_mat, self._select_mat.size, order='F') identity = sp.eye(self.args[0].size).tocsc() lowered = reshape(identity[select_vec] @ vec(self.args[0]), self._shape) return lowered.grad
def norm(x, p=2, axis=None): """Wrapper on the different norm atoms. Parameters ---------- x : Expression or numeric constant The value to take the norm of. If `x` is 2D and `axis` is None, this function constructs a matrix norm. p : int or str, optional The type of norm. Valid options include any positive integer, 'fro' (for frobenius), 'nuc' (sum of singular values), np.inf or 'inf' (infinity norm). axis : The axis along which to apply the norm, if any. Returns ------- Expression An Expression representing the norm. """ x = Expression.cast_to_const(x) # matrix norms take precedence num_nontrivial_idxs = sum([d > 1 for d in x.shape]) if axis is None and x.ndim == 2: if p == 1: # matrix 1-norm return cvxpy.atoms.max(norm1(x, axis=0)) # Frobenius norm elif p == 'fro' or (p == 2 and num_nontrivial_idxs == 1): return pnorm(vec(x), 2) elif p == 2: # matrix 2-norm is largest singular value return sigma_max(x) elif p == 'nuc': # the nuclear norm (sum of singular values) return normNuc(x) elif p in [np.inf, "inf", "Inf"]: # the matrix infinity-norm return cvxpy.atoms.max(norm1(x, axis=1)) else: raise RuntimeError('Unsupported matrix norm.') else: if p == 1 or x.is_scalar(): return norm1(x, axis=axis) elif p in [np.inf, "inf", "Inf"]: return norm_inf(x, axis) else: return pnorm(x, p, axis)
def diag(expr): """Extracts the diagonal from a matrix or makes a vector a diagonal matrix. Parameters ---------- expr : Expression or numeric constant A vector or square matrix. Returns ------- Expression An Expression representing the diagonal vector/matrix. """ expr = AffAtom.cast_to_const(expr) if expr.is_vector(): return diag_vec(vec(expr)) elif expr.ndim == 2 and expr.shape[0] == expr.shape[1]: return diag_mat(expr) else: raise ValueError("Argument to diag must be a vector or square matrix.")
def norm(x, p=2, axis=None): """Wrapper on the different norm atoms. Parameters ---------- x : Expression or numeric constant The value to take the norm of. p : int or str, optional The type of norm. Returns ------- Expression An Expression representing the norm. """ x = Expression.cast_to_const(x) # matrix norms take precedence if axis is None and x.ndim == 2: if p == 1: # matrix 1-norm return cvxpy.atoms.max(norm1(x, axis=0)) elif p == 2: # matrix 2-norm is largest singular value return sigma_max(x) elif p == 'nuc': # the nuclear norm (sum of singular values) return normNuc(x) elif p == 'fro': # Frobenius norm return pnorm(vec(x), 2) elif p in [np.inf, "inf", "Inf"]: # the matrix infinity-norm return cvxpy.atoms.max(norm1(x, axis=1)) else: raise RuntimeError('Unsupported matrix norm.') else: if p == 1 or x.is_scalar(): return norm1(x, axis=axis) elif p in [np.inf, "inf", "Inf"]: return norm_inf(x, axis) else: return pnorm(x, p, axis)
def quad_over_lin_canon(expr, args): x = vec(args[0]) y = args[1] numerator = sum(2 * xi for xi in x) return numerator - y, []
def optimize(self): self._pre_optimizer.optimize() self._pre_optimizer.update() points = self.graph.free_points landmarks = self.graph.landmarks num_points = len(points) point_dim = self.graph.point_dim num_landmarks = len(landmarks) landmark_dim = self.graph.landmark_dim transforms = [ equivalence.SumMass(self.graph.correspondence_map.set_map()), equivalence.ExpDistance(self._sigma), equivalence.Facing() ] E, W = equivalence.equivalence_matrix(landmarks, transforms=transforms) if E.shape[0] == 0: self.M = self._pre_optimizer.M self.P = self._pre_optimizer.P self.res_d = self._pre_optimizer.res_d self.res_t = self._pre_optimizer.res_t self.equivalence_pairs = [] return Am, Ap, d, sigma_d = self.graph.observation_system() Bp, t, sigma_t = self.graph.odometry_system() S_d, S_t = sp.sparse.diags( 1 / _sanitized_noise_array(sigma_d)), sp.sparse.diags( 1 / _sanitized_noise_array(sigma_t)) M = cp.Variable((landmark_dim, num_landmarks)) P = cp.Variable((point_dim, num_points)) M.value = self._pre_optimizer.M P.value = self._pre_optimizer.P objective = cp.Minimize(mixed_norm(W * E * M.T)) constraints = [ norm((Am * vec(M)) + (Ap * vec(P)) - d) <= 2 * np.linalg.norm(sigma_d + 1e-6), norm((Bp * vec(P)) - t) <= 2 * np.linalg.norm(sigma_t + 1e-6) ] problem = cp.Problem(objective, constraints) problem.solve(verbose=self._verbosity, solver=self._solver, warm_start=True) if problem.solution.status == 'infeasible': self.M = self._pre_optimizer.M self.P = self._pre_optimizer.P self.res_d = self._pre_optimizer.res_d self.res_t = self._pre_optimizer.res_t self.equivalence_pairs = [] return E_ = E[np.abs(np.linalg.norm(E * M.value.T, axis=1)) < 0.001, :] objective = cp.Minimize( sum_squares(S_d * ((Am * vec(M)) + (Ap * vec(P)) - d)) + sum_squares(S_t * ((Bp * vec(P)) - t))) constraints = [E_ * M.T == 0] if E_.shape[0] > 0 else [] problem = cp.Problem(objective, constraints) problem.solve(verbose=self._verbosity, solver=self._solver, warm_start=True) self.M = M.value self.P = P.value m = self.M.ravel(order='F') p = self.P.ravel(order='F') self.res_d = Am.dot(m) + Ap.dot(p) - d self.res_t = Bp.dot(p) - t self.equivalence_pairs = [(landmarks[i], landmarks[j]) for (i, j) in E_.tolil().rows]
def explicit_sum(expr): x = vec(expr) summation = x[0] for xi in x[1:]: summation += xi return summation