Merge branch 'merge-from-ReForm' into 'master'

Merge from re form

See merge request !19

NSolver/ConstraintTools.cc

@@ -7,6 +7,8 @@
 
 #include <CoMISo/Utils/MutablePriorityQueueT.hh>
 
+#include <Base/Debug/DebOut.hh>
+
 
 
 namespace COMISO {
@@ -55,6 +57,7 @@ void
 ConstraintTools::
 remove_dependent_linear_constraints_only_linear_equality( std::vector<NConstraintInterface*>& _constraints, const double _eps)
 {
+  DEB_enter_func;
   // make sure that constraints are available
   if(_constraints.empty()) return;
 
@@ -158,7 +161,8 @@ remove_dependent_linear_constraints_only_linear_equality( std::vector<NConstrain
     }
   }
 
-  std::cerr << "removed " << _constraints.size()-keep.size() << " dependent linear constraints out of " << _constraints.size() << std::endl;
+  DEB_line(2, "removed " << _constraints.size()-keep.size() << 
+    " dependent linear constraints out of " << _constraints.size());
 
   // 4. store result
   std::vector<NConstraintInterface*> new_constraints;

NSolver/NewtonSolver.cc

@@ -8,7 +8,7 @@
 
 #include "NewtonSolver.hh"
 #include <CoMISo/Solver/CholmodSolver.hh>
-
+#include <Base/Debug/DebTime.hh>
 //== NAMESPACES ===============================================================
 
 namespace COMISO {
@@ -21,6 +21,7 @@ int
 NewtonSolver::
 solve(NProblemGmmInterface* _problem)
 {
+  DEB_enter_func;
 #if COMISO_SUITESPARSE_AVAILABLE  
   
   // get problem size
@@ -44,8 +45,7 @@ solve(NProblemGmmInterface* _problem)
     // check for convergence
     if( gmm::vect_norm2(g) < eps_)
     {
-      std::cerr << "Newton Solver converged after "
-                << i << " iterations" << std::endl;
+      DEB_line(2, "Newton Solver converged after " << i << " iterations");
       _problem->store_result(P(x));
       return true;
     }
@@ -79,7 +79,7 @@ solve(NProblemGmmInterface* _problem)
           f = f_new;
           improvement = true;
 
-          std::cerr << "energy improved to " << f << std::endl;
+          DEB_line(2, "energy improved to " << f);
         }
       }
 
@@ -96,18 +96,18 @@ solve(NProblemGmmInterface* _problem)
       else
       {
         _problem->store_result(P(x));
-        std::cerr << "Newton solver reached max regularization but did not converge ... " << std::endl;
+        DEB_line(2, "Newton solver reached max regularization but did not "
+          "converge");
         return false;
       }
     }
   }
   _problem->store_result(P(x));
-  std::cerr << "Newton Solver did not converge!!! after "
-            << max_iters_ << " iterations." << std::endl;
+  DEB_line(2, "Newton Solver did not converge!!! after iterations.");
   return false;
 
 #else
-  std::cerr << "Warning: NewtonSolver requires not-available CholmodSolver...\n";
+  DEB_warning(1,"NewtonSolver requires not-available CholmodSolver");
   return false;
 #endif	    
 }
@@ -116,6 +116,260 @@ solve(NProblemGmmInterface* _problem)
 //-----------------------------------------------------------------------------
 
 
