AcceleratedQuadraticProxy.hh 7.99 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
//=============================================================================
//
//  CLASS AcceleratedQuadraticProxy
//
//=============================================================================


#ifndef COMISO_ACCELERATEDQUADRATICPROXY_HH
#define COMISO_ACCELERATEDQUADRATICPROXY_HH

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>

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

#include <CoMISo/Config/CoMISoDefines.hh>
David Bommes's avatar
David Bommes committed
17
#include <CoMISo/Utils/StopWatch.hh>
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
#include "NProblemInterface.hh"

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

	      

/** \class AcceleratedQuadraticProxy AcceleratedQuadraticProxy.hh <CoMISo/NSolver/AcceleratedQuadraticProxy.hh>

    Brief Description.
  
    A more elaborate description follows.
*/
Max Lyon's avatar
Max Lyon committed
36
class AcceleratedQuadraticProxy
37
38
39
40
41
42
43
44
45
46
47
{
public:

  typedef Eigen::VectorXd             VectorD;
  typedef Eigen::SparseMatrix<double> SMatrixD;
  typedef Eigen::Triplet<double>      Triplet;
   
  /// Default constructor
  AcceleratedQuadraticProxy(const double _eps = 1e-6, const int _max_iters = 1000, const double _accelerate = 100.0, const double _alpha_ls=0.2, const double _beta_ls = 0.6)
    : eps_(_eps), max_iters_(_max_iters), accelerate_(_accelerate), alpha_ls_(_alpha_ls), beta_ls_(_beta_ls) {}
 
48
49
50
51
52
53
54
55
56
57
  // solve without linear constraints
  int solve(NProblemInterface* _quadratic_problem, NProblemInterface* _nonlinear_problem, bool _update_factorization = true)
  {
    SMatrixD A(0,_quadratic_problem->n_unknowns());
    VectorD b(VectorD::Index(0));
    return solve(_quadratic_problem, _nonlinear_problem, A, b, _update_factorization);
  }

  // solve with linear constraints
  int solve(NProblemInterface* _quadratic_problem, NProblemInterface* _nonlinear_problem, const SMatrixD& _A, const VectorD& _b, bool _update_factorization = true)
58
  {
David Bommes's avatar
David Bommes committed
59
60
61
    // time solution procedure
    COMISO::StopWatch sw; sw.start();

62
    // number of unknowns
Max Lyon's avatar
Max Lyon committed
63
    size_t n = _quadratic_problem->n_unknowns();
64
    // number of constraints
Max Lyon's avatar
Max Lyon committed
65
    size_t m = _b.size();
66

67
68
    std::cerr << "optmize via AQP with " << n << " unknowns and " << m << " linear constraints" << std::endl;

69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
    // initialize vectors of unknowns (cache last two steps)
    VectorD x1(n);
    _quadratic_problem->initial_x(x1.data());
    VectorD x2 = x1;

    // storage of acceleration vector and 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 _quadratic problem is non-quadratic!)
    x_ls_ = x1;
    // resize temp gradient vector for line search
    g_.resize(n);

    const double theta = (1.0-std::sqrt(1.0/accelerate_))/(1.0+std::sqrt(1.0/accelerate_));

    // pre-factorize linear system
86
87
    if(_update_factorization)
      pre_factorize(_quadratic_problem, _A, _b);
88
89
90
91
92
93
94
95

    int iter=0;
    while( iter < max_iters_)
    {
      dx.head(n) = x1-x2;
      double t_max  = std::min(theta, 0.5*_nonlinear_problem->max_feasible_step(x1.data(), dx.data()));

      // accelerate and update x1 and x2 (x1 will be accelerated point and x2 the previous x1)
96
      x2 = x1 + dx.head(n)*t_max;
97
98
99
100
101
102
103
104
105
106
107
108
      x2.swap(x1);

      // solve KKT
      _quadratic_problem->eval_gradient(x1.data(), rhs.data());
      _nonlinear_problem->eval_gradient(x1.data(), g.data());
      // get gradient
      g += rhs.head(n);
      rhs.head(n) = -g;
      dx = lu_solver_.solve(rhs);

      t_max  = std::min(1.0, 0.5*_nonlinear_problem->max_feasible_step(x1.data(), dx.data()));
      double rel_df(0.0);
109
      double t = backtracking_line_search(_quadratic_problem, _nonlinear_problem, x1, g, dx, rel_df, t_max);
110
111
112

      x1 += dx.head(n)*t;

113
114
      std::cerr << "iter: " << iter << " eps = [f(x_old)-f(x_new)]/f(x_old) = " << rel_df << std::endl;

115
      // converged?
116
      if(rel_df < eps_)
117
        break;
118
119

      ++iter;
120
    }
121
122
123
124

    // store result
    _quadratic_problem->store_result(x1.data());

David Bommes's avatar
David Bommes committed
125
126
127
    double solution_time = sw.stop();
    std::cerr << "Accelerated Quadratic Proxy finished in " << solution_time/1000.0 << "s" << std::endl;

128
129
    // return success
    return 1;
130
131
132
133
134
135
136
  }

protected:

  void pre_factorize(NProblemInterface* _quadratic_problem, const SMatrixD& _A, const VectorD& _b)
  {
    const int n  = _quadratic_problem->n_unknowns();
Martin Marinov's avatar
Martin Marinov committed
137
    const int m  = static_cast<int>(_A.rows());
138
139
140
    const int nf = n+m;

    // get hessian of quadratic problem
Martin Marinov's avatar
Martin Marinov committed
141

142
143
144
145
146
147
148
149
    SMatrixD H(n,n);
    _quadratic_problem->eval_hessian(x_ls_.data(), H);

    // set up KKT matrix
    // create sparse matrix
    std::vector< Triplet > trips;
    trips.reserve(H.nonZeros() + 2*_A.nonZeros());

Martin Marinov's avatar
Martin Marinov committed
150
151
152
153
154
    const auto add_triplet = [&trips](
        const size_t _i, const size_t _j, const double _v)
    {
      trips.emplace_back(static_cast<int>(_i), static_cast<int>(_j), _v);
    };
155

Martin Marinov's avatar
Martin Marinov committed
156
157
158
159
160
161
    // add elements of H
    for (int k = 0; k < H.outerSize(); ++k)
    {
      for (SMatrixD::InnerIterator it(H, k); it; ++it)
        add_triplet(it.row(), it.col(), it.value());
    }
162
    // add elements of _A
Martin Marinov's avatar
Martin Marinov committed
163
164
165
    for (int k = 0; k < _A.outerSize(); ++k)
    {
      for (SMatrixD::InnerIterator it(_A, k); it; ++it)
166
167
      {
        // insert _A block below
Martin Marinov's avatar
Martin Marinov committed
168
        add_triplet(it.row() + n, it.col(), it.value());
169
170

        // insert _A^T block right
Martin Marinov's avatar
Martin Marinov committed
171
        add_triplet(it.col(), it.row() + n, it.value());
172
      }
Martin Marinov's avatar
Martin Marinov committed
173
    }
174
175
176
177
178
179
180
181
182
183
184
185
186
187

    // create KKT matrix
    SMatrixD KKT(nf,nf);
    KKT.setFromTriplets(trips.begin(), trips.end());

    // compute LU factorization
    lu_solver_.compute(KKT);

    if(lu_solver_.info() != Eigen::Success)
    {
//      DEB_line(2, "Eigen::SparseLU reported problem: " << lu_solver_.lastErrorMessage());
//      DEB_line(2, "-> re-try with regularized constraints...");
      std::cerr << "Eigen::SparseLU reported problem while factoring KKT system: " << lu_solver_.lastErrorMessage() << std::endl;

Martin Marinov's avatar
Martin Marinov committed
188
189
      for (int i = 0; i < m; ++i)
        add_triplet(n + i, n + i, 1e-9);
190
191
192
193
194
195
196
197
198
199
200
201
202

      // create KKT matrix
      KKT.setFromTriplets(trips.begin(), trips.end());

      // compute LU factorization
      lu_solver_.compute(KKT);

//      IGM_THROW_if(lu_solver_.info() != Eigen::Success, QGP_BOUNDED_DISTORTION_FAILURE);
    }
  }

  double backtracking_line_search(NProblemInterface* _quadratic_problem, NProblemInterface* _nonlinear_problem, VectorD& _x, VectorD& _g, VectorD& _dx, double& _rel_df, double _t_start = 1.0)
  {
Max Lyon's avatar
Max Lyon committed
203
    size_t n = _x.size();
204

205
206
207
208
    // pre-compute objective
    double fx = _quadratic_problem->eval_f(_x.data()) + _nonlinear_problem->eval_f(_x.data());

    // pre-compute dot product
209
210
211
    double gtdx = _g.transpose()*_dx.head(n);

    std::cerr << "Newton decrement: " << gtdx << std::endl;
212
213
214
215
216
217
218
219

    // 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
220
      x_ls_ = _x + _dx.head(n)*t;
221
222
223
224
      double fx_ls = _quadratic_problem->eval_f(x_ls_.data()) + _nonlinear_problem->eval_f(x_ls_.data());

      if( fx_ls <= fx + alpha_ls_*t*gtdx )
      {
225
226
227
228
        _rel_df = std::abs(1.0-fx_ls/fx);

        std::cerr << "LS improved objective function " << fx << " -> " << fx_ls << std::endl;

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
        return t;
      }
      else
        t *= beta_ls_;
    }

    std::cerr << "Warning: line search could not find a valid step within 100 iterations..." << std::endl;
    _rel_df = 0.0;
    return 0.0;
  }

private:
  double eps_;
  int    max_iters_;
  double accelerate_;
  double alpha_ls_;
  double beta_ls_;

  VectorD x_ls_;
  VectorD g_;

  // Sparse LU decomposition
  Eigen::SparseLU<SMatrixD> lu_solver_;
};


//=============================================================================
} // namespace COMISO
//=============================================================================
#endif // ACG_NEWTONSOLVER_HH defined
//=============================================================================