int SkeletonFitting::funcSize(void* p, int m, int n, const double* x, double* fvec, int iflag)
{
if (!p)
throw Exception("invalid data");
Payload* thisPayload = (Payload*)p;
Joint* joint = (JointHinge*)thisPayload->joint;
// compute energy
joint->addBoneSize(x[0]);
joint->updateForward(false);
fvec[0] = joint->computeForwardEnergy();
// reset
joint->addBoneSize(-x[0]);
joint->updateForward(false);
return iflag;
}
int SkeletonFitting::funcPos(void* p, int m, int n, const double* x, double* fvec, int iflag)
{
if (!p)
throw Exception("invalid data");
Payload* thisPayload = (Payload*)p;
Joint* joint = thisPayload->joint;
// store state
cv::Point3d pos = joint->getPos3d();
switch (thisPayload->axis) {
case 0:
joint->addPos3d(cv::Point3d(x[0], 0, 0));
break;
case 1:
joint->addPos3d(cv::Point3d(0, x[0], 0));
break;
case 2:
joint->addPos3d(cv::Point3d(0, 0, x[0]));
break;
default:
throw Exception("invalid parameter");
break;
}
// compute energy
joint->updateForward(false);
fvec[0] = joint->computeForwardEnergy();
// reset
joint->setPos3d(pos);
joint->updateForward(false);
return iflag;
}