def Basis2Angles(rot_mat):
'''
function Basis2Angles(rot_mat)
This function will take a "rotation matrix", whereby the columns form an orthonormal basis. The "rotation matrix" should describe the axes of the new coordinate system in terms of the global coordinate system. Matrix should be 3x3 and invertible.
[ e_1 e_2 e_3 ]
We are making the assumption that this rotation matrix is equivalent to three basis transformation in the follow order:
R_rot = R_z * R_y * R_x (order matters)
Returns a vector of size 3, which containes the following angles in order:
- theta, as part of rotation matrix:
[ 1 0 0 ]
R_x = [ 0 cos(theta) -sin(theta) ]
[ 0 sin(theta) cos(theta) ]
- phi
[ cos(phi) 0 sin(phi) ]
R_y = [ 0 1 0 ]
[ -sin(phi) 0 cos(phi) ]
- psi
[ cos(psi) -sin(psi) 0 ]
R_z = [ sin(psi) cos(psi) 0 ]
[ 0 0 1 ]
'''
phi = np.arcsin(-rot_mat[2, 0])
psi = np.arcsin((rot_mat[1, 0]) / (np.cos(phi)))
if rot_mat[0, 0] / np.cos(phi) < 0:
psi = np.pi - psi
theta = np.arcsin((rot_mat[2, 1]) / (np.cos(phi)))
if rot_mat[2, 2] / np.cos(phi) < 0:
theta = np.pi - theta
rot_mat_guess = Angles2Basis([theta, phi, psi])
error = rot_mat_guess - rot_mat
epsilon = 0.000009
error_binary = (error < epsilon)
if not error_binary.all():
phi = np.pi - phi
psi = np.arcsin((rot_mat[1, 0]) / (np.cos(phi)))
if rot_mat[0, 0] / np.cos(phi) < 0:
psi = np.pi - psi
theta = np.arcsin((rot_mat[2, 1]) / (np.cos(phi)))
# if rot_mat[2, 2] / np.cos(phi) < 0:
# theta = np.pi - theta
rot_mat_guess = Angles2Basis([theta, phi, psi])
error = rot_mat_guess - rot_mat
epsilon = 0.000009
error_binary = (error < epsilon)
assert error_binary.all()
return [theta, phi, psi]
def test_function_overloading():
a = pe.pseudo_Obs(17, 2.9, 'e1')
b = pe.pseudo_Obs(4, 0.8, 'e1')
fs = [
lambda x: x[0] + x[1], lambda x: x[1] + x[0], lambda x: x[0] - x[1],
lambda x: x[1] - x[0], lambda x: x[0] * x[1], lambda x: x[1] * x[0],
lambda x: x[0] / x[1], lambda x: x[1] / x[0], lambda x: np.exp(x[0]),
lambda x: np.sin(x[0]), lambda x: np.cos(x[0]), lambda x: np.tan(x[0]),
lambda x: np.log(x[0]), lambda x: np.sqrt(np.abs(x[0])),
lambda x: np.sinh(x[0]), lambda x: np.cosh(x[0]),
lambda x: np.tanh(x[0])
]
for i, f in enumerate(fs):
t1 = f([a, b])
t2 = pe.derived_observable(f, [a, b])
c = t2 - t1
assert c.is_zero()
assert np.log(np.exp(b)) == b
assert np.exp(np.log(b)) == b
assert np.sqrt(b**2) == b
assert np.sqrt(b)**2 == b
np.arcsin(1 / b)
np.arccos(1 / b)
np.arctan(1 / b)
np.arctanh(1 / b)
np.sinc(1 / b)
def _loss_fn(self, matrix, rels_reversed):
"""Given a numpy array with vectors for u, v and negative samples, computes loss value.
Parameters
----------
matrix : numpy.array
Array containing vectors for u, v and negative samples, of shape (2 + negative_size, dim).
rels_reversed : bool
Returns
-------
float
Computed loss value.
Warnings
--------
Only used for autograd gradients, since autograd requires a specific function signature.
"""
vector_u = matrix[0]
vectors_v = matrix[1:]
norm_u = grad_np.linalg.norm(vector_u)
norms_v = grad_np.linalg.norm(vectors_v, axis=1)
euclidean_dists = grad_np.linalg.norm(vector_u - vectors_v, axis=1)
dot_prod = (vector_u * vectors_v).sum(axis=1)
if not rels_reversed:
# u is x , v is y
cos_angle_child = (dot_prod * (1 + norm_u ** 2) - norm_u ** 2 * (1 + norms_v ** 2)) /\
(norm_u * euclidean_dists * grad_np.sqrt(1 + norms_v ** 2 * norm_u ** 2 - 2 * dot_prod))
angles_psi_parent = grad_np.arcsin(self.K * (1 - norm_u**2) /
norm_u) # scalar
else:
# v is x , u is y
cos_angle_child = (dot_prod * (1 + norms_v ** 2) - norms_v **2 * (1 + norm_u ** 2) ) /\
(norms_v * euclidean_dists * grad_np.sqrt(1 + norms_v**2 * norm_u**2 - 2 * dot_prod))
angles_psi_parent = grad_np.arcsin(self.K * (1 - norms_v**2) /
norms_v) # 1 + neg_size
# To avoid numerical errors
clipped_cos_angle_child = grad_np.maximum(cos_angle_child, -1 + EPS)
clipped_cos_angle_child = grad_np.minimum(clipped_cos_angle_child,
1 - EPS)
angles_child = grad_np.arccos(clipped_cos_angle_child) # 1 + neg_size
energy_vec = grad_np.maximum(0, angles_child - angles_psi_parent)
positive_term = energy_vec[0]
negative_terms = energy_vec[1:]
return positive_term + grad_np.maximum(
0, self.margin - negative_terms).sum()
def vInfDec(self, r, v, mu):
"""
declination of v infinity
"""
s = self.sVector(r, v, mu)
Dec = np.arcsin(s[2])
return Dec
def arccosine(
long1, lat1, long2,
lat2): #great-circle distance (long and lat given between [-1,1])
true_long1, true_long2 = long1 * pi, long2 * pi
true_lat1, true_lat2 = lat1 * pi / 2, lat2 * pi / 2
return (np.arcsin(
np.sqrt(
np.sin((true_lat2 - true_lat1) / 2)**2 + np.cos(true_lat1) *
np.cos(true_lat2) * np.sin((true_long2 - true_long1) / 2)**2)))
def acos_dist(coord_1, coord_2, R=R):
coord_1 *= pi
lat_1 = coord_1[0]
lat_2 = coord_2[0]
long_1 = coord_1[1]
long_2 = coord_2[1]
f = np.sin((lat_2 - lat_1) / 2) ** 2 + np.cos(lat_1) * np.cos(lat_2) * np.sin(
(long_1 - long_2) / 2) ** 2
return 2 * R * np.arcsin(np.sqrt(f))
def visit_Function(self, node):
f = node.value
if f == EXP:
return np.exp(self.visit(node.expr))
if (f == LOG) or (f == LN):
return np.log(self.visit(node.expr))
if f == LOG10:
return np.log10(self.visit(node.expr))
if f == SQRT:
return np.sqrt(self.visit(node.expr))
if f == ABS:
return np.abs(self.visit(node.expr))
if f == SIGN:
return np.sign(self.visit(node.expr))
if f == SIN:
return np.sin(self.visit(node.expr))
if f == COS:
return np.cos(self.visit(node.expr))
if f == TAN:
return np.tan(self.visit(node.expr))
if f == ASIN:
return np.arcsin(self.visit(node.expr))
if f == ACOS:
return np.arccos(self.visit(node.expr))
if f == ATAN:
return np.arctan(self.visit(node.expr))
if f == MAX:
raise NotImplementedError(MAX)
if f == MIN:
raise NotImplementedError(MIN)
if f == NORMCDF:
raise NotImplementedError(NORMCDF)
if f == NORMPDF:
raise NotImplementedError(NORMPDF)
if f == ERF:
return erf(self.visit(node.expr))
def compute_params(self, params, x, y):
n_params = params.shape[0]
vs = []
xa = agnp.vstack((1, x))
ya = agnp.vstack((1, y))
for i in range(n_params):
xyp = 2 * xa.T.dot(params[i, :, :].dot(ya))
xx = 2 * xa.T.dot(params[i, :, :].dot(xa))
ypyp = 2 * ya.T.dot(params[i, :, :].dot(ya))
vs.append(
agnp.sum((2 / agnp.pi) * agnp.arcsin(xyp / agnp.sqrt(
(1 + xx) * (1 + ypyp))),
axis=1))
return agnp.array(vs)
def test_trigonometric(): grad_test(lambda x: ti.tanh(x), lambda x: np.tanh(x)) grad_test(lambda x: ti.sin(x), lambda x: np.sin(x)) grad_test(lambda x: ti.cos(x), lambda x: np.cos(x)) grad_test(lambda x: ti.acos(x), lambda x: np.arccos(x)) grad_test(lambda x: ti.asin(x), lambda x: np.arcsin(x))
def test_arcsin():
fun = lambda x: 3.0 * np.arcsin(x)
check_grads(fun)(0.1)
def arcsin(a: Numeric):
return anp.arcsin(a)
def Basis2Angles_GD(rot_mat):
'''
function Basis2Angles_GD(rot_mat)
This function will take a "rotation matrix", whereby the columns form an orthonormal basis. The "rotation matrix" should describe the axes of the new coordinate system in terms of the global coordinate system. Matrix should be 3x3 and invertible.
[ e_1 e_2 e_3 ]
We are making the assumption that this rotation matrix is equivalent to three basis transformation in the follow order:
R_rot = R_z * R_y * R_x (order matters)
Returns a vector of size 3, which containes the following angles in order:
- theta, as part of rotation matrix:
[ 1 0 0 ]
R_x = [ 0 cos(theta) -sin(theta) ]
[ 0 sin(theta) cos(theta) ]
- phi
[ cos(phi) 0 sin(phi) ]
R_y = [ 0 cos(theta) 0 ]
[ -sin(phi) 0 cos(phi) ]
- psi
[ cos(psi) -sin(psi) 0 ]
R_z = [ sin(phi) cos(psi) 0 ]
[ 0 0 1 ]
'''
phi = np.arcsin(-rot_mat[2, 0])
psi = np.arcsin((rot_mat[1, 0]) / (np.cos(np.arcsin(-rot_mat[2, 0]))))
theta = np.arcsin((rot_mat[2, 1]) / (np.cos(np.arcsin(-rot_mat[2, 0]))))
def loss_fn(angle_array):
loss = (rot_mat[0, 0] - u_x(angle_array))**2 + (
rot_mat[1, 0] -
u_y(angle_array))**2 + (rot_mat[2, 0] - u_z(angle_array))**2 + (
rot_mat[0, 1] - v_x(angle_array))**2 + (
rot_mat[1, 1] - v_y(angle_array))**2 + (
rot_mat[2, 1] - v_z(angle_array))**2 + (
rot_mat[0, 2] - w_x(angle_array))**2 + (
rot_mat[1, 2] - w_y(angle_array))**2 + (
rot_mat[2, 2] - w_z(angle_array))**2
return loss
grad_loss = grad(loss_fn)
epsilon = 1e-12
learning_rate = 0.01
def learning_rate_scheduler(i):
learning_rate_update = learning_rate # * (2 / np.sqrt(i))
return learning_rate_update
i = 0
while loss_fn([theta, phi, psi]) > epsilon:
print('Iteration:\t', i + 1, '\t\tLoss:\t', loss_fn([theta, phi, psi]))
i = i + 1
if i < 50:
theta = theta - learning_rate * grad_loss([theta, phi, psi])[0]
phi = phi - learning_rate * grad_loss([theta, phi, psi])[1]
psi = psi - learning_rate * grad_loss([theta, phi, psi])[2]
else:
theta = theta - (i / 2) * learning_rate_scheduler(i) * grad_loss(
[theta, phi, psi])[0]
phi = phi - (i / 2) * learning_rate_scheduler(i) * grad_loss(
[theta, phi, psi])[1]
psi = psi - (i / 2) * learning_rate_scheduler(i) * grad_loss(
[theta, phi, psi])[2]
return [theta, phi, psi]
def test_arcsin():
fun = lambda x : 3.0 * np.arcsin(x)
check_grads(fun)(0.1)
lambda x: 0.4 * x * x - 3,
lambda x: (x - 3) * (x - 1),
lambda x: (x - 3) * (x - 1) + x * x,
])
@if_has_autograd
@ti.test()
def test_poly(tifunc):
grad_test(tifunc)
@pytest.mark.parametrize('tifunc,npfunc', [
(lambda x: ti.tanh(x), lambda x: np.tanh(x)),
(lambda x: ti.sin(x), lambda x: np.sin(x)),
(lambda x: ti.cos(x), lambda x: np.cos(x)),
(lambda x: ti.acos(x), lambda x: np.arccos(x)),
(lambda x: ti.asin(x), lambda x: np.arcsin(x)),
])
@if_has_autograd
@ti.test(exclude=[ti.vulkan])
def test_trigonometric(tifunc, npfunc):
grad_test(tifunc, npfunc)
@pytest.mark.parametrize('tifunc', [
lambda x: 1 / x,
lambda x: (x + 1) / (x - 1),
lambda x: (x + 1) * (x + 2) / ((x - 1) * (x + 3)),
])
@if_has_autograd
@ti.test()
def test_frac(tifunc):
def test_arcsin():
fun = lambda x : 3.0 * np.arcsin(x)
d_fun = grad(fun)
check_grads(fun, 0.1)
check_grads(d_fun, 0.2)
def test_arcsin():
fun = lambda x : 3.0 * np.arcsin(x)
d_fun = grad(fun)
check_grads(fun, 0.1)
check_grads(d_fun, 0.2)
def get_roll_pitch(y_in):
sq1, sq2, sq3, sq4, sq5, sq6, sq7, cq1, cq2, cq3, cq4, cq5, cq6, cq7 = get_sin_cos(y_in)
r_32 = -cq7*(cq5*sq2*sq3 - sq5*(cq2*sq4 - cq3*cq4*sq2)) - sq7*(cq6*(cq5*(cq2*sq4 - cq3*cq4*sq2) + sq2*sq3*sq5) + sq6*(cq2*cq4 + cq3*sq2*sq4))
r_33 = -cq6*(cq2*cq4 + cq3*sq2*sq4) + sq6*(cq5*(cq2*sq4 - cq3*cq4*sq2) + sq2*sq3*sq5)
r_31 = cq7*(cq6*(cq5*(cq2*sq4 - cq3*cq4*sq2) + sq2*sq3*sq5) + sq6*(cq2*cq4 + cq3*sq2*sq4)) - sq7*(cq5*sq2*sq3 - sq5*(cq2*sq4 - cq3*cq4*sq2))
return np.arctan2(r_32, r_33), -np.arcsin(r_31)
