/*!
* \brief affine3d2UrdfPose converts an Eigen affine 4x4 matrix o represent the pose into a urdf pose
* vparam pose eigen Affine3d pose
* \return urdf pose with position and rotation.
*/
RCS::Pose Affine3d2UrdfPose(const Eigen::Affine3d &pose) {
RCS::Pose p;
p.getOrigin().setX(pose.translation().x());
p.getOrigin().setY(pose.translation().y());
p.getOrigin().setZ(pose.translation().z());
Eigen::Quaterniond q (pose.rotation());
tf::Quaternion qtf(q.x(),q.y(),q.z(),q.w());
//std::cout << "Affine3d2UrdfPose Quaterion = \n" << q.x() << ":" << q.y() << ":" << q.z() << ":" << q.w() << std::endl;
p.setRotation(qtf);
//std::cout << "After Affine3d2UrdfPose Quaterion = \n" << p.getRotation().x() << ":" << p.getRotation().y() << ":" << p.getRotation().z() << ":" << p.getRotation().w() << std::endl;
#if 0
MatrixEXd m = pose.rotation();
Eigen::Quaterniond q = EMatrix2Quaterion(m);
Eigen::Quaterniond q(pose.rotation());
p.getRotation().setX(q.x());
p.getRotation().setY(q.y());
p.getRotation().setZ(q.z());
p.getRotation().setW(q.w());
#endif
return p;
}
EndCriteria::Type LevenbergMarquardt::minimize(Problem& P,
const EndCriteria& endCriteria) {
EndCriteria::Type ecType = EndCriteria::None;
P.reset();
Array x_ = P.currentValue();
currentProblem_ = &P;
initCostValues_ = P.costFunction().values(x_);
int m = initCostValues_.size();
int n = x_.size();
boost::scoped_array<double> xx(new double[n]);
std::copy(x_.begin(), x_.end(), xx.get());
boost::scoped_array<double> fvec(new double[m]);
boost::scoped_array<double> diag(new double[n]);
int mode = 1;
double factor = 1;
int nprint = 0;
int info = 0;
int nfev =0;
boost::scoped_array<double> fjac(new double[m*n]);
int ldfjac = m;
boost::scoped_array<int> ipvt(new int[n]);
boost::scoped_array<double> qtf(new double[n]);
boost::scoped_array<double> wa1(new double[n]);
boost::scoped_array<double> wa2(new double[n]);
boost::scoped_array<double> wa3(new double[n]);
boost::scoped_array<double> wa4(new double[m]);
// requirements; check here to get more detailed error messages.
QL_REQUIRE(n > 0, "no variables given");
QL_REQUIRE(m >= n,
"less functions (" << m <<
") than available variables (" << n << ")");
QL_REQUIRE(endCriteria.functionEpsilon() >= 0.0,
"negative f tolerance");
QL_REQUIRE(xtol_ >= 0.0, "negative x tolerance");
QL_REQUIRE(gtol_ >= 0.0, "negative g tolerance");
QL_REQUIRE(endCriteria.maxIterations() > 0,
"null number of evaluations");
// call lmdif to minimize the sum of the squares of m functions
// in n variables by the Levenberg-Marquardt algorithm.
MINPACK::LmdifCostFunction lmdifCostFunction =
boost::bind(&LevenbergMarquardt::fcn, this, _1, _2, _3, _4, _5);
MINPACK::lmdif(m, n, xx.get(), fvec.get(),
static_cast<double>(endCriteria.functionEpsilon()),
static_cast<double>(xtol_),
static_cast<double>(gtol_),
static_cast<int>(endCriteria.maxIterations()),
static_cast<double>(epsfcn_),
diag.get(), mode, factor,
nprint, &info, &nfev, fjac.get(),
ldfjac, ipvt.get(), qtf.get(),
wa1.get(), wa2.get(), wa3.get(), wa4.get(),
lmdifCostFunction);
info_ = info;
// check requirements & endCriteria evaluation
QL_REQUIRE(info != 0, "MINPACK: improper input parameters");
//QL_REQUIRE(info != 6, "MINPACK: ftol is too small. no further "
// "reduction in the sum of squares "
// "is possible.");
if (info != 6) ecType = QuantLib::EndCriteria::StationaryFunctionValue;
//QL_REQUIRE(info != 5, "MINPACK: number of calls to fcn has "
// "reached or exceeded maxfev.");
endCriteria.checkMaxIterations(nfev, ecType);
QL_REQUIRE(info != 7, "MINPACK: xtol is too small. no further "
"improvement in the approximate "
"solution x is possible.");
QL_REQUIRE(info != 8, "MINPACK: gtol is too small. fvec is "
"orthogonal to the columns of the "
"jacobian to machine precision.");
// set problem
std::copy(xx.get(), xx.get()+n, x_.begin());
P.setCurrentValue(x_);
P.setFunctionValue(P.costFunction().value(x_));
return ecType;
}
