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
164
165
    do
    {
      // get Newton search direction by solving LSE
166
      bool fact_ok = factorize(_problem, _A, _b, x, regularize_hessian, regularize_constraints, first_factorization);
167
168
      first_factorization = false;

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

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

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

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

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

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

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

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

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

      regularize_constraints *= 0.5;
      regularize_constraints_limit = regularize_constraints;
    }


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

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

    ++iter;
  }

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

  // return success
  return 1;
}


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


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

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

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

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

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

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

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

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


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


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

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


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


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

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