+ 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

@@ -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();

src/Mod/Sketcher/App/freegcs/GCS.cpp

@@ -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 &params, bool isFine)
+int System::solve(VEC_pD &params, 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)

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<Constraint *> clist_);
@@ -117,9 +127,9 @@ namespace GCS
 
         void initSolution(VEC_pD &params);
 
-        int solve(bool isFine=true);
+        int solve(bool isFine=true, Algorithm alg=DogLeg);
-        int solve(VEC_pD &params, bool isFine=true);
+        int solve(VEC_pD &params, bool isFine=true, Algorithm alg=DogLeg);
-        int solve(SubSystem *subsys, bool isFine=true);
+        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);

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