NewtonSolver.cc 12.2 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
#if COMISO_SUITESPARSE_AVAILABLE  
26
  converged_ = true;
27
  
28 29 30 31
  // get problem size
  int n = _problem->n_unknowns();

  // hesse matrix
David Bommes's avatar
David Bommes committed
32
  NProblemGmmInterface::SMatrixNP H;
33 34 35 36 37 38 39 40 41 42
  // 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;

43
  for(int i=0; i<max_iters_; ++i)
44 45 46
  {
    _problem->eval_gradient(P(x), P(g));
    // check for convergence
47
    if( gmm::vect_norm2(g) < eps_)
48
    {
49
      DEB_line(2, "Newton Solver converged after " << i << " iterations");
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 82
      _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;

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

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

#else
113
  DEB_warning(1,"NewtonSolver requires not-available CholmodSolver");
114
  converged_ = false;
115 116
  return false;
#endif	    
117 118 119 120 121 122
}


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


123 124 125
int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A, 
  const VectorD& _b)
{
126
  DEB_time_func_def;
127
  converged_ = false;
128

129
  const double KKT_res_eps = 1e-6;
130
  const int    max_KKT_regularization_iters = 40;
131 132
        double regularize_constraints_limit = 1e-6;
  const double max_allowed_constraint_violation2 = 1e-12;
133

134
  // number of unknowns
Max Lyon's avatar
Max Lyon committed
135
  size_t n = _problem->n_unknowns();
136
  // number of constraints
Max Lyon's avatar
Max Lyon committed
137
  size_t m = _b.size();
138

139
  DEB_line(2, "optimize via Newton with " << n << " unknowns and " << m <<
140
    " linear constraints");
141 142 143 144 145

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

146 147
  double initial_constraint_violation2 = (_A*x-_b).squaredNorm();

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

175 176 177 178 179 180 181 182 183
      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);
184

185 186
        // check numerical stability of KKT system and regularize if necessary
        kkt_res2 = (KKT_*dx-rhs).squaredNorm();
187
        constraint_res2 = (_A*dx.head(n)-rhs.tail(m)).squaredNorm();
188
      }
189

190
      if(!fact_ok || kkt_res2 > KKT_res_eps || constraint_res2 > max_allowed_constraint_violation2)
191
      {
Max Lyon's avatar
Max Lyon committed
192 193
        DEB_warning(2, "Numerical issues in KKT system");
        DEB_warning_if(!fact_ok, 2, "Factorization not ok");
194
        DEB_line_if(
195
            kkt_res2 > KKT_res_eps, 3, "KKT Residuum too high: " << kkt_res2);
196
        DEB_line_if(constraint_res2 > max_allowed_constraint_violation2, 3,
197
            "Constraint violation too high: " << constraint_res2);
198
        // alternate hessian and constraints regularization
199
        if(reg_iters % 2 == 0 || regularize_constraints >= regularize_constraints_limit)
200
        {
201
          DEB_line(2, "residual ^ 2 " << kkt_res2 << "->regularize hessian");
202 203 204 205 206
          if(regularize_hessian == 0.0)
            regularize_hessian = 1e-6;
          else
            regularize_hessian *= 2.0;
        }
207
        else
208
        {
209
          DEB_line(2, "residual^2 " << kkt_res2 << " -> regularize constraints");
210 211 212 213 214
          if(regularize_constraints == 0.0)
            regularize_constraints = 1e-8;
          else
            regularize_constraints *= 2.0;
        }
215
      }
216 217
      ++reg_iters;
    }
218
    while( (!fact_ok || kkt_res2 > KKT_res_eps || constraint_res2 > max_allowed_constraint_violation2) && reg_iters < max_KKT_regularization_iters);
219 220

    // no valid step could be found?
221
    if(kkt_res2 > KKT_res_eps || constraint_res2 > max_allowed_constraint_violation2 || reg_iters >= max_KKT_regularization_iters)
