alternating regularization for NewtonSolver

NSolver/NewtonSolver.cc

@@ -153,32 +153,58 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
   while( iter < max_iters_)
   {
     double kkt_res2(0.0);
+    int    reg_iters(0);
     do
     {
       // get Newton search direction by solving LSE
-      factorize(_problem, _A, _b, x, regularize_hessian, regularize_constraints, first_factorization);
+      bool fact_ok = factorize(_problem, _A, _b, x, regularize_hessian, regularize_constraints, first_factorization);
       first_factorization = false;
 
-      // get rhs
-      _problem->eval_gradient(x.data(), g.data());
-      rhs.head(n) = -g;
-      rhs.tail(m) = _b - _A*x;
+      if(fact_ok)
+      {
+        // get rhs
+        _problem->eval_gradient(x.data(), g.data());
+        rhs.head(n) = -g;
+        rhs.tail(m) = _b - _A*x;
+
+        // solve KKT system
+        solve_kkt_system(rhs, dx);
 
-      // solve KKT system
-      solve_kkt_system(rhs, dx);
+        // check numerical stability of KKT system and regularize if necessary
+        kkt_res2 = (KKT_*dx-rhs).squaredNorm();
+      }
 
-      // check numerical stability of KKT system and regularize if necessary
-      kkt_res2 = (KKT_*dx-rhs).squaredNorm();
-      if(kkt_res2 > KKT_res_eps)
+      if(!fact_ok || kkt_res2 > KKT_res_eps)
       {
-        DEB_line(2, "numerical issues in KKT system with residual^2 " << kkt_res2 << " -> regularize hessian");
-        if(regularize_hessian == 0.0)
-          regularize_hessian = 1e-5;
+        // alternatingly regularize hessian and constraints
+        if(reg_iters % 2 == 0)
+        {
+          DEB_line(2, "numerical issues in KKT system with residual^2 " << kkt_res2 << " -> regularize hessian");
+          if(regularize_hessian == 0.0)
+            regularize_hessian = 1e-6;
+          else
+            regularize_hessian *= 2.0;
+        }
         else
-          regularize_hessian *= 2.0;
+        {
+          DEB_line(2, "numerical issues in KKT system with residual^2 " << kkt_res2 << " -> regularize constraints");
+          if(regularize_constraints == 0.0)
+            regularize_constraints = 1e-8;
+          else
+            regularize_constraints *= 2.0;
+        }
       }
+      ++reg_iters;
+    }
+    while(kkt_res2 > KKT_res_eps && reg_iters < 10);
+
+    // no valid step could be found?
+    if(kkt_res2 > KKT_res_eps || reg_iters >= 10)
+    {
+      DEB_line(2, "numerical issues in KKT system could not be resolved -> terminating NewtonSolver with current solution");
+      _problem->store_result(x.data());
+      return 0;
     }
-    while(kkt_res2 > KKT_res_eps && regularize_hessian < 1.0);
 
     // get maximal reasonable step
     double t_max  = std::min(1.0, 
@@ -218,7 +244,7 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
 //-----------------------------------------------------------------------------
 
 
-void NewtonSolver::factorize(NProblemInterface* _problem, 
+bool NewtonSolver::factorize(NProblemInterface* _problem,
   const SMatrixD& _A, const VectorD& _b, const VectorD& _x, double& _regularize_hessian, double& _regularize_constraints,
   const bool _first_factorization)
 {
@@ -253,12 +279,12 @@ void NewtonSolver::factorize(NProblemInterface* _problem,
     }
 
   // regularize constraints
-  if(_regularize_constraints != 0.0)
+//  if(_regularize_constraints != 0.0)
     for( int i=0; i<m; ++i)
       trips.push_back(Triplet(n+i,n+i,_regularize_constraints));
 
   // regularize Hessian
-  if(_regularize_hessian != 0.0)
+//  if(_regularize_hessian != 0.0)
   {
     double ad(0.0);
     for( int i=0; i<n; ++i)
@@ -276,46 +302,7 @@ void NewtonSolver::factorize(NProblemInterface* _problem,
   if(_first_factorization)
     analyze_pattern(KKT_);
 
-  bool success = numerical_factorization(KKT_);
-
-  if(!success)
-  {
-    // ToDo: one round of regularization always sufficient?
-
-    double reg_h_old = _regularize_hessian;
-    double reg_c_old = _regularize_constraints;
-
-    // add more regularization
-    if(_regularize_hessian == 0.0)
-      _regularize_hessian = 1e-5;
-    else
-      _regularize_hessian *= 2.0;
-
-    if(_regularize_constraints == 0.0)
-      _regularize_constraints = 1e-8;
-    else
-      _regularize_constraints *= 2.0;
-
-
-    // print information
-    DEB_line(2, "Linear Solver reported problem while factoring KKT system: ");
-    DEB_line_if(solver_type_ == LS_EigenLU, 2, lu_solver_.lastErrorMessage());
-
-    // regularize full system
-    for( int i=0; i<n; ++i)
-      trips.push_back(Triplet(i,i,_regularize_hessian-reg_h_old));
-    for( int i=0; i<m; ++i)
-      trips.push_back(Triplet(n+i,n+i,_regularize_constraints-reg_c_old));
-
-    // create KKT matrix
-    KKT_.setFromTriplets(trips.begin(), trips.end());
-
-    // compute LU factorization
-    analyze_pattern(KKT_);
-    numerical_factorization(KKT_);
-
-//      IGM_THROW_if(lu_solver_.info() != Eigen::Success, QGP_BOUNDED_DISTORTION_FAILURE);
-  }
+  return numerical_factorization(KKT_);
 }
 
 

NSolver/NewtonSolver.hh

@@ -89,7 +89,7 @@ public:
 
 protected:
 
-  void factorize(NProblemInterface* _problem, const SMatrixD& _A, 
+  bool factorize(NProblemInterface* _problem, const SMatrixD& _A,
     const VectorD& _b, const VectorD& _x, double& _regularize_hessian,
     double& _regularize_constraints, const bool _first_factorization);