added qr solver to newton

NSolver/NewtonSolver.cc

@@ -121,7 +121,11 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
 {
   DEB_time_func_def;
 
-  const double KKT_res_eps = 1e-6;
+  // hack
+  solver_type_ = LS_SPQR;
+//  solver_type_ = LS_EigenQR;
+
+  const double KKT_res_eps = 1e-8;
 
   // number of unknowns
   size_t n = _problem->n_unknowns();
@@ -146,39 +150,51 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
   lu_solver_.isSymmetric(true);
 
   // start with no regularization
-  double regularize_hessian(0.0);
-  double regularize_constraints(0.0);
   int iter=0;
-  bool first_factorization = true;
   while( iter < max_iters_)
   {
+    bool qr_already_active = (solver_type_ == LS_SPQR);
     double kkt_res2(0.0);
-    do
+
+    // get Newton search direction by solving LSE
+    if(!factorize(_problem, _A, _b, x))
     {
-      // get Newton search direction by solving LSE
-      factorize(_problem, _A, _b, x, regularize_hessian, regularize_constraints, first_factorization);
-      first_factorization = false;
+      DEB_line(2, "KKT system could not be factored -> fallback to QR Solver");
+      solver_type_ = LS_SPQR;
+      if(qr_already_active || !factorize(_problem, _A, _b, x))
+      {
+        DEB_line(2, "KKT system could not be factored by QR -> quit with suboptimal solution!");
+        return 0;
+      }
+      qr_already_active = true;
+    }
 
-      // get rhs
-      _problem->eval_gradient(x.data(), g.data());
-      rhs.head(n) = -g;
-      rhs.tail(m) = _b - _A*x;
+    // 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();
-      if(kkt_res2 > KKT_res_eps)
+    // check numerical stability of KKT system and regularize if necessary
+    kkt_res2 = (KKT_*dx-rhs).squaredNorm();
+    if(kkt_res2 > KKT_res_eps)
+    {
+      DEB_line(2, "numerical issues in KKT system with residual^2 " << kkt_res2 << " -> fallback to QR solver");
+
+      // try again with QR solver
+      solver_type_ = LS_SPQR;
+      if( qr_already_active ||
+          !factorize(_problem, _A, _b, x) ||
+          !solve_kkt_system(rhs, dx)
+          || (kkt_res2 = (KKT_*dx-rhs).squaredNorm()) > 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;
-        else
-          regularize_hessian *= 2.0;
+        DEB_line(2, "KKT system could not be factored by QR -> quit with suboptimal solution!");
+        return 0;
       }
+      qr_already_active = true;
     }
-    while(kkt_res2 > KKT_res_eps && regularize_hessian < 1.0);
 
     // get maximal reasonable step
     double t_max  = std::min(1.0, 
@@ -192,6 +208,8 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
     // perform update
     x += dx.head(n)*t;
 
+    std::cerr << int(solver_type_) << std::endl;
+
     DEB_line(2, "iter: " << iter
       << ", f(x) = " << fx << ", t = " << t << " (tmax=" << t_max << ")"
       << (t < t_max ? " _clamped_" : "")
@@ -218,9 +236,8 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
 //-----------------------------------------------------------------------------
 
 
-void NewtonSolver::factorize(NProblemInterface* _problem, 
-  const SMatrixD& _A, const VectorD& _b, const VectorD& _x, double& _regularize_hessian, double& _regularize_constraints,
-  const bool _first_factorization)
+bool NewtonSolver::factorize(NProblemInterface* _problem,
+  const SMatrixD& _A, const VectorD& _b, const VectorD& _x)
 {
   DEB_enter_func;
   const int n  = _problem->n_unknowns();
@@ -252,70 +269,15 @@ void NewtonSolver::factorize(NProblemInterface* _problem,
       trips.push_back(Triplet(it.col(),it.row()+n,it.value()));
     }
 
-  // regularize constraints
-  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)
-  {
-    double ad(0.0);
-    for( int i=0; i<n; ++i)
-      ad += H.coeffRef(i,i);
-    ad *= _regularize_hessian/double(n);
-    for( int i=0; i<n; ++i)
-      trips.push_back(Triplet(i,i,ad));
-  }
-
   // create KKT matrix
   KKT_.resize(nf,nf);
   KKT_.setFromTriplets(trips.begin(), trips.end());
 
   // compute LU factorization
-  if(_first_factorization)
+//  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_);
 }
 
 
@@ -365,17 +327,33 @@ double NewtonSolver::backtracking_line_search(NProblemInterface* _problem,
 //-----------------------------------------------------------------------------
 
 
-void NewtonSolver::analyze_pattern(SMatrixD& _KKT)
+bool NewtonSolver::analyze_pattern(SMatrixD& _KKT)
 {
   DEB_enter_func;
   switch(solver_type_)
   {
-    case LS_EigenLU:      lu_solver_.analyzePattern(_KKT); break;
+    case LS_EigenLU:
+      lu_solver_.analyzePattern(_KKT);
+      return (lu_solver_.info() == Eigen::Success);
+
+    case LS_EigenQR:
+      qr_solver_.analyzePattern(_KKT);
+      return (qr_solver_.info() == Eigen::Success);
+
 #if COMISO_SUITESPARSE_AVAILABLE
-    case LS_Umfpack: umfpack_solver_.analyzePattern(_KKT); break;
+    case LS_Umfpack:
+      umfpack_solver_.analyzePattern(_KKT);
+      return (umfpack_solver_.info() == Eigen::Success);
+
+    case LS_SPQR:
+      return 1;
+//      spqr_solver_.analyzePattern(_KKT);
+//      return (spqr_solver_.info() == Eigen::Success);
 #endif
     default: DEB_warning(1, "selected linear solver not availble");
   }
+
+  return true;
 }
 
 
@@ -390,10 +368,19 @@ bool NewtonSolver::numerical_factorization(SMatrixD& _KKT)
     case LS_EigenLU:      
       lu_solver_.factorize(_KKT); 
       return (lu_solver_.info() == Eigen::Success);
+
+    case LS_EigenQR:
+      qr_solver_.factorize(_KKT);
+      return (qr_solver_.info() == Eigen::Success);
+
 #if COMISO_SUITESPARSE_AVAILABLE
     case LS_Umfpack: 
       umfpack_solver_.factorize(_KKT); 
       return (umfpack_solver_.info() == Eigen::Success);
+
+    case LS_SPQR:
+      spqr_solver_.compute(_KKT);
+      return (spqr_solver_.info() == Eigen::Success);
 #endif
     default: 
       DEB_warning(1, "selected linear solver not availble!"); 
@@ -405,14 +392,28 @@ bool NewtonSolver::numerical_factorization(SMatrixD& _KKT)
 //-----------------------------------------------------------------------------
 
 
-void NewtonSolver::solve_kkt_system(const VectorD& _rhs, VectorD& _dx)
+bool NewtonSolver::solve_kkt_system(const VectorD& _rhs, VectorD& _dx)
 {
   DEB_enter_func;
   switch(solver_type_)
   {
-    case LS_EigenLU: _dx =      lu_solver_.solve(_rhs); break;
+    case LS_EigenLU:
+      _dx = lu_solver_.solve(_rhs);
+      return (lu_solver_.info() == Eigen::Success);
+
+    case LS_EigenQR:
+      _dx = qr_solver_.solve(_rhs);
+      return (qr_solver_.info() == Eigen::Success);
+
 #if COMISO_SUITESPARSE_AVAILABLE
-    case LS_Umfpack: _dx = umfpack_solver_.solve(_rhs); break;
+    case LS_Umfpack:
+      _dx = umfpack_solver_.solve(_rhs);
+      return (umfpack_solver_.info() == Eigen::Success);
+
+    case LS_SPQR:
+      _dx = spqr_solver_.solve(_rhs);
+      return (spqr_solver_.info() == Eigen::Success);
+
 #endif
     default: DEB_warning(1, "selected linear solver not availble"); break;
   }

NSolver/NewtonSolver.hh

@@ -18,11 +18,15 @@
 #include "NProblemInterface.hh"
 #include "NProblemGmmInterface.hh"
 
+#include <Eigen/Sparse>
+#include <Eigen/SparseQR>
+
 //#include <Base/Debug/DebTime.hh>
 
 #if COMISO_SUITESPARSE_AVAILABLE
   #include <Eigen/UmfPackSupport>
   #include <Eigen/CholmodSupport>
+  #include <Eigen/SPQRSupport>
 #endif
 
 // ToDo: why is Metis not working yet?
@@ -50,7 +54,7 @@ class COMISODLLEXPORT NewtonSolver
 {
 public:
 
-  enum LinearSolver {LS_EigenLU, LS_Umfpack, LS_MUMPS};
+  enum LinearSolver {LS_EigenLU, LS_EigenQR, LS_Umfpack, LS_MUMPS, LS_SPQR};
 
   typedef Eigen::VectorXd             VectorD;
   typedef Eigen::SparseMatrix<double> SMatrixD;
@@ -89,19 +93,18 @@ public:
 
 protected:
 
-  void factorize(NProblemInterface* _problem, const SMatrixD& _A, 
-    const VectorD& _b, const VectorD& _x, double& _regularize_hessian,
-    double& _regularize_constraints, const bool _first_factorization);
+  bool factorize(NProblemInterface* _problem, const SMatrixD& _A,
+    const VectorD& _b, const VectorD& _x );
 
   double backtracking_line_search(NProblemInterface* _problem, VectorD& _x, 
     VectorD& _g, VectorD& _dx, double& _newton_decrement, double& _fx, 
     const double _t_start = 1.0);
 
-  void analyze_pattern(SMatrixD& _KKT);
+  bool analyze_pattern(SMatrixD& _KKT);
 
   bool numerical_factorization(SMatrixD& _KKT);
 
-  void solve_kkt_system(const VectorD& _rhs, VectorD& _dx);
+  bool solve_kkt_system(const VectorD& _rhs, VectorD& _dx);
 
   // deprecated function!
   // solve
@@ -146,8 +149,13 @@ private:
   // Sparse LU decomposition
   Eigen::SparseLU<SMatrixD> lu_solver_;
 
+  // Sparse QR decomposition
+  Eigen::SparseQR<SMatrixD, Eigen::AMDOrdering<int>> qr_solver_;
+
+
 #if COMISO_SUITESPARSE_AVAILABLE
   Eigen::UmfPackLU<SMatrixD> umfpack_solver_;
+  Eigen::SPQR<SMatrixD> spqr_solver_;
 #endif
 
   // deprecated