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
