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 calc_dihedral(coords3d, dihedral_ind, cos_tol=1e-9):
m, o, p, n = dihedral_ind
u_dash = coords3d[m] - coords3d[o]
v_dash = coords3d[n] - coords3d[p]
w_dash = coords3d[p] - coords3d[o]
u_norm = np.linalg.norm(u_dash)
v_norm = np.linalg.norm(v_dash)
w_norm = np.linalg.norm(w_dash)
u = u_dash / u_norm
v = v_dash / v_norm
w = w_dash / w_norm
phi_u = np.arccos(np.dot(u, w))
phi_v = np.arccos(-np.dot(w, v))
uxw = np.cross(u, w)
vxw = np.cross(v, w)
cos_dihed = np.dot(uxw, vxw) / (np.sin(phi_u) * np.sin(phi_v))
# Restrict cos_dihed to [-1, 1]
if cos_dihed >= 1 - cos_tol:
dihedral_rad = 0
elif cos_dihed <= -1 + cos_tol:
dihedral_rad = np.arccos(-1)
else:
dihedral_rad = np.arccos(cos_dihed)
if dihedral_rad != np.pi:
# wxv = np.cross(w, v)
# if wxv.dot(u) < 0:
if vxw.dot(u) < 0:
dihedral_rad *= -1
return dihedral_rad
def get_orthogonality_score(C_matrix, verbose=True):
"""
Gets the angle between each subspace and the other ones.
Note the we leave the diagonal as zeros, because the angles are 1 anyway
And it helps to have a more representative mean.
"""
in_degree = True
len_1, len_2 = C_matrix.shape
orthogonality_matrix = np.zeros((len_2, len_2))
for lat_i in range(0, len_2):
for lat_j in range(lat_i + 1, len_2):
angle = np.dot(C_matrix[:, lat_i], C_matrix[:, lat_j]) / (np.dot(
np.linalg.norm(C_matrix[:, lat_i]),
np.linalg.norm(C_matrix[:, lat_j])))
orthogonality_matrix[lat_i, lat_j] = np.arccos(np.abs(angle))
orthogonality_matrix[lat_j, lat_i] = np.arccos(np.abs(angle))
if in_degree:
orthogonality_matrix = 180 * orthogonality_matrix / np.pi
mean_per_sub_space = np.sum(np.abs(orthogonality_matrix), 1) / (len_2 - 1)
glob_mean = np.mean(mean_per_sub_space)
try:
all_non_diag = orthogonality_matrix.flatten()
all_non_diag = all_non_diag[np.nonzero(all_non_diag)]
tenth_percentil = np.percentile(all_non_diag, 25)
ninetith_percentil = np.percentile(all_non_diag, 75)
small_avr = np.average(
all_non_diag,
weights=(all_non_diag <= tenth_percentil).astype(int))
high_avr = np.average(
all_non_diag,
weights=(all_non_diag >= ninetith_percentil).astype(int))
except:
small_avr = glob_mean
high_avr = glob_mean
if verbose:
print(np.around(orthogonality_matrix, 2))
print("Mean abs angle per subspace: ", mean_per_sub_space)
print("Mean abs angle overall: ", glob_mean)
#print("Std abs angle overall: ", np.std(mean_per_sub_space))
# print(small_avr, high_avr)
if len_2 <= 1:
glob_mean = small_avr = high_avr = 0
return glob_mean, small_avr, high_avr
def Hapke_vect(SZA, VZA, DPHI, W, R, BB, CC, HH, B0, variant="2002"):
# En radian
theta0r = SZA * np.pi / 180
thetar = VZA * np.pi / 180
phir = DPHI * np.pi / 180
R = R * np.pi / 180
CTHETA = np.cos(theta0r) * np.cos(thetar) + np.sin(thetar) * np.sin(
theta0r) * np.cos(phir)
THETA = np.arccos(CTHETA)
MUP, MU, S = roughness(theta0r, thetar, phir, R)
P = (1 - CC) * (1 - BB**2) / ((1 + 2 * BB * CTHETA + BB**2)**(3 / 2))
P = P + CC * (1 - BB**2) / ((1 - 2 * BB * CTHETA + BB**2)**(3 / 2))
B = B0 * HH / (HH + np.tan(THETA / 2))
gamma = np.sqrt(1 - W)
# H0 = (1 + 2 * MUP) / (1 + 2 * MUP * gamma)
# H = (1 + 2 * MU) / (1 + 2 * MU * gamma)
if variant == "1993":
H0 = H_1993(MUP, gamma)
H = H_1993(MU, gamma)
else:
H0 = H_2002(MUP, gamma)
H = H_2002(MU, gamma)
REFF = W / 4 / (MU + MUP) * ((1 + B) * P + H0 * H - 1)
REFF = S * REFF * MUP / np.cos(theta0r)
return REFF
def solve(self, angles0, target):
joint_indexes = list(reversed(self._fk_solver.joint_indexes))
angles = angles0[:]
pmap = {
'x': [1., 0., 0., 0.],
'y': [0., 1., 0., 0.],
'z': [0., 0., 1., 0.]
}
prev_dist = sys.float_info.max
for _ in range(self.maxiter):
for i, idx in enumerate(joint_indexes):
axis = self._fk_solver.solve(
angles,
p=pmap[self._fk_solver.components[idx].axis],
index=idx)
pj = self._fk_solver.solve(angles, index=idx)
ee = self._fk_solver.solve(angles)
eorp = self.p_on_rot_plane(ee, pj, axis)
torp = self.p_on_rot_plane(target, pj, axis)
ve = (eorp - pj) / np.linalg.norm(eorp - pj)
vt = (torp - pj) / np.linalg.norm(torp - pj)
a = np.arccos(np.dot(vt, ve))
sign = 1 if np.dot(axis, np.cross(ve, vt)) > 0 else -1
angles[-(i + 1)] += (a * sign)
ee = self._fk_solver.solve(angles)
dist = np.linalg.norm(target - ee)
delta = prev_dist - dist
if delta < self.tol:
break
prev_dist = dist
return angles
def angles(self):
angles = []
for triplet in self.psf.angles:
c_atom = triplet.atom2
a_1 = triplet.atom1
a_2 = triplet.atom3
v1 = np.array(self.positions[a_1]) - np.array(
self.positions[c_atom])
v2 = np.array(self.positions[a_2]) - np.array(
self.positions[c_atom])
prod = np.dot(v1, v2)
if prod == 0:
angle = np.pi / 2
else:
prod /= (np.linalg.norm(v1) * np.linalg.norm(v2))
prod = np.clip(prod, -1, 1)
angle = np.arccos(prod)
angles.append([(a_1.type, c_atom.type, a_2.type),
np.degrees(angle)])
return angles
def dihedrals(self):
d_angles = []
for quad in self.psf.dihedrals:
a_1, a_2, a_3, a_4 = quad.atom1, quad.atom2, quad.atom3, quad.atom4
v1 = np.array(self.positions[a_2]) - np.array(self.positions[a_1])
v2 = np.array(self.positions[a_3]) - np.array(self.positions[a_1])
n1 = np.cross(v1, v2)
v3 = np.array(self.positions[a_4]) - np.array(self.positions[a_3])
v4 = -v2
n2 = np.cross(v3, v4)
prod = np.dot(n1, n2)
if prod == 0:
angle = 0
else:
prod /= (np.linalg.norm(n1) * np.linalg.norm(n2))
prod = np.clip(prod, -1, 1)
angle = np.arccos(prod)
d_angles.append([(a_1.type, a_2.type, a_3.type, a_4.type),
np.degrees(angle)])
return d_angles
def TAinf(self, x, mu):
"""
true anomaly of the incoming asymptote
"""
eMag = self.eMag(x, mu)
TAinf = -np.arccos(-1.0 / eMag)
return TAinf
def anglerotation(R):
"""
compute the angle of the rotation matrix R
:param R: the rotation matrix
:return: the angle of rotation in degree
"""
theta = np.rad2deg(np.arccos((np.trace(R) - 1) / 2))
return theta
def calc_bend(coords3d, angle_ind):
m, o, n = angle_ind
u_dash = coords3d[m] - coords3d[o]
v_dash = coords3d[n] - coords3d[o]
u_norm = np.linalg.norm(u_dash)
v_norm = np.linalg.norm(v_dash)
u = u_dash / u_norm
v = v_dash / v_norm
angle_rad = np.arccos(np.dot(u, v))
return angle_rad
def phi_prime(J, L, S, q, S1, S2):
A1 = np.sqrt(J**2 - (L - S)**2)
A2 = np.sqrt((L + S)**2 - J**2)
A3 = np.sqrt(S**2 - (S1 - S2)**2)
A4 = np.sqrt((S1 + S2)**2 - S**2)
term1 = J**2 - L**2 - S**2
term2 = S**2 * (1 + q)**2
term3 = -(S1**2 - S2**2) * (1 - q**2)
term4 = -4 * q * S**2 * L * xi
term5 = (1 - q**2) * A1 * A2 * A3 * A4
return np.arccos((term1 * (term2 + term3) + term4) / term5)
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 egrad(Y):
"""Derivative of the cost function."""
# tmp = -1*(np.ones(D.shape) - (Y.T@Y)**2 + np.eye(D.shape[0]))**(-0.5)
zero_tol = 1e-12
ip = acos_validate(Y.T @ Y)
tmp = np.ones(D.shape) - (ip)**2
idx = np.where(np.abs(tmp) < zero_tol) # Avoid division by zero.
tmp[idx] = 1
tmp = -1 * tmp**(-0.5)
fill_val = np.min(tmp) # All entries are negative.
tmp[idx] = fill_val # Make non-diagonal zeros large.
np.fill_diagonal(tmp, 0) # Ignore known zeros on diagonal.
return 2 * Y @ ((np.arccos(np.abs(ip)) - D) * tmp * np.sign(ip))
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 fubinistudy(X):
"""Distance matrix of X using Fubini-Study metric.
Parameters
----------
X : ndarray (complex, d,n)
Data.
Returns
-------
D : ndarray (real, n,n)
Distance matrix.
"""
D = np.arccos(np.sqrt((X.conj().T @ X) * (X.conj().T @ X).conj().T))
np.fill_diagonal(D, 0) # Things work better if diagonal is exactly zero.
return np.real(D)
def cart2sph(self, n):
temp = n[1]/n[0]
if n[0]==0:
if n[1]<0:
phi = -np.pi/2
else:
phi = np.pi/2
else:
if n[0]>0:
phi = np.arctan(temp)
elif n[1]<0:
phi = np.arctan(temp) - np.pi
else:
phi = np.arctan(temp) + np.pi
# phi = np.arctan() #arctan(y/x)
theta = np.arccos(n[2]) #arccos(z)
return [phi, theta]
def ensemble_transfer_matrix_pi_half(NAtoms, g1d, gprime, gm, Delta1, Deltap,
Omega):
"""
NAtoms: The number of atoms
g1d: \Gamma_{1D}, the decay rate into the guided mode
gprime: \Gamma', the decay rate into all the other modes
gm: \Gamma_m, the decay rate from the meta-stable state
Delta1: The detuning of the carrier frequency \omega_L of the
quantum field from the atomic transition frequency \omega_{ab}.
Delta1 = \omega_L - \omega_{ab}
Deltap: The detuning of the classical drive frequency \omega_p from
the transition bc. Deltap = \omega_p - \omega_{bc}
"""
kd = 0.5 * np.pi
Mf = np.mat([[np.exp(1j * kd), 0], [0, np.exp(-1j * kd)]])
beta3 = Lambda_type_minus_r_over_t(g1d, gprime, gm, Delta1, Deltap, Omega)
M3 = np.mat([[1 - beta3, -beta3], [beta3, 1 + beta3]])
beta2 = two_level_minus_r_over_t(g1d, gprime, Delta1)
M2 = np.mat([[1 - beta2, -beta2], [beta2, 1 + beta2]])
Mcell = Mf * M2 * Mf * M3
NCells = NAtoms / 2
#diag, V = np.linalg.eig(Mcell)
#DN = np.diag(diag**(NCells))
#V_inv = np.linalg.inv(V)
#return V*DN*V_inv
theta = np.arccos(0.5 * np.trace(Mcell))
Id = np.mat([[1, 0], [0, 1]])
Mensemble\
= np.mat([[np.cos(NCells*theta)+1j*1j*np.sin(NCells*theta)/np.sin(theta)*(Mcell[1,1]-Mcell[0,0])/2,
-1j*1j*np.sin(NCells*theta)/np.sin(theta)*Mcell[0,1]],
[-1j*1j*np.sin(NCells*theta)/np.sin(theta)*Mcell[1,0],
np.cos(NCells*theta)-1j*1j*np.sin(NCells*theta)/np.sin(theta)*(Mcell[1,1]-Mcell[0,0])/2]])
return Mensemble
def impropers(self):
i_angles = []
for quad in self.psf.impropers: # a_1 bonded to a_2, a_1 is central atom
a_1, a_2, a_3, a_4 = quad.atom1, quad.atom2, quad.atom3, quad.atom4
v1 = np.array(self.positions[a_3]) - np.array(self.positions[a_1])
v2 = np.array(self.positions[a_4]) - np.array(self.positions[a_1])
n = np.cross(v1, v2)
v3 = np.array(self.positions[a_2]) - np.array(self.positions[a_1])
prod = np.dot(n, v3)
if prod == 0:
angle = np.pi / 2
else:
prod /= (np.linalg.norm(n) * np.linalg.norm(v3))
prod = np.clip(prod, -1, 1)
angle = np.pi / 2 - np.arccos(prod)
i_angles.append([(a_1.type, a_2.type, a_3.type, a_4.type),
np.degrees(angle)])
return i_angles
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_dist(self):
s = self.manifold
x = s.random_point()
y = s.random_point()
correct_dist = np.arccos(np.tensordot(x, y))
np_testing.assert_almost_equal(correct_dist, s.dist(x, y))
def test_arccos():
fun = lambda x: 3.0 * np.arccos(x)
check_grads(fun)(0.1)
kps = []
for j in range(xy[0].shape[1]):
x0, y0 = int(xy[0][0, j]), int(xy[0][1, j])
x, y = int(xy[i][0, j]), int(xy[i][1, j])
s = keypoints0[j].size
theta = keypoints0[j].angle
dx_, dy_ = np.cos(np.deg2rad(theta)), -np.sin(np.deg2rad(theta))
x_0, y_0, _ = tuple(
h_apply(H[0, i], (x0 + s * dx_, y0 + s * dy_, 1)))
x_1, y_1, _ = tuple(
h_apply(H[0, i], (x0 - s * dy_, y0 + s * dx_, 1)))
x_2, y_2, _ = tuple(
h_apply(H[0, i], (x0 + s * dy_, y0 - s * dx_, 1)))
x_3, y_3, _ = tuple(
h_apply(H[0, i], (x0 - s * dx_, y0 - s * dy_, 1)))
mytheta = np.arccos(
(x_0 - x_3) / np.sqrt((x_0 - x_3)**2 + (y_0 - y_3)**2))
dxt, dyt = np.cos(mytheta), -np.sin(mytheta)
xt, yt = int(x + s * dxt / 2), int(y + s * dyt / 2)
# Plot things
c = color_list[j % len(color_list)]
cv2.line(dispimg, (x, y), (int(x_0), int(y_0)),
c,
thickness=line_sz)
cv2.line(dispimg, (x, y), (int(x_1), int(y_1)),
c,
thickness=line_sz)
cv2.line(dispimg, (x, y), (int(x_2), int(y_2)),
c,
thickness=line_sz)
cv2.line(dispimg, (x, y), (int(x_3), int(y_3)),
lambda x: x * x * x * x,
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()
color='r',
alpha=0.3,
label='Projection')
ax.grid('off')
# Obtain closed for solution by performing eigendecomposition
e, q = np.linalg.eigh(cov)
U_closed = q[:, -components:][:, ::-1]
print('Closed form solution is given as: \n', U_closed)
print('Optimized solution is found as: \n', U, end='\n\n')
# Find the matrix T such that UA = U_closed
T = np.linalg.lstsq(U, U_closed, rcond=None)[0]
# Assuming this is a rotation matrix, compute the angle and confirm this is indeed a rotation
angle = np.arccos(T[0, 0])
rotation = np.array([[np.cos(angle), np.sin(angle)],
[-np.sin(angle), np.cos(angle)]])
print('T matrix found as: \n', T)
print('rotation matrix associated with an angle of ',
np.round(angle, 4),
' rad : \n',
rotation,
end='\n\n')
#Take a step further and solve the problem using a pymanopt to solve the optimization as a constrained problem
from pymanopt import Problem
from pymanopt.solvers import ConjugateGradient
from pymanopt.manifolds import Stiefel
def arccos(a: Numeric):
return anp.arccos(a)
def angle_between(v1, v2):
v1_u = unit_vector(v1)
v2_u = unit_vector(v2)
return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))
def create_cam_distribution_in_YZ(cam=None,
plane_size=(0.3, 0.3),
theta_params=(0, 180, 10),
r_params=(0.25, 1.0, 4),
plot=False):
"""
cam distritubution in YZ plane
:param cam:
:param plane_size:
:param theta_params:
:param phi_params:
:param r_params:
:param plot:
:return:
"""
if cam == None:
# Create an initial camera on the center of the world
cam = Camera()
f = 800
cam.set_K(fx=f, fy=f, cx=320, cy=240) # Camera Matrix
cam.img_width = 320 * 2
cam.img_height = 240 * 2
# we create a default plane with 4 points with a side lenght of w (meters)
plane = Plane(origin=np.array([0, 0, 0]),
normal=np.array([0, 0, 1]),
size=plane_size,
n=(2, 2))
# We extend the size of this plane to account for the deviation from a uniform pattern
# plane.size = (plane.size[0] + deviation, plane.size[1] + deviation)
d_space = np.linspace(r_params[0], r_params[1], r_params[2])
t_list = []
for d in d_space:
xx, yy, zz = uniform_halfCircle_in_YZ(theta_params, d,
False) # YZ plane
sphere_points = np.array(
[xx.ravel(), yy.ravel(), zz.ravel()], dtype=np.float32)
t_list.append(sphere_points)
t_space = np.hstack(t_list)
acc_row = r_params[2]
acc_col = theta_params[2]
accuracy_mat = np.zeros([
acc_row, acc_col
]) # accuracy_mat is used to describe accuracy degree for marker area
cams = []
for t in t_space.T:
cam = cam.clone()
cam.set_t(-t[0], -t[1], -t[2])
cam.set_R_mat(Rt_matrix_from_euler_t.R_matrix_from_euler_t(0.0, 0, 0))
cam.look_at([0, 0, 0])
radius = sqrt(t[0] * t[0] + t[1] * t[1] + t[2] * t[2])
angle = np.rad2deg(np.arccos(t[1] / radius))
cam.set_radius(radius)
cam.set_angle(angle)
plane.set_origin(np.array([0, 0, 0]))
plane.uniform()
objectPoints = plane.get_points()
# print "objectPoints",objectPoints
imagePoints = cam.project(objectPoints)
# print "imagePoints\n",imagePoints
if ((imagePoints[0, :] < cam.img_width) &
(imagePoints[0, :] > 0)).all():
if ((imagePoints[1, :] < cam.img_height) &
(imagePoints[1, :] > 0)).all():
cams.append(cam)
if plot:
planes = []
plane.uniform()
planes.append(plane)
# plot3D(cams, planes) #TODO comment because of from mayavi import mlab
return cams, accuracy_mat
# ==============================Test=================================================
#cams = create_cam_distribution(cam = None, plane_size = (0.3,0.3), theta_params = (0,360,10), phi_params = (0,70,5), r_params = (0.25,1.0,4), plot=True)
#create_cam_distribution_in_YZ(cam = None, plane_size = (0.3,0.3), theta_params = (0,180,3), r_params = (0.3,0.9,3), plot=False)
# print "cams size: ",len(cams)
# -----------------------------Test for cam look at method------------------------------
# cam = Camera()
# f = 800
# cam.set_K(fx = f, fy = f, cx = 320, cy = 240) #Camera Matrix
# cam.img_width = 320*2
# cam.img_height = 240*2
# cam.set_t(1,1,1,"world")
# cam.set_R_mat(Rt_matrix_from_euler_t.R_matrix_from_euler_t(0,np.deg2rad(0),0))
# cam.look_at([0,0,0])
# plane_size = (0.3,0.3)
# plane = Plane(origin=np.array([0, 0, 0] ), normal = np.array([0, 0, 1]), size=plane_size, n = (2,2))
# plane.set_origin(np.array([0, 0, 0]))
# plane.uniform()
# planes = []
# planes.append(plane)
# cams = []
# cams.append(cam)
# plot3D(cams,planes)
#
# print "cam.R",cam.R
# print "cam.Rt",cam.Rt
# print "cam.P",cam.P
# ------------------Code End-----------Test for cam look at method------------------------------
# create_cam_distribution_rotation_around_Z(cam=None, plane_size=(0.3, 0.3), theta_params=(0, 360, 10), phi_params=(45, 45, 1),
# r_params=(3.0, 3.0, 1), plot=False)
# create_cam_distribution_square_in_XY(cam=None, plane_size=(0.3, 0.3), theta_params=(0, 360, 5), phi_params=(45, 45, 1),
# r_params=(3.0, 3.0, 1), plot=False)
def cost(Y):
"""Weighted Frobenius norm cost function."""
ip = acos_validate(Y.T @ Y)
return 0.5 * np.linalg.norm(D - np.arccos(np.abs(ip)))**2
def test_arccos():
fun = lambda x : 3.0 * np.arccos(x)
d_fun = grad(fun)
check_grads(fun, 0.1)
check_grads(d_fun, 0.2)
def cart_pol(position, delta):
return np.arccos(position[0] / delta)
def test_arccos():
fun = lambda x : 3.0 * np.arccos(x)
d_fun = grad(fun)
check_grads(fun, 0.1)
check_grads(d_fun, 0.2)
def test_arccos():
fun = lambda x : 3.0 * np.arccos(x)
check_grads(fun)(0.1)