222
    {
223 224
      DEB_error("numerical issues in KKT system could not be resolved "
        "-> terminating NewtonSolver with current solution");
225 226
      _problem->store_result(x.data());
      return 0;
227
    }
228 229

    // get maximal reasonable step
230 231
    double t_max  = std::min(1.0, 
      0.5 * _problem->max_feasible_step(x.data(), dx.data()));
232 233 234 235

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

238 239
    // perform update
    x += dx.head(n)*t;
240

241 242 243 244
    double constraint_violation2 = (_A*x-_b).squaredNorm();

    if(constraint_violation2 > 2*initial_constraint_violation2 && constraint_violation2 > max_allowed_constraint_violation2)
    {
245 246
      DEB_warning(2, "Numerical issues in KKT system lead to "
        "constraint violation -> recovery phase");
247 248 249 250 251 252 253
      // restore old solution
      x -= dx.head(n)*t;

      regularize_constraints *= 0.5;
      regularize_constraints_limit = regularize_constraints;
    }

254 255 256 257 258 259 260
    DEB_line(4,
        "iter: " << iter << ", f(x) = " << fx << ", t = " << t
                 << " (tmax=" << t_max << ")" << (t < t_max ? " _clamped_" : "")
                 << ", eps = [Newton decrement] = " << newton_decrement
                 << ", constraint violation prior = " << rhs.tail(m).norm()
                 << ", after = " << (_b - _A * x).norm()
                 << ", KKT residual^2 = " << kkt_res2);
261 262

    // converged?
263
    if (newton_decrement < eps_ || std::abs(t) < eps_ls_)
264 265
    {
      converged_ = true;
266
      break;
267
    }
268 269 270 271 272 273 274 275 276 277 278 279 280 281 282

    ++iter;
  }

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

  // return success
  return 1;
}


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


283
bool NewtonSolver::factorize(NProblemInterface* _problem,
284
  const SMatrixD& _A, const VectorD& _b, const VectorD& _x, double& _regularize_hessian, double& _regularize_constraints,
285 286 287 288
  const bool _first_factorization)
{
  DEB_enter_func;
  const int n  = _problem->n_unknowns();
289
  const int m  = static_cast<int>(_A.rows());
290 291 292 293 294 295 296 297 298 299 300 301 302 303
  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)
304
      trips.push_back(Triplet(static_cast<int>(it.row()),static_cast<int>(it.col()),it.value()));
305 306 307 308 309 310

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

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

317
  // regularize constraints
318
//  if(_regularize_constraints != 0.0)
319
    for( int i=0; i<m; ++i)
320 321 322
      trips.push_back(Triplet(n+i,n+i,_regularize_constraints));

  // regularize Hessian
323
//  if(_regularize_hessian != 0.0)
324 325 326 327 328 329 330 331
  {
    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));
  }
332 333

  // create KKT matrix
334 335
  KKT_.resize(nf,nf);
  KKT_.setFromTriplets(trips.begin(), trips.end());
336 337 338

  // compute LU factorization
  if(_first_factorization)
339
    analyze_pattern(KKT_);
340

341
  return numerical_factorization(KKT_);
342 343 344 345 346 347 348 349 350 351 352
}


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


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
353
  size_t n = _x.size();
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

  // 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_)
  {
413
    case LS_EigenLU:
414 415 416
      lu_solver_.factorize(_KKT); 
      return (lu_solver_.info() == Eigen::Success);
#if COMISO_SUITESPARSE_AVAILABLE
417
    case LS_Umfpack:
418 419 420 421 422 423 424 425 426 427 428 429 430
      umfpack_solver_.factorize(_KKT); 
      return (umfpack_solver_.info() == Eigen::Success);
#endif
    default: 
      DEB_warning(1, "selected linear solver not availble!"); 
      return false;
  }
}


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


431
void NewtonSolver::solve_kkt_system(const VectorD& _rhs, VectorD& _dx)
432 433 434 435 436 437 438 439 440 441 442
{
  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;
  }
}
443 444

//=============================================================================
David Bommes's avatar
David Bommes committed
445
} // namespace COMISO
446
//=============================================================================