def dual_improved_omega(P_i, P_j):
"""
Implementation of the approximation of omega when using the improved fast covariance intersection algorithm for
two tracks.
Follows the formula given on slide 24 of: http://www.ftm.mw.tum.de/uploads/media/17_Seeliger.pdf
:param P_i: The covariance matrix of track i as a 2D numpy-array
:param P_j: The covariance matrix of track j as a 2D numpy-array
:return: omega, the weighting of the track i (first parameter). j should be weighted (1-omega)
"""
# Calculate the inverse of both matrices first instead of doing this multiple times
P_i_inv = inverse(P_i)
P_j_inv = inverse(P_j)
# Now, calculate w using these two matrices
# Splitting this into two steps (the entire thing is a big division)
# First step is calculate everything before the division
# Second step is calculate everything after the division
# The following lines describe the operations with +/-/* , the actual code uses numpy functions:
# w = det(P_i_inv + P_j_inv) - det(P_j_inv) + det(P_i_inv)
# w /= 2 * det(P_i_inv + P_j_inv)
w = np.add(np.subtract(det(np.add(P_i_inv, P_j_inv)), det(P_j_inv)),
det(P_i_inv))
w = np.divide(w, dot(2, det(np.add(P_i_inv, P_j_inv))))
return w
def transf_indices(g_c, transformation, blocks, almost_zero = 10e-9):
"""
Finds the new blockstructure of G_loc using the unitary transformation u.
transformation is a matrix over the cluster sites
"""
n_sites = len(transformation)
sites = range(n_sites)
g_block_structure = zeros([n_sites, n_sites], int)
u = array(transformation)
u_inv = inverse(u)
blockdiag_len = list()
_blockdiag_len = 0
for s, b in g_c:
for i, l, m in product(sites, sites, range(len(b.data[:,0,0]))):
if abs(sum_list([sum_list([u[i, j] * b.data[m, j, k] * u_inv[k, l] for j in sites]) for k in sites])) > almost_zero:
g_block_structure[i, l] = 1
for i in sites:
block_is_zero = True
for j in range(i, n_sites, 1):
for k in range(0, i + 1, 1):
if i != j and (g_block_structure[i - k, j] != 0 or g_block_structure[j, i - k] != 0):
block_is_zero = False
if block_is_zero:
blockdiag_len.append(_blockdiag_len)
_blockdiag_len = 0
else:
_blockdiag_len += 1
return [(str(i) + '-' + s, range(blockdiag_len[i] + 1)) for s in blocks for i in range(len(blockdiag_len))]
def Broyden_Method(x0,h=0.001,max_int=100):
System_Value = Non_Linear_System(x0)
System_Value = list(abs(i) for i in System_Value)
erro_alvo = [0.00001,0.00001,0.00001]
var_count = 0
while(System_Value > erro_alvo and var_count < max_int):
F_x0 = numpy.array(Non_Linear_System(x0))
A0 = Jacobiano(x0,h)
Inv_A0 = inverse(A0) # Inversa da Matriz Jacobiana
x1 = x0 - Inv_A0@F_x0
F_x1 = numpy.array(Non_Linear_System(x1))
y1 = F_x1 - F_x0
s1 = x1 - x0
c1 = (s1@Inv_A0)@y1
Inv_A1 = Inv_A0 + (1/c1)*((s1-Inv_A0@y1)*(s1@Inv_A0))
x2 = x1 - Inv_A1@F_x1
x0 = x2
System_Value = Non_Linear_System(x0)
System_Value = list(abs(i) for i in System_Value)
var_count = var_count + 1
xn = list(round(i,10) for i in x0)
print("Número de Iterações: %.i" %var_count)
return(print("Solução do Sistema Não Linear: ", xn))
def cal_phi(self, Y_value, reduced_matrix):
"""
\mathcal{A}_i(\boldsymbol{\theta}) = 2(\sum_{l \in p_i} \theta_l)^2 \sum_{k=1}^{|L|}b_{k,i}^2
\phi_{i}=\frac{\sqrt{\mathcal{A}_{i}(\boldsymbol{\theta})}}{\sum_{j=1}^{|L|} \sqrt{\mathcal{A}_{j}(\boldsymbol{\theta})}}
计算A_i,计算\phi_i,
:return:
"""
assert isinstance(Y_value, list)
# assert isinstance(reduced_matrix,np.array)
# 求规约矩阵的逆矩阵
inverse_matrix = inverse(reduced_matrix)
A = []
Phi = []
for i in range(len(Y_value)):
# 取出逆矩阵的列平方和
sum_col = 0
for k in range(len(inverse_matrix[:, i])):
sum_col += inverse_matrix[:, i][k]**2
value = 2 * (Y_value[i]**2) * sum_col
A.append(value)
# 计算 A 的平方根和
sum_A = 0
for i in range(len(A)):
sum_A += np.sqrt(A[i])
for i in range(len(Y_value)):
Phi.append(np.sqrt(A[i]) / sum_A)
# print("the sum of phi:",sum(Phi))
return Phi
def llsq(vectors, flux):
A = np.matrix(vectors).transpose()
y = np.matrix(flux).transpose()
At = A.transpose()
coeffs = inverse(At * A) * At * y
coeffs = np.array(0.0 - coeffs)
return coeffs
def CalcJacobian(self):
"""
Use crude numerical differences to approximate Jacobian
return inverse
"""
jacobian = zeros((self.numberActive, self.numberActive), Float)
for cont in self.activeControllers:
cont.SaveBase()
self.errors = self.GetErrors()
delta = 0.1
for i in range(self.numberActive):
self.flowsheet.InfoMessage('ContDerivCalc', (self.flowsheet.GetPath(), i))
cont = self.activeControllers[i]
cont.SetOutput(delta)
self.flowsheet.InnerSolve()
jacobian[:,i] = (self.GetErrors() - self.errors)/delta
cont.SetOutput(0.0)
self.flowsheet.InnerSolve()
try:
self.jacobian = inverse(jacobian)
except:
raise Error.SimError('CouldNotInvertJacobian', self.flowsheet.GetPath())
def inverse(self):
"Returns the inverse of a rank-2 tensor."
if self.rank == 2:
from numpy.linalg import inv as inverse
return Tensor(inverse(self.array))
else:
raise ValueError, 'Undefined operation'
def KF_update(self, X_intermediate, P_intermediate, Z, H, R):
'''**INPUT:**
X_intermediate: The *initially* predicted state based on previous state
P_intermediate: The *initially* predicted covariance matrix of state X
Z: The observed measurement
H: Maps the observed measurement into the appropriate scale/units
R: The measurement noise covariance matrix
**LOCAL VARIABLES:**
X_mean: the mean of the state, with corrected scale/units
X_covar: the covariance of the state, with corrected scale/units
K_gain: the Kalman Filter gain, which determines how much correction is needed
**OUTPUT:**
X_final: the final *corrected/updated* predicted state at current timestep
P_final: the final *corrected/updated* state covariance matrix
(Note that these essentially become X_prior and P_prior when inputed into KF_predict_step for the next timestep.)
'''
X_mean = dot(H, X_intermediate)
X_covar = dot(H, dot(P_intermediate, H.T))
K_gain = dot(P_intermediate, dot(H.T, inverse(X_covar + R)))
X_final = X_intermediate + dot(K_gain, (Z - X_mean))
P_final = P_intermediate - dot(K_gain, dot(H, K_gain.T))
return (X_final, P_final)
def inverse(self):
"Returns the inverse of a rank-2 tensor."
if self.rank == 2:
from numpy.linalg import inv as inverse
return Tensor(inverse(self.array))
else:
raise ValueError, 'Undefined operation'
def solve_newton(self, x0=None, t=1., tol=.0001, max_iter=500):
"""
Estimate the parameters of the time dependent model given
some observed time series. It is assumed that the first
argument to the model is time while the remaining args
are parameters
:param x0: a starting point for the optimization
:param t: newton step size multiplier
:param tol: stopping criterion; tolerance on the norm of the gradient
:param max_iter: maximum number of iterations to perform
:return: Estimates of the parameters that would be input
to the time dependent model
"""
if x0 is None:
x0 = self.initial_guess()
grad_norm = norm(self.objective.gradient(x0))
x = array(x0, copy=True, dtype=datatype)
self.store_iteration(x)
k = 0
while grad_norm > tol:
h = self.objective.hessian(x)
hinv = inverse(h)
g = self.objective.gradient(x)
grad_norm = norm(g)
direction = array(matrix(hinv) * matrix(g)).squeeze()
x -= t * direction
self.store_iteration(x)
k += 1
if k >= max_iter:
break
self.optimal_point = x
self.optimal_value = self.objective.value(x)
def interpolation_rbf(xs, x, y, sigma):
# get inverse of similarity matrix for x and get weights
x = x.reshape(x.size, 1)
y = y.reshape(y.size, 1)
inv_sim_matrix = inverse(phi_sigma(distance_matrix(x, x), sigma))
weights = np.dot(inv_sim_matrix, y)
return weights
def g_c(g, transformation, transf_indices, blocks):
u = array(transformation)
u_inv = inverse(u)
sites = range(len(u))
g_c = BlockGf(name_block_generator = [(s, GfImFreq(indices = sites, mesh = g[transf_indices[0][0]].mesh)) for s in blocks], make_copies = False)
for s, i, j in product(blocks, sites, sites):
g_c[s][i, j] << sum_list([sum_list([u_inv[i, k] * _unblocked_g_transf(g, s, k, l, transf_indices) * u[l, j] for k in sites]) for l in sites])
return g_c
def is_generator_unique(Q):
"""Conservatively tests whether a transition rate matrix uniquely yields
its transition probability matrix"""
if not Q.shape[0] in (3, 4):
raise NotImplementedError("Only Q of 3x3 or 4x4 supported")
assert _is_Q_ok(Q), "Q must be a valid transition rate matrix"
e, V = eigenvectors(Q)
n = len(e)
# Assert that the matrix is diagonalisable
if not allclose(V.dot(diag(e)).dot(inverse(V)), Q):
raise ArithmeticError("matrix not diagonalisable")
# Find the Perron-Frobenius eigenvalue
PF_EV = argmin([norm(ones(n) / n - v / v.sum()) for v in V.T])
# Don't mess with the P-F eigenvalue - it has a special job to do
ix = list(range(0, PF_EV)) + list(range(PF_EV + 1, n))
real_close = []
expe = exp(e)
for i, j in combinations(ix, 2):
if isclose(e.real[i], e.real[j]):
real_close.append((i, j))
# Can't deal with non-primary roots yet
if isclose(expe[i], expe[j]):
raise NotImplementedError("non-primary root detected:\n" + repr(Q))
# If the real parts of the eigenvalues are distinct, we're ok
# For each candidate complex conjugate pair, check for equivalent Qs
for i, j in real_close:
s = zeros(n)
s[i] = 1.0
s[j] = -1.0
gen = 2.0 * pi * complex(0.0, 1.0) * V.dot(diag(s)).dot(inverse(V))
Qtest = Q + gen
if _is_Q_ok(Qtest):
return False
Qtest = Q - gen
if _is_Q_ok(Qtest):
return False
return True
def interpolate3DTransform(matrixList, indexList, percent):
""" This function gets input of two list and a percent value.
Return value is a 4x4 matrix corresponding to percent% of the transformation.
matrixList: a list of 4x4 transformation matrix
indexList : a list of sorted index (positive float number)
percent : a positive float number.
if only one matrix in the matrix list:
percent = 0.0 means no transformation (identity)
1.0 means 100% of the transformation (returns mat)
0.58 means 58% of translation and rotatetion 58% of rotation angle
along the same rotation axis
percent can go above 1.0
If matrixList has more than one matrix:
matrixList=[M1, M2, M3] #Attention: All M uses the same reference frame
indexList =[0.2, 0.5, 1.0] #Attention: assume the list sorted ascendingly
p = 0.5 means apply M2
p = 0.8 means apply M3
p = 0.9 means apply M2 first, then apply 50% of M'. M' is the transformation
from M2 to M3. 50% = (0.9-0.8) / (1.0-0.8)
M2 x M' = M3
--> M2.inverse x M2 x M'= M2.inverse x M3
--> M'= M2.inverse x M
"""
listLen = len(matrixList)
if listLen != len(indexList):
raise ValueError("matrix list should have same length of index list")
if listLen == 0:
raise ValueError("no matrix found in the matrix list")
offset = -1
for i in range(listLen):
if indexList[i] >= percent:
offset = i
break
prevMat = nextMat = N.identity(4,'f')
if offset == -1:
prevMat = matrixList[-1]
p = percent/indexList[-1]
return _interpolateMat(matrixList[-1], p)
elif offset == 0:
nextMat = matrixList[0]
p = percent/indexList[0]
return _interpolateMat(N.array(matrixList[0]), p)
else:
prevMat = matrixList[offset-1]
nextMat = matrixList[offset]
p = (percent-indexList[offset-1])/(
indexList[offset]-indexList[offset-1])
from numpy.linalg import inv as inverse
M = N.dot(inverse(prevMat), nextMat)
Mat = _interpolateMat(M, p)
return N.dot(prevMat, Mat)
def interpolate3DTransform(matrixList, indexList, percent):
""" This function gets input of two list and a percent value.
Return value is a 4x4 matrix corresponding to percent% of the transformation.
matrixList: a list of 4x4 transformation matrix
indexList : a list of sorted index (positive float number)
percent : a positive float number.
if only one matrix in the matrix list:
percent = 0.0 means no transformation (identity)
1.0 means 100% of the transformation (returns mat)
0.58 means 58% of translation and rotatetion 58% of rotation angle
along the same rotation axis
percent can go above 1.0
If matrixList has more than one matrix:
matrixList=[M1, M2, M3] #Attention: All M uses the same reference frame
indexList =[0.2, 0.5, 1.0] #Attention: assume the list sorted ascendingly
p = 0.5 means apply M2
p = 0.8 means apply M3
p = 0.9 means apply M2 first, then apply 50% of M'. M' is the transformation
from M2 to M3. 50% = (0.9-0.8) / (1.0-0.8)
M2 x M' = M3
--> M2.inverse x M2 x M'= M2.inverse x M3
--> M'= M2.inverse x M
"""
listLen = len(matrixList)
if listLen != len(indexList):
raise ValueError("matrix list should have same length of index list")
if listLen == 0:
raise ValueError("no matrix found in the matrix list")
offset = -1
for i in range(listLen):
if indexList[i] >= percent:
offset = i
break
prevMat = nextMat = N.identity(4, 'f')
if offset == -1:
prevMat = matrixList[-1]
p = percent / indexList[-1]
return _interpolateMat(matrixList[-1], p)
elif offset == 0:
nextMat = matrixList[0]
p = percent / indexList[0]
return _interpolateMat(N.array(matrixList[0]), p)
else:
prevMat = matrixList[offset - 1]
nextMat = matrixList[offset]
p = (percent - indexList[offset - 1]) / (indexList[offset] -
indexList[offset - 1])
from numpy.linalg import inv as inverse
M = N.dot(inverse(prevMat), nextMat)
Mat = _interpolateMat(M, p)
return N.dot(prevMat, Mat)
def is_generator_unique(Q):
"""Conservatively tests whether a transition rate matrix uniquely yields
its transition probability matrix"""
assert Q.shape[0] in (3, 4), 'Q must be 3x3 or 4x4'
assert _is_Q_ok(Q), 'Q must be a valid transition rate matrix'
e, V = eigenvectors(Q)
n = len(e)
# Assert that the matrix is diagonalisable
if not allclose(V.dot(diag(e)).dot(inverse(V)), Q):
raise ArithmeticError('matrix not diagonalisable')
# Find the Perron-Frobenius eigenvalue
PF_EV = argmin([norm(ones(n)/n-v/v.sum()) for v in V.T])
# Don't mess with the P-F eigenvalue - it has a special job to do
ix = range(0,PF_EV) + range(PF_EV+1,n)
real_close = []
expe = exp(e)
for i, j in combinations(ix, 2):
if isclose(e.real[i], e.real[j]):
real_close.append((i, j))
# Can't deal with non-primary roots yet
if isclose(expe[i], expe[j]):
raise NotImplementedError('non-primary root detected:\n'+repr(Q))
# If the real parts of the eigenvalues are distinct, we're ok
# For each candidate complex conjugate pair, check for equivalent Qs
for i, j in real_close:
s = zeros(n)
s[i] = 1.
s[j] = -1.
gen = 2.*pi*complex(0.,1.)*V.dot(diag(s)).dot(inverse(V))
Qtest = Q + gen
if _is_Q_ok(Qtest):
return False
Qtest = Q - gen
if _is_Q_ok(Qtest):
return False
return True
def logm(P):
"""Returns logarithm of a matrix.
This method should work if the matrix is positive definite and
diagonalizable.
"""
roots, ev = eigenvectors(P)
evI = inverse(ev.T)
evT = ev
log_roots = log(roots)
return innerproduct(evT * log_roots, evI)
def logm(P):
"""Returns logarithm of a matrix.
This method should work if the matrix is positive definite and
diagonalizable.
"""
roots, ev = eigenvectors(P)
evI = inverse(ev.T)
evT = ev
log_roots = log(roots)
return innerproduct(evT * log_roots, evI)
def __init__(self, data):
self.x, self.y = data
# Solving using the analytical solution
self.x = np.concatenate((np.ones((self.x.shape[0], 1)), self.x),
axis=1)
self.w = np.dot(inverse(np.dot(self.x.transpose, self.x)),
np.dot(self.x.transpose, self.y))
#self.b = self.x[1,] # First row is just b
# shouldn't it be like below?
self.b = self.w[:1]
self.w = self.w[1:] # w is all but the first row.
def multivariate_normal_prob(x, cov, mean=None):
"""Returns multivariate normal probability density of vector x.
Formula: http://www.riskglossary.com/link/joint_normal_distribution.htm
"""
if mean is None:
mean = zeros(len(x))
diff_row = x-mean
diff_col = diff_row[:,NewAxis]
numerator = exp(-0.5 * dot(dot(diff_row, inverse(cov)), diff_col))
denominator = sqrt((2*pi)**(len(x)) * determinant(cov))
return numerator/denominator
def multivariate_normal_prob(x, cov, mean=None):
"""Returns multivariate normal probability density of vector x.
Formula: http://www.riskglossary.com/link/joint_normal_distribution.htm
"""
if mean is None:
mean = zeros(len(x))
diff_row = x - mean
diff_col = diff_row[:, NewAxis]
numerator = exp(-0.5 * dot(dot(diff_row, inverse(cov)), diff_col))
denominator = sqrt((2 * pi)**(len(x)) * determinant(cov))
return numerator / denominator
def dual_cov_intersection(P_i, P_j, x_i, x_j, omega_fct=dual_fast_omega):
"""
Implementation of the covariance intersection algorithm for two tracks, according to the formula given on slide 24
of: http://www.ftm.mw.tum.de/uploads/media/17_Seeliger.pdf
One parameter is a function to determine w/omega:
In the basic CI algorithm, this is a convex optimization problem
In the FCI/I-FCI algorithms, an approximate solution is calculated
This omega_function should take two arguments: P_i and P_j (in that order). These parameters are the two covariance
matrices of the tracks.
This implementation follows the following naming:
The two tracks are called i and j
P is the final covariance matrix, P_i is the cov-matrix of track i
x is the estimated result, x_i the tracking of track i
w is the omega in the CI algorithm, i.e. the weighting of the two tracks based on their covariances.
track i will be weighted w, while j will be weighted (1-w)
:param P_i: The covariance matrix of track i as a 2D numpy-array
:param P_j: The covariance matrix of track j as a 2D numpy-array
:param x_i: The tracking estimation of track i as a 2D numpy-array
:param x_j: The tracking estimation of track j as a 2D numpy-array
:param omega_fct: Function that takes P_i and P_j as arguments and calculates w using this
:return: (x, P, w) [estimation, covariance matrix estimation, omega used)
"""
# All code is provided twice:
# Commented and spaced is the readable version of the algorithm
# The actual code is the same, but using numpy operations instead of +/-/*
w = omega_fct(P_i, P_j)
# print("using w = "+str(w))
# Calculate P and P^(-1)=P_inv first
# P_inv = w * inverse(P_i) + (1-w)*inverse(P_j)
P_inv = np.add(dot(w, inverse(P_i)), dot((1 - w), inverse(P_j)))
P = inverse(P_inv)
# Now, proceed by calculating x
# x = P * (w * inverse(P_i) * x_i + (1-w)*inverse(P_j)*x_j)
x = dot(
P,
np.add(dot(dot(w, inverse(P_i)), x_i),
dot(dot((1 - w), inverse(P_j)), x_j)))
return x, P, w
def Cmatrix(self):
'''
Calculate the C matrix
'''
self.C = []
self.gamma = []
self.f1001 = []
self.f1010 = []
S = getattr(self, "S")
R11 = getattr(self, "R11")
R12 = getattr(self, "R12")
R21 = getattr(self, "R21")
R22 = getattr(self, "R22")
BETX = getattr(self, "BETX")
BETY = getattr(self, "BETY")
ALFX = getattr(self, "ALFX")
ALFY = getattr(self, "ALFY")
J = numpy.reshape(numpy.array([0, 1, -1, 0]), (2, 2))
for j in range(0, len(S)):
R = numpy.array([[R11[j], R12[j]], [R21[j], R22[j]]])
C = matrixmultiply(-J, matrixmultiply(numpy.transpose(R), J))
C = (1 / numpy.sqrt(1 + determinant(R))) * C
g11 = 1 / numpy.sqrt(BETX[j])
g12 = 0
g21 = ALFX[j] / numpy.sqrt(BETX[j])
g22 = numpy.sqrt(BETX[j])
Ga = numpy.reshape(numpy.array([g11, g12, g21, g22]), (2, 2))
g11 = 1 / numpy.sqrt(BETY[j])
g12 = 0
g21 = ALFY[j] / numpy.sqrt(BETY[j])
g22 = numpy.sqrt(BETY[j])
Gb = numpy.reshape(numpy.array([g11, g12, g21, g22]), (2, 2))
C = matrixmultiply(Ga, matrixmultiply(C, inverse(Gb)))
gamma = 1 - determinant(C)
self.gamma.append(gamma)
C = numpy.ravel(C)
self.C.append(C)
self.f1001.append(((C[0] + C[3]) * 1j + (C[1] - C[2])) / 4 / gamma)
self.f1010.append(
((C[0] - C[3]) * 1j + (-C[1] - C[2])) / 4 / gamma)
self.F1001R = numpy.array(self.f1001).real
self.F1001I = numpy.array(self.f1001).imag
self.F1010R = numpy.array(self.f1010).real
self.F1010I = numpy.array(self.f1010).imag
self.F1001W = numpy.sqrt(self.F1001R**2 + self.F1001I**2)
self.F1010W = numpy.sqrt(self.F1010R**2 + self.F1010I**2)
self.GAMMAC = numpy.array(self.gamma)
def g_transf(g, transformation, transf_indices, blocks):
u = array(transformation)
u_inv = inverse(u)
g_transf = BlockGf(name_block_generator = [(ind, GfImFreq(indices = dict(transf_indices)[ind], mesh = g[blocks[0]].mesh)) for ind in dict(transf_indices).keys()], make_copies = False)
for ss in blocks:
i = 0
for s in [transf_indices[ii][0] for ii in range(len(transf_indices))]:
if ss in s:
for ib in dict(transf_indices)[s]:
for jb in dict(transf_indices)[s]:
g_transf[s][ib, jb] << sum_list([sum_list([u[i + ib, k] * g[ss][k, l] * u_inv[l, i + jb] for k in range(len(u))]) for l in range(len(u))])
i += 1
return g_transf
def Cmatrix(self):
'''
Calculate the C matrix
'''
self.C = []
self.gamma = []
self.f1001 = []
self.f1010 = []
S = getattr(self, "S")
R11 = getattr(self, "R11")
R12 = getattr(self, "R12")
R21 = getattr(self, "R21")
R22 = getattr(self, "R22")
BETX = getattr(self, "BETX")
BETY = getattr(self, "BETY")
ALFX = getattr(self, "ALFX")
ALFY = getattr(self, "ALFY")
J = numpy.reshape(numpy.array([0, 1, -1, 0]), (2, 2))
for j in range(0, len(S)):
R = numpy.array([[R11[j], R12[j]], [R21[j], R22[j]]])
C = matrixmultiply(-J, matrixmultiply(numpy.transpose(R), J))
C = (1 / numpy.sqrt(1 + determinant(R))) * C
g11 = 1 / numpy.sqrt(BETX[j])
g12 = 0
g21 = ALFX[j] / numpy.sqrt(BETX[j])
g22 = numpy.sqrt(BETX[j])
Ga = numpy.reshape(numpy.array([g11, g12, g21, g22]), (2, 2))
g11 = 1 / numpy.sqrt(BETY[j])
g12 = 0
g21 = ALFY[j] / numpy.sqrt(BETY[j])
g22 = numpy.sqrt(BETY[j])
Gb = numpy.reshape(numpy.array([g11, g12, g21, g22]), (2, 2))
C = matrixmultiply(Ga, matrixmultiply(C, inverse(Gb)))
gamma = 1 - determinant(C)
self.gamma.append(gamma)
C = numpy.ravel(C)
self.C.append(C)
self.f1001.append(((C[0] + C[3]) * 1j + (C[1] - C[2])) / 4 / gamma)
self.f1010.append(((C[0] - C[3]) * 1j + (-C[1] - C[2])) / 4 / gamma)
self.F1001R = numpy.array(self.f1001).real
self.F1001I = numpy.array(self.f1001).imag
self.F1010R = numpy.array(self.f1010).real
self.F1010I = numpy.array(self.f1010).imag
self.F1001W = numpy.sqrt(self.F1001R ** 2 + self.F1001I ** 2)
self.F1010W = numpy.sqrt(self.F1010R ** 2 + self.F1010I ** 2)
def frame_transform(f1, f2):
from numpy import transpose, dot as matrix_multiply
ft1 = transpose(f1)
ft2 = transpose(f2)
from numpy.linalg import inv as inverse
ft1_inv = inverse(ft1)
r = matrix_multiply(ft2, ft1_inv)
import chimera
xf = chimera.Xform.xform(r[0][0], r[0][1], r[0][2], 0,
r[1][0], r[1][1], r[1][2], 0,
r[2][0], r[2][1], r[2][2], 0,
orthogonalize = True)
return xf
def calc_dNdX(J, dNdXi):
"""
Shape function derivatives w.r.t. spatial coordinates
dN_I/dX_i = dN_I/dXi_j dXi_j/dX_i
Note that the inverse Jacobian is defined as
J'_ij = dXi_j/dX_i
"""
Jinv = inverse(J)
dNdX = empty((4,2))
for I,i in product(range(0,4), range(0,2)):
dNdX[I,i] = 0
for j in range(0,2):
dNdX[I,i] += dNdXi[I,j] * Jinv[i,j]
return dNdX
def g_c(g, transformation, transf_indices, blocks, blockstates):
u = array(transformation)
u_inv = inverse(u)
g_c = BlockGf(name_block_generator = [(s, GfImFreq(indices = blockstates, mesh = g[transf_indices[0][0]].mesh)) for s in blocks], make_copies = False)
initial_blockdim = len(blockstates)
momenta = [transf_indices[i][0] for i in range(len(transf_indices))]
to_site = lambda initial_index: initial_index %int(initial_blockdim /2)
to_nambu = lambda initial_index: initial_index /int(initial_blockdim /2)
nambu_size = dict([(transf_indices[i][0], len(transf_indices[i][1])) for i in range(len(transf_indices))])
gf = lambda mom, i, j: g[mom][to_nambu(i), to_nambu(j)] if to_nambu(i)<nambu_size[mom] and to_nambu(j)<nambu_size[mom] else 0
momentum_nr = dict([(momentum, i) for i, momentum in enumerate(momenta)])
for i, j in product(blockstates, blockstates):
g_c[blocks[0]][i, j] << sum_list([u_inv[to_site(i), momentum_nr[momentum]] * gf(momentum, i, j) * u[momentum_nr[momentum], to_site(j)] for momentum in momenta])
return g_c
def logm(P):
"""Returns logarithm of a matrix.
This method should work if the matrix is positive definite and
diagonalizable.
"""
roots, ev = eigenvectors(P)
evI = inverse(ev.T)
evT = ev
if not allclose(P, innerproduct(evT * roots, evI)):
raise ArithmeticError("eigendecomposition failed")
log_roots = log(roots)
return innerproduct(evT * log_roots, evI)
def logm(P):
"""Returns logarithm of a matrix.
This method should work if the matrix is positive definite and
diagonalizable.
"""
roots, ev = eigenvectors(P)
evI = inverse(ev.T)
evT = ev
if not allclose(P, innerproduct(evT * roots, evI)):
raise ArithmeticError("eigendecomposition failed")
log_roots = log(roots)
return innerproduct(evT * log_roots, evI)
def _get_diagonalized(self):
"""Gets diagonalization of self as u, v, w; caches values."""
if not hasattr(self, '_diag_cache'):
error_tolerance = 1e-4 #amount of error allowed in product
eigenvalues, eigenvectors = eig(self._data)
u = transpose(eigenvectors)
v = eigenvalues
w = inverse(u)
#check that the diagonalization actually worked by multiplying
#the results back together
result = dot(dot(u,v),w)
if abs(sum(ravel(result))) > error_tolerance:
raise ValueError, "Diagonalization failed with erroneous result."
self._diag_cache = u, v, w
return self._diag_cache
def _get_diagonalized(self):
"""Gets diagonalization of self as u, v, w; caches values."""
if not hasattr(self, '_diag_cache'):
error_tolerance = 1e-4 #amount of error allowed in product
eigenvalues, eigenvectors = eig(self._data)
u = transpose(eigenvectors)
v = eigenvalues
w = inverse(u)
#check that the diagonalization actually worked by multiplying
#the results back together
result = dot(dot(u, v), w)
if abs(sum(ravel(result))) > error_tolerance:
raise ValueError, "Diagonalization failed with erroneous result."
self._diag_cache = u, v, w
return self._diag_cache
def Solve(self, parent, uObj, numberOfEquations, max_num_iterations):
self.converged = 0
self.useNumericJac = 1
x0 = uObj.GetMyVariableValues()
x = x0
# Set initial deltaX to 1.0 to ensure that convergence check works
deltaX = ones(numberOfEquations, Float)
idex = 0
try:
while idex < max_num_iterations:
#for idex in range(max_num_iterations):
stepX = parent.CalculateStep(x)
if stepX == None: return 0
rhs = parent.CalculateRHS(x)
if self.useNumericJac == 1:
jacobian = parent.CalculateJacobian(x)
elif self.useNumericJac == 0:
try:
jacobian = parent.CalculateJacobian1(x)
except:
pass
jacobian = inverse(jacobian)
deltaX = dot(jacobian, stepX)
#deltaX = self.GetSolutionSolver(numberOfEquations, jacobian, stepX) # rhs)
self.converged = self.CheckForConvergence(
numberOfEquations, rhs, stepX, deltaX)
if idex == max_num_iterations - 1: #show intermidiate results
if self.useNumericJac == 0:
try:
parentPath = parent.GetPath()
parent.InfoMessage('ShowIntermidiateResults',
(parentPath, ))
uObj.SetMyVariableValues(x)
return 2
except:
return 0
else:
self.useNumericJac = 0
x = x0
deltaX = zeros(numberOfEquations, Float)
idex = 0
elif self.converged:
uObj.SetMyVariableValues(x)
return 1
parent.UpdateX(x, deltaX)
idex = idex + 1
except ArithmeticError, e:
return 0
def Solve(self, parent, uObj, numberOfEquations, max_num_iterations):
self.converged = 0
self.useNumericJac = 1
x0 = uObj.GetMyVariableValues()
x = x0
# Set initial deltaX to 1.0 to ensure that convergence check works
deltaX = ones(numberOfEquations, Float)
idex =0
try:
while idex < max_num_iterations:
#for idex in range(max_num_iterations):
stepX = parent.CalculateStep(x)
if stepX == None: return 0
rhs = parent.CalculateRHS(x)
if self.useNumericJac == 1:
jacobian=parent.CalculateJacobian(x)
elif self.useNumericJac == 0:
try:
jacobian=parent.CalculateJacobian1(x)
except: pass
jacobian = inverse(jacobian)
deltaX = dot(jacobian, stepX)
#deltaX = self.GetSolutionSolver(numberOfEquations, jacobian, stepX) # rhs)
self.converged = self.CheckForConvergence(numberOfEquations, rhs, stepX, deltaX)
if idex == max_num_iterations-1: #show intermidiate results
if self.useNumericJac == 0:
try:
parentPath = parent.GetPath()
parent.InfoMessage('ShowIntermidiateResults', (parentPath,))
uObj.SetMyVariableValues(x)
return 2
except:
return 0
else:
self.useNumericJac = 0
x = x0
deltaX = zeros(numberOfEquations, Float)
idex = 0
elif self.converged:
uObj.SetMyVariableValues(x)
return 1
parent.UpdateX(x, deltaX)
idex = idex + 1
except ArithmeticError, e:
return 0
def plot(data, index, target):
one = np.ones((data.shape[0], 1), dtype=float)
train = np.concatenate((one, data[:, [index]]), axis=1)
alpha, beta = 1, 1
I = np.identity(train.shape[1], dtype=float)
cov = inverse(alpha * I + beta * (np.matmul(train.transpose(), train)))
Mean = beta * np.matmul(cov, np.matmul(train.transpose(), target))
slope = Mean[1][0]
intercept = Mean[0][0]
a, b, c, d = cov[0][0], cov[0][1], cov[1][0], cov[1][1]
x = np.linspace(1, 1000, 500)
plt.xlabel('x')
plt.ylabel('prediction')
plt.title('posterior predictive inferences')
plt.plot(x, Standard(x, slope, intercept), color='g')
plt.plot(x, pos_deviation(x, a, b, c, d, slope, intercept), color='r')
plt.plot(x, neg_deviation(x, a, b, c, d, slope, intercept), color='r')
plt.show()
def predict(self, u, z):
# u = control_vector,
# z = measurement_vector
# Prediction step
# Predicted State Estimate X_p = AX + Bu
X_p = self.A*self.X+self.B*u
# Predicted Probability Estimate V_p = ACA_t + W
V_p = self.A*self.C*self.AT+self.W
# Observation step
IC = 1
# Kalman Gain K
K = V_p*self.HT*inverse(self.H*V_p*self.HT+self.Q)
self.X = X_p + K * (z-self.H*X_p)
self.C = V_p - K * self.H*V_p
self.K = K
self.V = V_p
def set_hamiltonian(self, u_hubbard, mu, u_hubbard_non_loc, *args, **kwargs):
"""
transforms H_loc
u_hubbard: Hubbard interaction
mu: chemical potential that goes explicitly into the Hamiltonian (i.e. mu_DC)
"""
spinspace = range(2)
dim = len(self.transf_mat)
u = self.transf_mat
u_inv = inverse(self.transf_mat)
u_c = CoulombTensorHubbard(-u_hubbard, dim, spinspace)
u_c_transf = u_c.transform(u)
assert dot(u, u_inv).all() == identity(len(u)).all(), 'transformation not unitary'
mu_matrices = [(mu - u_hubbard) * identity(dim), -mu * identity(dim)]
self.hamiltonian = 0
self.hamiltonian -= sum_list([sum_list([sum_list([self._unblocked_c_dag(s, i) * m_transform(mu_matrix, u, i, j) * self._unblocked_c(s, j) for j in range(dim)]) for i in range(dim)]) for s, mu_matrix in zip(spinspace, mu_matrices)])
for i, j, k, l, s1, s2 in product(*[range(dim)]*4 + [spinspace]*2):
self.hamiltonian += u_c_transf[i, j, k, l, s1, s2] * self._unblocked_c_dag(s1, i) * self._unblocked_c_dag(s2, j) * self._unblocked_c(s2, l) * self._unblocked_c(s1, k)
def Polynomial_Regression(k, input, target, test_data, y_test):
#Create features to capture powers upto k
phi = np.ones((input.shape[0], 1), dtype=float)
for i in range(1, k + 1):
phi = np.concatenate((phi, np.power(input, i)), axis=1)
alpha, beta = 1, 1
I = np.identity(k + 1, dtype=float)
covar = inverse(alpha * I + beta * (np.matmul(phi.transpose(), phi)))
mean = beta * np.matmul(covar, np.matmul(phi.transpose(), target))
ans = []
for i in range(test_data.shape[0]):
sum = 0
for j in range(k + 1):
sum = sum + (math.pow(test_data[i][0], j)) * mean[j][0]
ans.append(sum)
ans = np.array(ans)
ans = ans.transpose()
deviation = Error(ans, y_test)
return deviation
def problem5(x_end,t_change=.1,iters=1200,Q_scale=.1): b = 10E-4 F = np.array(sparse.diags(np.array([[1,1,1-b,1-b],[t_change,t_change]]),[0,2]).todense()) F_inv = la.inverse(F) u = np.zeros(4) g = 9.8 u[-1] -= g*t_change Q = np.eye(4)*Q_scale x_old = x0 x = [] for i in xrange(iters): x_new = np.dot(F_inv,x_old) - np.dot(F_inv,u) - np.dot(F_inv,np.dot(la.cholesky(Q),np.random.normal(size=4))) x_old = x_new x.append(x_new) print np.array(x) #print problem5
def triangle_map(tri1, tri2):
from numpy import zeros, subtract, dot as matrix_multiply, float
f1 = zeros((3,3), float)
f1[:,0], f1[:,1] = subtract(tri1[1], tri1[0]), subtract(tri1[2], tri1[0])
f1[:,2] = cross_product(f1[:,0], f1[:,1])
f2 = zeros((3,3), float)
f2[:,0], f2[:,1] = subtract(tri2[1], tri2[0]), subtract(tri2[2], tri2[0])
f2[:,2] = cross_product(f2[:,0], f2[:,1])
from numpy.linalg import inv as inverse
f1inv = inverse(f1)
tmap = zeros((3,4), float)
tmap[:,:3] = matrix_multiply(f2, f1inv)
tmap[:,3] = subtract(tri2[0], matrix_multiply(tmap[:,:3],tri1[0]))
return tmap
def Cmatrix(self):
'''
Calculate the C matrix
'''
self.C = []
self.gamma = []
self.f1001 = []
self.f1010 = []
J = reshape(array([0,1,-1,0]),(2,2))
for j in range(0,len(self.S)):
R = array([[self.R11[j],self.R12[j]],[self.R21[j],self.R22[j]]])
#print R
C = matrixmultiply(-J,matrixmultiply(transpose(R),J))
C = (1/sqrt(1+determinant(R)))*C
g11 = 1/sqrt(self.BETX[j])
g12 = 0
g21 = self.ALFX[j]/sqrt(self.BETX[j])
g22 = sqrt(self.BETX[j])
Ga = reshape(array([g11,g12,g21,g22]),(2,2))
g11 = 1/sqrt(self.BETY[j])
g12 = 0
g21 = self.ALFY[j]/sqrt(self.BETY[j])
g22 = sqrt(self.BETY[j])
Gb = reshape(array([g11,g12,g21,g22]),(2,2))
C = matrixmultiply(Ga, matrixmultiply(C, inverse(Gb)))
gamma=1-determinant(C)
self.gamma.append(gamma)
C = ravel(C)
self.C.append(C)
self.f1001.append(((C[0]+C[3])*1j + (C[1]-C[2]))/4/gamma)
self.f1010.append(((C[0]-C[3])*1j +(-C[1]-C[2]))/4/gamma)
self.F1001R=array(self.f1001).real
self.F1001I=array(self.f1001).imag
self.F1010R=array(self.f1010).real
self.F1010I=array(self.f1010).imag
self.F1001W=sqrt(self.F1001R**2+self.F1001I**2)
self.F1010W=sqrt(self.F1010R**2+self.F1010I**2)
def fitNewtonRaphson(x, y, theta=None, epochs=10):
if theta is None:
theta = np.zeros((1,3))
x = np.concatenate((np.ones((x.shape[0], 1)), x), axis=1)
x = np.asarray([x]).swapaxes(0, 1)
y = np.asarray([y]).swapaxes(0, 1)
errors = []
for i in range(0, epochs):
z = np.inner(theta, x).swapaxes(0, 1)
a = sigmoid(z)
errors.append((y - a).mean())
grad = ((y-a) * x).sum(axis=0)
hessian = -(a * (1 - a) * np.einsum('ikj,ijk->ijk', x, x)).sum(axis=0)
theta = theta - np.dot(inverse(hessian), grad.T).T
return theta, errors
def interpolate3DTransform1(matrixList, indexList, percent):
# MS version that does not assume identity as fist matrix and does
# not wrap around
if percent <= indexList[0]:
return matrixList[0]
if percent >=indexList[-1]:
return matrixList[-1]
listLen = len(indexList)
for i in range(listLen):
if indexList[i] > percent:
break
prevMat = matrixList[i-1]
nextMat = matrixList[i]
from numpy.linalg import inv as inverse
M = N.dot(inverse(prevMat), nextMat)
p = (percent-indexList[i-1]) / (indexList[i]-indexList[i-1])
Mat = _interpolateMat(M, p)
return N.dot(prevMat, Mat)
def triangle_map(tri1, tri2):
from numpy import zeros, subtract, dot as matrix_multiply, float, float64
f1 = zeros((3, 3), float)
f1[:, 0], f1[:, 1] = subtract(tri1[1], tri1[0]), subtract(tri1[2], tri1[0])
f1[:, 2] = cross_product(f1[:, 0], f1[:, 1])
f2 = zeros((3, 3), float)
f2[:, 0], f2[:, 1] = subtract(tri2[1], tri2[0]), subtract(tri2[2], tri2[0])
f2[:, 2] = cross_product(f2[:, 0], f2[:, 1])
from numpy.linalg import inv as inverse
f1inv = inverse(f1)
tmap = zeros((3, 4), float64)
tmap[:, :3] = matrix_multiply(f2, f1inv)
tmap[:, 3] = subtract(tri2[0], matrix_multiply(tmap[:, :3], tri1[0]))
from chimerax.geometry import Place
tf = Place(matrix=tmap)
return tf
def interpolate3DTransform1(matrixList, indexList, percent):
# MS version that does not assume identity as fist matrix and does
# not wrap around
if percent <= indexList[0]:
return matrixList[0]
if percent >= indexList[-1]:
return matrixList[-1]
listLen = len(indexList)
for i in range(listLen):
if indexList[i] > percent:
break
prevMat = matrixList[i - 1]
nextMat = matrixList[i]
from numpy.linalg import inv as inverse
M = N.dot(inverse(prevMat), nextMat)
p = (percent - indexList[i - 1]) / (indexList[i] - indexList[i - 1])
Mat = _interpolateMat(M, p)
return N.dot(prevMat, Mat)
def g_transf(g, transformation, transf_indices, blocks):
assert len(blocks) == 1, 'nambu permits up/down blockstructures, choose one block!'
initial_blockdim = len(g[blocks[0]].data[0,:,:])
momenta = [transf_indices[i][0] for i in range(len(transf_indices))]
nambu_blocksize_momentum = [len(transf_indices[i][1]) for i in range(len(transf_indices))]
to_site = lambda initial_index: initial_index % int(initial_blockdim /2)
inds_map_block = dict()
inds_map_block[0] = range(0, int(initial_blockdim /2))
inds_map_block[1] = range(int(initial_blockdim /2), initial_blockdim)
inds_map_diag = dict()
inds_map_diag[0] = range(0, int(initial_blockdim /2))
u = array(transformation)
u_inv = inverse(u)
g_transf = BlockGf(name_block_generator = [(ind, GfImFreq(indices = dict(transf_indices)[ind], mesh = g[blocks[0]].mesh)) for ind in dict(transf_indices).keys()], make_copies = False)
assert len(u)*2==initial_blockdim, 'blockstates must be twice as large as the site-space'
for momentum_nr, momentum in enumerate(momenta):
if nambu_blocksize_momentum[momentum_nr] == 1:
inds_map = inds_map_diag
else:
inds_map = inds_map_block
for i_nambu, j_nambu in product(inds_map.keys(), inds_map.keys()):
g_transf[momentum][i_nambu, j_nambu] << sum_list([sum_list([u[momentum_nr, to_site(i)] * g[blocks[0]][i, j] * u_inv[to_site(j), momentum_nr] for i in inds_map[i_nambu]]) for j in inds_map[j_nambu]])
return g_transf
def Ksol(self, X, U):
time=self.time
P1 = np.eye(X.shape[1]).flatten()
solver = ode(self.peqns).set_integrator('dopri5')
solver.set_initial_value(P1,time[0]).set_f_params(self.A_interp,
self.B_interp,
self.Rk,
self.Qk)
k = 0
t=time
soln = [P1]
while solver.successful() and solver.t < t[-1]:
k += 1
solver.integrate(t[k])
soln.append(solver.y)
# Convert the list to a numpy array.
psoln = np.array(soln).reshape(time.shape[0],X.shape[1],X.shape[1])
K=np.empty((time.shape[0],X.shape[1],X.shape[1]))
for tindex,t in np.ndenumerate(time):
K[tindex,:,:]=matmult(inverse(self.Rk),self.B_current[tindex].T,psoln[tindex])
self.K=K
return K
def problem5(x_end, t_change=.1, iters=1200, Q_scale=.1):
b = 10E-4
F = np.array(
sparse.diags(np.array([[1, 1, 1 - b, 1 - b], [t_change, t_change]]),
[0, 2]).todense())
F_inv = la.inverse(F)
u = np.zeros(4)
g = 9.8
u[-1] -= g * t_change
Q = np.eye(4) * Q_scale
x_old = x0
x = []
for i in xrange(iters):
x_new = np.dot(F_inv, x_old) - np.dot(F_inv, u) - np.dot(
F_inv, np.dot(la.cholesky(Q), np.random.normal(size=4)))
x_old = x_new
x.append(x_new)
print np.array(x)
#print problem5
def matrixLinReg(dataFrame,xLabel,tLabel,order):
df = dataFrame.copy() # copy dataframe so don't alter original dataset
#need to add 1s to matrix for intercept calculations - can do in the data frame or in the matrix
df['1s'] = 1
#note that 1s are at the start - will effect which item of matrix w is the intercept
x = df.as_matrix(columns = ['1s'])
t = df.as_matrix(columns = [tLabel])
#for loop to add higher order terms
for i in range(1,order+1):
xn = df[xLabel]**i
x = np.insert(x,i,xn,axis=1)
#matrix calculations needed
xT = x.transpose()
xTx = np.dot(xT,x)
invxTx = inverse(xTx)
w = np.dot(np.dot(invxTx,xT),t)
return w
def set_hamiltonian(self, u_hubbard, mu, u_hubbard_non_loc, blocks, *args, **kwargs):
"""
transforms H_loc
express operators in site-basis by operators in new basis
u: Hubbard interaction, can also be non-local(matrix-valued) within the cluster
mu: chemical potential that goes explicitly into the Hamiltonian (i.e. mu_DC)
"""
dim = len(self.transf_mat)
u = self.transf_mat
u_inv = inverse(self.transf_mat)
u_c = CoulombTensorHubbard(u_hubbard, dim, blocks)
u_c_transf = u_c.transform(u)
if u_hubbard_non_loc:
u_c_nl = NNCoulombTensorHubbard(u_hubbard_non_loc, dim, blocks)
u_c_nl_transf = u_c_nl.transform(u)
assert dot(u, u_inv).all() == identity(len(u)).all(), 'transformation not unitary'
sites = range(dim)
mu_matrix = - mu * identity(dim)
self.hamiltonian = sum_list([sum_list([sum_list([self._unblocked_c_dag(s, i) * m_transform(mu_matrix, u, i, j) * self._unblocked_c(s, j) for j in sites]) for i in sites]) for s in self.blocks])
for i, j, k, l, s1, s2 in product(*[sites]*4 + [self.blocks]*2):
self.hamiltonian += u_c_transf[i, j, k, l, s1, s2] * self._unblocked_c_dag(s1, i) * self._unblocked_c_dag(s2, j) * self._unblocked_c(s2, l) * self._unblocked_c(s1, k)
if u_hubbard_non_loc:
for i, j, k, l, s1, s2 in product(*[sites]*4 + [self.blocks]*2):
self.hamiltonian += u_c_nl_transf[i, j, k, l, s1, s2] * self._unblocked_c_dag(s1, i) * self._unblocked_c_dag(s2, j) * self._unblocked_c(s2, l) * self._unblocked_c(s1, k)
def ACL(tree):
"""Returns a normalized dictionary of sequence weights {seq_id: weight}
tree: a PhyloNode object
The ACL method is named after Altschul, Carroll and Lipman, who published a
paper on sequence weighting in 1989.
The ACL method is based on an idea of Felsenstein (1973). Imagine
electrical current flows from the root of the tree down the edges and
out the leaves. If the edge lengths are proportional to their electrical
resistances, current flowing out each leaf equals the leaf weight.
The first step in the calculation of the weight vector is calculating a
variance-covariance matrix. The variance of a leaf is proportional to the
distance from the root to that leaf. The covariance of two leaves is
proportional to the distance from the root to the last common ancestor
of the two leaves.
The second step in the calculation results in a vector of weights. Suppose
there are n leaves on the tree. Let i be the vector of size n, all of whose
elements are 1.0. The weight vector is calculated as:
w = (inverse(M)*i)/(transpose(i)*inverse(M)*i)
See Altschul 1989
"""
#clip branch lengths to avoid error due to negative or zero branch lengths
_clip_branch_lengths(tree)
#get a list of sequence IDs (in the order that the tree will be traversed)
seqs = []
for n in tree.tips():
seqs.append(n.Name)
#initialize the variance-covariance matrix
m = zeros([len(seqs),len(seqs)],Float64)
#calculate (co)variances
#variance of a node is defined as the distance from the root to the leaf
#covariance of two nodes is defined as the distance from the root to the
#last common ancestor of the two leaves.
for x in tree.tips():
for y in tree.tips():
idx_x = seqs.index(x.Name)
idx_y = seqs.index(y.Name)
if idx_x == idx_y:
m[idx_x,idx_y] = x.distance(tree)
else:
lca = x.lastCommonAncestor(y)
dist_lca_root = lca.distance(tree)
m[idx_x,idx_y] = dist_lca_root
m[idx_y,idx_x] = dist_lca_root
#get the inverse of the variance-covariance matrix
inv = inverse(m)
#build vector i (vector or ones, length = # of leaves in the tree)
i = ones(len(seqs),Float64)
numerator = matrixmultiply(inv, i)
denominator = matrixmultiply(matrixmultiply(transpose(i),inv),i)
weight_vector = numerator/denominator
#return a Weights object (is dict {seq_id: weight})
return Weights(dict(list(zip(seqs,weight_vector))))
def SolveNonLinearEquations(parent, u, numMethSettings, lastConvX, lastX, lastJac, lastConvJac=None):
#parent.InfoMessage('In:', (time.asctime(), time.time()))
#Load numerical method settings
maxIter = numMethSettings.maxIter
damp = numMethSettings.dampingFactor
stayThreshold = numMethSettings.stayThreshold
tolerance = numMethSettings.tolerance
maxStep = numMethSettings.maxStep
minimErr = numMethSettings.minimizeErr
tryToRestart = numMethSettings.tryToRestart
tryLastConverged = numMethSettings.tryLastConverged
if hasattr(numMethSettings, 'solveMethod'):
solveMethod = numMethSettings.solveMethod
else:
if hasattr(parent, 'CalculateJacobian'):
solveMethod = NR
else:
solveMethod = SECANT
if hasattr(numMethSettings, 'monitorConv'): monitorConv = numMethSettings.monitorConv
else: monitorConv = False
if hasattr(numMethSettings, 'freqJacMsg'): freqJacMsg = numMethSettings.freqJacMsg
else: freqJacMsg = 10
#Load stuff from the unknowns
nuEquations = u.GetNumberOfUnknowns()
x = u.GetValues()
isFix = u.GetIsFixed()
initx = u.GetInitValues()
scaleFactors = u.GetScaleFactors()
lowBounds = u.GetLowBounds()
highBounds = u.GetHighBounds()
#Init arrays
jacobian = zeros((nuEquations, nuEquations), Float)
deltaX = ones(nuEquations, Float) #Set initial deltaX to 1.0 to ensure that convergence check works
rhs = zeros(nuEquations, Float)
oldRhs = zeros(nuEquations, Float)
#Init some vars
iter = 0
converged = False
needsAFirstCalculation = True
maxError = None
parentPath = parent.GetPath()
#Start from last vals?
if tryToRestart:
if lastX and len(lastX) == nuEquations:
#Should it really waste the time checking on the specs?
for i in range(nuEquations):
if not isFix[i]:
x[i] = lastX[i]
#Use last converged?
elif tryLastConverged:
if lastConvX and len(lastConvX) == nuEquations:
try:
#Keep the specs:
for i in range(nuEquations):
if isFix[i]:
lastConvX[i] = x[i]
parent.CalculateRHS(lastConvX, oldRhs, isFix, initx)
maxErr = max(abs(oldRhs))
if maxErr < stayThreshold:
x = Numeric.array(lastConvX)
initx = Numeric.array(lastConvX)
u.SetInitValues(lastConvX)
rhs[:] = oldRhs[:]
needsAFirstCalculation = False
except:
pass
#Lets do stuff for the first iteration
if needsAFirstCalculation:
#print 'FirstCalcIn:', time.asctime()
try:
parent.CalculateRHS(x, rhs, isFix, initx)
#Check to see if the previous solution is better
if not tryToRestart and maxError != None and maxError < max(abs(rhs)):
x = Numeric.array(lastConvX)
initx = Numeric.array(lastConvX)
u.SetInitValues(lastConvX)
rhs[:] = oldRhs[:] #OldRhs was used to store rhs for the last converged solution
except:
parent.InfoMessage('CouldNotInitialize', (parentPath,))
return x, rhs, converged, jacobian
try:
converged = CheckForConvergence(rhs, scaleFactors, tolerance)
if converged:
parent.InfoMessage('Converged', (parentPath, iter))
return x, rhs, converged, lastConvJac
#parent.InfoMessage('FirstJacobianIn:', (time.asctime(), time.time()))
doCrudeDiff = True
parent.InfoMessage('TowerCalcJacobian', (parentPath,))
if solveMethod == NR and hasattr(parent, 'CalculateJacobian'):
parent.CalculateJacobian(x, jacobian, isFix, initx)
jacobian = inverse(jacobian)
doCrudeDiff = False
elif solveMethod == BROYDEN:
if tryToRestart and lastX and len(lastX) == nuEquations:
if lastJac:
jacobian[:] = lastJac[:]
doCrudeDiff = False
elif tryLastConverged and lastConvX and len(lastConvX) == nuEquations:
if lastConvJac:
jacobian[:] = lastConvJac[:]
doCrudeDiff = False
elif hasattr(parent, 'CalculateJacobian'):
parent.CalculateJacobian(x, jacobian, isFix, initx)
jacobian = inverse(jacobian)
doCrudeDiff = False
if doCrudeDiff:
xForJac = array(x, Float)
rhsForJac = array(rhs, Float)
shift = 0.0001
#Pass a message every x calculations, so the solver doesn't look dead
distCnt = 0
msgEvery = freqJacMsg
for j in range(nuEquations):
distCnt +=1
if distCnt == msgEvery:
parent.InfoMessage('CalcDisturbance', (j+1, nuEquations, parentPath))
distCnt = 0
old = xForJac[j]
xForJac[j] = xForJac[j] + shift * scaleFactors[j]
parent.CalculateRHS(xForJac, rhsForJac, isFix, initx)
jacobian[:, j] = (rhsForJac - rhs)/(shift * scaleFactors[j])
xForJac[j] = old
jacobian = inverse(jacobian)
deltaX = -dot(jacobian, rhs)
#parent.InfoMessage('FirstJacobianOut:', (time.asctime(), time.time()))
except:
#Bye
parent.InfoMessage('CouldNotInvertJacobian', (parentPath,))
return x, rhs, converged, jacobian
maxStep = None
errHistory = []
currMaxErr = max(abs(rhs))
while iter <= maxIter:
iter += 1
oldRhs[:] = rhs[:]
stepLength = 1.0*damp
currx = Numeric.array(x)
maxErr = currMaxErr
#Make sure the step is not too large
if maxStep != None:
for i in range(len(deltaX)):
if deltaX[i]/scaleFactors[i] > maxStep:
deltaX[i] = maxStep*scaleFactors[i]
elif deltaX[i]/scaleFactors[i] < -maxStep:
deltaX[i] = -maxStep*scaleFactors[i]
cntHere = 0
#Loop until max error (max rhs) decreases
while 1:
actualDeltaX = stepLength*deltaX
x = UpdateX(currx, actualDeltaX, lowBounds, highBounds)
if hasattr(parent, 'SanityCheck'):
parent.SanityCheck(x, initx)
try:
parent.CalculateRHS(x, rhs, isFix, initx)
converged = CheckForConvergence(rhs, scaleFactors, tolerance)
if converged:
parent.InfoMessage('Converged', (parentPath, iter))
#parent.InfoMessage('Out:', (time.asctime(), time.time()))
return x, rhs, converged, jacobian
currMaxErr = max(abs(rhs))
if minimErr and currMaxErr > maxErr:
stepLength *= 0.25
cntHere += 1
else:
break
except:
stepLength *= 0.25
if stepLength < 0.0000001:
parent.InfoMessage('CouldNotConverge', (parentPath, iter))
return x, rhs, False, jacobian
parent.InfoMessage('EqnBasedUOpError', (parentPath, iter, currMaxErr))
if monitorConv:
#Monitor if it is not converging and it is just wasting time
if cntHere > 4 and currMaxErr > 1000.0*tolerance:
errHistory.append(currMaxErr)
#It needed to reduce the error 7 times more than 4 times in a row.
#Indicator there could be something wrong
if len(errHistory) > 4:
parent.InfoMessage('NotConverging', (parentPath,))
return x, rhs, converged, jacobian
else:
errHistory = []
try:
#parent.InfoMessage('JacobianIn:', (time.asctime(), time.time()))
parent.InfoMessage('TowerCalcJacobian', (parentPath,))
if solveMethod == NR and hasattr(parent, 'CalculateJacobian'):
jacobian = zeros((nuEquations, nuEquations), Float)
parent.CalculateJacobian(x, jacobian, isFix, initx)
jacobian = inverse(jacobian)
elif solveMethod == BROYDEN:
xLastBr = array(x)
rhsLastBr = array(rhs)
usedBr = True
dF = rhs - oldRhs
dotdxB = dot(actualDeltaX, jacobian)
denom = dot(dotdxB, dF)
if not abs(denom) < TINYESTVALUE:
jacobian = jacobian + outerproduct((actualDeltaX - dot(jacobian, dF)), dotdxB)/denom
else: #Do Secant
xForJac = array(x, Float)
rhsForJac = array(rhs, Float)
shift = 0.0001
#Pass a message every x calculations, so the solver doesn't look dead
distCnt = 0
msgEvery = freqJacMsg
for j in range(nuEquations):
distCnt +=1
if distCnt == msgEvery:
parent.InfoMessage('CalcDisturbance', (j+1, nuEquations, parentPath))
distCnt = 0
old = xForJac[j]
xForJac[j] = xForJac[j] + shift * scaleFactors[j]
parent.CalculateRHS(xForJac, rhsForJac, isFix, initx)
jacobian[:, j] = (rhsForJac - rhs)/(shift * scaleFactors[j])
xForJac[j] = old
jacobian = inverse(jacobian)
deltaX = -dot(jacobian, rhs)
#parent.InfoMessage('JacobianOut:', (time.asctime(), time.time()))
except:
parent.InfoMessage('CouldNotInvertJacobian', (parentPath,))
break
parent.InfoMessage('CouldNotConverge', (parentPath, iter))
return x, rhs, converged, jacobian
def EulerImplicit(parent, yInit, odeSettings, yMin=None, yMax=None, yScale=None):
"""Integrate with Euler explicit"""
MAXTRY = 40
LARGEVALITER = 1000
#Settings for implicit iteration
#Default are fairly loose settings
if hasattr(odeSettings, 'maxIter'): maxIter = odeSettings.maxIter
else: maxIter = 15
if hasattr(odeSettings, 'dampingFactor'): damping = odeSettings.dampingFactor
else: damping = 1.0
damping = 1.0
if hasattr(odeSettings, 'tolerance'): tol = odeSettings.tolerance
else: tol = 1.0E-3
h = odeSettings.step
xEnd = odeSettings.end
xInit = odeSettings.init
CalcDerivativesMethod = parent.CalculateDerivatives
Validate = None
if hasattr(parent, 'ValidateStepResults'): Validate = parent.ValidateStepResults
nuEquations = len(yInit)
jacobian = zeros((nuEquations, nuEquations), Float)
#Did scale values came in?
autoScale = False
if yScale == None:
autoScale = True
#Iterate along the whole distance
h = Numeric.sign(xEnd-xInit) * abs(h) #Make sure the sign of h makes sense
hBase = h #Original value of h
hNext = h #Initialize hNext as h
hMin = TINYESTVALUE #Always positive
x = xInit
y = array(yInit)
stepCnt = 0
converged = False
loadResults = True
path = parent.GetPath()
xNextStore = xInit
while stepCnt < LARGEVALITER and ((x-xEnd)*(xEnd-xInit) < 0.0):
stepCnt += 1
#Calculate derivatives right where we are
parent.InfoMessage('CalculatingStep', (stepCnt, path, x, xInit, xEnd))
if ( hBase > 0.0 and x >= xNextStore ) or ( hBase < 0.0 and x <= xNextStore ):
loadResults = True
xNextStore += hBase
else:
loadResults = False
if yMin != None and min(y-yMin) < 0.0:
#See if it is a round off problem.
if hasattr(parent, 'RoundValues'):
y = parent.RoundValues(y, yMin, yMax, yScale)
dy_dx = CalcDerivativesMethod(x, y, loadResults)
#Set h as the estimated next h
h = hNext
#Make sure it won't go over
if (x + h - xEnd) * (x + h - xInit) > 0.0: h = xEnd - x
#Scale values
if autoScale:
C = ones(len(y), Float)
yScale = Numeric.maximum(C, absolute(y))
#Iterate until a proper step size is found
innerCnt = 0
ySave = array(y, Float)
#The implicit algorithm implies that the derivatives are evaluated at h
#but sometimes it is necessary to evaluate the derivatives at h-epsilon
#to avoid convergence problems.
#In this case hEval = h-epsilon
hEval = h
doImplicit = 1
while innerCnt <= MAXTRY:
innerCnt += 1
try:
#Initial guess of yNext as explicit Euler
yNext = ySave + h*dy_dx
#Make sure it is
yNext = parent.StepToBoundaries(x+h, yNext, h)
yNext = Numeric.clip(yNext, yMin, yMax)
#Iterate in the implicit step with quasi Newton Raphson
#with approximate Jacobian
iter = 0
convImplicit = False
while iter < maxIter:
iter += 1
yNextNew = ySave + h*CalcDerivativesMethod(x + hEval, yNext)
yNextNew = parent.StepToBoundaries(x+h, yNextNew, h)
#if Validate: yNextNew = Validate(x+h, yNextNew)
rhs = yNextNew - yNext
rhs /= yScale
if max(Numeric.absolute(rhs)) < tol:
convImplicit = True
y = Numeric.clip(yNextNew, yMin, yMax)
break
#Calculate Jacobian with crude differentials
yNextForJac = array(yNext, Float)
rhsForJac = array(rhs, Float)
shift = 0.0001
shift = shift * yScale
for j in xrange(nuEquations):
old = yNextForJac[j]
yNextForJac[j] = yNextForJac[j] + shift[j]
yNextNewForJac = ySave + h*CalcDerivativesMethod(x+hEval, yNextForJac)
rhsForJac = (yNextNewForJac - yNextForJac) / yScale
for k in xrange(nuEquations):
jacobian[k][j] = (rhsForJac[k] - rhs[k])/(shift[j])
yNextForJac[j] = old
#Invert Jacobian and get the new estimate for y
jacobian = inverse(jacobian)
deltaX = -dot(jacobian, rhs)*damping
yNext = UpdateX(yNext, deltaX)
yNext = parent.StepToBoundaries(x+h, yNext, h)
yNext = Numeric.clip(yNext, yMin, yMax)
if convImplicit:
if h >= 0.0: hNext = min(hBase, h*4.0)
else: hNext = max(hBase, h*4.0)
y = yNext
break
else:
#Reduce the step
h *= 0.25
if abs(h) <= hMin:
raise SimError('StepSizeTooSmall', (path, h))
except:
#Reduce the step
h *= 0.25
if abs(h) <= hMin:
raise SimError('StepSizeTooSmall', (path, h))
if not convImplicit:
raise SimError('StepSizeTooSmall', (path, h))
x += h
##Decide if we keep on iterating####################################################
if ((x-xEnd)*(xEnd-xInit) >= 0.0):
#Calculate derivatives yet again just so final results are loaded.
#Not the best way to do things but good enough for now
loadResults = True
dy_dx = CalcDerivativesMethod(xEnd, y, loadResults)
converged = True
break
####################################################################################
if not converged:
parent.InfoMessage('ODEMaxSteps', (stepCnt, path))
return converged
def _compute_rectification_homographies(self):
"""
This method a C++ to Python translation of
void doppia::CpuPreprocessor::compute_rectification_homographies(...)
"""
assert self.stereo_calibration.IsInitialized()
# rectification: http://profs.sci.univr.it/~fusiello/demo/rect/
# in this function 1 refers to left and 2 refers to right
left_camera_calibration = self.stereo_calibration.left_camera
right_camera_calibration = self.stereo_calibration.right_camera
K1 = left_camera_calibration.internal_parameters
K1 = camera_internal_parameters_to_array(K1)
R1 = left_camera_calibration.pose.rotation
R1 = rotation_to_array(R1)
K2 = right_camera_calibration.internal_parameters
K2 = camera_internal_parameters_to_array(K2)
R2 = right_camera_calibration.pose.rotation
R2 = rotation_to_array(R2)
# optical centers (unchanged)
# focal_center1 and focal_center2 are Vector3f
focal_center1 = get_focal_point(left_camera_calibration.pose)
focal_center2 = get_focal_point(right_camera_calibration.pose)
# Q1 and Q2 are Matrix3f
Q1 = inverse(K1 * R1)
Q2 = inverse(K2 * R2)
# new x axis (direction of the baseline)
v1 = normalize(focal_center2 - focal_center1) # Normalise by dividing through by the magnitude.
v1 = v1.transpose()
# new y axes (old y)
#v2 = (R1.row(1) + R2.row(1))*0.5
v2 = (R1[1,:] + R2[1,:])*0.5
v2 = normalize(v2)
# new z axes (orthogonal to baseline and y)
v3 = cross(v1,v2)
v3 = normalize(v3)
v2 = cross(v3,v1)
v2 = normalize(v2)
# new extrinsic parameters
R1[0, :] = v1 # R1.row(0)
R1[1, :] = v2
R1[2, :] = v3
K1 = (K1 + K2) * 0.5
K2 = K1
# Q1new and Q2new are Matrix3f
Q1new = K1 * R1
Q2new = K1 * R1
Q1new = inverse(Q1new * Q1)
Q2new = inverse(Q2new * Q2)
# do centering
# FIXME why 5 iterations ?
for i in range(5):
# new intrinsic parameters (arbitrary)
K1 = (K1 + K2) * 0.5
K2 = K1
K1[0,2] += (Q1new[0, 2] + Q2new[0, 2]) / 4
K2[0,2] += (Q1new[0, 2] + Q2new[0, 2]) / 4
Q1new = K1 * R1
Q2new = K2 * R1
Q1new = inverse(Q1new * Q1)
Q2new = inverse(Q2new * Q2)
use_terrible_hack = True
#use_terrible_hack = False
if use_terrible_hack:
print("!"*10 + "USING 30 pixels offset in _compute_rectification_homographies" + "!"*50)
x_offset = 30
Q1new[0, 2] += x_offset # [pixels]
Q2new[0, 2] -= x_offset # [pixels]
# left, right rectified MetricCamera(K, radial_distortion_parameters, R, t)
#metric_camera1_rectified.set(K1, zeros< double >(3), R1, -R1.transpose() * focal_center1)
#metric_camera2_rectified.set(K2, zeros< double >(3), R1, -R1.transpose() * focal_center2)
self.left_rectification_homography = Q1new
self.right_rectification_homography = Q2new
self.left_rectification_inverse_homography = inverse(self.left_rectification_homography)
self.right_rectification_inverse_homography = inverse(self.right_rectification_homography)
self.left_K_rectified = K1
self.right_K_rectified = K2
return
def reference_rect(I, R=None, f=None, c=None, k=None, alpha=None, KK_new=None):
# original version, translated from MATLAB. superseded by above
"""
arguments:
I is image
optional arguments:
R is 3x3 rotation (of affine transformation) matrix, defaults to eye(3)
f is focal length (horizontal and vertical), defaults to (1,1)
c is image center (horizontal and vertical), defaults to (0,0)
k is nonlinear parameters, defaults to (0,0,0,0,0)
"""
I = nx.asarray(I)
if R is None:
R = nx.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
else:
R = nx.asarray(R)
if f is None:
f = (1, 1)
else:
f = nx.asarray(f)
if c is None:
c = (0, 0)
else:
c = nx.asarray(c)
if k is None:
k = (0, 0, 0, 0, 0)
else:
k = nx.asarray(k)
if KK_new is None:
KK_new = nx.array([[f[0], 0, c[0]], [0, f[1], c[1]], [0, 0, 1]])
else:
KK_new = nx.asarray(KK_new)
if alpha is None:
alpha = 0
# Note: R is the motion of the points in space
# So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame.
nr, nc = I.shape # must be 2D (grayscale) image
# put I in matlab uni-dimensional index format
I = I.copy()
I = nx.transpose(I).copy()
I.ravel()
Irec = 255.0 * nx.ones((nr * nc, ))
mx, my = nx.meshgrid(nx.arange(nc), nx.arange(nr))
px = nx.reshape(mx, (nc * nr, ))
py = nx.reshape(my, (nc * nr, ))
## A = linalg.inverse(KK_new)
## b = nx.array( [px,
## py,
## nx.ones(px.shape)])
rays = nx.dot(linalg.inverse(KK_new), nx.array([px, py,
nx.ones(px.shape)]))
# print 'rays',rays
# Rotation: (or affine transformation):
rays2 = nx.dot(nx.transpose(R), rays)
# print 'rays2',rays2
x = nx.array([rays2[0, :] / rays2[2, :], rays2[1, :] / rays2[2, ]])
# print 'x',x
# Add distortion
xd = apply_distortion(x, k)
# print 'xd',xd
# Reconvert in pixels:
px2 = f[0] * (xd[0, :] + alpha * xd[1, :]) + c[0]
py2 = f[1] * xd[1, :] + c[1]
# print 'px2',px2
# print 'py2',py2
# Interpolate between the closest pixels:
px_0 = nx.floor(px2)
py_0 = nx.floor(py2)
if 0:
py_1 = py_0 + 1
tmpA = nx.where((px_0 >= 0) & (px_0 <= (nc - 2)) & (py_0 >= 0)
& (py_0 <= (nr - 2)))
if type(tmpA) == tuple:
# numarray behavior
good_points = tmpA[0]
else:
# numpy behavior
good_points = tmpA
px2 = px2[good_points]
py2 = py2[good_points]
px_0 = px_0[good_points]
py_0 = py_0[good_points]
alpha_x = px2 - px_0
# print 'alpha_x',alpha_x
alpha_y = py2 - py_0
a1 = (1 - alpha_y) * (1 - alpha_x)
a2 = (1 - alpha_y) * alpha_x
a3 = alpha_y * (1 - alpha_x)
a4 = alpha_y * alpha_x
# print 'a2',a2
ind_lu = (px_0 * nr + py_0).astype(nx.Int)
ind_ru = ((px_0 + 1) * nr + py_0).astype(nx.Int)
ind_ld = (px_0 * nr + (py_0 + 1)).astype(nx.Int)
ind_rd = ((px_0 + 1) * nr + (py_0 + 1)).astype(nx.Int)
ind_new = ((px[good_points]) * nr + py[good_points]).astype(nx.Int)
Ir = nx.ravel(I)
Irec[ind_new] = (a1 * Ir[ind_lu] + a2 * Ir[ind_ru] + a3 * Ir[ind_ld] +
a4 * Ir[ind_rd])
# convert matlab unidimensional format into numarray format
Irec = nx.reshape(Irec, (nc, nr))
Irec = nx.transpose(Irec)
return Irec