+int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A, 
+  const VectorD& _b)
+{
+  DEB_time_func_def;
+
+  // number of unknowns
+  int n = _problem->n_unknowns();
+  // number of constraints
+  int m = _b.size();
+
+  DEB_line(2, "optimize via Newton with " << n << " unknowns and " << m << " linear constraints");
+
+  // initialize vectors of unknowns
+  VectorD x(n);
+  _problem->initial_x(x.data());
+
+  // storage of update vector dx and rhs of KKT system
+  VectorD dx(n+m), rhs(n+m), g(n);
+  rhs.setZero();
+
+  // resize temp vector for line search (and set to x1 to approx Hessian correctly if problem is non-quadratic!)
+  x_ls_ = x;
+
+  // indicate that system matrix is symmetric
+  lu_solver_.isSymmetric(true);
+
+  // start with no regularization
+  double regularize(0.0);
+  int iter=0;
+  while( iter < max_iters_)
+  {
+    // get Newton search direction by solving LSE
+    bool first_factorization = (iter ==0);
+    factorize(_problem, _A, _b, x, regularize, first_factorization);
+
+    // 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);
+
+    // get maximal reasonable step
+    double t_max  = std::min(1.0, 0.5*_problem->max_feasible_step(x.data(), dx.data()));
+
+    // perform line-search
+    double newton_decrement(0.0);
+    double fx(0.0);
+    double t = backtracking_line_search(_problem, x, g, dx, newton_decrement, fx, t_max);
+
+    x += dx.head(n)*t;
+
+    DEB_line(2, "iter: " << iter
+                << ", f(x) = " << fx
+                << ", t = " << t << " (tmax=" << t_max << ")"
+                << ", eps = [Newton decrement] = " << newton_decrement
+                << ", constraint violation  = " << rhs.tail(m).norm());
+
+    // converged?
+    if(newton_decrement < eps_ || std::abs(t) <= eps_ls_)
+      break;
+
+    ++iter;
+  }
+
+  // store result
+  _problem->store_result(x.data());
+
+  // return success
+  return 1;
+}
+
+
+//-----------------------------------------------------------------------------
+
+
+void NewtonSolver::factorize(NProblemInterface* _problem, 
+  const SMatrixD& _A, const VectorD& _b, const VectorD& _x, double& _regularize, 
+  const bool _first_factorization)
+{
+  DEB_enter_func;
+  const int n  = _problem->n_unknowns();
+  const int m  = _A.rows();
+  const int nf = n+m;
+
+  // get hessian of quadratic problem
+  SMatrixD H(n,n);
+  _problem->eval_hessian(_x.data(), H);
+
+  // set up KKT matrix
+  // create sparse matrix
+  std::vector< Triplet > trips;
+  trips.reserve(H.nonZeros() + 2*_A.nonZeros());
+
+  // add elements of H
+  for (int k=0; k<H.outerSize(); ++k)
+    for (SMatrixD::InnerIterator it(H,k); it; ++it)
+      trips.push_back(Triplet(it.row(),it.col(),it.value()));
+
+  // add elements of _A
+  for (int k=0; k<_A.outerSize(); ++k)
+    for (SMatrixD::InnerIterator it(_A,k); it; ++it)
+    {
+      // insert _A block below
+      trips.push_back(Triplet(it.row()+n,it.col(),it.value()));
+
+      // insert _A^T block right
+      trips.push_back(Triplet(it.col(),it.row()+n,it.value()));
+    }
+
+  if(_regularize != 0.0)
+    for( int i=0; i<m; ++i)
+      trips.push_back(Triplet(n+i,n+i,_regularize));
+
+  // create KKT matrix
+  SMatrixD KKT(nf,nf);
+  KKT.setFromTriplets(trips.begin(), trips.end());
+
+  // compute LU factorization
+  if(_first_factorization)
+    analyze_pattern(KKT);
+
+  bool success = numerical_factorization(KKT);
+
+  if(!success)
+  {
+    // add more regularization
+    if(_regularize == 0.0)
+      _regularize = 1e-8;
+    else
+      _regularize *= 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());
+
+//      for( int i=0; i<m; ++i)
+//        trips.push_back(Triplet(n+i,n+i,_regularize));
+
+    // regularize full system
+    for( int i=0; i<n+m; ++i)
+      trips.push_back(Triplet(i,i,_regularize));
+
+    // 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);
+  }
+}
+
+
+//-----------------------------------------------------------------------------
+
+
+double NewtonSolver::backtracking_line_search(NProblemInterface* _problem, 
+  VectorD& _x, VectorD& _g, VectorD& _dx, double& _newton_decrement, 
+  double& _fx, const double _t_start)
+{
+  DEB_enter_func;
+  int n = _x.size();
+
+  // pre-compute objective
+  double fx = _problem->eval_f(_x.data());
+
+  // pre-compute dot product
+  double gtdx = _g.transpose()*_dx.head(n);
+  _newton_decrement = std::abs(gtdx);
+
+  // current step size
+  double t = _t_start;
+
+  // backtracking (stable in case of NAN and with max 100 iterations)
+  for(int i=0; i<100; ++i)
+  {
+    // current update
+    x_ls_ = _x + _dx.head(n)*t;
+    double fx_ls = _problem->eval_f(x_ls_.data());
+
+    if( fx_ls <= fx + alpha_ls_*t*gtdx )
+    {
+      _fx = fx_ls;
+      return t;
+    }
+    else
+      t *= beta_ls_;
+  }
+
+  DEB_warning(1, "line search could not find a valid step within 100 "
+    "iterations");
+  _fx = fx;
+  return 0.0;
+}
+
+
+//-----------------------------------------------------------------------------
+
+
+void NewtonSolver::analyze_pattern(SMatrixD& _KKT)
+{
+  DEB_enter_func;
+  switch(solver_type_)
+  {
+    case LS_EigenLU:      lu_solver_.analyzePattern(_KKT); break;
+#if COMISO_SUITESPARSE_AVAILABLE
+    case LS_Umfpack: umfpack_solver_.analyzePattern(_KKT); break;
+#endif
+    default: DEB_warning(1, "selected linear solver not availble");
+  }
+}
+
+
+//-----------------------------------------------------------------------------
+
+
+bool NewtonSolver::numerical_factorization(SMatrixD& _KKT)
+{
+  DEB_enter_func;
+  switch(solver_type_)
+  {
+    case LS_EigenLU:      
+      lu_solver_.factorize(_KKT); 
+      return (lu_solver_.info() == Eigen::Success);
+#if COMISO_SUITESPARSE_AVAILABLE
+    case LS_Umfpack: 
+      umfpack_solver_.factorize(_KKT); 
+      return (umfpack_solver_.info() == Eigen::Success);
+#endif
+    default: 
+      DEB_warning(1, "selected linear solver not availble!"); 
+      return false;
+  }
+}
+
+
+//-----------------------------------------------------------------------------
+
+
+void NewtonSolver::solve_kkt_system( VectorD& _rhs, VectorD& _dx)
+{
+  DEB_enter_func;
+  switch(solver_type_)
+  {
+    case LS_EigenLU: _dx =      lu_solver_.solve(_rhs); break;
+#if COMISO_SUITESPARSE_AVAILABLE
+    case LS_Umfpack: _dx = umfpack_solver_.solve(_rhs); break;
+#endif
+    default: DEB_warning(1, "selected linear solver not availble"); break;
+  }
+}
 
 //=============================================================================
 } // namespace COMISO

