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 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)