+ add Levenberg-Marquardt and DogLeg algorithms in freegcs (from ickby)
+ use fallback solvers in Sketch::solve and ask for users feedback + improve tooltip text git-svn-id: https://free-cad.svn.sourceforge.net/svnroot/free-cad/trunk@5112 e8eeb9e2-ec13-0410-a4a9-efa5cf37419d
This commit is contained in:
logari81
parent
6a9dff8129
commit
5f186325e5
|
|
@ -1303,22 +1303,51 @@ int Sketch::addSymmetricConstraint(int geoId1, PointPos pos1, int geoId2, PointP
|
|||
|
||||
int Sketch::solve() {
|
||||
|
||||
if (!isInitMove) {
|
||||
GCSsys.clearByTag(-1);
|
||||
GCSsys.clearByTag(-2);
|
||||
InitParameters.resize(Parameters.size());
|
||||
int i=0;
|
||||
for (std::vector<double*>::iterator it = Parameters.begin(); it != Parameters.end(); ++it, i++) {
|
||||
InitParameters[i] = **it;
|
||||
GCSsys.addConstraintEqual(*it, &InitParameters[i], -2);
|
||||
}
|
||||
GCSsys.initSolution(Parameters);
|
||||
}
|
||||
|
||||
Base::TimeInfo start_time;
|
||||
|
||||
// solving with freegcs
|
||||
int ret = GCSsys.solve();
|
||||
// (either with SQP solver for two subsystems or
|
||||
// with the default DogLeg solver for a single subsystem)
|
||||
int ret = GCSsys.solve(GCS::DogLeg);
|
||||
|
||||
if (ret != GCS::Success && !isInitMove) {
|
||||
// if we are not in dragging mode and the solver fails we try
|
||||
// alternative solvers
|
||||
ret = GCSsys.solve(GCS::BFGS);
|
||||
if (ret == GCS::Success) {
|
||||
Base::Console().Warning("Important: the BFGS solver succeeded where the DogLeg solver had failed.\n");
|
||||
Base::Console().Warning("If you see this message please report a way of reproducing this result at\n");
|
||||
Base::Console().Warning("https://sourceforge.net/apps/mantisbt/free-cad/main_page.php\n");
|
||||
}
|
||||
else {
|
||||
ret = GCSsys.solve(GCS::LevenbergMarquardt);
|
||||
if (ret == GCS::Success) {
|
||||
Base::Console().Warning("Important: the LevenbergMarquardt solver succeeded where the DogLeg and BFGS solvers have failed.\n");
|
||||
Base::Console().Warning("If you see this message please report a way of reproducing this result at\n");
|
||||
Base::Console().Warning("https://sourceforge.net/apps/mantisbt/free-cad/main_page.php\n");
|
||||
} else {
|
||||
// last resort: augment the system with a second subsystem and use the SQP solver
|
||||
GCSsys.clearByTag(-1);
|
||||
GCSsys.clearByTag(-2);
|
||||
InitParameters.resize(Parameters.size());
|
||||
int i=0;
|
||||
for (std::vector<double*>::iterator it = Parameters.begin(); it != Parameters.end(); ++it, i++) {
|
||||
InitParameters[i] = **it;
|
||||
GCSsys.addConstraintEqual(*it, &InitParameters[i], -2);
|
||||
}
|
||||
GCSsys.initSolution(Parameters);
|
||||
|
||||
ret = GCSsys.solve();
|
||||
if (ret == GCS::Success) {
|
||||
Base::Console().Warning("Important: the SQP solver succeeded where all single subsystem solvers have failed.\n");
|
||||
Base::Console().Warning("If you see this message please report a way of reproducing this result at\n");
|
||||
Base::Console().Warning("https://sourceforge.net/apps/mantisbt/free-cad/main_page.php\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// if successfully solve write the parameter back
|
||||
if (ret == GCS::Success) {
|
||||
GCSsys.applySolution();
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
***************************************************************************/
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <cfloat>
|
||||
|
||||
#include "GCS.h"
|
||||
#include "qp_eq.h"
|
||||
|
|
@ -484,13 +485,13 @@ void System::resetToReference()
|
|||
*(it->first) = it->second;
|
||||
}
|
||||
|
||||
int System::solve(VEC_pD ¶ms, bool isFine)
|
||||
int System::solve(VEC_pD ¶ms, bool isFine, Algorithm alg)
|
||||
{
|
||||
initSolution(params);
|
||||
return solve(isFine);
|
||||
return solve(isFine, alg);
|
||||
}
|
||||
|
||||
int System::solve(bool isFine)
|
||||
int System::solve(bool isFine, Algorithm alg)
|
||||
{
|
||||
if (subsys0) {
|
||||
resetToReference();
|
||||
|
|
@ -505,21 +506,31 @@ int System::solve(bool isFine)
|
|||
else if (subsys1)
|
||||
return solve(subsys0, subsys1, isFine);
|
||||
else
|
||||
return solve(subsys0, isFine);
|
||||
return solve(subsys0, isFine, alg);
|
||||
}
|
||||
else if (subsys1) {
|
||||
resetToReference();
|
||||
if (subsys2)
|
||||
return solve(subsys1, subsys2, isFine);
|
||||
else
|
||||
return solve(subsys1, isFine);
|
||||
return solve(subsys1, isFine, alg);
|
||||
}
|
||||
else
|
||||
// return success in order to permit coincidence constraints to be applied
|
||||
return Success;
|
||||
}
|
||||
|
||||
int System::solve(SubSystem *subsys, bool isFine)
|
||||
int System::solve(SubSystem *subsys, bool isFine, Algorithm alg)
|
||||
{
|
||||
if (alg == BFGS)
|
||||
return solve_BFGS(subsys, isFine);
|
||||
else if (alg == LevenbergMarquardt)
|
||||
return solve_LM(subsys);
|
||||
else if (alg == DogLeg)
|
||||
return solve_DL(subsys);
|
||||
}
|
||||
|
||||
int System::solve_BFGS(SubSystem *subsys, bool isFine)
|
||||
{
|
||||
int xsize = subsys->pSize();
|
||||
if (xsize == 0)
|
||||
|
|
@ -594,6 +605,307 @@ int System::solve(SubSystem *subsys, bool isFine)
|
|||
return Failed;
|
||||
}
|
||||
|
||||
int System::solve_LM(SubSystem* subsys)
|
||||
{
|
||||
int xsize = subsys->pSize();
|
||||
int csize = subsys->cSize();
|
||||
|
||||
if (xsize == 0)
|
||||
return Success;
|
||||
|
||||
int itmax = MaxIterations*xsize;
|
||||
|
||||
Eigen::VectorXd e(csize), e_new(csize); // vector of all function errors (every constraint is one function)
|
||||
Eigen::MatrixXd J(csize, xsize); // Jacobi of the subsystem
|
||||
Eigen::VectorXd x(xsize), h(xsize), x_new(xsize), g(xsize);
|
||||
Eigen::MatrixXd A;
|
||||
Eigen::VectorXd diag_A(xsize);
|
||||
|
||||
subsys->redirectParams();
|
||||
|
||||
subsys->getParams(x);
|
||||
subsys->calcResidual(e);
|
||||
e*=-1;
|
||||
|
||||
double eps=1e-10, eps1=1e-80;
|
||||
double tau=1e-3;
|
||||
double nu=2, mu=0;
|
||||
int k=0, stop=0;
|
||||
for (k=0; k < itmax && !stop; ++k) {
|
||||
|
||||
// check error
|
||||
// (logari81) why not using:
|
||||
// if (e.squaredNorm() <= eps) { // error is small
|
||||
if (e.dot(e) <= eps) { // error is small
|
||||
stop=1;
|
||||
break;
|
||||
}
|
||||
|
||||
// J^T J, J^T e
|
||||
subsys->calcJacobi(J);;
|
||||
|
||||
A = J.transpose()*J;
|
||||
g = J.transpose()*e;
|
||||
|
||||
// Compute ||J^T e||_inf
|
||||
double g_inf = -DBL_EPSILON; // (logari81) is this initialization correct?
|
||||
for (int i=0; i < xsize; i++) {
|
||||
if (g_inf < g(i))
|
||||
g_inf = g(i);
|
||||
diag_A(i) = A(i,i); // save diagonal entries so that augmentation can be later canceled
|
||||
}
|
||||
// (logari81) to be replaced with (if max(abs(g)) == max(g)):
|
||||
// g_inf = g.lpNorm<Eigen::Infinity>();
|
||||
// diag_A = A.diagonal();
|
||||
|
||||
// check for convergence
|
||||
if (g_inf <= eps1) {
|
||||
stop=2;
|
||||
break;
|
||||
}
|
||||
|
||||
// compute initial damping factor
|
||||
if (k==0) {
|
||||
double temp=-DBL_EPSILON;
|
||||
for (int i=0; i < xsize; i++) {
|
||||
double z=A(i,i);
|
||||
if (z > temp) temp=z; // find max diagonal element
|
||||
}
|
||||
mu=tau*temp;
|
||||
}
|
||||
// (logari81) to be replaced with (diagonal of A is positive):
|
||||
// if (k==0)
|
||||
// mu = tau * A.diagonal().lpNorm<Eigen::Infinity>();
|
||||
|
||||
// determine increment using adaptive damping
|
||||
while (1) {
|
||||
// augment normal equations A = A+uI
|
||||
for (int i=0; i < xsize; ++i)
|
||||
A(i,i) += mu;
|
||||
// (logari81) to be replaced with:
|
||||
// mu = tau * A.diagonal().lpNorm<Eigen::Infinity>();
|
||||
|
||||
//solve augmented functions A*h=-g
|
||||
h = A.fullPivLu().solve(g);
|
||||
double rel_error = (A*h - g).norm() / g.norm();
|
||||
|
||||
// check if solving workes
|
||||
if (rel_error < 1e-5) {
|
||||
|
||||
// compute par's new estimate and ||d_par||^2
|
||||
x_new = x + h;
|
||||
double h_norm = h.dot(h);
|
||||
// (logari81) why not using:
|
||||
// h_norm = h.squaredNorm();
|
||||
|
||||
if (h_norm <= eps1*(x.norm()*eps1)) { // relative change in p is small, stop
|
||||
// (logari81) why not write:
|
||||
// if (h_norm <= eps1*eps1*x.norm()) { // relative change in p is small, stop
|
||||
stop=3;
|
||||
break;
|
||||
}
|
||||
else if (h_norm >= (x.norm()+eps1)/(DBL_EPSILON*DBL_EPSILON)) { // almost singular
|
||||
stop=4;
|
||||
break;
|
||||
}
|
||||
|
||||
subsys->setParams(x_new);
|
||||
subsys->calcResidual(e_new);
|
||||
e_new *= -1;
|
||||
|
||||
double error_new = e_new.dot(e_new);
|
||||
double dF = e.dot(e) - error_new;
|
||||
// (logari81) why not using:
|
||||
// double error_new = e_new.squaredNorm();
|
||||
// double dF = e.squaredNorm() - error_new;
|
||||
double dL = h.dot(mu*h+g);
|
||||
|
||||
if (dF>0. && dL>0.) { // reduction in error, increment is accepted
|
||||
double tmp=(2.0*dF/dL)-1.0;
|
||||
tmp = 1.0 - pow(tmp, 3);
|
||||
if (tmp <= 1./3.)
|
||||
mu *= 1./3.;
|
||||
else
|
||||
mu *= tmp;
|
||||
// (logari81) to be replaced with:
|
||||
// mu *= std::max(1./3.,tmp);
|
||||
nu=2;
|
||||
|
||||
// update par's estimate
|
||||
x = x_new;
|
||||
e = e_new;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// if this point is reached, either the linear system could not be solved or
|
||||
// the error did not reduce; in any case, the increment must be rejected
|
||||
|
||||
mu*=nu;
|
||||
nu*=2.0;
|
||||
for (int i=0; i < xsize; ++i) // restore diagonal J^T J entries
|
||||
A(i,i)=diag_A(i);
|
||||
}
|
||||
}
|
||||
|
||||
if (k >= itmax)
|
||||
stop = 5;
|
||||
|
||||
subsys->revertParams();
|
||||
|
||||
return (stop == 1) ? Success : Failed;
|
||||
}
|
||||
|
||||
|
||||
int System::solve_DL(SubSystem* subsys)
|
||||
{
|
||||
double tolg=1e-80, tolx=1e-80, tolf=1e-10;
|
||||
double error, error_new;
|
||||
|
||||
int xsize = subsys->pSize();
|
||||
int csize = subsys->cSize();
|
||||
|
||||
int itmax = MaxIterations*xsize;
|
||||
|
||||
Eigen::VectorXd x(xsize), x_new(xsize);
|
||||
Eigen::VectorXd fx(csize), fx_new(csize);
|
||||
Eigen::MatrixXd Jx(csize, xsize), Jx_new(csize, xsize);
|
||||
Eigen::VectorXd g(xsize), h_sd(xsize), h_gn(xsize), h_dl(xsize);
|
||||
|
||||
subsys->redirectParams();
|
||||
|
||||
subsys->getParams(x);
|
||||
subsys->calcResidual(fx, error);
|
||||
subsys->calcJacobi(Jx);
|
||||
|
||||
g = -1*(Jx.transpose()*fx);
|
||||
// (logari81) why not:
|
||||
// g = Jx.transpose()*(-fx);
|
||||
|
||||
// get the infinity norm fx_inf and g_inf
|
||||
double fx_inf=0, g_inf=0;
|
||||
for (int i=0; i < xsize; i++)
|
||||
g_inf = fabs(g(i)) > g_inf ? fabs(g(i)) : g_inf;
|
||||
for (int i=0; i < csize; i++)
|
||||
fx_inf = fabs(fx(i)) > fx_inf ? fabs(fx(i)) : fx_inf;
|
||||
// (logari81) to be replaced with:
|
||||
// double g_inf = g.lpNorm<Eigen::Infinity>();
|
||||
// double f_inf = f.lpNorm<Eigen::Infinity>();
|
||||
|
||||
double delta=0.1;
|
||||
double alpha=0.;
|
||||
double nu=2.;
|
||||
int k=0, stop=0, reduce=0;
|
||||
while (!stop) {
|
||||
|
||||
// check if finished
|
||||
if (fx_inf <= tolf)
|
||||
stop = 1;
|
||||
else if (g_inf <= tolg)
|
||||
stop = 2;
|
||||
else if (delta <= tolx*(tolx + x.norm()))
|
||||
stop = 2;
|
||||
else if (k >= itmax)
|
||||
stop = 4;
|
||||
else {
|
||||
// get the steepest descent direction
|
||||
alpha = pow(g.norm()/(Jx*g).norm() ,2);
|
||||
h_sd = alpha*g;
|
||||
|
||||
// get the gauss-newton step
|
||||
h_gn = Jx.fullPivLu().solve(-1*fx);
|
||||
double rel_error = (Jx*h_gn + fx).norm() / fx.norm();
|
||||
if (rel_error > 1e15)
|
||||
break;
|
||||
|
||||
// compute the dogleg step
|
||||
if (h_gn.norm() < delta) {
|
||||
h_dl = h_gn;
|
||||
if (h_dl.norm() <= tolx*(tolx + x.norm())) {
|
||||
stop = 5;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else if (alpha*g.norm() >= delta) {
|
||||
h_dl = (delta/(alpha*g.norm()))*h_sd;
|
||||
}
|
||||
else {
|
||||
//compute beta
|
||||
double beta = 0;
|
||||
Eigen::VectorXd b = h_gn - h_sd;
|
||||
double bb = (b.transpose()*b).norm();
|
||||
double gb = (h_sd.transpose()*b).norm();
|
||||
double c = (delta + h_sd.norm())*(delta - h_sd.norm());
|
||||
|
||||
if (gb > 0)
|
||||
beta = c / (gb + sqrt(pow(gb,2) + c * bb));
|
||||
else
|
||||
beta = (sqrt(pow(gb,2) + c * bb) - gb)/bb;
|
||||
|
||||
// and update h_dl and dL with beta
|
||||
h_dl = h_sd + beta*b;
|
||||
}
|
||||
}
|
||||
|
||||
// see if we are already finished
|
||||
if (stop)
|
||||
break;
|
||||
|
||||
// get the new values
|
||||
x_new = x + h_dl;
|
||||
subsys->setParams(x_new);
|
||||
subsys->calcResidual(fx_new, error_new);
|
||||
subsys->calcJacobi(Jx_new);
|
||||
|
||||
// calculate the linear model and the update ratio
|
||||
double dL = error - 0.5*pow((fx + Jx*h_dl).norm(),2);
|
||||
double dF = error - error_new;
|
||||
double rho = dL/dF;
|
||||
|
||||
if (dF > 0 && dL > 0) {
|
||||
x = x_new;
|
||||
Jx = Jx_new;
|
||||
fx = fx_new;
|
||||
error = error_new;
|
||||
|
||||
g = -1*(Jx.transpose()*fx);
|
||||
|
||||
// get infinity norms
|
||||
fx_inf = g_inf = 0.;
|
||||
for (int i=0; i < xsize; i++)
|
||||
g_inf = fabs(g(i)) > g_inf ? fabs(g(i)) : g_inf;
|
||||
for (int i=0; i < csize; i++)
|
||||
fx_inf = fabs(fx(i)) > fx_inf ? fabs(fx(i)) : fx_inf;
|
||||
// (logari81) to be replaced with:
|
||||
// g_inf = g.lpNorm<Eigen::Infinity>();
|
||||
// f_inf = f.lpNorm<Eigen::Infinity>();
|
||||
}
|
||||
else
|
||||
rho = -1;
|
||||
|
||||
// update delta
|
||||
if (fabs(rho-1.) < 0.2 && h_dl.norm() > delta/3. && reduce <= 0) {
|
||||
delta = 3*delta;
|
||||
nu = 2;
|
||||
reduce = 0;
|
||||
}
|
||||
else if (rho < 0.25) {
|
||||
delta = delta/nu;
|
||||
nu = 2*nu;
|
||||
reduce = 2;
|
||||
}
|
||||
else
|
||||
reduce--;
|
||||
|
||||
// count this iteration and start again
|
||||
k++;
|
||||
}
|
||||
|
||||
subsys->revertParams();
|
||||
|
||||
return (stop == 1) ? Success : Failed;
|
||||
}
|
||||
|
||||
// The following solver variant solves a system compound of two subsystems
|
||||
// treating the first of them as of higher priority than the second
|
||||
int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool isFine)
|
||||
|
|
|
|||
|
|
@ -38,6 +38,12 @@ namespace GCS
|
|||
Failed = 2 // Failed to find any solution
|
||||
};
|
||||
|
||||
enum Algorithm {
|
||||
BFGS = 0,
|
||||
LevenbergMarquardt = 1,
|
||||
DogLeg = 2
|
||||
};
|
||||
|
||||
class System
|
||||
{
|
||||
// This is the main class. It holds all constraints and information
|
||||
|
|
@ -60,6 +66,10 @@ namespace GCS
|
|||
MAP_pD_pD reductionmap; // for simplification of equality constraints
|
||||
|
||||
bool init;
|
||||
|
||||
int solve_BFGS(SubSystem *subsys, bool isFine);
|
||||
int solve_LM(SubSystem *subsys);
|
||||
int solve_DL(SubSystem *subsys);
|
||||
public:
|
||||
System();
|
||||
System(std::vector<Constraint *> clist_);
|
||||
|
|
@ -117,9 +127,9 @@ namespace GCS
|
|||
|
||||
void initSolution(VEC_pD ¶ms);
|
||||
|
||||
int solve(bool isFine=true);
|
||||
int solve(VEC_pD ¶ms, bool isFine=true);
|
||||
int solve(SubSystem *subsys, bool isFine=true);
|
||||
int solve(bool isFine=true, Algorithm alg=DogLeg);
|
||||
int solve(VEC_pD ¶ms, bool isFine=true, Algorithm alg=DogLeg);
|
||||
int solve(SubSystem *subsys, bool isFine=true, Algorithm alg=DogLeg);
|
||||
int solve(SubSystem *subsysA, SubSystem *subsysB, bool isFine=true);
|
||||
|
||||
void getSubSystems(std::vector<SubSystem *> &subsysvec);
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ CmdSketcherNewSketch::CmdSketcherNewSketch()
|
|||
sAppModule = "Sketcher";
|
||||
sGroup = QT_TR_NOOP("Sketcher");
|
||||
sMenuText = QT_TR_NOOP("Create sketch");
|
||||
sToolTipText = QT_TR_NOOP("Create a new sketch");
|
||||
sToolTipText = QT_TR_NOOP("Create a new or edit the selected sketch");
|
||||
sWhatsThis = sToolTipText;
|
||||
sStatusTip = sToolTipText;
|
||||
sPixmap = "Sketcher_NewSketch";
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user