def _solve_mle_scale(x):
xbar = stats.mean(x)
# Very rough guess of the scale parameter:
s0 = stats.std(x)
if s0 == 0:
# The x values are all the same.
return s0
# Find an interval in which there is a sign change of
# gumbel_min._mle_scale_func.
s1 = s0
s2 = s0
sign2 = mpmath.sign(_mle_scale_func(s2, x, xbar))
while True:
s1 = 0.9*s1
sign1 = mpmath.sign(_mle_scale_func(s1, x, xbar))
if (sign1 * sign2) <= 0:
break
s2 = 1.1*s2
sign2 = mpmath.sign(_mle_scale_func(s2, x, xbar))
if (sign1 * sign2) <= 0:
break
# Did we stumble across the root while looking for an interval
# with a sign change? Not likely, but check anyway...
if sign1 == 0:
return s1
if sign2 == 0:
return s2
root = mpmath.findroot(lambda t: _mle_scale_func(t, x, xbar),
[s1, s2], solver='anderson')
return root
def scanRoots(self, numSteps):
found = []
coords = mp.linspace(-1.0, 1.0, numSteps)
for ndx in range(len(coords) - 1):
ax = coords[ndx]
bx = coords[ndx + 1]
aerr = self.evalEstimate(ax) - self.evalFunc(ax)
berr = self.evalEstimate(bx) - self.evalFunc(bx)
#print(f'bucket: {ax} <{aerr}>')
#print(f' to: {bx} <{berr}>')
if mp.sign(aerr) != mp.sign(berr):
(rx, rerr) = self.findRoot(ax, bx, aerr, berr)
#print(f' root in range: [{ax}, {bx}]: {rx} <{rerr}>')
found.append((ax, bx, rx, rerr))
return found
def mini(r):
nonlocal txi, tv
xi = [r]
y = f(r)
v = r*f(r) + mpmath.quad(f, [r, mpmath.inf])
if verbose:
print('Trying r={0} (v={1})'.format(r, v), file=sys.stderr)
for i in itertools.count():
xm1 = xi[i]
h = v / xm1
y += h
if y >= maximum or mpmath.almosteq(y, maximum, abs_eps=mpmath.mp.eps * 2**10):
break
# We solve for x via secant method instead of using f's inverse.
x = mpmath.findroot(lambda x: f(x) - y, xm1)
xi.append(x)
xi.append(mpmath.mpf())
if len(xi) == nseg:
if mpmath.almosteq(y, maximum, abs_eps=mpmath.mp.eps * 2**10):
txi, tv = xi[::-1], v
return 0
# If y > maximum, then v is too large, which means r is too far
# left, so we want to return a negative value. The opposite holds
# true when y < maximum.
return maximum - y
return len(xi) - nseg + h*mpmath.sign(len(xi) - nseg)
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 newTon(f, fd, precision, E, maxIterations, a, b):
mp.mp.dps = precision
a = DEC(a)
b = DEC(b)
iterator = 0
middle = DEC(a) + (DEC(b) - DEC(a)) / 2
if mp.sign(f(a)) == mp.sign(f(b)):
print("no root")
return maxsize, maxsize
while abs(f(middle)) > E and iterator < maxIterations:
middle = DEC(middle) - (DEC(f(middle)) / DEC(fd(middle)))
iterator += 1
return middle, iterator
def secant(f, precision, maxIterations, E, a, b):
mp.mp.dps = precision
if mp.sign(f(a)) == mp.sign(f(b)):
print("no root")
return (maxsize, maxsize)
a = DEC(a + 1e-4)
b = DEC(b - 1e-4)
iterator = 0
while abs(DEC(a) - DEC(b)) > E and maxIterations > iterator:
middle = DEC(b) - f(b) * (DEC(b) - DEC(a)) / (f(b) - f(a))
a = b
b = middle
return middle, iterator
def bisection(f, a, b, precision, E):
mp.mp.dps = precision
if mp.sign(f(a)) == mp.sign(f(b)):
print("no root")
return (maxsize, maxsize)
middle = DEC(a) + (DEC(b) - DEC(a)) / 2
numOfSteps = 0
while abs(DEC(a) - DEC(b)) > E:
middle = DEC(a) + (DEC(b) - DEC(a)) / 2
if mp.sign(f(middle)) != mp.sign(f(a)):
b = middle
else:
a = middle
numOfSteps += 1
return (middle, numOfSteps)
def AitkensExtrapolation(a, MaxIter):
"""
A simple implementation of aitkens extrapolation (Aitken's delta-squared process)
a : series of an integral sum, or something that is assumingly converging.
TODO: prevent division by zero errors and other approximation errors
"""
N = len(a)
assert (type(MaxIter) == int)
assert (N - 2 * MaxIter > 0.0)
if len(a) < 3 - 0.01:
return a[-1]
elif MaxIter > 1.01 and N > 3 + 0.01:
while N > 3 - 0.01 and MaxIter > 1.01:
A = [0] * (N - 2)
for i in range(N - 2):
denom = (a[i] + a[i + 2] - 2 * a[i + 1])
if denom == 0.0:
A[i] = mp.sign(a[i] * a[i + 2] -
a[i + 1]**2) * mp.sign(denom) * mp.inf
print("Warning: Denominator of zero at iteration n=" +
str((len(a) - N) / 2) + " i=" + str(i) +
" ; Setting A_n[i] = " + str(A[i]))
else:
A[i] = (a[i] * a[i + 2] - a[i + 1]**2) / (a[i] + a[i + 2] -
2 * a[i + 1])
# prep for new cycle
N -= 2
a = A
MaxIter -= 1
return A[-1]
elif MaxIter == 1 and N > 3 - 0.01:
denom = (a[-3] + a[-1] - 2 * a[-2])
if denom == 0.0:
print("Warning: Denominator of zero at iteration N=" + 1 + " i=" +
str(len(a) - 2) + " ; Setting A_N[i] = " + str(mp.inf))
return mp.sign(a[-3] * a[-1] - a[-2]**2) * mp.sign(denom) * mp.inf
else:
return (a[-3] * a[-1] - a[-2]**2) / (a[-3] + a[-1] - 2 * a[-2])
else:
return a[-1]
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 my_secant(eq, p1, p2, debug=False):
tol = mp.mpf(0.1)
max_count = 10000
sol = 0
for count in range(max_count):
if debug: print (count + 1), ''
if p1 == p2:
sol = p1
break
y1 = eq(p1)
y2 = eq(p2)
if debug: print '-->', p1, '->', y1
if debug: print '-->', p2, '->', y2
if abs(y1) < abs(y2):
sol = p1
err = abs(y1)
else:
sol = p2
err = abs(y2)
if err < tol:
break
if mp.sign(y1) * mp.sign(y2) < 0:
# p3 = (p1+p2)/mpf(2)
x1 = mp.log(p1)
x2 = mp.log(p2)
# x1 = p1
# x2 = p2
# x3 = (x2*y1 - x1*y2)/(y1-y2)
# if x3 == x1 or x3 == x2:
x3 = (x1 + x2) / mp.mpf(2)
p3 = mp.exp(x3)
# p3 = x3
if p3 == p1 or p3 == p2:
break
y3 = eq(p3)
if debug: print '--->', x1, x2, x3, p3, '->', y3
if mp.sign(y3) == mp.sign(y1):
p1 = p3
else:
p2 = p3
elif mp.sign(y1) * mp.sign(y2) == 0:
if y1 == 0:
sol = p1
elif y2 == 0:
sol = p2
else:
raise Exception('Strange: sign returns zero! without zeros $)')
break
else:
raise Exception('Functin has same sign on both ends')
if debug: print 'Solution:', sol
return sol
def skewness(xi, mu=0, sigma=1):
"""
Skewness of the generalized extreme value distribution.
"""
_validate_sigma(sigma)
xi = mpmath.mpf(xi)
mu = mpmath.mpf(mu)
sigma = mpmath.mpf(sigma)
if xi == 0:
return 12 * mpmath.sqrt(6) * mpmath.zeta(3) / mpmath.pi**3
elif 3 * xi < 1:
g1 = mpmath.gamma(mpmath.mp.one - xi)
g2 = mpmath.gamma(mpmath.mp.one - 2 * xi)
g3 = mpmath.gamma(mpmath.mp.one - 3 * xi)
num = g3 - 3 * g2 * g1 + 2 * g1**3
den = mpmath.power(g2 - g1**2, 1.5)
return mpmath.sign(xi) * num / den
else:
return mpmath.inf
def find_roots(self, fn, ub):
# Start with a coarse evaluation and fine tune the mesh until the number of
# detected sign changes no longer increases
init = True
cont = False
nchangepnts = 0
i = 1
while init or cont:
init = False
domain = np.linspace(0, ub, 1000 * i)
cf0 = list(map(fn, domain))
signs = np.array(list((map(lambda t: float(mp.sign(t)), cf0))))
dsign = ((np.roll(signs, 1) - signs) != 0).astype(int)
dsign[0] = 0
changepnts = np.nonzero(dsign)[0]
if changepnts.size > nchangepnts:
cont = True
nchangepnts = changepnts.size
else:
cont = False
halfchangepnts = ((changepnts[1:] + changepnts[:-1]) / 2).astype(int)
partpnts = np.zeros(halfchangepnts.size + 2, dtype=int)
partpnts[-1] = (len(cf0) - 1)
partpnts[1:-1] = halfchangepnts
roots = []
# Search for the root within each pair of partpnts
for i in range(len(partpnts[:-1])):
roots.append(
mp.findroot(fn, (domain[partpnts[i]], domain[partpnts[i + 1]]),
solver='anderson'))
return roots
def get_BCMatrix_3sec_axt_det(self):
mp.mp.dps = self.prec
#use symbolic mode to create symbolic BCMatrix that is then transformed with sympy lambdify into a mp.math matrix
if self.mode == "symbolic":
matrix_start = time.time()
mat = self.BCMatrix_3sec_axt_lambda(self.tau11, self.tau21,
self.tau12, self.tau22,
self.tau13, self.tau23,
self.E_1, self.E_2, self.E_3,
self.I_1, self.I_2, self.I_3,
self.parameters['L1'], self.parameters['L2'], self.parameters['L3'])
self.matrix_time = time.time() - matrix_start
else:
matrix_start = time.time()
self.get_BC_Matrix()
mat = self.BCM
self.matrix_time = time.time() - matrix_start
det_start = time.time()
det = mp.det(mat)
self.det_time = time.time() - det_start
self.kappa = 0
# calculate condition number kappa from stretching factors in sigma by using SVD
# sigma = mp.svd_r(mat, compute_uv=False)
# zeros in sigma represent a matrix without full rank, meaning the BCMatrix is singular due to low precision
# if min(sigma) == 0:
# kappa = max(sigma) / min(sigma)
# kappa_float = np.float(mp.log10(kappa))
# self.kappa = kappa_float
if det == 0:
warnings.warn("BCMatrix is singular, a much higher precision is needed!")
self.kappa = np.inf
return -np.inf
else:
# check legitemacy of log transform around the zero transition (log transform predicts +-1 transition?)
return np.float64(mp.log(mp.fabs(det)) * mp.sign(det))
def cdf(k, x):
k, x = map(mpf, (k, x))
return .5 + .5 * mp.sign(x) * mp.betainc(
k / 2, .5, x1=1 / (1 + x**2 / k), regularized=True)
def pearsonr(x, y, alternative='two-sided'):
"""
Pearson's correlation coefficient.
Returns the correlation coefficient r and the p-value.
x and y must be one-dimensional sequences with the same lengths.
The function assumes all the values in x and y are finite
(no `inf`, no `nan`).
Examples
--------
>>> from mpsci.stats import pearsonr
>>> import mpmath
>>> mpmath.mp.dps = 25
>>> x = [1, 2, 3, 5, 8, 10]
>>> y = [0.25, 2, 2, 2.5, 2.4, 5.5]
Compute the correlation coefficent and p-value.
>>> r, p = pearsonr(x, y)
>>> r
mpf('0.8645211772786436751458124677')
>>> p
mpf('0.02628844331049414042317641803')
Compute a one-sided p-value. The correlation coefficient is the
same; only the p-value is different.
>>> r, p = pearsonr(x, y, alternative='greater')
>>> r
mpf('0.8645211772786436751458124677')
>>> p
mpf('0.01314422165524707021158820901')
"""
if alternative not in ['two-sided', 'less', 'greater']:
raise ValueError("alternative must be 'two-sided', 'less', or "
"'greater'.")
if len(x) != len(y):
raise ValueError('lengths of x and y must be the same.')
if all(x[0] == t for t in x[1:]) or all(y[0] == t for t in y[1:]):
return mpmath.nan, mpmath.nan
if len(x) == 2:
return mpmath.sign(x[1] - x[0]) * mpmath.sign(y[1] -
y[0]), mpmath.mpf(1)
x = [mpmath.mp.mpf(float(t)) for t in x]
y = [mpmath.mp.mpf(float(t)) for t in y]
xmean = sum(x) / len(x)
ymean = sum(y) / len(y)
xm = [t - xmean for t in x]
ym = [t - ymean for t in y]
num = sum(s * t for s, t in zip(xm, ym))
den = mpmath.sqrt(sum(t**2 for t in xm) * sum(t**2 for t in ym))
r = num / den
n = len(x)
a = mpmath.mpf(float(n)) / 2 - 1
if alternative == 'two-sided':
p = 2 * mpmath.betainc(a, a, x2=0.5 * (1 - abs(r))) / mpmath.beta(a, a)
elif alternative == 'less':
p = mpmath.betainc(a, a, x2=0.5 * (1 + r)) / mpmath.beta(a, a)
else:
# alternative == 'greater'
p = mpmath.betainc(a, a, x2=0.5 * (1 - r)) / mpmath.beta(a, a)
return r, p
['0.0', '0.49062853002881556', '0.0', '0.49062866702173223', '0.0', '0.0', '0.0', '0.0', '0.0', '0.0', '0.0', '0.0'],
['0.88203688184375317', '0.47118036787131636', '1.1227920515867501', '0.51055067437658265', '-0.85084783450537795', '-0.52541218344982159', '-1.1569572051744931', '-0.5818504744392446', '0.0', '0.0', '0.0', '0.0'],
['-0.2311745312671405', '0.43275245877020069', '0.2504907968164292', '0.55087396761260321', '0.2906577462388459', '-0.4706885789091225', '-0.32187947109667221', '-0.64002830562602743', '0.0', '0.0', '0.0', '0.0'],
['157946997.87865532', '84374617.543281037', '-201059316.76444157', '-91424738.551285949', '-88143244.788399123', '-54429867.271798491', '119854592.2077822', '60276604.033337957', '0.0', '0.0', '0.0', '0.0'],
['41396594.577003489', '-77493303.391669111', '44855597.608227799', '98645464.576438188', '-30110574.226551734', '48760797.110106846', '-33344995.45515151', '-66303516.870942348', '0.0', '0.0', '0.0', '0.0'],
['0.0', '0.0', '0.0', '0.0', '0.44788407496498205', '0.89409163702204616', '1.6770999492103481', '1.3463521974733625', '-0.55597812186530204', '-0.83119692492641679', '-1.5213239822127666', '-1.146484478244564'],
['0.0', '0.0', '0.0', '0.0', '-0.49461102793907139', '0.2477692370032844', '0.74480154656609981', '0.92777108267950682', '0.4078089254411196', '-0.2727787286589548', '-0.56249815132223653', '-0.74640515750124306'],
['0.0', '0.0', '0.0', '0.0', '46398373.546327205', '92623069.401249781', '-173738604.68242844', '-139474902.67964052', '-99559414.172426251', '-148843049.12926965', '272424764.68263935', '205301939.52756113'],
['0.0', '0.0', '0.0', '0.0', '51239033.752749977', '-25667556.080590032', '77157465.496696528', '96112132.99799967', '-73026646.399300336', '48846689.025947514', '-100727016.92738359', '-133659399.1599524'],
['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', '0.0', '0.0', '0.098749536123081272', '0.99511231984910943', '2.2934688986468432', '2.0639766445045748'],
['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', '0.0', '0.0', '-0.48823049470113306', '0.048449339749094783', '1.0126461098572671', '1.1252415885989009']])
# referrence mpmath determinant computation
start_1 = time.time()
det_1 = np.float64(mp.log(mp.fabs(mp.det(A))) * mp.sign(mp.det(A)))
time_1 = time.time() - start_1
# chebyshev approximation
start_2 = time.time()
sv_vector = mp.matrix(12, 2)
A_abs = mp.matrix(12)
for i in range(12):
for j in range(12):
A_abs[i, j] = abs(A[i, j])
for i in range(12):
sv_vector[i, 0] = A_abs[i, i] - sum(A_abs[i, :])
sv_vector[i, 1] = A_abs[i, i] - sum(A_abs[:, i])
def get_Theta2_3(self, wc=None, wk=0, cfg=1):
'''Return the two possible values of Theta 2 and Theta 3 given the wrist
center (wc).
Parameters:
- *wc*: wrist center position.
- *wk*: working position (0:frontware-1:backward).
- *cfg*: elbow configuration (0:down-1:up).
'''
if wc is None:
wc = self.wc
# Get x, y proyected coords
Q = []
projected = hypotenusePita(wc[0], wc[1])
x = (
projected - 0.35, # worikng a head
projected + 0.35) # workin backward
x = x[wk]
y = wc[2] - 0.75
# Define sides of triangle
A = 1.501
C = 1.25
# get the Cos of the angles
B = hypotenusePita(x, y)
cosA = cosLaw_cosC(B, C, A)
cosB = cosLaw_cosC(C, A, B)
# get the ineers angles
if cosA >= 1:
cosA = 1 * mp.sign(cosA)
if abs(cosB) >= 1:
cosB = 1 * mp.sign(cosB)
a = atan2(mp.sqrt(1 - cosA**2), cosA)
b = atan2(mp.sqrt(1 - cosB**2), cosB)
# get the complementary angles
alpha = atan2(y, x)
beta = 0.036
# Solve Theta2 and Theta3
if wk:
theta2 = -(np.pi / 2 - alpha - a)
theta3 = -(np.pi / 2 - b - beta)
theta3_ = -(np.pi - theta3 + 2 * beta)
theta2_ = -(np.pi / 2 - alpha + a)
if cfg:
# cfg 1: elbow up
Q = [theta2.evalf(), theta3_.evalf()]
else:
# cfg 2: elbow down
Q = [theta2_.evalf(), theta3.evalf()]
else:
theta2 = np.pi / 2 - alpha - a
theta3 = np.pi / 2 - b - beta
theta2_ = np.pi / 2 - alpha + a
theta3_ = -(np.pi + theta3 + 2 * beta)
if cfg:
# cfg 1: elbow up
Q = [theta2.evalf(), theta3.evalf()]
else:
# cfg-2: elbow down
Q = [theta2_.evalf(), theta3_.evalf()]
return Q
def high_precision_pvalue(df, r):
r = r if np.abs(r) != 1.0 else mp.mpf(0.9999999999999999) * mp.sign(r)
den = ((1.0 - r) * (1.0 + r))
t = r * np.sqrt(df / den)
return t_sf(t, df) * 2
def cdf(k, x):
k, x = map(mpf, (k, x))
return .5 + .5*mp.sign(x)*mp.betainc(k/2, .5, x1=1/(1+x**2/k),
regularized=True)
def getSign( n ):
if isinstance( n, RPNMeasurement ):
return sign( n.value )
else:
return sign( n )
def getSignOperator(n):
if isinstance(n, RPNMeasurement):
return sign(n.value)
return sign(n)
def __Pprime_from_P(self,z,P): from mpmath import sqrt,sign,phase,pi txt = '' # 1 - We start by computing Pprime without knowing its sign g2, g3 = self.__invariants Pprime = sqrt(4*P*P*P - g2*P - g3) # 2 - We now resolve the sign ambiguity using the method in (Abramowitz 18.8) # 2.1 - First we reduce to studying the behaviour in the fundamental parallelogram om,omp,om2,om2p = self.__omegas if g3<0: #DOES NOT WORK .... REDUCTION TO FPP PROBABLY FAILED Pprime_r = Pprime*1j z_r = z*1j om,omp,om2,om2p = -1j*omp, 1j*om,-1j*om2p,1j*om2 if self.Delta >= 0: T1,T2 = 2 * om, 2 * omp else: T1,T2 = 2 *(om + omp), 2 * omp z_r = self.reduce_to_fpp2(z_r,T1,T2) else: z_r = z T1,T2 = self.__periods z_r = self.reduce_to_fpp2(z_r,T1,T2) Pprime_r = Pprime # 2.2 - We then proceed separately according to the sign of Delta if self.Delta > 0: # 2.2.1 - We reduce the study of the sign to the fundamental rectangle (in this case 1/4 FPP) x = z_r.real y = z_r.imag if (x>om.real and y>omp.imag): #R3 z_r = 2*om2-z_r Pprime_r = - Pprime elif (x>om.real): #R4 z_r = (2*om-z_r).conjugate() Pprime_r = -Pprime.conjugate() elif (y>omp.imag): #R2 z_r = (z_r-2*omp).conjugate() Pprime_r = Pprime.conjugate() else: #R1 Pprime_r = Pprime # 2.2.2 - We apply Abramowitz criteria z_r = z_r/om a = omp.imag/om.real x = z_r.real y = z_r.imag if (y<=0.4 and x<=0.4): #region B zmac = -2.0/z_r/z_r/z_r flag = (sign(zmac.real)==sign(Pprime_r.real)) elif (y>=0.4 and x<=0.5): #region A (first case) flag = (Pprime_r.real >= 0) else: #elswhere flag = (Pprime_r.imag >= 0) if not flag: Pprime = -Pprime else: # 2.2.1 - We reduce the study of the sign to the fundamental rectangle # using formulas 18.2.22 - 18.2.33 from Abramowitz x = z_r.real y = z_r.imag if (x<=om2.real and y>=0.0): #R1 Pprime_r = Pprime txt = 'R1' elif (x<=om2.real and y<0.0): #R2 z_r = z_r.conjugate() Pprime_r = Pprime.conjugate() txt = 'R2' elif (x>om2.real and y<=0.0): #R3 z_r = 2*om2 - z_r Pprime_r = - Pprime txt = 'R3' else: #R4 z_r = (2*om2 - z_r).conjugate() Pprime_r = - Pprime.conjugate() txt = 'R4' if z_r.imag > om2p.imag / 2.0: #We move from 1/4 FPP to fundamental rectangle z_r = 2*omp - z_r Pprime_r = -Pprime_r txt = txt + ' - FR' # 2.2.2 - We then apply Abramowitz criteria by first reducing to the case # where the real half-period is unity z_r = z_r/om2 # and by then applying the criterion a = om2p.imag/om2.real x = z_r.real y = z_r.imag if (y<=0.4 and x<=0.4): #region B zmac = -2.0/z_r/z_r/z_r flag = (sign(zmac.real)==sign(Pprime_r.real)) txt = txt + ' - Region B' elif a>=1.05: if (y>=0.4 and x<=0.5): #as region A of Delta>0 flag = (Pprime_r.real >= 0) txt = txt + ' - Region A real' else: flag = (Pprime_r.imag >= 0) txt = txt + ' - Region A imag' elif a>1: if(y>=0.4 and x<=0.4): flag = (Pprime_r.real >= 0) txt = txt + ' - case 1' elif (x >0.4 and x <=0.5 and y >0.4 and y <=0.5): arg = phase(Pprime_r) flag = (arg > -pi/4 and arg < 3.0/4.0*pi) txt = txt + ' - case 2 (arg)' else: flag = (Pprime_r.imag >= 0) txt = txt + ' - case 3 (elsewhere)' else: print "BOH" if not flag: Pprime = -Pprime print txt return Pprime
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)