NSolver/NewtonSolver.hh

@@ -79,82 +79,7 @@ public:
   // solve with linear constraints
   // Warning: so far only feasible starting points with (_A*_problem->initial_x() == b) are supported!
   // Extension to infeasible starting points is planned
-  int solve(NProblemInterface* _problem, const SMatrixD& _A, const VectorD& _b)
-  {
-    // time solution procedure
-    COMISO::StopWatch sw; sw.start();
-
-    // number of unknowns
-    int n = _problem->n_unknowns();
-    // number of constraints
-    int m = _b.size();
-
-    std::cerr << "optmize via Newton with " << n << " unknowns and " << m << " linear constraints" << std::endl;
-
-    // initialize vectors of unknowns
-    VectorD x(n);
-    _problem->initial_x(x.data());
-
-    // storage of update vector dx and rhs of KKT system
-    VectorD dx(n+m), rhs(n+m), g(n);
-    rhs.setZero();
-
-    // resize temp vector for line search (and set to x1 to approx Hessian correctly if problem is non-quadratic!)
-    x_ls_ = x;
-
-    // indicate that system matrix is symmetric
-    lu_solver_.isSymmetric(true);
-
-    // start with no regularization
-    double regularize(0.0);
-    int iter=0;
-    while( iter < max_iters_)
-    {
-      // get Newton search direction by solving LSE
-      bool first_factorization = (iter ==0);
-      factorize(_problem, _A, _b, x, regularize, first_factorization);
-
-      // 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);
-
-      // get maximal reasonable step
-      double t_max  = std::min(1.0, 0.5*_problem->max_feasible_step(x.data(), dx.data()));
-
-      // perform line-search
-      double newton_decrement(0.0);
-      double fx(0.0);
-      double t = backtracking_line_search(_problem, x, g, dx, newton_decrement, fx, t_max);
-
-      x += dx.head(n)*t;
-
-      std::cerr << "iter: " << iter
-                << ", f(x) = " << fx
-                << ", t = " << t << " (tmax=" << t_max << ")"
-                << ", eps = [Newton decrement] = " << newton_decrement
-                << ", constraint violation  = " << rhs.tail(m).norm()
-                << std::endl;
-
-      // converged?
-      if(newton_decrement < eps_ || std::abs(t) <= eps_ls_)
-        break;
-
-      ++iter;
-    }
-
-    // store result
-    _problem->store_result(x.data());
-
-    double solution_time = sw.stop();
-    std::cerr << "Newton Method finished in " << solution_time/1000.0 << "s" << std::endl;
-
-    // return success
-    return 1;
-  }
+  int solve(NProblemInterface* _problem, const SMatrixD& _A, const VectorD& _b);
 
   // select internal linear solver
   void set_linearsolver(LinearSolver _st)
