def testDerivatives(skelDict, chanValues, effectorData, effectorTargets):
''' skelDict specifies a skeleton.
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.
'''
rootMat = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]],
dtype=np.float32)
effectorJoints, effectorOffsets, effectorWeights, usedChannels, 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.where(effectorWeights.reshape(-1) != 0)[0]
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) * 1.0
steps[np.where(jointChans[usedChannels] < 3)[0]] = 30.
delta = np.zeros((numUsedChannels), dtype=np.float32)
JJTB = np.zeros((numEffectors * 12), dtype=np.float32)
Character.pose_skeleton_with_chan_mats(channelMats, Gs, chanValues,
rootMat)
bestScore = ISCV.pose_effectors(effectors, residual, Gs, effectorJoints,
effectorOffsets, effectorWeights,
effectorTargets)
print(bestScore, chanValues[usedChannels])
ISCV.derror_dchannel(derrors, channelMats, usedChannels, usedCAEs,
usedCAEsSplits, jointChans, effectors,
effectorWeights)
JT = derrors.reshape(numUsedChannels, -1)
B = residual.reshape(-1)
residual2 = np.zeros(residual.shape, dtype=np.float32)
# JT[ci] = dresidual_dci
for uci in xrange(numUsedChannels):
ci = usedChannels[uci]
tmp = chanValues[ci]
d_ci = max(0.001, abs(chanValues[ci] * 0.001))
chanValues[ci] += d_ci
Character.pose_skeleton(Gs, skelDict, chanValues, rootMat)
bestScore = ISCV.pose_effectors(effectors, residual2, Gs,
effectorJoints, effectorOffsets,
effectorWeights, effectorTargets)
#print (bestScore)
diff = (residual2.reshape(-1) - B) / -d_ci
if np.dot(JT[uci], diff) / np.sqrt(1e-10 + np.dot(JT[uci], JT[uci]) *
np.dot(diff, diff)) < 0.99:
print(
uci, ci,
np.dot(JT[uci], diff) /
np.sqrt(1e-10 + np.dot(JT[uci], JT[uci]) * np.dot(diff, diff)))
chanValues[ci] = tmp
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)