NewtonSolver.cc 11.7 KB
Newer Older
1 2 3 4 5 6 7 8 9 10
//=============================================================================
//
//  CLASS NewtonSolver - IMPLEMENTATION
//
//=============================================================================

//== INCLUDES =================================================================

#include "NewtonSolver.hh"
#include <CoMISo/Solver/CholmodSolver.hh>
11
#include <Base/Debug/DebTime.hh>
12 13
//== NAMESPACES ===============================================================

David Bommes's avatar
David Bommes committed
14
namespace COMISO {
15 16 17 18 19 20 21

//== IMPLEMENTATION ========================================================== 


// solve
int
NewtonSolver::
22
solve(NProblemGmmInterface* _problem)
23
{
24
  DEB_enter_func;
25 26
#if COMISO_SUITESPARSE_AVAILABLE  
  
27 28 29 30
  // get problem size
  int n = _problem->n_unknowns();

  // hesse matrix
David Bommes's avatar
David Bommes committed
31
  NProblemGmmInterface::SMatrixNP H;
32 33 34 35 36 37 38 39 40 41
  // gradient
  std::vector<double> x(n), x_new(n), dx(n), g(n);

  // get initial x, initial grad and initial f
  _problem->initial_x(P(x));
  double f = _problem->eval_f(P(x));

  double reg = 1e-3;
  COMISO::CholmodSolver chol;

42
  for(int i=0; i<max_iters_; ++i)
43 44 45
  {
    _problem->eval_gradient(P(x), P(g));
    // check for convergence
46
    if( gmm::vect_norm2(g) < eps_)
47
    {
48
      DEB_line(2, "Newton Solver converged after " << i << " iterations");
49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81
      _problem->store_result(P(x));
      return true;
    }

    // get current hessian
    _problem->eval_hessian(P(x), H);

    // regularize
    double reg_comp = reg*gmm::mat_trace(H)/double(n);
    for(int j=0; j<n; ++j)
      H(j,j) += reg_comp;

    // solve linear system
    bool factorization_ok = false;
    if(constant_hessian_structure_ && i != 0)
      factorization_ok = chol.update_system_gmm(H);
    else
      factorization_ok = chol.calc_system_gmm(H);

    bool improvement = false;
    if(factorization_ok)
      if(chol.solve( dx, g))
      {
        gmm::add(x, gmm::scaled(dx,-1.0),x_new);
        double f_new = _problem->eval_f(P(x_new));

        if( f_new < f)
        {
          // swap x and x_new (and f and f_new)
          x_new.swap(x);
          f = f_new;
          improvement = true;

82
          DEB_line(2, "energy improved to " << f);
83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98
        }
      }

    // adapt regularization
    if(improvement)
    {
      if(reg > 1e-9)
        reg *= 0.1;
    }
    else
    {
      if(reg < 1e4)
        reg *= 10.0;
      else
      {
        _problem->store_result(P(x));
99 100
        DEB_line(2, "Newton solver reached max regularization but did not "
          "converge");
101 102 103 104 105
        return false;
      }
    }
  }
  _problem->store_result(P(x));
106
  DEB_line(2, "Newton Solver did not converge!!! after iterations.");
107
  return false;
108 109

#else
110
  DEB_warning(1,"NewtonSolver requires not-available CholmodSolver");
111 112
  return false;
#endif	    
113 114 115 116 117 118
}


//-----------------------------------------------------------------------------


119 120 121 122 123
int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A, 
  const VectorD& _b)
{
  DEB_time_func_def;

124
  const double KKT_res_eps = 1e-6;
125
  const int    max_KKT_regularization_iters = 40;
126 127
        double regularize_constraints_limit = 1e-6;
  const double max_allowed_constraint_violation2 = 1e-12;
128

129
  // number of unknowns
Max Lyon's avatar
Max Lyon committed
130
  size_t n = _problem->n_unknowns();
131
  // number of constraints
Max Lyon's avatar
Max Lyon committed
132
  size_t m = _b.size();
133

134 135
  DEB_line(2, "optimize via Newton with " << n << " unknowns and " << m << 
    " linear constraints");
136 137 138 139 140

  // initialize vectors of unknowns
  VectorD x(n);
  _problem->initial_x(x.data());

141 142
  double initial_constraint_violation2 = (_A*x-_b).squaredNorm();

143 144 145 146 147 148 149 150 151 152 153
  // 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
154 155
  double regularize_hessian(0.0);
  double regularize_constraints(0.0);
156
  int iter=0;
157
  bool first_factorization = true;
158 159
  while( iter < max_iters_)
  {
160
    double kkt_res2(0.0);
161
    double constraint_res2(0.0);
162
    int    reg_iters(0);
163
    bool fact_ok = true;
164 165 166
    do
    {
      // get Newton search direction by solving LSE
167
      fact_ok = factorize(_problem, _A, _b, x, regularize_hessian, regularize_constraints, first_factorization);
168 169
      first_factorization = false;

170 171 172 173 174 175 176 177 178
      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);
179

180 181
        // check numerical stability of KKT system and regularize if necessary
        kkt_res2 = (KKT_*dx-rhs).squaredNorm();
182
        constraint_res2 = (_A*dx.head(n)-rhs.tail(m)).squaredNorm();
183
      }
184

185
      if(!fact_ok || kkt_res2 > KKT_res_eps || constraint_res2 > max_allowed_constraint_violation2)
186
      {
187 188
        DEB_warning(2, "Numerical issues in KKT system"); 
        // alternate hessian and constraints regularization
189
        if(reg_iters % 2 == 0 || regularize_constraints >= regularize_constraints_limit)
190
        {
191
          DEB_line(2, "residual ^ 2 " << kkt_res2 << "->regularize hessian");
192 193 194 195 196
          if(regularize_hessian == 0.0)
            regularize_hessian = 1e-6;
          else
            regularize_hessian *= 2.0;
        }
197
        else
198
        {
199
          DEB_line(2, "residual^2 " << kkt_res2 << " -> regularize constraints");
200 201 202 203 204
          if(regularize_constraints == 0.0)
            regularize_constraints = 1e-8;
          else
            regularize_constraints *= 2.0;
        }
205
      }
206 207
      ++reg_iters;
    }
