def test_msvd():
output("""\
msvd_:{[b;x]
$[3<>count usv:.qml.msvd x;::;
not (.qml.mdim[u:usv 0]~2#d 0) and (.qml.mdim[v:usv 2]~2#d 1) and
.qml.mdim[s:usv 1]~d:.qml.mdim x;::;
not mortho[u] and mortho[v] and
all[0<=f:.qml.mdiag s] and mzero s _'til d 0;::;
not mzero x-.qml.mm[u] .qml.mm[s] flip v;::;
b;1b;(p*/:m#/:u;m#m#/:s;
(p:(f>.qml.eps*f[0]*max d)*1-2*0>m#u 0)*/:(m:min d)#/:v)]};""")
for A in subjects:
U, S, V = map(mp.matrix, mp.svd(mp.matrix(A)))
m = min(A.rows, A.cols)
p = mp.diag([mp.sign(s * u) for s, u in zip(mp.chop(S), U[0, :])])
U *= p
S = mp.diag(S)
V = V.T * p
test("msvd_[0b", A, (U, S, V))
reps(250)
for Aq in large_subjects:
test("msvd_[1b", qstr(Aq), qstr("1b"))
reps(10000)
def test_msvd():
output("""\
msvd_:{[b;x]
$[3<>count usv:.qml.msvd x;::;
not (.qml.mdim[u:usv 0]~2#d 0) and (.qml.mdim[v:usv 2]~2#d 1) and
.qml.mdim[s:usv 1]~d:.qml.mdim x;::;
not mortho[u] and mortho[v] and
all[0<=f:.qml.mdiag s] and mzero s _'til d 0;::;
not mzero x-.qml.mm[u] .qml.mm[s] flip v;::;
b;1b;(p*/:m#/:u;m#m#/:s;
(p:(f>.qml.eps*f[0]*max d)*1-2*0>m#u 0)*/:(m:min d)#/:v)]};""")
for A in subjects:
U, S, V = map(mp.matrix, mp.svd(mp.matrix(A)))
m = min(A.rows, A.cols)
p = mp.diag([mp.sign(s * u) for s, u in zip(mp.chop(S), U[0, :])])
U *= p
S = mp.diag(S)
V = V.T * p
test("msvd_[0b", A, (U, S, V))
reps(250)
for Aq in large_subjects:
test("msvd_[1b", qstr(Aq), qstr("1b"))
reps(10000)
def psolve(M, b):
U, S, V = mp.svd(M)
if False:
print("V")
testHermitian(V)
print("U")
testHermitian(U)
sys.exit(1)
Mpinv = V.transpose_conj() * (mp.diag(S)**(-1)) * U.transpose_conj()
ws = Mpinv*mp.matrix(b)
return [ws[i] for i in range(len(ws))]
def test_mev():
output("""\
reim:{$[0>type x;1 0*x;2=count x;x;'`]};
mc:{((x[0]*y 0)-x[1]*y 1;(x[0]*y 1)+x[1]*y 0)};
mmc:{((.qml.mm[x 0]y 0)-.qml.mm[x 1]y 1;(.qml.mm[x 0]y 1)+.qml.mm[x 1]y 0)};
mev_:{[b;x]
if[2<>count wv:.qml.mev x;'`length];
if[not all over prec>=abs
mmc[flip vc;flip(flip')(reim'')flip x]-
flip(w:reim'[wv 0])mc'vc:(flip')(reim'')(v:wv 1);'`check];
/ Normalize sign; LAPACK already normalized to real
v*:1-2*0>{x a?max a:abs x}each vc[;0];
(?'[prec>=abs w[;1];w[;0];w];?'[b;v;0n])};""")
for A in eigenvalue_subjects:
if A.rows <= 3:
V = []
for w, n, r in A.eigenvects():
w = sp.simplify(sp.expand_complex(w))
if len(r) == 1:
r = r[0]
r = sp.simplify(sp.expand_complex(r))
r = r.normalized() / sp.sign(max(r, key=abs))
r = sp.simplify(sp.expand_complex(r))
else:
r = None
V.extend([(w, r)] * n)
V.sort(key=lambda (x, _): (-abs(x), -sp.im(x)))
else:
Am = mp.matrix(A)
# extra precision for complex pairs to be equal in sort
with mp.extradps(mp.mp.dps):
W, R = mp.eig(Am)
V = []
for w, r in zip(W, (R.column(i) for i in range(R.cols))):
w = mp.chop(w)
with mp.extradps(mp.mp.dps):
_, S, _ = mp.svd(Am - w * mp.eye(A.rows))
if sum(x == 0 for x in mp.chop(S)) == 1:
# nullity 1, so normalized eigenvector is unique
r /= mp.norm(r) * mp.sign(max(r, key=abs))
r = mp.chop(r)
else:
r = None
V.append((w, r))
V.sort(key=lambda (x, _): (-abs(x), -x.imag))
W, R = zip(*V)
test("mev_[%sb" % "".join("0" if r is None else "1" for r in R),
A, (W, [r if r is None else list(r) for r in R]),
complex_pair=True)
def test_mev():
output("""\
reim:{$[0>type x;1 0*x;2=count x;x;'`]};
mc:{((x[0]*y 0)-x[1]*y 1;(x[0]*y 1)+x[1]*y 0)};
mmc:{((.qml.mm[x 0]y 0)-.qml.mm[x 1]y 1;(.qml.mm[x 0]y 1)+.qml.mm[x 1]y 0)};
mev_:{[b;x]
if[2<>count wv:.qml.mev x;'`length];
if[not all over prec>=abs
mmc[flip vc;flip(flip')(reim'')flip x]-
flip(w:reim'[wv 0])mc'vc:(flip')(reim'')(v:wv 1);'`check];
/ Normalize sign; LAPACK already normalized to real
v*:1-2*0>{x a?max a:abs x}each vc[;0];
(?'[prec>=abs w[;1];w[;0];w];?'[b;v;0n])};""")
for A in eigenvalue_subjects:
if A.rows <= 3:
V = []
for w, n, r in A.eigenvects():
w = sp.simplify(sp.expand_complex(w))
if len(r) == 1:
r = r[0]
r = sp.simplify(sp.expand_complex(r))
r = r.normalized() / sp.sign(max(r, key=abs))
r = sp.simplify(sp.expand_complex(r))
else:
r = None
V.extend([(w, r)]*n)
V.sort(key=lambda (x, _): (-abs(x), -sp.im(x)))
else:
Am = mp.matrix(A)
# extra precision for complex pairs to be equal in sort
with mp.extradps(mp.mp.dps):
W, R = mp.eig(Am)
V = []
for w, r in zip(W, (R.column(i) for i in range(R.cols))):
w = mp.chop(w)
with mp.extradps(mp.mp.dps):
_, S, _ = mp.svd(Am - w*mp.eye(A.rows))
if sum(x == 0 for x in mp.chop(S)) == 1:
# nullity 1, so normalized eigenvector is unique
r /= mp.norm(r) * mp.sign(max(r, key=abs))
r = mp.chop(r)
else:
r = None
V.append((w, r))
V.sort(key=lambda (x, _): (-abs(x), -x.imag))
W, R = zip(*V)
test("mev_[%sb" % "".join("0" if r is None else "1" for r in R), A,
(W, [r if r is None else list(r) for r in R]), complex_pair=True)
def subspace_angle_V_M_mp(mu, lam, dps=100):
r"""Multiprecision implementation computing the subspace angle between V(mu) and M(lam)
"""
n = len(mu)
lam = np.atleast_1d(lam)
m = len(lam)
with mp.workdps(dps):
mu = mp.matrix(mu)
lam = mp.matrix(lam)
# Construct the Cauchy mass matrix
#M = mp.zeros(n,n)
#for i,j in product(range(n), range(n)):
# M[i,j] = (mu[i] + mu[j].conjugate())**(-1)
#ewL, QL = mp.eighe(M)
ewL, QL = cauchy_eigen(mu, dps)
# Construct the right hand side Mhat matrix
Mhat = mp.zeros(2 * m, 2 * m)
for i, j in product(range(m), range(m)):
Mhat[i, j] = (-lam[i] - lam[j].conjugate())**(-1)
Mhat[i, m + j] = (-lam[i] - lam[j].conjugate())**(-2)
Mhat[m + i, j] = (-lam[i] - lam[j].conjugate())**(-2).conjugate()
Mhat[m + i, m + j] = 2 * (-lam[i] - lam[j].conjugate())**(-3)
# Construct the interior matrix
A = mp.zeros(n, 2 * m)
for i, j in product(range(n), range(m)):
A[i, j] = (mu[i] - lam[j])**(-1)
A[i, m + j] = (mu[i] - lam[j])**(-2)
ewR, QR = mp.eighe(Mhat)
AA = mp.diag([1 / mp.sqrt(ewL[i])
for i in range(n)]) * (QL.H * A * QR) * mp.diag(
[1 / mp.sqrt(ewR[i]) for i in range(2 * m)])
U, s, VH = mp.svd(AA)
phi = np.array([float(mp.acos(si)) for si in s])
return phi
def ff(x, y): return np.float128(mp.svd(mp.matrix((x+(1j) * y) * np.eye(args[0][-2], args[0][-1]) - args[0][2]), compute_uv=False).tolist()[-1][0]) result = (args[0][0],
def FindRelativeMotionMultiPoints_mpmath(self, base_frame_ind,
targ_frame_ind, pnt_ids,
pnt_depthes):
eltype = self.elem_type
points_num = len(pnt_ids)
assert points_num == len(pnt_depthes)
# estimage camera position [R,T] given distances to all 3D points pj in frame1
x_img_skew = mpmath.matrix(3, 3)
A = mpmath.matrix(3 * points_num, 12)
for i, pnt_id in enumerate(pnt_ids):
pnt_ind = pnt_id
pnt_life = self.points_life[pnt_ind]
x1 = pnt_life.points_list_meter[base_frame_ind]
x_img = pnt_life.points_list_meter[targ_frame_ind]
skewSymmeticMatWithAdapter(x_img, lambda x: mpmath.mpf(str(x)),
x_img_skew)
A[3 * i:3 * (i + 1), 0:3] = x_img_skew * mpmath.mpf(str(x1[0]))
A[3 * i:3 * (i + 1), 3:6] = x_img_skew * mpmath.mpf(str(x1[1]))
A[3 * i:3 * (i + 1), 6:9] = x_img_skew * mpmath.mpf(str(x1[2]))
depth = mpmath.mpf(str(pnt_depthes[i]))
alph = 1 / depth
A[3 * i:3 * (i + 1), 9:12] = x_img_skew * alph
#
u1, dVec1, vt1 = mpmath.svd_r(A)
# take the last column of V
cam_R_noisy = mpmath.matrix(3, 3)
for col in range(0, 3):
for row in range(0, 3):
cam_R_noisy[row, col] = vt1[11, col * 3 + row]
cam_Tvec_noisy = mpmath.matrix(3, 1)
for row in range(0, 3):
cam_Tvec_noisy[row, 0] = vt1[11, 9 + row]
# project noisy [R,T] onto SO(3) (see MASKS, formula 8.41 and 8.42)
u2, dVec2, vt2 = mpmath.svd(cam_R_noisy)
no_guts = u2 * vt2
no_guts_det = mpmath.det(no_guts)
sign1 = mpmath.sign(no_guts_det)
frame_R = no_guts * sign1
det_den = dVec2[0] * dVec2[1] * dVec2[2]
if math.isclose(0, det_den):
return False, None
t_factor = sign1 / rootCube(det_den)
cam_Tvec = cam_Tvec_noisy * t_factor
if sum(1 for a in cam_Tvec if not math.isfinite(a)) > 0:
print("error: nan")
return False, None
# convert results into default precision type
rel_R = np.zeros((3, 3), dtype=eltype)
rel_T = np.zeros(3, dtype=eltype)
for row in range(0, 3):
for col in range(0, 3):
rel_R[row, col] = float(frame_R[row, col])
rel_T[row] = cam_Tvec[row, 0]
p_err = [""]
assert IsSpecialOrthogonal(rel_R, p_err), p_err[0]
return True, (rel_R, rel_T)