NewtonSolver.cc 10.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
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
124
int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A, 
  const VectorD& _b)
{
  DEB_time_func_def;

  // number of unknowns
125
  const int n = _problem->n_unknowns();
126
  // number of constraints
127
  const int m = _b.size();
128

129
130
  DEB_line(2, "optimize via Newton with " << n << " unknowns and " << m << 
    " linear constraints");
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163

  // 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
164
165
    double t_max  = std::min(1.0, 
      0.5 * _problem->max_feasible_step(x.data(), dx.data()));
166
167
168
169

    // perform line-search
    double newton_decrement(0.0);
    double fx(0.0);
170
    //double t = backtracking_line_search(_problem, x, g, dx, newton_decrement, fx, t_max);
171
172
    const VectorD x0 = x;
    const double fx0 = _problem->eval_f(x0.data());
173
174
175
    
    double t = t_max;
    bool x_ok = false;
176
177
178
    
    for (int i = 0; i < 10; ++i < 3 ? t *= 0.95 : t /= 2)
    {// clamp back t very mildly the first 3 steps and then aggressively (/ 2)
179
180
181
182
183
184
185
      x = x0 + dx.head(n) * t;
      fx = _problem->eval_f(x.data());
      if (fx > fx0) // function value is larger
        continue;
      newton_decrement = fx0 - fx;

      //check constraint violation
186
187
      const VectorD r = _b - _A*x;
      const double cnstr_vltn = r.norm();
188
189
190
      if (cnstr_vltn > 1e-8)
        continue;
      x_ok = true;
191
      break; // exit here to avoid changing t
192
    }
193

194
195
196
197
198
199
200
    if (!x_ok)
    {
      DEB_line(2, "Line search failed, pulling back to the intial solution");
      t = 0;
      x = x0;
      fx = fx0;
    }
201
202

    DEB_line(2, "iter: " << iter
203
204
205
206
207
      << ", 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());
208
209

    // converged?
210
    if(newton_decrement < eps_ || std::abs(t) < eps_ls_)
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
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
      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;
  }
}


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


392
void NewtonSolver::solve_kkt_system(const VectorD& _rhs, VectorD& _dx)
393
394
395
396
397
398
399
400
401
402
403
{
  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;
  }
}
404
405

//=============================================================================
David Bommes's avatar
David Bommes committed
406
} // namespace COMISO
407
//=============================================================================