208
    while( (!fact_ok || kkt_res2 > KKT_res_eps || constraint_res2 > max_allowed_constraint_violation2) && reg_iters < max_KKT_regularization_iters);
209 210

    // no valid step could be found?
211
    if(kkt_res2 > KKT_res_eps || constraint_res2 > max_allowed_constraint_violation2 || reg_iters >= max_KKT_regularization_iters)
212
    {
213 214
      DEB_error("numerical issues in KKT system could not be resolved "
        "-> terminating NewtonSolver with current solution");
215 216
      _problem->store_result(x.data());
      return 0;
217
    }
218 219

    // get maximal reasonable step
220 221
    double t_max  = std::min(1.0, 
      0.5 * _problem->max_feasible_step(x.data(), dx.data()));
222 223 224 225

    // perform line-search
    double newton_decrement(0.0);
    double fx(0.0);
226
    double t = backtracking_line_search(_problem, x, g, dx, newton_decrement, fx, t_max);
227

228 229
    // perform update
    x += dx.head(n)*t;
230

231 232 233 234
    double constraint_violation2 = (_A*x-_b).squaredNorm();

    if(constraint_violation2 > 2*initial_constraint_violation2 && constraint_violation2 > max_allowed_constraint_violation2)
    {
235 236
      DEB_warning(2, "Numerical issues in KKT system lead to "
        "constraint violation -> recovery phase");
237 238 239 240 241 242 243 244
      // restore old solution
      x -= dx.head(n)*t;

      regularize_constraints *= 0.5;
      regularize_constraints_limit = regularize_constraints;
    }


245
    DEB_line(2, "iter: " << iter
246 247 248 249
      << ", f(x) = " << fx << ", t = " << t << " (tmax=" << t_max << ")"
      << (t < t_max ? " _clamped_" : "")
      << ", eps = [Newton decrement] = " << newton_decrement
      << ", constraint violation prior = " << rhs.tail(m).norm()
250 251
      << ", after = " << (_b - _A*x).norm()
      << ", KKT residual^2 = " << kkt_res2);
252 253

    // converged?
254
    if(newton_decrement < eps_ || std::abs(t) < eps_ls_)
255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270
      break;

    ++iter;
  }

  // store result
  _problem->store_result(x.data());

  // return success
  return 1;
}


//-----------------------------------------------------------------------------


271
bool NewtonSolver::factorize(NProblemInterface* _problem,
272
  const SMatrixD& _A, const VectorD& _b, const VectorD& _x, double& _regularize_hessian, double& _regularize_constraints,
273 274 275 276
  const bool _first_factorization)
{
  DEB_enter_func;
  const int n  = _problem->n_unknowns();
277
  const int m  = static_cast<int>(_A.rows());
278 279 280 281 282 283 284 285 286 287 288 289 290 291
  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)
292
      trips.push_back(Triplet(static_cast<int>(it.row()),static_cast<int>(it.col()),it.value()));
293 294 295 296 297 298

  // add elements of _A
  for (int k=0; k<_A.outerSize(); ++k)
    for (SMatrixD::InnerIterator it(_A,k); it; ++it)
    {
      // insert _A block below
299
      trips.push_back(Triplet(static_cast<int>(it.row())+n,static_cast<int>(it.col()),it.value()));
300 301

      // insert _A^T block right
302
      trips.push_back(Triplet(static_cast<int>(it.col()),static_cast<int>(it.row())+n,it.value()));
303 304
    }

305
  // regularize constraints
306
//  if(_regularize_constraints != 0.0)
307
    for( int i=0; i<m; ++i)
308 309 310
      trips.push_back(Triplet(n+i,n+i,_regularize_constraints));

  // regularize Hessian
311
//  if(_regularize_hessian != 0.0)
312 313 314 315 316 317 318 319
  {
    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));
  }
320 321

  // create KKT matrix
322 323
  KKT_.resize(nf,nf);
  KKT_.setFromTriplets(trips.begin(), trips.end());
324 325 326

  // compute LU factorization
  if(_first_factorization)
327
    analyze_pattern(KKT_);
328

329
  return numerical_factorization(KKT_);
330 331 332 333 334 335 336 337 338 339 340
}


//-----------------------------------------------------------------------------


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;
Max Lyon's avatar
Max Lyon committed
341
  size_t n = _x.size();
342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418

  // 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;
  }
}


//-----------------------------------------------------------------------------


419
void NewtonSolver::solve_kkt_system(const VectorD& _rhs, VectorD& _dx)
420 421 422 423 424 425 426 427 428 429 430
{
  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;
  }
}
431 432

//=============================================================================
David Bommes's avatar
David Bommes committed
433
} // namespace COMISO
434
//=============================================================================