From 5f186325e502502a734b5cec222da8da96e6a6c0 Mon Sep 17 00:00:00 2001 From: logari81 Date: Thu, 10 Nov 2011 18:29:52 +0000 Subject: [PATCH] + 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 --- src/Mod/Sketcher/App/Sketch.cpp | 55 +++-- src/Mod/Sketcher/App/freegcs/GCS.cpp | 324 ++++++++++++++++++++++++++- src/Mod/Sketcher/App/freegcs/GCS.h | 16 +- src/Mod/Sketcher/Gui/Command.cpp | 2 +- 4 files changed, 374 insertions(+), 23 deletions(-) diff --git a/src/Mod/Sketcher/App/Sketch.cpp b/src/Mod/Sketcher/App/Sketch.cpp index 6b5ce7e647..54c53a07de 100644 --- a/src/Mod/Sketcher/App/Sketch.cpp +++ b/src/Mod/Sketcher/App/Sketch.cpp @@ -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::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::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(); diff --git a/src/Mod/Sketcher/App/freegcs/GCS.cpp b/src/Mod/Sketcher/App/freegcs/GCS.cpp index 0bacc4927e..8ace3da19e 100644 --- a/src/Mod/Sketcher/App/freegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/freegcs/GCS.cpp @@ -21,6 +21,7 @@ ***************************************************************************/ #include #include +#include #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(); +// 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(); + + // 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(); + + //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(); +// double f_inf = f.lpNorm(); + + 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(); +// f_inf = f.lpNorm(); + } + 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) diff --git a/src/Mod/Sketcher/App/freegcs/GCS.h b/src/Mod/Sketcher/App/freegcs/GCS.h index aa2dc981ad..8ba89a29e6 100644 --- a/src/Mod/Sketcher/App/freegcs/GCS.h +++ b/src/Mod/Sketcher/App/freegcs/GCS.h @@ -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 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 &subsysvec); diff --git a/src/Mod/Sketcher/Gui/Command.cpp b/src/Mod/Sketcher/Gui/Command.cpp index 752fdb3762..c9eda945ec 100644 --- a/src/Mod/Sketcher/Gui/Command.cpp +++ b/src/Mod/Sketcher/Gui/Command.cpp @@ -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";