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; }