def check_SO(x):
''' Checks that the given value is a rotation matrix of arbitrary size. '''
check_orthogonal(x)
determinant = np.linalg.det(x * 1.0) # XXX: voodoo
# lapack_lite.LapackError:
# Parameter a has non-native byte order in lapack_lite.dgetrf
assert_allclose(determinant, 1.0)
def SE2_from_SE3(pose, check_exact=True, z_atol=1e-6):
'''
Projects a pose in SE3 to SE2.
If check_exact is True, it will check that z = 0 and axis ~= [0,0,1].
'''
rotation, translation = rotation_translation_from_pose(pose)
axis, angle = axis_angle_from_rotation(rotation)
if check_exact:
sit = '\n pose %s' % pose
sit += '\n axis: %s' % axis
sit += '\n angle: %s' % angle
err_msg = ('I expect that z=0 when projecting to SE2 '
'(check_exact=True).')
err_msg += sit
assert_allclose(translation[2], 0, atol=z_atol, err_msg=err_msg)
# normalize angle z
axis2 = axis * np.sign(axis[2])
err_msg = ('I expect that the rotation is around [0,0,1] '
'when projecting to SE2 (check_exact=True).')
err_msg += sit
assert_allclose(axis2, [0, 0, 1],
rtol=GeometryConstants.rtol_SE2_from_SE3,
atol=GeometryConstants.rtol_SE2_from_SE3, # XXX
err_msg=err_msg)
angle = angle * np.sign(axis[2])
return SE2_from_translation_angle(translation[0:2], angle)
def check_SO(x):
''' Checks that the given value is a rotation matrix of arbitrary size. '''
check_orthogonal(x)
determinant = np.linalg.det(x * 1.0) # XXX: voodoo
# lapack_lite.LapackError:
# Parameter a has non-native byte order in lapack_lite.dgetrf
assert_allclose(determinant, 1.0)
def distance(self, y0, y1):
"""
Compare Y1(s) with best matching pixel in Y2(s_n)
where s_s \in (the neighbourhood of s)
"""
Y1 = y0.resize(self.size).get_values()
Y2 = y1.resize(self.size).get_values()
Y2_unrolled = self.fs.image2unrolledneighbors(Y2)
Y1_repeated = self.fs.image2repeated(Y1)
assert_allclose(Y2_unrolled.shape, Y1_repeated.shape)
diff1 = np.abs(Y2_unrolled - Y1_repeated)
myres = np.mean(np.min(diff1, axis=1))
if False:
# old method, equivalent
neighbor_indices_flat = self.fs.neighbor_indices_flat
nchannels = Y1.shape[2]
nsensel = Y1[:, :, 0].size
best = np.zeros((nsensel, Y1.shape[2]))
for c in range(nchannels):
y1_flat = Y1[:, :, c].astype(np.int16).flat
y2_flat = Y2[:, :, c].astype(np.int16).flat
for k in range(nsensel):
a = y1_flat[k].astype(np.float)
b = y2_flat[neighbor_indices_flat[k]]
diff = np.abs(a - b)
best[k, c] = np.min(diff)
res = np.mean(best)#/self.maxval_distance_neighborhood_bestmatch
assert_allclose(res, myres)
return myres
def SE2_from_SE3(pose, check_exact=True, z_atol=1e-6):
'''
Projects a pose in SE3 to SE2.
If check_exact is True, it will check that z = 0 and axis ~= [0,0,1].
'''
rotation, translation = rotation_translation_from_pose(pose)
axis, angle = axis_angle_from_rotation(rotation)
if check_exact:
sit = '\n pose %s' % pose
sit += '\n axis: %s' % axis
sit += '\n angle: %s' % angle
err_msg = ('I expect that z=0 when projecting to SE2 '
'(check_exact=True).')
err_msg += sit
assert_allclose(translation[2], 0, atol=z_atol, err_msg=err_msg)
# normalize angle z
axis2 = axis * np.sign(axis[2])
err_msg = ('I expect that the rotation is around [0,0,1] '
'when projecting to SE2 (check_exact=True).')
err_msg += sit
assert_allclose(
axis2,
[0, 0, 1],
rtol=GeometryConstants.rtol_SE2_from_SE3,
atol=GeometryConstants.rtol_SE2_from_SE3, # XXX
err_msg=err_msg)
angle = angle * np.sign(axis[2])
return SE2_from_translation_angle(translation[0:2], angle)
def check_orthogonal(x):
''' Check that the argument is an orthogonal matrix. '''
N = x.shape[0]
I = np.eye(N)
rtol = 10E-10 # XXX:
atol = 10E-7 # XXX:
assert_allclose(I, np.dot(x, x.T), rtol=rtol, atol=atol)
assert_allclose(I, np.dot(x.T, x), rtol=rtol, atol=atol)
def check_orthogonal(x):
''' Check that the argument is an orthogonal matrix. '''
N = x.shape[0]
I = np.eye(N)
rtol = 10E-10 # XXX:
atol = 10E-7 # XXX:
assert_allclose(I, np.dot(x, x.T), rtol=rtol, atol=atol)
assert_allclose(I, np.dot(x.T, x), rtol=rtol, atol=atol)
def se2_from_se3(vel, check_exact=True, z_atol=1e-6):
# TODO: testing this
M, v, Z, zero = extract_pieces(vel) # @UnusedVariable
M1 = M[:2, :2]
v1 = v[:2]
if check_exact:
assert_allclose(v[2], 0, atol=z_atol)
return combine_pieces(M1, v1, Z[:2], zero)
def belongs(self, x):
if x.shape != self.shape:
raise ValueError('Expected shape %r, not %r.' %
(self.shape, x.shape))
# TODO: make contract
assert np.all(np.isreal(x)), "Expected real vector"
proj = self.project(x)
assert_allclose(proj, x, atol=1e-8) # XXX: tol
def se2_from_se3(vel, check_exact=True, z_atol=1e-6):
# TODO: testing this
M, v, Z, zero = extract_pieces(vel) # @UnusedVariable
M1 = M[:2, :2]
v1 = v[:2]
if check_exact:
assert_allclose(v[2], 0, atol=z_atol)
return combine_pieces(M1, v1, Z[:2], zero)
def belongs(self, x):
if x.shape != self.shape:
raise ValueError('Expected shape %r, not %r.' %
(self.shape, x.shape))
# TODO: make contract
assert np.all(np.isreal(x)), "Expected real vector"
proj = self.project(x)
assert_allclose(proj, x, atol=1e-8) # XXX: tol
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 double_center(P):
n = P.shape[0]
grand_mean = P.mean()
row_mean = np.zeros(n)
col_mean = np.zeros(n)
for i in range(n):
row_mean[i] = P[i, :].mean()
col_mean[i] = P[:, i].mean()
R = row_mean.reshape(n, 1).repeat(n, axis=1)
assert R.shape == (n, n)
C = col_mean.reshape(1, n).repeat(n, axis=0)
assert C.shape == (n, n)
B2 = -0.5 * (P - R - C + grand_mean)
if False:
B = np.zeros(P.shape)
for i, j in itertools.product(range(n), range(n)):
B[i, j] = -0.5 * (P[i, j] - col_mean[j] - row_mean[i] + grand_mean)
assert_allclose(B2, B)
return B2
def double_center(P):
n = P.shape[0]
grand_mean = P.mean()
row_mean = np.zeros(n)
col_mean = np.zeros(n)
for i in range(n):
row_mean[i] = P[i, :].mean()
col_mean[i] = P[:, i].mean()
R = row_mean.reshape(n, 1).repeat(n, axis=1)
assert R.shape == (n, n)
C = col_mean.reshape(1, n).repeat(n, axis=0)
assert C.shape == (n, n)
B2 = -0.5 * (P - R - C + grand_mean)
if False:
B = np.zeros(P.shape)
for i, j in itertools.product(range(n), range(n)):
B[i, j] = -0.5 * (P[i, j] - col_mean[j] - row_mean[i] + grand_mean)
assert_allclose(B2, B)
return B2
def check_se(M):
''' Checks that the input is in the special euclidean Lie algebra. '''
omega, v, Z, zero = extract_pieces(M) # @UnusedVariable
check_skew_symmetric(omega)
assert_allclose(Z, 0, err_msg='I expect the lower-right to be 0.')
assert_allclose(zero, 0, err_msg='I expect the bottom component to be 0.')
def check_se(M):
''' Checks that the input is in the special euclidean Lie algebra. '''
omega, v, Z, zero = extract_pieces(M) # @UnusedVariable
check_skew_symmetric(omega)
assert_allclose(Z, 0, err_msg='I expect the lower-right to be 0.')
assert_allclose(zero, 0, err_msg='I expect the bottom component to be 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 check_SE(M):
''' Checks that the argument is in the special euclidean group. '''
R, t, zero, one = extract_pieces(M) # @UnusedVariable
check_SO(R)
assert_allclose(one, 1, err_msg='I expect the lower-right to be 1')
assert_allclose(zero, 0, err_msg='I expect the bottom component to be 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 check_SE(M):
''' Checks that the argument is in the special euclidean group. '''
R, t, zero, one = extract_pieces(M) # @UnusedVariable
check_SO(R)
assert_allclose(one, 1, err_msg='I expect the lower-right to be 1')
assert_allclose(zero, 0, err_msg='I expect the bottom component to be 0.')