@@ -164,158 +89,19 @@ public:
 
 protected:
 
-  void factorize(NProblemInterface* _problem, const SMatrixD& _A, const VectorD& _b, const VectorD& _x, double& _regularize, const bool _first_factorization)
-  {
-    const int n  = _problem->n_unknowns();
-    const int m  = _A.rows();
-    const int nf = n+m;
-
-    // get hessian of quadratic problem
-    SMatrixD H(n,n);
-    _problem->eval_hessian(_x.data(), H);
-
-    // set up KKT matrix
-    // create sparse matrix
-    std::vector< Triplet > trips;
-    trips.reserve(H.nonZeros() + 2*_A.nonZeros());
-
-    // add elements of H
-    for (int k=0; k<H.outerSize(); ++k)
-      for (SMatrixD::InnerIterator it(H,k); it; ++it)
-        trips.push_back(Triplet(it.row(),it.col(),it.value()));
-
-    // add elements of _A
-    for (int k=0; k<_A.outerSize(); ++k)
-      for (SMatrixD::InnerIterator it(_A,k); it; ++it)
-      {
-        // insert _A block below
-        trips.push_back(Triplet(it.row()+n,it.col(),it.value()));
-
-        // insert _A^T block right
-        trips.push_back(Triplet(it.col(),it.row()+n,it.value()));
-      }
-
-    if(_regularize != 0.0)
-      for( int i=0; i<m; ++i)
-        trips.push_back(Triplet(n+i,n+i,_regularize));
-
-    // create KKT matrix
-    SMatrixD KKT(nf,nf);
-    KKT.setFromTriplets(trips.begin(), trips.end());
-
-    // compute LU factorization
-    if(_first_factorization)
-      analyze_pattern(KKT);
-
-    bool success = numerical_factorization(KKT);
-
-    if(!success)
-    {
-      // add more regularization
-      if(_regularize == 0.0)
-        _regularize = 1e-8;
-      else
-        _regularize *= 2.0;
-
-      // print information
-      std::cerr << "Linear Solver reported problem while factoring KKT system: ";
-      if(solver_type_ == LS_EigenLU)
-        std::cerr << lu_solver_.lastErrorMessage();
-      std::cerr << std::endl;
-
-//      DEB_line(2, "Linear Solver reported problem while factoring KKT system: ");
-//      if(solver_type_ == LS_EigenLU)
-//        DEB_line(2, lu_solver_.lastErrorMessage());
-
-//      for( int i=0; i<m; ++i)
-//        trips.push_back(Triplet(n+i,n+i,_regularize));
-
-      // regularize full system
-      for( int i=0; i<n+m; ++i)
-        trips.push_back(Triplet(i,i,_regularize));
-
-      // 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);
-    }
-  }
+  void factorize(NProblemInterface* _problem, const SMatrixD& _A, 
+    const VectorD& _b, const VectorD& _x, double& _regularize, 
+    const bool _first_factorization);
 
