def distances(self, a, b):
""" Returns linear, angular distance. """
_, vel = self.logmap(a, b)
W, v, _, _ = extract_pieces(vel)
dist1 = np.linalg.norm(v)
dist2 = self.algebra.son.norm(W)
return np.array([dist1, dist2])
def belongs(self, x):
# TODO: more checks
if not isinstance(x, np.ndarray):
msg = 'Expected a numpy array (%s)' % describe_type(x)
raise ValueError(msg)
if not x.shape == (self.n, self.n):
msg = ('Expected shape %dx%d instead of (%s)' %
(self.n, self.n, x.shape))
raise ValueError(msg)
R, t, zero, one = extract_pieces(x) # @UnusedVariable
self.SOn.belongs(R)
assert_allclose(zero, 0, err_msg='I expect the lower row to be 0.')
assert_allclose(one, 1)
def vector_from_algebra(self, a):
""" Note that it returns (omega, vx, vy) or (w1,w2,w3,vx,vy,vz) """
W, v, zero, zero = extract_pieces(a) # @UnusedVariable
if self.n == 3:
assert v.size == 2
V = np.zeros(3)
V[0] = self.son.vector_from_algebra(W)
V[1:3] = v
return V
elif self.n == 4:
assert v.size == 3
V = np.zeros(6)
V[0:3] = self.son.vector_from_algebra(W)
V[3:6] = v
return V
else:
assert False, 'Not implemented for n>=4.'
def vector_from_algebra(self, a):
""" Note that it returns (omega, vx, vy) or (w1,w2,w3,vx,vy,vz) """
W, v, zero, zero = extract_pieces(a) # @UnusedVariable
if self.n == 3:
assert v.size == 2
V = np.zeros(3)
V[0] = self.son.vector_from_algebra(W)
V[1:3] = v
return V
elif self.n == 4:
assert v.size == 3
V = np.zeros(6)
V[0:3] = self.son.vector_from_algebra(W)
V[3:6] = v
return V
else:
assert False, 'Not implemented for n>=4.'
def belongs(self, x):
# TODO: explicit
R, t, zero, one = extract_pieces(x) # @UnusedVariable
assert_allclose(R, np.eye(self.n - 1))
assert_allclose(zero, 0, err_msg='I expect the lower row to be 0.')
assert_allclose(one, 1, err_msg='Bottom-right must be 1.')
def se2_project_from_se3(b):
W, v, zero, one = extract_pieces(b) # @UnusedVariable
W = so2_project_from_so3(W)
v = v[0:2]
return combine_pieces(W, v, v * 0, 0)
def SE2_project_from_SE3(b):
R, t, zero, one = extract_pieces(b) # @UnusedVariable
R = SO2_project_from_SO3(R)
t = t[0:2]
return combine_pieces(R, t, t * 0, 1)
def project(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
W = self.son.project(W)
return combine_pieces(W, v, v * 0, 0)
def so2_project_from_se2(b):
return extract_pieces(b)[0]
def vector_from_algebra(self, a):
W, v, zero, zero = extract_pieces(a) # @UnusedVariable
if v.shape == ():
v = v.reshape(1)
assert v.size == self.n - 1
return v
def project(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
return combine_pieces(W * 0, v, v * 0, 0)
def norm(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
return np.linalg.norm(v)
def vector_from_algebra(self, a):
W, v, zero, zero = extract_pieces(a) # @UnusedVariable
if v.shape == ():
v = v.reshape(1)
assert v.size == self.n - 1
return v
def project(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
return combine_pieces(W * 0, v, v * 0, 0)
def belongs(self, x):
# TODO: explicit
R, t, zero, one = extract_pieces(x) # @UnusedVariable
assert_allclose(R, np.eye(self.n - 1))
assert_allclose(zero, 0, err_msg='I expect the lower row to be 0.')
assert_allclose(one, 1, err_msg='Bottom-right must be 1.')
def norm(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
return np.linalg.norm(v) + self.alpha * self.son.norm(W)
def so3_project_from_se3(b):
return extract_pieces(b)[0]
def se3_from_se2(a):
W, v, zero, one = extract_pieces(a) # @UnusedVariable
W = so3_from_so2(W)
v = np.array([v[0], v[1], 0])
return combine_pieces(W, v, v * 0, 0)
def project(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
W = self.son.project(W)
return combine_pieces(W, v, v * 0, 0)
