///-------------------------------------------------------------
/// Calculates gradient for a given frame.
///-------------------------------------------------------------
Vecd IKSolver::CalculateGradient(int frameNum)
{
// setup initial gradient to be zero
// the dimensions should be equal to the number of DOFs we have
Vecd gradient;
gradient.SetSize(mModel->GetDofCount());
gradient.MakeZero();
// loop over all of our constraints, getting Jacobian and
// combining to form final value to add to total gradient
for (int i = 0; i < mConstraintList.size(); i++)
{
// get constraint
Constraint & constraint = mConstraintList[i];
Vec4d constraintVec(constraint.GetConstraintValue(), 1.0);
// get Jacobian - TODO
ConstraintJacobian jacobian(mModel, constraint);
Matd jacobianMatrix = jacobian.CalculateJacobian();
// calculate current value and add to gradient
Vecd currentValue = jacobianMatrix * constraintVec;
gradient = gradient + currentValue;
}
// final computation of doubling after above calculation
gradient = 2.0 * gradient;
return gradient;
}
///-------------------------------------------------------------
/// Saves dofs for later playback
///-------------------------------------------------------------
void IKSolver::SaveDofs(int frameNum)
{
// get dofs
int dofCount = mModel->GetDofCount();
Vecd dofs;
dofs.SetSize(dofCount);
mModel->mDofList.GetDofs(&dofs);
UI->mFrameToDofMap[frameNum] = dofs;
}
void Solution(void *v)
{
Model* model = UI->mData->mSelectedModel;
C3dFileInfo* c3d = model->mOpenedC3dFile;
int numDofs = model->GetDofCount();
int numCons = model->GetHandleCount() * 3;
int frameNum = 0;
double alpha = 0.1;
cout << "The Jacobian will be " << numCons << " by " << numDofs << endl;
for (int i = 0; i < 1000; i++) {
std::vector<Vec4d*> handles = model->ComputeJacobian(frameNum);
Matd J = model->mJacobian;
Matd Jt = trans(J);
Vecd C;
C.SetSize(numCons);
for (int i = 0; i < model->GetHandleCount(); i++) {
for (int j = 0; j < 3; j++) {
// cout << "Prevec " << i << ":" << j << C << endl;
Vec4d v = *(handles[i]);
// cout << "Homogeneous " << v << endl;
double h = v[j]/v[3];
double m = c3d->GetMarkerPos(frameNum, i)[j];
// cout << "Calculated m" << endl;
C[i*3 + j] = h - m;
}
}
// cout << "Objective vector " << C << endl;
Vecd dF = 2 * Jt * C;
Vecd q;
q.SetSize(numDofs);
model->mDofList.GetDofs(&q);
Vecd qnew = q - alpha*dF;
model->SetDofs(qnew);
}
for (int i = 0; i < UI->mData->mSelectedModel->GetDofCount(); i++) {
cout << UI->mData->mSelectedModel->mDofList.mDofs[i]->GetName() << endl;
}
}
void ArticulatedBody::InitModel()
{
mMarkerCount = mHandleList.size();
mNodeCount = mLimbs.size();
Mat4d invHeadMatrix = vl_I;
for(int i = 0; i < ((TransformNode*)mChildren[0])->mTransforms.size(); i++)
invHeadMatrix *= ((TransformNode*)mChildren[0])->mTransforms[i]->GetTransform();
invHeadMatrix = inv(invHeadMatrix);
UpdateUpMatrix(vl_I, invHeadMatrix);
mRoot = (TransformNode*)mChildren[0];
int nDof = GetDofCount();
Vecd dofVec;
dofVec.SetSize(nDof);
mDofList.GetDofs(&dofVec);
SetDofs(dofVec);
mRoot->mParentNode = NULL;
ParentPointer(mRoot);
}
///-------------------------------------------------------------
/// Main solving loop.
/// Loops over all valid frames and performs IK solving.
///-------------------------------------------------------------
void IKSolver::SolveLoop()
{
// clear saved frame data
UI->mFrameToDofMap.clear();
int maxFrames = mModel->mOpenedC3dFile->GetFrameCount();
#ifdef _DEBUG
std::ofstream logFile("logs/loop_log.txt");
logFile << "Num Frames: " << maxFrames << std::endl << std::endl;
//std::ofstream dofFile("logs/dofs.txt");
#endif
// loop over all valid frames
// this is limited by the number of frames in the constraint file, but it can also be
// limited by a max iteration parameter set for the solver
for (int frameCounter = 0; frameCounter < maxFrames /*&& frameCounter < mMaxNumFrames*/; frameCounter++)
{
std::cout << "Starting frame " << frameCounter << std::endl;
// calculate constraint values
CreateConstraints(frameCounter);
CalculateConstraints(frameCounter);
// evaluate objective function
double objectiveFunction = EvaluateObjectiveFunction(frameCounter);
// various variables for our main loop
double localStepSize = mStepSize;
double localEpsilon = mEpsilon;
int iterations = 0;
int decreaseStepCounter = 0;
// main solving loop
while (objectiveFunction > localEpsilon )//&& iterations < mMaxIterations)
{
#ifdef _DEBUG
// print out some information every 30 frames
if (iterations % mPrintFrequency == 0)
{
std::cout << "Iteration " << iterations << "\tObjective: " << objectiveFunction
<< "\tStep: " << localStepSize << "\tEpsilon: " << localEpsilon << std::endl;
}
#endif
#ifdef _DEBUG
if (iterations % mPrintFrequency == 0)
{
logFile << "Frame: " << frameCounter << std::endl;
logFile << "Iteration: " << iterations << std::endl;
logFile << "Step Size: " << localStepSize << std::endl;
logFile << "Epsilon: " << localEpsilon << std::endl;
logFile << "Objective Function: " << objectiveFunction << std::endl;
}
#endif
// calculate gradient
Vecd gradient = CalculateGradient(frameCounter);
// get old dofs
Vecd oldDofs;
oldDofs.SetSize(mModel->GetDofCount());
mModel->mDofList.GetDofs(&oldDofs);
// move dofs
Vecd newDofs = oldDofs - localStepSize * gradient;
// update dofs
mModel->SetDofs(newDofs);
#ifdef _DEBUG
if (iterations % mPrintFrequency == 0)
{
logFile << "Gradient: " << gradient << std::endl;
logFile << "Old Dofs: " << oldDofs << std::endl;
logFile << "New Dofs: " << newDofs << std::endl;
logFile << std::endl;
}
#endif
// calculate new constraint values
CalculateConstraints(frameCounter);
// calculate new objective function value
double newObjectiveFunction = EvaluateObjectiveFunction(frameCounter);
// make sure we always decrease so that we don't get stuck in some infinite loop
if (newObjectiveFunction < objectiveFunction)
{
// adaptive step size
// if difference between objective functions is greater than epsilon
// and certain number of iterations have passed, increase step size
if (newObjectiveFunction > mEpsilon &&
iterations > 0 && iterations % mStepIncreaseFrequency == 0)
{
localStepSize *= mStepIncreaseFactor;
#ifdef _DEBUG
std::cout << "Increased step size to " << localStepSize << std::endl;
#endif
}
objectiveFunction = newObjectiveFunction;
}
else
{
// count how many times we've decrease step this frame
decreaseStepCounter++;
// decrease step size
localStepSize /= mStepDecreaseFactor;
#ifdef _DEBUG
std::cout << "Decreased step size to " << localStepSize << std::endl;
#endif
// based on how many times we've had to decrease the step, choose different options
if (decreaseStepCounter < mMaxIterations)
{
// reset to old dofs and try again
mModel->SetDofs(oldDofs);
}
else if (decreaseStepCounter < mEpsilonIncreaseFrequency*mMaxIterations)
{
// try increasing epsilon
localEpsilon *= mEpsilonIncreaseFactor;
#ifdef _DEBUG
std::cout << "Increased epsilon to " << localEpsilon << std::endl;
#endif
// reset to old dofs and try again
mModel->SetDofs(oldDofs);
}
else
{
// at this point, give up; it isn't worth it
objectiveFunction = 0.0;
#ifdef _DEBUG
std::cout << "Too many iterations; moving on..." << std::endl;
#endif
}
}
// update iteration counter
iterations++;
}
UI->mFrameCounter_cou->value(frameCounter); // update frame counter
SaveDofs(frameCounter); // save dofs for later playback
#ifdef _DEBUG
// commented out because it slows things down more than really necessary
// and same info can be placed into main log file
// write dofs to file
//Vecd dofs;
//dofs.SetSize(mModel->GetDofCount());
//mModel->mDofList.GetDofs(&dofs);
//dofFile << frameCounter << std::endl << dofs << std::endl << std::endl;
#endif
UI->mGLWindow->flush(); // update screen
std::cout << "Ending frame " << frameCounter;
#ifdef _DEBUG
std::cout << " after " << iterations << " iterations";
#endif
std::cout << std::endl;
}
#ifdef _DEBUG
//dofFile.close();
logFile.close();
#endif
}
void Solver::solve() {
// Utility constants
const int sIFreq = 20;
const double sIF = 4.0;
const double sDF = 2.0;
const int eIFreq = 2;
const double eIF = 2.0;
// Loop over all valid frames
for (int f = 0; f < mMaxFrames; f++)
{
// Build C[]
buildConstraintMat(f);
//Objective function value
double o = 0.0;
Markers &handles = mModel->mHandleList;
for (int i = 0; i < constraintIDs.size(); i++)
{
// Get handle and target position for the given constraint
Marker *h = handles[constraintIDs[i]];
Vec3d cPos = mModel->mOpenedC3dFile->GetMarkerPos(f, constraintIDs[i]);
// Calculate values and store
Vec3d cVal = h->mGlobalPos - cPos;
double cLength = sqrlen(cVal);
//Store results
constraintVals.push_back(cVal);
constraintLengths.push_back(cLength);
//Update objective function
o += cLength;
}
// Main loop
double e = mEps;
double s = mStep;
int iterCount = 0;
int stepCount = 0;
while (o > e) //while Objective Function > Epsilon
{
// Compute gradient -- TODO: Move this somewhere else for the sake of keeping this compact!
Vecd gradient;
gradient.SetSize(mModel->GetDofCount());
gradient.MakeZero();
for (int i = 0; i < constraintIDs.size(); i++)
{
//Get Constraint
Vec4d cVec(constraintVals[i], 1.0);
//Get Jacobian
Matd J = computeJ(mModel->mHandleList, constraintIDs[i]);
//Calculate current and add to gradient matrix
Vecd currentVal = J * cVec;
gradient = gradient + currentVal;
}
// Finally, double it
gradient = 2.0 * gradient;
// Get previous DOFs
Vecd prevDofs;
prevDofs.SetSize(mModel->GetDofCount());
mModel->mDofList.GetDofs(&prevDofs);
// Update DOFs
Vecd nextDofs = prevDofs - s * gradient;
mModel->SetDofs(nextDofs);
// Recalculate constraints and objective function
double newO = 0.0;
Markers &handles = mModel->mHandleList;
for (int i = 0; i < constraintIDs.size(); i++)
{
// Get handle and target position for the given constraint
Marker *h = handles[constraintIDs[i]];
Vec3d cPos = mModel->mOpenedC3dFile->GetMarkerPos(f, constraintIDs[i]);
// Calculate values and store
Vec3d cVal = h->mGlobalPos - cPos;
double cLength = sqrlen(cVal);
//Store results
constraintVals.push_back(cVal);
constraintLengths.push_back(cLength);
//Update objective function
newO += cLength;
}
// Attempting to make sure we don't go infinite...
if (newO < o)
{
if (newO > mEps && iterCount > 0 && iterCount % 20 == 0)
{
s *= 3.0;
}
o = newO;
}
else
{
stepCount++;
// Decrease step size
s = s / 2.0;
// based on how many times we've had to decrease the step size, choose different options
if (stepCount < mMaxIters)
{
// Reset to the previous dofs and try again
mModel->SetDofs(prevDofs);
}
else if (stepCount < eIFreq*mMaxIters)
{
// Try increasing epsilon
e *= 3.0;
// Reset to the previous dofs and try again
mModel->SetDofs(prevDofs);
}
else
{
// Otherwise, halt or we will never finish
o = 0.0;
}
}
iterCount++;
}
// Update the frame counter
UI->mFrameCounter_cou->value(f);
//Refresh the screen
UI->mGLWindow->flush();
}
}
