def solve_psd(A,b,overwrite_b=False,return_chol=False):
from scipy.linalg.lapack import dposv
if return_chol:
L, x, _ = dposv(A,b,lower=1,overwrite_b=overwrite_b)
return np.tril(L), x
else:
return dposv(A,b,overwrite_b=overwrite_b)[1]
def solve_psd(A,b,overwrite_b=False,return_chol=False):
from scipy.linalg.lapack import dposv
if return_chol:
L, x, _ = dposv(A,b,lower=1,overwrite_b=overwrite_b)
return np.tril(L), x
else:
return dposv(A,b,overwrite_b=overwrite_b)[1]
def LAPACK_solve_ls_with_normal_equation(A, b):
# The corresponding procedure in LAPACK is https://www.netlib.org/lapack/lug/node27.html
ATA = np.matmul(A.T, A)
ATb = np.matmul(A.T, b)
# ATA = dgemm(1, A.T, A)
# ATb = dgemm(1, A.T, b)
_, x, _ = dposv(ATA, ATb)
# _, _, x, _ = dgesv(ATA, ATb)
return x
def solve_psd(A,
b,
chol=None,
lower=True,
overwrite_b=False,
overwrite_A=False):
if chol is None:
return lapack.dposv(A,
b,
overwrite_b=overwrite_b,
overwrite_a=overwrite_A)[1]
else:
return lapack.dpotrs(chol, b, lower, overwrite_b)[0]
def solve_psd(A,b,chol=None,lower=True,overwrite_b=False,overwrite_A=False):
if chol is None:
return lapack.dposv(A,b,overwrite_b=overwrite_b,overwrite_a=overwrite_A)[1]
else:
return lapack.dpotrs(chol,b,lower,overwrite_b)[0]
unifs = utils.pgev(Y, Loc, Scale, Shape)
cen = unifs < prob_below
cen_above = unifs > prob_above
# 3. Eigendecomposition of the correlation matrix
n_covariates = len(beta_loc0)
theta_c = np.array([range, nu])
Dist = distance.squareform(distance.pdist(Stations_local))
tmp_vec = np.ones(n_s)
Cor = utils.corr_fn(Dist, theta_c)
# eig_Cor = np.linalg.eigh(Cor) #For symmetric matrices
# V = eig_Cor[1]
# d = eig_Cor[0]
cholesky_inv = lapack.dposv(Cor, tmp_vec)
# 4. Process data given initial values
# X = data_all['X'][subset_indices,:]
# X_s = data_all['X_s'][subset_indices,:]
# Z = data_all['Z'][subset_indices,:]
X = utils.gev_2_RW_me(Y, xp, surv_p, tau_sqd, phi, gamma, Loc, Scale,
Shape)
R = data_all['R_at_knots'][local_fit_no, :]
Z = np.empty((n_s, n_t))
Z[:] = np.nan
for idx in np.arange(n_t):
X_s_tmp = X[:, idx] - np.sqrt(tau_sqd) * norm.rvs(size=n_s)
lower_limit = R[idx]**phi
X_s_tmp[X_s_tmp < lower_limit] = lower_limit + 0.01
def solveIK1Ray(skelDict,
effectorData,
x3ds,
effectorIndices_3d,
E,
effectorIndices_2d,
outerIts=10,
rootMat=None):
"""
solveIK routine form Label.py - Has Single ray constraint equations enables
Given effectors (joint, offset, weight) and constraints for those (3d and 2d), solve for the skeleton pose.
Effector offsets, weights and targets are 3-vectors
Args:
skelDict (GskelDict): The Skeleton to process
effectorData (big o'l structure!):
effectorJoints, effectorOffsets, effectorWeights, usedChannels, usedChannelWeights, usedCAEs, usedCAEsSplits
x3ds (float[][3]): 3D Reconstructions
effectorIndices_3d (?): What's this?
E (): Equations for 1-Ray constraints, or MDMA.
effectorIndices_2d (?): What's this?
outerIts (int): IK Iterations to solve the skeleton. Default = 10
rootMat (float[3][4]): reference frame of the Skeleton. Default = None
Returns:
None: The result is an update of the skelDict to the solution - chanValues, channelMats, and Gs.
Requires:
Character.pose_skeleton_with_chan_mats
ISCV.derror_dchannel_single_ray
ISCV.JTJ_single_ray
"""
if rootMat is None: rootMat = np.eye(3, 4, dtype=np.float32)
effectorJoints, effectorOffsets, effectorWeightsOld, usedChannels, usedChannelWeights, usedCAEs, usedCAEsSplits = effectorData
chanValues = skelDict['chanValues']
jointParents = skelDict['jointParents']
Gs = skelDict['Gs']
Ls = skelDict['Ls']
jointChans = skelDict['jointChans']
jointChanSplits = skelDict['jointChanSplits']
numChannels = jointChanSplits[-1]
numEffectors = len(effectorJoints)
num3ds = len(effectorIndices_3d)
num2ds = len(effectorIndices_2d)
effectorOffsets = np.copy(effectorOffsets[:, :, 3])
effectorWeights = np.zeros(numEffectors, dtype=np.float32)
effectorWeights[
effectorIndices_3d] = 1 # TODO Why does this fail? effectorWeightsOld[effectorIndices_3d,0,3]
effectorWeights[
effectorIndices_2d] = 1 # effectorWeightsOld[effectorIndices_2d,0,3]
numUsedChannels = len(usedChannels)
channelMats = np.zeros((numChannels, 3, 4), dtype=np.float32)
effectors = np.zeros((numEffectors, 3), dtype=np.float32)
residual = np.zeros((num3ds, 3), dtype=np.float32)
residual2 = np.zeros((num2ds, 2), dtype=np.float32)
derrors = np.zeros((numUsedChannels, numEffectors, 3), dtype=np.float32)
delta = np.zeros((numUsedChannels), dtype=np.float32)
JTJ = np.zeros((numUsedChannels, numUsedChannels), dtype=np.float32)
JTB = np.zeros((numUsedChannels), dtype=np.float32)
JT = derrors.reshape(numUsedChannels, -1)
JTJdiag = np.diag_indices_from(JTJ)
for it in xrange(outerIts):
# TODO, only usedChannels are changing, only update the matrices that have changed after the first iteration.
# updates the channelMats and Gs
Character.pose_skeleton_with_chan_mats(channelMats, Gs, skelDict,
chanValues, rootMat)
bestScore = ISCV.pose_effectors_single_ray(
effectors, residual, residual2, Gs, effectorJoints,
effectorOffsets, effectorWeights, x3ds, effectorIndices_3d, E,
effectorIndices_2d)
if np.sum(residual * residual) + np.sum(
residual2 * residual2) <= 1e-5 * (num3ds + num2ds):
break # early termination
ISCV.derror_dchannel_single_ray(derrors, channelMats, usedChannels,
usedChannelWeights, usedCAEs,
usedCAEsSplits, jointChans, effectors,
effectorWeights)
# J = d_effectors/dc
# err(c) = x3ds - effectors[effectorIndices_3d], e0 + E effectors[effectorIndices_2d]; err(c+delta) = x3ds - effectors[effectorIndices_3d] - J[effectorIndices_3d] delta, e0 + E effectors[effectorIndices_2d] + E J[effectorIndices_2d] delta = 0
# J dc = B; (J[effectorIndices_3d] ; E J[effectorIndices_2d]) dc = B ; e0
# DLS method : solve (JTJ + k^2 I) delta = JTB
ISCV.JTJ_single_ray(
JTJ, JTB, JT, residual, effectorIndices_3d, E, effectorIndices_2d,
residual2) #np.dot(JT, B, out=JTB); np.dot(JT, JT.T, out=JTJ)
JTJ[JTJdiag] += 1
JTJ[JTJdiag] *= 1.1
# delta[:] = np.linalg.solve(JTJ, JTB)
_, delta[:], _ = LAPACK.dposv(JTJ, JTB) # Use Positive Definite Solver
chanValues[usedChannels] += delta
# TODO: add channel limits
# # J_transpose method, 3d only: scaling problems with translation
#JT = derrors[:,effectorIndices_3d,:].reshape(numUsedChannels,-1)
#np.dot(JT, B, out=delta)
#np.dot(JT.T,delta,out=JJTB)
#delta *= np.dot(B,JJTB)/(np.dot(JJTB,JJTB)+1)
#delta[:3] *= 100000.
#testScale = ISCV.Jtranspose_SR(delta, JJTB, JT, residual,effectorIndices_3d,residual2,effectorIndices_2d)
Character.pose_skeleton(Gs, skelDict, chanValues, rootMat)
def solveIK(skelDict,
chanValues,
effectorData,
effectorTargets,
outerIts=10,
rootMat=None):
"""
Given an initial skeleton pose (chanValues), effectors (ie constraints: joint, offset, weight, target), solve for the skeleton pose.
Effector weights and targets are 3x4 matrices.
* Setting 1 in the weight's 4th column makes a position constraint.
* Setting 100 in the weight's first 3 columns makes an orientation constraint.
Args:
skelDict (GskelDict): The Skeleton to process
chanValues (float[]): Initial pose of the skeleton as Translation and many rotations applied to joints in the skelDict.
effectorData (big o'l structure!):
effectorJoints, effectorOffsets, effectorWeights, usedChannels, usedChannelWeights, usedCAEs, usedCAEsSplits
effectorTargets (?): What's this?
outerIts (int): IK Iterations to solve the skeleton. Default = 10
rootMat (float[3][4]): reference frame of the Skeleton. Default = None
Returns:
None: The result is an update of the skelDict to the solution - chanValues, channelMats, and Gs.
Requires:
Character.pose_skeleton_with_chan_mats
ISCV.pose_effectors
ISCV.derror_dchannel
ISCV.JTJ
"""
effectorJoints, effectorOffsets, effectorWeights, usedChannels, usedChannelWeights, usedCAEs, usedCAEsSplits = effectorData
jointParents = skelDict['jointParents']
Gs = skelDict['Gs']
Ls = skelDict['Ls']
jointChans = skelDict['jointChans']
jointChanSplits = skelDict['jointChanSplits']
numChannels = jointChanSplits[-1]
numEffectors = len(effectorJoints)
numUsedChannels = len(usedChannels)
channelMats = np.zeros((numChannels, 3, 4), dtype=np.float32)
#usedEffectors = np.array(np.where(np.sum(effectorWeights,axis=(1,2)) != 0)[0], dtype=np.int32)
usedEffectors = np.array(np.where(effectorWeights.reshape(-1) != 0)[0],
dtype=np.int32)
# numUsedEffectors= len(usedEffectors)
effectors = np.zeros((numEffectors, 3, 4), dtype=np.float32)
residual = np.zeros((numEffectors, 3, 4), dtype=np.float32)
derrors = np.zeros((numUsedChannels, numEffectors, 3, 4), dtype=np.float32)
# steps = np.ones((numUsedChannels),dtype=np.float32)*0.2
# steps[np.where(jointChans[usedChannels] < 3)[0]] = 30.
# steps = 1.0/steps
delta = np.zeros((numUsedChannels), dtype=np.float32)
# JJTB = np.zeros((numEffectors*12),dtype=np.float32)
JTJ = np.zeros((numUsedChannels, numUsedChannels), dtype=np.float32)
JTB = np.zeros((numUsedChannels), dtype=np.float32)
JT = derrors.reshape(numUsedChannels, -1)
JTJdiag = np.diag_indices_from(JTJ)
B = residual.reshape(-1)
# TODO, calculate the exact requirements on the tolerance
B_len = len(B)
tolerance = 0.00001
it_eps = (B_len**0.5) * tolerance
for it in xrange(outerIts):
# TODO, only usedChannels are changing, only update the matrices that have changed after the first iteration.
# TODO Look into damping, possibly clip residuals?
# updates the channelMats and Gs
Character.pose_skeleton_with_chan_mats(channelMats, Gs, skelDict,
chanValues, rootMat)
bestScore = ISCV.pose_effectors(effectors, residual, Gs,
effectorJoints, effectorOffsets,
effectorWeights, effectorTargets)
if np.linalg.norm(B) < it_eps: break # early termination
ISCV.derror_dchannel(derrors, channelMats, usedChannels,
usedChannelWeights, usedCAEs, usedCAEsSplits,
jointChans, effectors, effectorWeights)
# if True: # DLS method : solve (JTJ + k^2 I) delta = JTB
ISCV.JTJ(
JTJ, JTB, JT, B,
usedEffectors) #np.dot(JT, B, out=JTB); np.dot(JT, JT.T, out=JTJ)
JTJ[JTJdiag] += 1
JTJ[JTJdiag] *= 1.1
_, delta[:], _ = LAPACK.dposv(JTJ, JTB) # Use Positive Definite Solver
# Use General Solver
# delta[:] = np.linalg.solve(JTJ, JTB)
# elif it==0: # SVD method: solve J delta = B
# delta[:] = np.linalg.lstsq(JT.T[usedEffectors], B[usedEffectors], rcond=0.0001)[0].reshape(-1)
# else: # J transpose method
# testScale = ISCV.J_transpose(delta, JJTB, JT, B)
# #np.dot(JT, B, out=delta); np.dot(JT.T,delta,out=JJTB); delta *= np.dot(B,JJTB)/(np.dot(JJTB,JJTB)+1.0)
#scale = np.max(np.abs(delta*steps))
#if scale > 1.0: delta *= 1.0/scale
#np.clip(delta,-steps,steps,out=delta)
chanValues[usedChannels] += delta
# TODO: add channel limits
#bestScore = ISCV.lineSearch(chanValues, usedChannels, delta, Gs, Ls, jointParents, jointChans, jointChanSplits,
# rootMat, effectorJoints, effectorOffsets, effectorWeights, effectorTargets, innerIts, bestScore)
#print np.mean(B*B)
Character.pose_skeleton(Gs, skelDict, chanValues, rootMat)