-  double backtracking_line_search(NProblemInterface* _problem, VectorD& _x, VectorD& _g, VectorD& _dx, double& _newton_decrement, double& _fx, const double _t_start = 1.0)
-  {
-    int n = _x.size();
-
-    // pre-compute objective
-    double fx = _problem->eval_f(_x.data());
-
-    // pre-compute dot product
-    double gtdx = _g.transpose()*_dx.head(n);
-    _newton_decrement = std::abs(gtdx);
-
-    // current step size
-    double t = _t_start;
-
-    // backtracking (stable in case of NAN and with max 100 iterations)
-    for(int i=0; i<100; ++i)
-    {
-      // current update
-      x_ls_ = _x + _dx.head(n)*t;
-      double fx_ls = _problem->eval_f(x_ls_.data());
-
-      if( fx_ls <= fx + alpha_ls_*t*gtdx )
-      {
-        _fx = fx_ls;
-        return t;
-      }
-      else
-        t *= beta_ls_;
-    }
-
-    std::cerr << "Warning: line search could not find a valid step within 100 iterations..." << std::endl;
-    _fx = fx;
-    return 0.0;
-  }
-
-  void analyze_pattern(SMatrixD& _KKT)
-  {
-    switch(solver_type_)
-    {
-      case LS_EigenLU:      lu_solver_.analyzePattern(_KKT); break;
-#if COMISO_SUITESPARSE_AVAILABLE
-      case LS_Umfpack: umfpack_solver_.analyzePattern(_KKT); break;
-#endif
-      default: std::cerr <<"Warning: selected linear solver not availble!"; break;
-    }
-  }
+  double backtracking_line_search(NProblemInterface* _problem, VectorD& _x, 
+    VectorD& _g, VectorD& _dx, double& _newton_decrement, double& _fx, 
+    const double _t_start = 1.0);
 
-  bool numerical_factorization(SMatrixD& _KKT)
-  {
-    switch(solver_type_)
-    {
-      case LS_EigenLU:      lu_solver_.factorize(_KKT); return (lu_solver_.info() == Eigen::Success);
-#if COMISO_SUITESPARSE_AVAILABLE
-      case LS_Umfpack: umfpack_solver_.factorize(_KKT); return (umfpack_solver_.info() == Eigen::Success);
-#endif
-      default: std::cerr <<"Warning: selected linear solver not availble!"; return false;
-    }
-  }
+  void analyze_pattern(SMatrixD& _KKT);
 
-  void solve_kkt_system( VectorD& _rhs, VectorD& _dx)
-  {
-    switch(solver_type_)
-    {
-      case LS_EigenLU: _dx =      lu_solver_.solve(_rhs); break;
-#if COMISO_SUITESPARSE_AVAILABLE
-      case LS_Umfpack: _dx = umfpack_solver_.solve(_rhs); break;
-#endif
-      default: std::cerr <<"Warning: selected linear solver not availble!"; break;
-    }
-  }
+  bool numerical_factorization(SMatrixD& _KKT);
 
+  void solve_kkt_system(VectorD& _rhs, VectorD& _dx);
 
   // deprecated function!
   // solve