LSSTApplications  10.0-2-g4f67435,11.0.rc2+1,11.0.rc2+12,11.0.rc2+3,11.0.rc2+4,11.0.rc2+5,11.0.rc2+6,11.0.rc2+7,11.0.rc2+8
LSSTDataManagementBasePackage
NLSolver.cc
Go to the documentation of this file.
1 // -*- LSST-C++ -*-
2 
3 /*
4  * LSST Data Management System
5  * Copyright 2008, 2009, 2010 LSST Corporation.
6  *
7  * This product includes software developed by the
8  * LSST Project (http://www.lsst.org/).
9  *
10  * This program is free software: you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation, either version 3 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the LSST License Statement and
21  * the GNU General Public License along with this program. If not,
22  * see <http://www.lsstcorp.org/LegalNotices/>.
23  */
24 // The algorithms contained in this file are taken from the paper
25 // "Methods for Nonlinear Least-Squares Problems", by Madsen, Nielsen,
26 // and Tingleff (2004).
27 // A copy of this paper should be included with the code in the file
28 // madsen04.pdf. Please refer to this paper for more details about
29 // how the algorithms work.
30 
31 #include <iostream>
32 #include <limits>
33 #include <algorithm>
34 
36 
37 #define dbg if(_nlOut) (*_nlOut)
38 #define xdbg if(_verbose >= 1 && _nlOut) (*_nlOut)
39 #define xxdbg if(_verbose >= 2 && _nlOut) (*_nlOut)
40 
41 namespace lsst {
42 namespace meas {
43 namespace algorithms {
44 namespace shapelet {
45 
46 #ifdef USE_TMV
47 
49  _method(NEWTON),
50  _fTol(1.e-8), _gTol(1.e-8), _minStep(1.e-8), _maxIter(200),
51  _tau(1.e-3), _delta0(1.),
52  _nlOut(0), _verbose(0),
53  _hasDirectH(false), _shouldUseCh(true), _shouldUseSvd(false)
54  {}
55 
57  const tmv::Vector<double>& x, const tmv::Vector<double>& f,
58  tmv::Matrix<double>& df) const
59  {
60  const double sqrtEps = sqrt(std::numeric_limits<double>::epsilon());
61  // Do a finite difference calculation for J.
62  // This function is virtual, so if there is a better way to
63  // calculate J, then you should override this version.
64 
65  tmv::Vector<double> x2 = x;
66  tmv::Vector<double> f2(f.size());
67  tmv::Vector<double> f1(f.size());
68  for(size_t j=0;j<x.size();++j) {
69  const double dx = sqrtEps * (x.norm() + 1.);
70  x2(j) += dx;
71  this->calculateF(x2,f2);
72  x2(j) -= 2.*dx;
73  this->calculateF(x2,f1);
74  df.col(j) = (f2-f1)/(2.*dx);
75  x2(j) = x(j);
76  }
77  }
78 
79  bool NLSolver::testJ(
80  const tmv::Vector<double>& x, tmv::Vector<double>& f,
81  std::ostream* os, double relErr) const
82  {
83  const double sqrtEps = sqrt(std::numeric_limits<double>::epsilon());
84 
85  this->calculateF(x,f);
86  _pJ.reset(new tmv::Matrix<double>(f.size(),x.size()));
87  tmv::Matrix<double>& J = *_pJ;
88  this->calculateJ(x,f,J);
89  tmv::Matrix<double> Jn(f.size(),x.size());
90  NLSolver::calculateJ(x,f,Jn);
91  double err = MaxAbsElement(J-Jn) / Jn.norm();
92  if (!relErr) relErr = 10.*sqrtEps;
93  if (os) {
94  *os << "TestJ:\n";
95  if (_verbose >= 1) {
96  *os << "x = "<<x<<std::endl;
97  *os << "f = "<<f<<std::endl;
98  *os << "Direct J = "<<J<<std::endl;
99  *os << "Numeric J = "<<Jn<<std::endl;
100  }
101  *os << "MaxAbsElement(J-J_num) / J.norm() = "<<err<<std::endl;
102  *os << "cf. relerr = "<<relErr<<std::endl;
103  if (err >= relErr) {
104  tmv::Matrix<double> diff = J-Jn;
105  *os << "J-J_num = "<<diff;
106  double maxel = diff.maxAbsElement();
107  *os << "Max element = "<<maxel<<std::endl;
108  for(size_t i=0;i<diff.colsize();++i) {
109  for(size_t j=0;j<diff.rowsize();++j) {
110  if (std::abs(diff(i,j)) > 0.9*maxel) {
111  *os<<"J("<<i<<','<<j<<") = "<<J(i,j)<<" ";
112  *os<<"J_num("<<i<<','<<j<<") = "<<Jn(i,j)<<" ";
113  *os<<"diff = "<<J(i,j)-Jn(i,j)<<std::endl;
114  }
115  }
116  }
117  }
118  }
119  return err < relErr;
120  }
121 
122  class NoDefinedH : public std::runtime_error
123  {
124  public :
125  NoDefinedH() :
126  std::runtime_error("calculateH is undefined in NLSolver")
127  {}
128  };
129 
130  void NLSolver::calculateH(
131  const tmv::Vector<double>& , const tmv::Vector<double>& ,
132  const tmv::Matrix<double>& , tmv::SymMatrix<double>& ) const
133  {
134 #ifdef NOTHROW
135  std::cerr<<"H is undefined\n";
136  exit(1);
137 #else
138  throw NoDefinedH();
139 #endif
140  }
141 
142 
143  // H(i,j) = d^2 Q / dx_i dx_j
144  // where Q = 1/2 Sum_k |f_k|^2
145  // H = JT J + Sum_k f_k d^2f_k/(dx_i dx_j)
146  void NLSolver::calculateNumericH(
147  const tmv::Vector<double>& x,
148  const tmv::Vector<double>& f,
149  tmv::SymMatrix<double>& h) const
150  {
151  // Do a finite difference calculation for H.
152 
153  const double sqrtEps = sqrt(std::numeric_limits<double>::epsilon());
154  const double dx = sqrt(sqrtEps) * (x.norm() + 1.);
155  double q0 = 0.5 * f.normSq();
156 
157  tmv::Vector<double> x2 = x;
158  tmv::Vector<double> f2(f.size());
159  for(size_t i=0;i<x.size();++i) {
160  x2(i) = x(i) + dx;
161  this->calculateF(x2,f2);
162  double q2a = 0.5*f2.normSq();
163  x2(i) = x(i) - dx;
164  this->calculateF(x2,f2);
165  double q2b = 0.5*f2.normSq();
166 
167  h(i,i) = (q2a + q2b - 2.*q0) / (dx*dx);
168  x2(i) = x(i);
169 
170  for(size_t j=i+1;j<x.size();++j) {
171  x2(i) = x(i) + dx;
172  x2(j) = x(j) + dx;
173  this->calculateF(x2,f2);
174  q2a = 0.5*f2.normSq();
175 
176  x2(i) = x(i) + dx;
177  x2(j) = x(j) - dx;
178  this->calculateF(x2,f2);
179  q2b = 0.5*f2.normSq();
180 
181  x2(i) = x(i) - dx;
182  x2(j) = x(j) + dx;
183  this->calculateF(x2,f2);
184  double q2c = 0.5*f2.normSq();
185 
186  x2(i) = x(i) - dx;
187  x2(j) = x(j) - dx;
188  this->calculateF(x2,f2);
189  double q2d = 0.5*f2.normSq();
190 
191  h(i,j) = (q2a - q2b - q2c + q2d) / (4.*dx*dx);
192  x2(i) = x(i);
193  x2(j) = x(j);
194  }
195  }
196  }
197 
198 
199 #define CHECKF(normInfF) \
200  do { \
201  double checkfTemp = (normInfF); \
202  if (!(checkfTemp > _fTol)) { \
203  dbg<<"Found ||f|| ~= 0\n"; \
204  dbg<<"||f||_inf = "<<checkfTemp<<" < "<<_fTol<<std::endl; \
205  return true; \
206  } \
207  } while (false)
208 
209 #define CHECKG(normInfG) \
210  do { \
211  double checkgTemp = (normInfG); \
212  if (!(checkgTemp > _gTol)) { \
213  dbg<<"Found local minimum of ||f||\n"; \
214  dbg<<"||g||_inf = "<<checkgTemp<<" < "<<_gTol<<std::endl; \
215  return true; \
216  } \
217  } while (false)
218 
219 #define SHOWFAILFG \
220  do { \
221  dbg<<"||f||_inf = "<<f.normInf()<<" !< "<<_fTol<<std::endl; \
222  dbg<<"||g||_inf = "<<g.normInf()<<" !< "<<_gTol<<std::endl; \
223  } while (false)
224 
225 #define CHECKSTEP(normH) \
226  do { \
227  double checkStepTemp1 = (normH); \
228  double checkStepTemp2 = _minStep*(x.norm()+_minStep); \
229  if (!(checkStepTemp1 > checkStepTemp2)) { \
230  dbg<<"Step size became too small\n"; \
231  dbg<<"||h|| = "<<checkStepTemp1<<" < "<<checkStepTemp2<<std::endl; \
232  SHOWFAILFG; \
233  return false; \
234  } \
235  } while (false)
236 
237  bool NLSolver::solveNewton(
238  tmv::Vector<double>& x, tmv::Vector<double>& f) const
239  // This is a simple descent method which uses either the
240  // Newton direction or the steepest descent direction.
241  {
242  const double gamma1 = 0.1;
243  const double gamma2 = 0.5;
244  dbg<<"Start Solve_Newton\n";
245 
246  _pJ.reset(new tmv::Matrix<double>(f.size(),x.size()));
247  tmv::Matrix<double>& J = *_pJ;
248  tmv::Vector<double> g(x.size());
249  tmv::Vector<double> h(x.size());
250  tmv::Vector<double> xNew(x.size());
251  tmv::Vector<double> fNew(f.size());
252  tmv::Vector<double> gNew(x.size());
253 
254  xdbg<<"x = "<<x<<std::endl;
255  this->calculateF(x,f);
256  xdbg<<"f = "<<f<<std::endl;
257  CHECKF(f.normInf());
258  double Q = 0.5*f.normSq();
259  xdbg<<"Q = "<<Q<<std::endl;
260  this->calculateJ(x,f,J);
261  if (_shouldUseSvd) J.divideUsing(tmv::SV);
262  J.saveDiv();
263  xdbg<<"J = "<<J<<std::endl;
264  g = J.transpose() * f;
265  xdbg<<"g = "<<g<<std::endl;
266  CHECKG(g.normInf());
267  double alpha = Q/g.normSq();
268  bool shouldUseNewton = true;
269 
270  dbg<<"iter |f|inf Q |g|inf alpha\n";
271  for(int k=0;k<_maxIter;++k) {
272  shouldUseNewton = true;
273  dbg<<k<<" "<<f.normInf()<<" "<<Q<<" "<<g.normInf()<<" "<<
274  alpha<<std::endl;
275 
276  h = -f/J;
277  xdbg<<"h = "<<h<<std::endl;
278  double normH = h.norm();
279  CHECKSTEP(normH);
280 
281  // phi(alpha) = Q(x + alpha h)
282  // phi'(alpha) = fT J h = gT h
283  // where g is measured at xNew, not x
284  // m = phi'(0)
285  double m = h*g;
286  double normG = g.norm();
287  double m2 = -normG*normG;
288 
289  if ((k%5 == 0 && m >= 0.) || (k%5 != 0 && m/normH >= -0.01*normG)) {
290  // i.e. either m >= 0 or |m/normH| < 0.01 * |m2/normH2|
291  shouldUseNewton = false;
292  xdbg<<"Newton is not a good descent direction - use steepest descent\n";
293  h = -g;
294  CHECKSTEP(normG);
295  m = m2;
296  } else {
297  xdbg<<"m = "<<m<<", Steepest m = "<<m2<<std::endl;
298  xdbg<<"m/h.norm() = "<<m/normH<<", Steepest m/h.norm() = "<<-normG<<std::endl;
299  }
300 
301  if (shouldUseNewton && alpha > 0.1) alpha = 1.0;
302  for(int k2=0;k2<=_maxIter;++k2) {
303  if (k2 == _maxIter) {
304  dbg<<"Maximum iterations exceeded in subloop of Newton method\n";
305  dbg<<"This can happen when there is a singularity (or close to it)\n";
306  dbg<<"along the gradient direction:\n";
307  if (_shouldUseSvd) {
308  dbg<<"J Singular values = \n"<<J.svd().getS().diag()<<std::endl;
309  dbg<<"V = \n"<<J.svd().getV()<<std::endl;
310  }
311  SHOWFAILFG;
312  return false;
313  }
314  xNew = x + alpha*h;
315  if (alpha < _minStep) {
316  dbg<<"alpha became too small ("<<alpha<<" < "<<_minStep<<")\n";
317  SHOWFAILFG;
318  return false;
319  }
320  xdbg<<"xnew = "<<xNew<<std::endl;
321  this->calculateF(xNew,fNew);
322  xdbg<<"fnew = "<<fNew<<std::endl;
323  double QNew = 0.5*fNew.normSq();
324  xdbg<<"Qnew = "<<QNew<<std::endl;
325 
326  // Check that phi has decreased significantly
327  // Require phi(alpha) <= phi(0) + gamma1 phi'(0) alpha
328  if (QNew > Q + gamma1 * m * alpha) {
329  alpha /= 2;
330  shouldUseNewton = false;
331  xdbg<<"Qnew not small enough: alpha => "<<alpha<<std::endl;
332  continue;
333  }
334  this->calculateJ(xNew,fNew,J);
335  J.unsetDiv();
336  xdbg<<"Jnew = "<<J<<std::endl;
337  gNew = J.transpose() * fNew;
338  xdbg<<"gnew = "<<gNew<<std::endl;
339 
340  // Check that alpha is not too small
341  // Require phi'(alpha) >= gamma2 phi'(0)
342  double mNew = h*gNew;
343  if (mNew < gamma2 * m) {
344  alpha *= 3.;
345  shouldUseNewton = false;
346  xdbg<<"New slope not shallow enough: alpha => "<<alpha<<std::endl;
347  xdbg<<"(m = "<<m<<", mnew = "<<mNew<<")\n";
348  continue;
349  }
350  xdbg<<"Good choice\n";
351  x = xNew; f = fNew; Q = QNew; g = gNew;
352  break;
353  }
354  CHECKF(f.normInf());
355  CHECKG(g.normInf());
356  }
357  dbg<<"Maximum iterations exceeded in Newton method\n";
358  SHOWFAILFG;
359  return false;
360  }
361 
362  bool NLSolver::solveLM(
363  tmv::Vector<double>& x, tmv::Vector<double>& f) const
364  // This is the Levenberg-Marquardt method
365  {
366  dbg<<"Start Solve_LM\n";
367 
368  _pJ.reset(new tmv::Matrix<double>(f.size(),x.size()));
369  tmv::Matrix<double>& J = *_pJ;
370  tmv::Vector<double> h(x.size());
371  tmv::Vector<double> xNew(x.size());
372  tmv::Vector<double> fNew(f.size());
373  tmv::Vector<double> gNew(x.size());
374 
375  xdbg<<"x = "<<x<<std::endl;
376  this->calculateF(x,f);
377  xdbg<<"f = "<<f<<std::endl;
378  CHECKF(f.normInf());
379  double Q = 0.5*f.normSq();
380  xdbg<<"Q = "<<Q<<std::endl;
381  this->calculateJ(x,f,J);
382  if (_shouldUseSvd) J.divideUsing(tmv::SV);
383  J.saveDiv();
384  xdbg<<"J = "<<J<<std::endl;
385  tmv::Vector<double> g = J.transpose() * f;
386  xdbg<<"g = "<<g<<std::endl;
387  CHECKG(g.normInf());
388 
389  tmv::SymMatrix<double> A = J.transpose() * J;
390  xdbg<<"JT J = "<<A<<std::endl;
391  if (_shouldUseSvd) A.divideUsing(tmv::SV);
392  else if (_shouldUseCh) A.divideUsing(tmv::CH);
393  else A.divideUsing(tmv::LU);
394  double mu = _tau * A.diag().normInf();
395  xdbg<<"initial mu = "<<_tau<<" * "<<A.diag().normInf()<<" = "<<mu<<std::endl;
396  A += mu;
397  A.saveDiv();
398  double nu = 2.;
399 
400  dbg<<"iter |f|inf Q |g|inf mu\n";
401  for(int k=0;k<_maxIter;++k) {
402  dbg<<k<<" "<<f.normInf()<<" "<<Q<<" "<<g.normInf()<<" "<<mu<<std::endl;
403  xdbg<<"k = "<<k<<std::endl;
404  xdbg<<"mu = "<<mu<<std::endl;
405  xdbg<<"A = "<<A<<std::endl;
406 #ifndef NOTHROW
407  try {
408 #endif
409  h = -g/A;
410 #ifndef NOTHROW
411  } catch (tmv::NonPosDef) {
412  xdbg<<"NonPosDef caught - switching division to LU method.\n";
413  // Once the Cholesky decomp fails, just use LU from that point on.
414  A.divideUsing(tmv::LU);
415  h = -g/A;
416  }
417 #endif
418  xdbg<<"h = "<<h<<std::endl;
419  CHECKSTEP(h.norm());
420 
421  xNew = x + h;
422  xdbg<<"xnew = "<<xNew<<std::endl;
423  this->calculateF(xNew,fNew);
424  xdbg<<"fnew = "<<fNew<<std::endl;
425  double QNew = 0.5*fNew.normSq();
426  xdbg<<"Qnew = "<<QNew<<std::endl;
427 
428  if (QNew < Q) {
429  xdbg<<"improved\n";
430  x = xNew; f = fNew;
431  CHECKF(f.normInf());
432 
433  this->calculateJ(x,f,J);
434  J.unsetDiv();
435  A = J.transpose() * J;
436  gNew = J.transpose() * f;
437  xdbg<<"gnew = "<<gNew<<std::endl;
438  CHECKG(gNew.normInf());
439 
440  // Use g as a temporary for (g - mu*h)
441  g -= mu*h;
442  double rho = (Q-QNew) / (-0.5*h*g);
443  xdbg<<"rho = "<<Q-QNew<<" / "<<(-0.5*h*g)<<" = "<<rho<<std::endl;
444  mu *= std::max(1./3.,1.-std::pow(2.*rho-1.,3)); nu = 2.;
445  xdbg<<"mu *= "<<std::max(1./3.,1.-std::pow(2.*rho-1.,3))<<" = "<<mu<<std::endl;
446  A += mu;
447  A.unsetDiv();
448  Q = QNew; g = gNew;
449  } else {
450  xdbg<<"not improved\n";
451  A += mu*(nu-1.); mu *= nu; nu *= 2.;
452  A.unsetDiv();
453  xdbg<<"mu *= (nu = "<<nu<<") = "<<mu<<std::endl;
454  }
455  }
456  dbg<<"Maximum iterations exceeded in LM method\n";
457  SHOWFAILFG;
458  return false;
459  }
460 
462  tmv::Vector<double>& x, tmv::Vector<double>& f) const
463  // This is the Dogleg method
464  {
465  dbg<<"Start Solve_Dogleg\n";
466  _pJ.reset(new tmv::Matrix<double>(f.size(),x.size()));
467  tmv::Matrix<double>& J = *_pJ;
468  tmv::Vector<double> h(x.size());
469  tmv::Vector<double> temp(x.size());
470  tmv::Vector<double> xNew(x.size());
471  tmv::Vector<double> fNew(f.size());
472 
473  xdbg<<"x = "<<x<<std::endl;
474  this->calculateF(x,f);
475  xdbg<<"f = "<<f<<std::endl;
476  CHECKF(f.normInf());
477  double Q = 0.5*f.normSq();
478  xdbg<<"Q = "<<Q<<std::endl;
479  this->calculateJ(x,f,J);
480  if (_shouldUseSvd) J.divideUsing(tmv::SV);
481  J.saveDiv();
482  xdbg<<"J = "<<J<<std::endl;
483  xdbg<<"J.svd = "<<J.svd().getS().diag()<<std::endl;
484 
485  tmv::Vector<double> g = J.transpose() * f;
486  xdbg<<"g = "<<g<<std::endl;
487  CHECKG(g.normInf());
488 
489  double delta = _delta0;
490  int maxnsing = std::min(f.size(),x.size());
491  int nsing = maxnsing;
492 
493  dbg<<"iter |f|inf Q |g|inf delta\n";
494  for(int k=0;k<_maxIter;++k) {
495  dbg<<k<<" "<<f.normInf()<<" "<<Q<<" "<<g.normInf()<<" "<<delta<<std::endl;
496  xxdbg<<"f = "<<f<<std::endl;
497  xxdbg<<"g = "<<g<<std::endl;
498  xxdbg<<"J = "<<J<<std::endl;
499  if (_shouldUseSvd)
500  xxdbg<<"J.svd = "<<J.svd().getS().diag()<<std::endl;
501  if (_shouldUseSvd && nsing == maxnsing && nsing > 1 &&
502  J.svd().isSingular()) {
503  xdbg<<"Singular J, so try lowering number of singular values.\n";
504  nsing = J.svd().getKMax();
505  xdbg<<"J Singular values = \n"<<J.svd().getS().diag()<<std::endl;
506  xdbg<<"nsing -> "<<nsing<<std::endl;
507  }
508  h = -f/J;
509  xdbg<<"h_newton = "<<h<<std::endl;
510 
511  double normsqg = g.normSq();
512  double normH = h.norm();
513  double normH1 = normH;
514  double rhoDenom;
515 
516  if (normH <= delta) {
517  xdbg<<"|h| < delta\n";
518  rhoDenom = Q;
519  xdbg<<"rhodenom = "<<rhoDenom<<std::endl;
520  } else {
521  xxdbg<<"normsqg = "<<normsqg<<std::endl;
522  xxdbg<<"(J*g) = "<<J*g<<std::endl;
523  xxdbg<<"normsq = "<<(J*g).normSq()<<std::endl;
524  double alpha = normsqg / (J*g).normSq();
525  xdbg<<"alpha = "<<alpha<<std::endl;
526  double normG = sqrt(normsqg);
527  xxdbg<<"normG = "<<normG<<std::endl;
528  if (normG >= delta / alpha) {
529  xdbg<<"|g| > delta/alpha\n";
530  h = -(delta / normG) * g;
531  xdbg<<"h_gradient = "<<h<<std::endl;
532  rhoDenom = delta*(2.*alpha*normG-delta)/(2.*alpha);
533  xdbg<<"rhodenom = "<<rhoDenom<<std::endl;
534  } else {
535  xdbg<<"dogleg\n";
536  temp = h + alpha*g;
537  double a = temp.normSq();
538  double b = -alpha * g * temp;
539  double c = alpha*alpha*g.normSq()-delta*delta;
540  // beta is the solution of 0 = a beta^2 + 2b beta + c
541  xdbg<<"a,b,c = "<<a<<" "<<b<<" "<<c<<std::endl;
542  double beta = (b <= 0) ?
543  (-b + sqrt(b*b - a*c)) / a :
544  -c / (b + sqrt(b*b - a*c));
545  xdbg<<"beta = "<<beta<<std::endl;
546  h = -alpha*g + beta*temp;
547  xdbg<<"h_dogleg = "<<h<<std::endl;
548  xdbg<<"h.norm() = "<<h.norm()<<" delta = "<<delta<<std::endl;
549  rhoDenom =
550  0.5*alpha*std::pow((1.-beta)*normG,2) +
551  beta*(2.-beta)*Q;
552  xdbg<<"rhodenom = "<<rhoDenom<<std::endl;
553  }
554  normH = h.norm();
555  }
556 
557  CHECKSTEP(normH);
558 
559  xNew = x + h;
560  xdbg<<"xnew = "<<xNew<<std::endl;
561  this->calculateF(xNew,fNew);
562  xdbg<<"fnew = "<<fNew<<std::endl;
563  double QNew = 0.5*fNew.normSq();
564  xdbg<<"Qnew = "<<QNew<<std::endl;
565 
566  bool deltaok = false;
567  if (QNew < Q) {
568  double rho = (Q-QNew) / rhoDenom;
569  xdbg<<"rho = "<<Q-QNew<<" / "<<rhoDenom<<" = "<<rho<<std::endl;
570  x = xNew; f = fNew; Q = QNew;
571  CHECKF(f.normInf());
572  this->calculateJ(x,f,J);
573  J.unsetDiv();
574  g = J.transpose() * f;
575  xdbg<<"g = "<<g<<std::endl;
576  CHECKG(g.normInf());
577  if (rho > 0.75) {
578  delta = std::max(delta,3.*normH);
579  deltaok = true;
580  }
581  }
582  if (deltaok) {
583  nsing = maxnsing;
584  } else {
585  double normsqh = normH1*normH1;
586  if (_shouldUseSvd &&
587  delta < normH1 &&
588  normsqg < 0.01 * normsqh && nsing > 1) {
589 
590  dbg<<"normsqg == "<<normsqg/normsqh<<
591  " * normsqh, so try lowering number of singular values.\n";
592  --nsing;
593  dbg<<"nsing -> "<<nsing<<std::endl;
594  dbg<<"J Singular values = \n"<<J.svd().getS().diag()<<std::endl;
595  J.svd().top(nsing);
596  } else {
597  delta /= 2.;
598  double min_delta = _minStep * (x.norm()+_minStep);
599  if (delta < min_delta) {
600  dbg<<"delta became too small ("<<
601  delta<<" < "<<min_delta<<")\n";
602  SHOWFAILFG;
603  return false;
604  }
605  }
606  }
607  }
608  dbg<<"Maximum iterations exceeded in Dogleg method\n";
609  SHOWFAILFG;
610  return false;
611  }
612 
614  tmv::Vector<double>& x, tmv::Vector<double>& f) const
615  // This is the Hybrid method which starts with the L-M method,
616  // but switches to a quasi-newton method if ||f|| isn't approaching 0.
617  {
618  const double sqrtEps = sqrt(std::numeric_limits<double>::epsilon());
619 
620  dbg<<"Start Solve_Hybrid\n";
621  _pJ.reset(new tmv::Matrix<double>(f.size(),x.size()));
622  tmv::Matrix<double>& J = *_pJ;
623  tmv::Vector<double> h(x.size());
624  tmv::Vector<double> xNew(x.size());
625  tmv::Vector<double> fNew(f.size());
626  tmv::Vector<double> gNew(x.size());
627  tmv::Matrix<double> JNew(f.size(),x.size());
628  tmv::Vector<double> y(x.size());
629  tmv::Vector<double> v(x.size());
630 
631  xdbg<<"x = "<<x<<std::endl;
632  this->calculateF(x,f);
633  xdbg<<"f = "<<f<<std::endl;
634  double normInfF = f.normInf();
635  CHECKF(normInfF);
636  double Q = 0.5*f.normSq();
637  xdbg<<"Q = "<<Q<<std::endl;
638  this->calculateJ(x,f,J);
639  if (_shouldUseSvd) J.divideUsing(tmv::SV);
640  J.saveDiv();
641  xdbg<<"J = "<<J<<std::endl;
642 
643  tmv::SymMatrix<double> A = J.transpose()*J;
644  xdbg<<"A = "<<A<<std::endl;
645  if (_shouldUseSvd) A.divideUsing(tmv::SV);
646  else if (_shouldUseCh) A.divideUsing(tmv::CH);
647  else A.divideUsing(tmv::LU);
648  A.saveDiv();
649  tmv::SymMatrix<double> H(x.size());
650  if (_shouldUseSvd) H.divideUsing(tmv::SV);
651  else if (_shouldUseCh) H.divideUsing(tmv::CH);
652  else H.divideUsing(tmv::LU);
653  H.saveDiv();
654  bool shouldUseDirectH = _hasDirectH;
655  xdbg<<"shouldUseDirectH = "<<shouldUseDirectH<<std::endl;
656  if (shouldUseDirectH) {
657 #ifndef NOTHROW
658  try {
659 #endif
660  xdbg<<"try calculateH\n";
661  this->calculateH(x,f,J,H);
662 #ifndef NOTHROW
663  } catch(NoDefinedH) {
664  dbg<<"No direct H calculation - calculate on the fly\n";
665  shouldUseDirectH = false;
666  H.setToIdentity();
667  }
668 #endif
669  } else {
670  xdbg<<"setToIdent\n";
671  H.setToIdentity();
672  }
673  xdbg<<"After calculate H = "<<H<<std::endl;
674 
675  tmv::Vector<double> g = J.transpose() * f;
676  xdbg<<"g = "<<g<<std::endl;
677  double normInfG = g.normInf();
678  CHECKG(normInfG);
679 
680  double mu = _tau * A.diag().normInf();
681  A += mu;
682  double nu = 2.;
683  double delta = _delta0;
684  bool shouldUseQuasiNewton = false;
685  int count = 0;
686 
687  dbg<<"iter |f|inf Q |g|inf mu delta LM/QN\n";
688  for(int k=0;k<_maxIter;++k) {
689  dbg<<k<<" "<<normInfF<<" "<<Q<<" "<<normInfG<<" "<< mu<<" "<<
690  delta<<" "<<(shouldUseQuasiNewton?"QN":"LM")<<std::endl;
691  xdbg<<"k = "<<k<<std::endl;
692  xdbg<<"mu = "<<mu<<std::endl;
693  xdbg<<"delta = "<<delta<<std::endl;
694  xdbg<<"A = "<<A<<std::endl;
695  xdbg<<"H = "<<H<<std::endl;
696  xdbg<<"method = "<<(shouldUseQuasiNewton ? "quasinewton\n" : "LM\n");
697  bool isBetter = false;
698  bool shouldSwitchMethod = false;
699 
700  if (shouldUseQuasiNewton) {
701 #ifndef NOTHROW
702  try {
703 #endif
704  h = -g/H;
705 #ifndef NOTHROW
706  } catch (tmv::NonPosDef) {
707  xdbg<<"NonPosDef caught - switching division to LU method for H\n";
708  H.divideUsing(tmv::LU);
709  h = -g/H;
710  }
711 #endif
712  } else {
713 #ifndef NOTHROW
714  try {
715 #endif
716  h = -g/A;
717 #ifndef NOTHROW
718  } catch (tmv::NonPosDef) {
719  xdbg<<"NonPosDef caught - switching division to LU method for A\n";
720  A.divideUsing(tmv::LU);
721  h = -g/A;
722  }
723 #endif
724  }
725 
726  xdbg<<"h = "<<h<<std::endl;
727  double normH = h.norm();
728  CHECKSTEP(normH);
729  if (shouldUseQuasiNewton && normH > delta) h *= delta/normH;
730 
731  xNew = x + h;
732  xdbg<<"xnew = "<<xNew<<std::endl;
733  this->calculateF(xNew,fNew);
734  xdbg<<"fnew = "<<fNew<<std::endl;
735  double QNew = 0.5*fNew.normSq();
736  xdbg<<"Qnew = "<<QNew<<std::endl;
737 
738  double normInfGNew = 0.;
739  bool isJNewSet = false;
740  bool isGNewSet = false;
741  if (!shouldUseDirectH || shouldUseQuasiNewton || QNew < Q) {
742  this->calculateJ(xNew,fNew,JNew);
743  xdbg<<"Jnew = "<<JNew<<std::endl;
744  isJNewSet = true;
745  }
746  if (shouldUseQuasiNewton || QNew < Q) {
747  if (!isJNewSet) dbg<<"Error: JNew should be set!\n";
748  gNew = JNew.transpose() * fNew;
749  xdbg<<"gnew = "<<gNew<<std::endl;
750  normInfGNew = gNew.normInf();
751  xdbg<<"NormInf(gnew) = "<<NormInf(gNew)<<std::endl;
752  isGNewSet = true;
753  }
754 
755  if (shouldUseQuasiNewton) {
756  xdbg<<"quasinewton\n";
757  if (!isGNewSet) dbg<<"Error: gNew should be set!\n";
758  isBetter =
759  (QNew < Q) ||
760  (QNew <= (1.+sqrtEps)*Q && normInfGNew < normInfG);
761  xdbg<<"better = "<<isBetter<<std::endl;
762  shouldSwitchMethod = (normInfGNew >= normInfG);
763  xdbg<<"switchmethod = "<<shouldSwitchMethod<<std::endl;
764  if (QNew < Q) {
765  double rho = (Q-QNew) / (-h*g-0.5*(J*h).normSq());
766  if (rho > 0.75) {
767  delta = std::max(delta,3.*normH);
768  } else if (rho < 0.25) {
769  delta /= 2.;
770  double min_delta = _minStep * (x.norm()+_minStep);
771  if (delta < min_delta) {
772  dbg<<"delta became too small ("<<
773  delta<<" < "<<min_delta<<")\n";
774  SHOWFAILFG;
775  return false;
776  }
777  }
778  } else {
779  delta /= 2.;
780  double min_delta = _minStep * (x.norm()+_minStep);
781  if (delta < min_delta) {
782  dbg<<"delta became too small ("<<
783  delta<<" < "<<min_delta<<")\n";
784  SHOWFAILFG;
785  return false;
786  }
787  }
788  } else {
789  xdbg<<"LM\n";
790  if (QNew < Q) {
791  isBetter = true;
792  // we don't need the g vector anymore, so use this space
793  // to calculate g-mu*h
794  //double rho = (Q-QNew) / (0.5*h*(mu*h-g));
795  g -= mu*h;
796  double rho = (Q-QNew) / (-0.5*h*g);
797  mu *= std::max(1./3.,1.-std::pow(2.*rho-1.,3)); nu = 2.;
798  if (!isGNewSet) dbg<<"Error: gNew should be set!\n";
799  xdbg<<"check1: "<<normInfGNew<<" <? "<<0.02*QNew<<std::endl;
800  xdbg<<"check2: "<<Q-QNew<<" <? "<<0.02*QNew<<std::endl;
801  if (std::min(normInfGNew,Q-QNew) < 0.02 * QNew) {
802  ++count;
803  if (count == 3) shouldSwitchMethod = true;
804  } else {
805  count = 0;
806  }
807  if (count != 3) {
808  if (!isJNewSet) dbg<<"Error: JNew should be set!\n";
809  A = JNew.transpose() * JNew;
810  A += mu;
811  }
812  } else {
813  A += mu*(nu-1.); mu *= nu; nu *= 2.;
814  count = 0;
815  // MJ: try this?
816  shouldSwitchMethod = (nu >= 32.);
817  }
818  A.unsetDiv();
819  xdbg<<"better = "<<isBetter<<std::endl;
820  xdbg<<"switchmethod = "<<shouldSwitchMethod<<std::endl;
821  xdbg<<"count = "<<count<<std::endl;
822  }
823 
824  if (!shouldUseDirectH) {
825  if (!isJNewSet) dbg<<"Error: JNew should be set!\n";
826  y = JNew.transpose()*(JNew*h) + (JNew-J).transpose()*fNew;
827  double hy = h*y;
828  xdbg<<"hy = "<<hy<<std::endl;
829  if (hy > 0.) {
830  v = H*h;
831  xdbg<<"v = "<<v<<std::endl;
832  xdbg<<"y = "<<y<<std::endl;
833  xdbg<<"hv = "<<h*v<<std::endl;
834  H -= (1./(h*v)) * (v^v);
835  xdbg<<"H -> "<<H<<std::endl;
836  H += (1./hy) * (y^y);
837  H.unsetDiv();
838  xdbg<<"H -> "<<H<<std::endl;
839  }
840  }
841 
842  if (isBetter) {
843  xdbg<<"better"<<std::endl;
844  x = xNew; f = fNew; Q = QNew; normInfF = f.normInf();
845  if (isJNewSet) J = JNew;
846  else this->calculateJ(x,f,J);
847  if (isGNewSet) { g = gNew; normInfG = normInfGNew; }
848  else { g = J.transpose() * f; normInfG = g.normInf(); }
849  J.unsetDiv();
850  if (shouldUseDirectH && shouldUseQuasiNewton && !shouldSwitchMethod)
851  this->calculateH(x,f,J,H);
852  CHECKF(normInfF);
853  CHECKG(normInfG);
854  }
855  if (shouldSwitchMethod) {
856  if (shouldUseQuasiNewton) {
857  xdbg<<"switch to LM\n";
858  A = J.transpose() * J;
859  //mu = _tau * A.diag().normInf();
860  A += mu;
861  A.unsetDiv();
862  shouldUseQuasiNewton = false;
863  count = 0;
864  } else {
865  xdbg<<"switch to quasinewton\n";
866  delta = std::max(1.5*_minStep*(x.norm()+_minStep),0.2*normH);
867  if (shouldUseDirectH) {
868  this->calculateH(x,f,J,H);
869  H.unsetDiv();
870  }
871  shouldUseQuasiNewton = true;
872  }
873  }
874  }
875  dbg<<"Maximum iterations exceeded in Hybrid method\n";
876  SHOWFAILFG;
877  return false;
878  }
879 
880  bool NLSolver::solveSecantLM(
881  tmv::Vector<double>& x, tmv::Vector<double>& f) const
882  // This is the Secant version of the Levenberg-Marquardt method
883  {
884  dbg<<"Start Solve_SecantLM\n";
885  _pJ.reset(new tmv::Matrix<double>(f.size(),x.size()));
886  tmv::Matrix<double>& J = *_pJ;
887  tmv::Vector<double> h(x.size());
888  tmv::Vector<double> xNew(x.size());
889  tmv::Vector<double> fNew(f.size());
890  tmv::Vector<double> gNew(x.size());
891 
892  xdbg<<"x = "<<x<<std::endl;
893  this->calculateF(x,f);
894  xdbg<<"f = "<<f<<std::endl;
895  CHECKF(f.normInf());
896  double Q = 0.5*f.normSq();
897  xdbg<<"Q = "<<Q<<std::endl;
898  this->calculateJ(x,f,J);
899  if (_shouldUseSvd) J.divideUsing(tmv::SV);
900  xdbg<<"J = "<<J<<std::endl;
901  tmv::SymMatrix<double> A = J.transpose() * J;
902  if (_shouldUseSvd) A.divideUsing(tmv::SV);
903  else if (_shouldUseCh) A.divideUsing(tmv::CH);
904  else A.divideUsing(tmv::LU);
905  tmv::Vector<double> g = J.transpose() * f;
906  xdbg<<"g = "<<g<<std::endl;
907  CHECKG(g.normInf());
908 
909  double mu = _tau * A.diag().normInf();
910  A += mu;
911  double nu = 2.;
912 
913  dbg<<"iter |f|inf Q |g|inf mu\n";
914  for(int k=0,j=0;k<_maxIter;++k) {
915  dbg<<k<<" "<<f.normInf()<<" "<<Q<<" "<<g.normInf()<<" "<<
916  mu<<std::endl;
917  xdbg<<"k = "<<k<<std::endl;
918  xdbg<<"mu = "<<mu<<std::endl;
919  xdbg<<"J = "<<J<<std::endl;
920 #ifndef NOTHROW
921  try {
922 #endif
923  h = -g/A;
924 #ifndef NOTHROW
925  } catch (tmv::NonPosDef) {
926  xdbg<<"NonPosDef caught - switching division to LU method.\n";
927  A.divideUsing(tmv::LU);
928  h = -g/A;
929  }
930 #endif
931  xdbg<<"h = "<<h<<std::endl;
932  double normH = h.norm();
933  CHECKSTEP(normH);
934 
935  xdbg<<"j = "<<j<<std::endl;
936  if (h(j) < 0.8 * normH) {
937  xNew = x;
938  double eta = _minStep * (x.norm() + 1.);
939  xNew(j) += eta;
940  this->calculateF(xNew,fNew);
941  J.col(j) = (fNew-f)/eta;
942  xdbg<<"J -> "<<J<<std::endl;
943  }
944  j = (j+1)%J.ncols();
945 
946  xNew = x + h;
947  xdbg<<"xnew = "<<xNew<<std::endl;
948  this->calculateF(xNew,fNew);
949  xdbg<<"fnew = "<<fNew<<std::endl;
950  double QNew = 0.5*fNew.normSq();
951  xdbg<<"Qnew = "<<QNew<<std::endl;
952  J += (1./h.normSq()) * ((fNew - f - J*h) ^ h);
953  xdbg<<"J -> "<<J<<std::endl;
954 
955  if (QNew < Q) {
956  x = xNew; f = fNew;
957  CHECKF(f.normInf());
958 
959  A = J.transpose() * J;
960  gNew = J.transpose() * f;
961  CHECKG(g.normInf());
962 
963  g -= mu*h;
964  double rho = (Q-QNew) / (-0.5*h*g);
965  xdbg<<"rho = "<<Q-QNew<<" / "<<(-0.5*h*g)<<" = "<<rho<<std::endl;
966  mu *= std::max(1./3.,1.-std::pow(2.*rho-1.,3)); nu = 2.;
967  xdbg<<"mu = "<<mu<<std::endl;
968  A += mu;
969  Q = QNew; g = gNew;
970  } else {
971  A += mu*(nu-1.); mu *= nu; nu *= 2.;
972  }
973  }
974  dbg<<"Maximum iterations exceeded in Secant LM method\n";
975  SHOWFAILFG;
976  return false;
977  }
978 
979  bool NLSolver::solveSecantDogleg(
980  tmv::Vector<double>& x, tmv::Vector<double>& f) const
981  // This is the Secant version of the Dogleg method
982  {
983  const double sqrtEps = sqrt(std::numeric_limits<double>::epsilon());
984 
985  dbg<<"Start Solve_SecantDogleg\n";
986  _pJ.reset(new tmv::Matrix<double>(f.size(),x.size()));
987  tmv::Matrix<double>& J = *_pJ;
988  tmv::Vector<double> h(x.size());
989  tmv::Vector<double> temp(x.size());
990  tmv::Vector<double> xNew(x.size());
991  tmv::Vector<double> fNew(f.size());
992  tmv::Vector<double> y(f.size());
993  tmv::Vector<double> djodjy(f.size());
994 
995  xdbg<<"x = "<<x<<std::endl;
996  this->calculateF(x,f);
997  xdbg<<"f = "<<f<<std::endl;
998  CHECKF(f.normInf());
999  double Q = 0.5*f.normSq();
1000  xdbg<<"Q = "<<Q<<std::endl;
1001  this->calculateJ(x,f,J);
1002  if (_shouldUseSvd) J.divideUsing(tmv::SV);
1003  tmv::Matrix<double> D = J.inverse();
1004 
1005  tmv::Vector<double> g = J.transpose() * f;
1006  xdbg<<"g = "<<g<<std::endl;
1007  CHECKG(g.normInf());
1008  double delta = _delta0;
1009 
1010  dbg<<"iter |f|inf Q |g|inf delta\n";
1011  for(int k=0,j=0;k<_maxIter;++k) {
1012  dbg<<k<<" "<<f.normInf()<<" "<<Q<<" "<<g.normInf()<<" "<<
1013  delta<<std::endl;
1014  h = -D*f;
1015  xdbg<<"h = "<<h<<std::endl;
1016 
1017  double normsqg = g.normSq();
1018  double alpha = normsqg / (J*g).normSq();
1019  double normH = h.norm();
1020  double rhoDenom;
1021 
1022  if (normH <= delta) {
1023  xdbg<<"|h| < delta \n";
1024  rhoDenom = Q;
1025  } else {
1026  double normG = sqrt(normsqg);
1027  if (normG >= delta / alpha) {
1028  xdbg<<"|g| > delta/alpha \n";
1029  h = -(delta / normG) * g;
1030  xdbg<<"h = "<<h<<std::endl;
1031  rhoDenom = delta*(2.*alpha*normG-delta)/(2.*alpha);
1032  } else {
1033  xdbg<<"dogleg\n";
1034  temp = h + alpha*g;
1035  double a = temp.normSq();
1036  double b = -alpha * g * temp;
1037  double c = alpha*alpha*g.normSq()-delta*delta;
1038  // beta is the solution of 0 = a beta^2 + 2b beta + c
1039  double beta = (b <= 0) ?
1040  (-b + sqrt(b*b - a*c)) / a :
1041  -c / (b + sqrt(b*b - a*c));
1042  xdbg<<"alpha = "<<alpha<<std::endl;
1043  xdbg<<"beta = "<<beta<<std::endl;
1044  h = -alpha*g + beta*temp;
1045  xdbg<<"h = "<<h<<std::endl;
1046  rhoDenom =
1047  0.5*alpha*std::pow((1.-beta)*normG,2) +
1048  beta*(2.-beta)*Q;
1049  }
1050  normH = h.norm();
1051  }
1052 
1053  CHECKSTEP(normH);
1054 
1055  bool resetd = false;
1056  if (h(j) < 0.8 * normH) {
1057  xNew = x;
1058  double eta = _minStep * (x.norm() + 1.);
1059  xNew(j) += eta;
1060  this->calculateF(xNew,fNew);
1061  y = fNew-f;
1062  J.col(j) = y/eta;
1063  double djy = D.row(j)*y;
1064  if (djy < sqrtEps*eta) {
1065  resetd = true;
1066  } else {
1067  djodjy = D.row(j)/djy;
1068  D -= ((D*y) ^ djodjy);
1069  D.row(j) += eta*djodjy;
1070  }
1071  }
1072  j = (j+1)%J.ncols();
1073 
1074  xNew = x + h;
1075  this->calculateF(xNew,fNew);
1076  double QNew = 0.5*fNew.normSq();
1077 
1078  y = fNew - f;
1079  J += (1./h.normSq()) * ((fNew - f - J*h) ^ h);
1080  double hDy = h*D*y;
1081  if (resetd || hDy < sqrtEps*h.norm()) {
1082  D = J.inverse();
1083  } else {
1084  D += 1./(hDy) * ((h-D*y) ^ (h*D));
1085  }
1086 
1087  if (QNew < Q) {
1088  double rho = (Q-QNew) / rhoDenom;
1089  xdbg<<"rho = "<<Q-QNew<<" / "<<rhoDenom<<" = "<<rho<<std::endl;
1090  x = xNew; f = fNew; Q = QNew;
1091  CHECKF(f.normInf());
1092  g = J.transpose() * f;
1093  xdbg<<"g = "<<g<<std::endl;
1094  CHECKG(g.normInf());
1095  if (rho > 0.75) {
1096  delta = std::max(delta,3.*normH);
1097  } else if (rho < 0.25) {
1098  delta /= 2.;
1099  double min_delta = _minStep * (x.norm()+_minStep);
1100  if (delta < min_delta) {
1101  dbg<<"delta became too small ("<<
1102  delta<<" < "<<min_delta<<")\n";
1103  SHOWFAILFG;
1104  return false;
1105  }
1106  }
1107  } else {
1108  delta /= 2.;
1109  double min_delta = _minStep * (x.norm()+_minStep);
1110  if (delta < min_delta) {
1111  dbg<<"delta became too small ("<<delta<<" < "<<min_delta<<")\n";
1112  SHOWFAILFG;
1113  return false;
1114  }
1115  }
1116  }
1117  dbg<<"Maximum iterations exceeded in Secant Dogleg method\n";
1118  SHOWFAILFG;
1119  return false;
1120  }
1121 
1122  bool NLSolver::solve(
1123  tmv::Vector<double>& x, tmv::Vector<double>& f) const
1124  // On input, x is the initial guess
1125  // On output, if return is true, then
1126  // x is the solution for which either f.norm() ~= 0
1127  // or f is a local minimum.
1128  {
1129 #ifndef NOTHROW
1130  try {
1131 #endif
1132  switch (_method) {
1133  case HYBRID : return solveHybrid(x,f);
1134  case DOGLEG : return solveDogleg(x,f);
1135  case LM : return solveLM(x,f);
1136  case NEWTON : return solveNewton(x,f);
1137  case SECANT_LM : return solveSecantLM(x,f);
1138  case SECANT_DOGLEG : return solveSecantDogleg(x,f);
1139  default : dbg<<"Unknown method\n"; return false;
1140  }
1141 #ifndef NOTHROW
1142  }
1143 #if 0
1144  catch (int) {}
1145 #else
1146  catch (tmv::Singular& e) {
1147  dbg<<"Singular matrix encountered in NLSolver::Solve\n";
1148  dbg<<e<<std::endl;
1149  } catch (tmv::Error& e) {
1150  dbg<<"TMV error encountered in NLSolver::Solve\n";
1151  dbg<<e<<std::endl;
1152  } catch (...) {
1153  dbg<<"Error encountered in NLSolver::Solve\n";
1154  }
1155 #endif
1156  return false;
1157 #endif
1158  }
1159 
1160  void NLSolver::getCovariance(tmv::Matrix<double>& cov) const
1161  {
1162  const double sqrtEps = sqrt(std::numeric_limits<double>::epsilon());
1163  if (!_pJ.get()) {
1164  throw std::runtime_error(
1165  "J not set before calling getCovariance");
1166  }
1167  tmv::Matrix<double>& J = *_pJ;
1168  // This might have changed between solve and getCovariance:
1169  // And we need to set the threshold to sqrt(eps) rather than eps
1170  if (_shouldUseSvd) {
1171  J.divideUsing(tmv::SV);
1172  J.svd().thresh(sqrtEps);
1173  }
1174  J.makeInverseATA(cov);
1175  //dbg<<"getCovariance:\n";
1176  //dbg<<"J = "<<J<<std::endl;
1177  //dbg<<"JtJ = "<<J.adjoint()*J<<std::endl;
1178  //dbg<<"(JtJ)^-1 = "<<(J.adjoint()*J).inverse()<<std::endl;
1179  //dbg<<"cov = "<<cov<<std::endl;
1180  }
1181 
1182  void NLSolver::getInverseCovariance(tmv::Matrix<double>& invCov) const
1183  {
1184  if (!_pJ.get()) {
1185  throw std::runtime_error(
1186  "J not set before calling getInverseCovariance");
1187  }
1188  tmv::Matrix<double>& J = *_pJ;
1189  invCov = J.transpose() * J;
1190  }
1191 
1192 #else // USE_EIGEN
1193 
1195  _method(HYBRID),
1196  _fTol(1.e-8), _gTol(1.e-8), _minStep(1.e-8), _maxIter(200),
1197  _tau(1.e-3), _delta0(1.),
1198  _nlOut(0), _verbose(0)
1199  {}
1200 
1202  const DVector& x, const DVector& f, DMatrix& df) const
1203  {
1204  const double sqrtEps = sqrt(std::numeric_limits<double>::epsilon());
1205  // Do a finite difference calculation for J.
1206  // This function is virtual, so if there is a better way to
1207  // calculate J, then you should override this version.
1208 
1209  DVector x2 = x;
1210  DVector f2(f.size());
1211  DVector f1(f.size());
1212  const int n = x.size();
1213  for(int j=0;j<n;++j) {
1214  const double dx = sqrtEps * (x.norm() + 1.);
1215  x2(j) += dx;
1216  this->calculateF(x2,f2);
1217  x2(j) -= 2.*dx;
1218  this->calculateF(x2,f1);
1219  df.col(j) = (f2-f1)/(2.*dx);
1220  x2(j) = x(j);
1221  }
1222  }
1223 
1225  const DVector& x, DVector& f,
1226  std::ostream* os, double relErr) const
1227  {
1228  const double sqrtEps = sqrt(std::numeric_limits<double>::epsilon());
1229 
1230  this->calculateF(x,f);
1231  _pJ.reset(new DMatrix(f.size(),x.size()));
1232  DMatrix& J = *_pJ;
1233  this->calculateJ(x,f,J);
1234  DMatrix Jn(f.size(),x.size());
1235  NLSolver::calculateJ(x,f,Jn);
1236  double err = (J-Jn).TMV_maxAbsElement() / Jn.norm();
1237  if (!relErr) relErr = 10.*sqrtEps;
1238  if (os) {
1239  *os << "TestJ:\n";
1240  if (_verbose >= 1) {
1241  *os << "x = "<<x<<std::endl;
1242  *os << "f = "<<f<<std::endl;
1243  *os << "Direct J = "<<J<<std::endl;
1244  *os << "Numeric J = "<<Jn<<std::endl;
1245  }
1246  *os << "MaxAbsElement(J-J_num) / J.norm() = "<<err<<std::endl;
1247  *os << "cf. relerr = "<<relErr<<std::endl;
1248  if (err >= relErr) {
1249  DMatrix diff = J-Jn;
1250  *os << "J-J_num = "<<diff;
1251  double maxel = diff.TMV_maxAbsElement();
1252  *os << "Max element = "<<maxel<<std::endl;
1253  const int m = diff.TMV_colsize();
1254  const int n = diff.TMV_rowsize();
1255  for(int i=0;i<m;++i) {
1256  for(int j=0;j<n;++j) {
1257  if (std::abs(diff(i,j)) > 0.9*maxel) {
1258  *os<<"J("<<i<<','<<j<<") = "<<J(i,j)<<" ";
1259  *os<<"J_num("<<i<<','<<j<<") = "<<Jn(i,j)<<" ";
1260  *os<<"diff = "<<J(i,j)-Jn(i,j)<<std::endl;
1261  }
1262  }
1263  }
1264  }
1265  }
1266  return err < relErr;
1267  }
1268 
1269 #define CHECKF(normInfF) \
1270  do { \
1271  double checkfTemp = (normInfF); \
1272  if (!(checkfTemp > _fTol)) { \
1273  dbg<<"Found ||f|| ~= 0\n"; \
1274  dbg<<"||f||_inf = "<<checkfTemp<<" < "<<_fTol<<std::endl; \
1275  return true; \
1276  } \
1277  } while (false)
1278 
1279 #define CHECKG(normInfG) \
1280  do { \
1281  double checkgTemp = (normInfG); \
1282  if (!(checkgTemp > _gTol)) { \
1283  dbg<<"Found local minimum of ||f||\n"; \
1284  dbg<<"||g||_inf = "<<checkgTemp<<" < "<<_gTol<<std::endl; \
1285  return true; \
1286  } \
1287  } while (false)
1288 
1289 #define SHOWFAILFG \
1290  do { \
1291  dbg<<"||f||_inf = "<<f.TMV_normInf()<<" !< "<<_fTol<<std::endl; \
1292  dbg<<"||g||_inf = "<<g.TMV_normInf()<<" !< "<<_gTol<<std::endl; \
1293  } while (false)
1294 
1295 #define CHECKSTEP(normH) \
1296  do { \
1297  double checkStepTemp1 = (normH); \
1298  double checkStepTemp2 = _minStep*(x.norm()+_minStep); \
1299  if (!(checkStepTemp1 > checkStepTemp2)) { \
1300  dbg<<"Step size became too small\n"; \
1301  dbg<<"||h|| = "<<checkStepTemp1<<" < "<<checkStepTemp2<<std::endl; \
1302  SHOWFAILFG; \
1303  return false; \
1304  } \
1305  } while (false)
1306 
1308  // This is the Dogleg method
1309  {
1310  dbg<<"Start Solve_Dogleg\n";
1311  _pJ.reset(new DMatrix(f.size(),x.size()));
1312  DMatrix& J = *_pJ;
1313  DVector h(x.size());
1314  DVector temp(x.size());
1315  DVector xNew(x.size());
1316  DVector fNew(f.size());
1317 
1318  xdbg<<"x = "<<x.transpose()<<std::endl;
1319  this->calculateF(x,f);
1320  xdbg<<"f = "<<f.transpose()<<std::endl;
1321  CHECKF(f.TMV_normInf());
1322  double Q = 0.5*f.TMV_normSq();
1323  xdbg<<"Q = "<<Q<<std::endl;
1324  this->calculateJ(x,f,J);
1325  xdbg<<"J = "<<J<<std::endl;
1326 
1327  DVector g = J.transpose() * f;
1328  xdbg<<"g = "<<g.transpose()<<std::endl;
1329  CHECKG(g.TMV_normInf());
1330 
1331  double delta = _delta0;
1332  int maxnsing = std::min(f.size(),x.size());
1333  int nsing = maxnsing;
1334 
1335  dbg<<"iter |f|inf Q |g|inf delta\n";
1336  for(int k=0;k<_maxIter;++k) {
1337  dbg<<k<<" "<<f.TMV_normInf()<<" "<<Q<<" "<<g.TMV_normInf()<<" "<<delta<<std::endl;
1338  //h = -f/J;
1339  h = J.lu().solve(-f);
1340  xdbg<<"h = "<<h.transpose()<<std::endl;
1341 
1342  double normsqg = g.TMV_normSq();
1343  double normH = h.norm();
1344  double rhoDenom;
1345 
1346  if (normH <= delta) {
1347  xdbg<<"|h| < delta\n";
1348  rhoDenom = Q;
1349  xdbg<<"rhodenom = "<<rhoDenom<<std::endl;
1350  } else {
1351  double alpha = normsqg / (J*g).TMV_normSq();
1352  double normG = sqrt(normsqg);
1353  if (normG >= delta / alpha) {
1354  xdbg<<"|g| > delta/alpha\n";
1355  h = -(delta / normG) * g;
1356  xdbg<<"h = "<<h.transpose()<<std::endl;
1357  rhoDenom = delta*(2.*alpha*normG-delta)/(2.*alpha);
1358  xdbg<<"rhodenom = "<<rhoDenom<<std::endl;
1359  } else {
1360  xdbg<<"dogleg\n";
1361  temp = h + alpha*g;
1362  double a = temp.TMV_normSq();
1363  double b = -alpha * (g.transpose() * temp)(0,0);
1364  double c = alpha*alpha*g.TMV_normSq()-delta*delta;
1365  // beta is the solution of 0 = a beta^2 + 2b beta + c
1366  xdbg<<"a,b,c = "<<a<<" "<<b<<" "<<c<<std::endl;
1367  double beta = (b <= 0) ?
1368  (-b + sqrt(b*b - a*c)) / a :
1369  -c / (b + sqrt(b*b - a*c));
1370  xdbg<<"alpha = "<<alpha<<std::endl;
1371  xdbg<<"beta = "<<beta<<std::endl;
1372  h = -alpha*g + beta*temp;
1373  xdbg<<"h = "<<h.transpose()<<std::endl;
1374  xdbg<<"h.norm() = "<<h.norm()<<" delta = "<<delta<<std::endl;
1375  rhoDenom =
1376  0.5*alpha*std::pow((1.-beta)*normG,2) +
1377  beta*(2.-beta)*Q;
1378  xdbg<<"rhodenom = "<<rhoDenom<<std::endl;
1379  }
1380  normH = h.norm();
1381  }
1382 
1383  CHECKSTEP(normH);
1384 
1385  xNew = x + h;
1386  xdbg<<"xnew = "<<xNew.transpose()<<std::endl;
1387  this->calculateF(xNew,fNew);
1388  xdbg<<"fnew = "<<fNew.transpose()<<std::endl;
1389  double QNew = 0.5*fNew.TMV_normSq();
1390  xdbg<<"Qnew = "<<QNew<<std::endl;
1391 
1392  bool deltaok = false;
1393  if (QNew < Q) {
1394  double rho = (Q-QNew) / rhoDenom;
1395  xdbg<<"rho = "<<Q-QNew<<" / "<<rhoDenom<<" = "<<rho<<std::endl;
1396  x = xNew; f = fNew; Q = QNew;
1397  CHECKF(f.TMV_normInf());
1398  this->calculateJ(x,f,J);
1399  g = J.transpose() * f;
1400  xdbg<<"g = "<<g.transpose()<<std::endl;
1401  CHECKG(g.TMV_normInf());
1402  if (rho > 0.75) {
1403  delta = std::max(delta,3.*normH);
1404  deltaok = true;
1405  }
1406  }
1407  if (deltaok) {
1408  nsing = maxnsing;
1409  } else {
1410  delta /= 2.;
1411  double min_delta = _minStep * (x.norm()+_minStep);
1412  if (delta < min_delta) {
1413  dbg<<"delta became too small ("<<
1414  delta<<" < "<<min_delta<<")\n";
1415  SHOWFAILFG;
1416  return false;
1417  }
1418  }
1419  }
1420  assert(nsing >= 0); // make compiler happy about unused variable
1421  dbg<<"Maximum iterations exceeded in Dogleg method\n";
1422  SHOWFAILFG;
1423  return false;
1424  }
1425 
1427  // This is the Hybrid method which starts with the L-M method,
1428  // but switches to a quasi-newton method if ||f|| isn't approaching 0.
1429  {
1430  const double sqrtEps = sqrt(std::numeric_limits<double>::epsilon());
1431 
1432  dbg<<"Start Solve_Hybrid\n";
1433  _pJ.reset(new DMatrix(f.size(),x.size()));
1434  DMatrix& J = *_pJ;
1435  DVector h(x.size());
1436  DVector xNew(x.size());
1437  DVector fNew(f.size());
1438  DVector gNew(x.size());
1439  DMatrix JNew(f.size(),x.size());
1440  DVector y(x.size());
1441  DVector v(x.size());
1442 
1443  xdbg<<"x = "<<x.transpose()<<std::endl;
1444  this->calculateF(x,f);
1445  xdbg<<"f = "<<f.transpose()<<std::endl;
1446  double normInfF = f.TMV_normInf();
1447  CHECKF(normInfF);
1448  double Q = 0.5*f.TMV_normSq();
1449  xdbg<<"Q = "<<Q<<std::endl;
1450  this->calculateJ(x,f,J);
1451  xdbg<<"J = "<<J<<std::endl;
1452 
1453  DMatrix A = J.transpose()*J;
1454  xdbg<<"A = "<<A<<std::endl;
1455  DMatrix H(x.size(),x.size());
1456  xdbg<<"setToIdent\n";
1457  H.TMV_setToIdentity();
1458  xdbg<<"After calculate H = "<<H<<std::endl;
1459 
1460  DVector g = J.transpose() * f;
1461  xdbg<<"g = "<<g.transpose()<<std::endl;
1462  double normInfG = g.TMV_normInf();
1463  CHECKG(normInfG);
1464 
1465  double mu = _tau * A.TMV_diag().TMV_normInf();
1466  A.EIGEN_diag() += mu;
1467  double nu = 2.;
1468  double delta = _delta0;
1469  bool shouldUseQuasiNewton = false;
1470  int count = 0;
1471 
1472  dbg<<"iter |f|inf Q |g|inf mu delta LM/QN\n";
1473  for(int k=0;k<_maxIter;++k) {
1474  dbg<<k<<" "<<normInfF<<" "<<Q<<" "<<normInfG<<" "<< mu<<" "<<
1475  delta<<" "<<(shouldUseQuasiNewton?"QN":"LM")<<std::endl;
1476  xdbg<<"k = "<<k<<std::endl;
1477  xdbg<<"mu = "<<mu<<std::endl;
1478  xdbg<<"delta = "<<delta<<std::endl;
1479  xdbg<<"A = "<<A<<std::endl;
1480  xdbg<<"H = "<<H<<std::endl;
1481  xdbg<<"method = "<<(shouldUseQuasiNewton ? "quasinewton\n" : "LM\n");
1482  bool isBetter = false;
1483  bool shouldSwitchMethod = false;
1484 
1485  if (shouldUseQuasiNewton) {
1486  //h = -g/H;
1487  h = H.ldlt().solve(-g);
1488  } else {
1489  //h = -g/A;
1490  h = A.ldlt().solve(-g);
1491  }
1492 
1493  xdbg<<"h = "<<h.transpose()<<std::endl;
1494  double normH = h.norm();
1495  CHECKSTEP(normH);
1496  if (shouldUseQuasiNewton && normH > delta) h *= delta/normH;
1497 
1498  xNew = x + h;
1499  xdbg<<"xnew = "<<xNew.transpose()<<std::endl;
1500  this->calculateF(xNew,fNew);
1501  xdbg<<"fnew = "<<fNew.transpose()<<std::endl;
1502  double QNew = 0.5*fNew.TMV_normSq();
1503  xdbg<<"Qnew = "<<QNew<<std::endl;
1504 
1505  this->calculateJ(xNew,fNew,JNew);
1506  xdbg<<"Jnew = "<<JNew<<std::endl;
1507  if (shouldUseQuasiNewton || QNew < Q) {
1508  gNew = JNew.transpose() * fNew;
1509  xdbg<<"gnew = "<<gNew.transpose()<<std::endl;
1510  }
1511  double normInfGNew = gNew.TMV_normInf();
1512 
1513  if (shouldUseQuasiNewton) {
1514  xdbg<<"quasinewton\n";
1515  isBetter =
1516  (QNew < Q) ||
1517  (QNew <= (1.+sqrtEps)*Q && normInfGNew < normInfG);
1518  xdbg<<"better = "<<isBetter<<std::endl;
1519  shouldSwitchMethod = (normInfGNew >= normInfG);
1520  xdbg<<"switchmethod = "<<shouldSwitchMethod<<std::endl;
1521  if (QNew < Q) {
1522  double rho = (Q-QNew) / (-(h.transpose()*g)(0,0)-0.5*(J*h).TMV_normSq());
1523  if (rho > 0.75) {
1524  delta = std::max(delta,3.*normH);
1525  } else if (rho < 0.25) {
1526  delta /= 2.;
1527  double min_delta = _minStep * (x.norm()+_minStep);
1528  if (delta < min_delta) {
1529  dbg<<"delta became too small ("<<
1530  delta<<" < "<<min_delta<<")\n";
1531  SHOWFAILFG;
1532  return false;
1533  }
1534  }
1535  } else {
1536  delta /= 2.;
1537  double min_delta = _minStep * (x.norm()+_minStep);
1538  if (delta < min_delta) {
1539  dbg<<"delta became too small ("<<
1540  delta<<" < "<<min_delta<<")\n";
1541  SHOWFAILFG;
1542  return false;
1543  }
1544  }
1545  } else {
1546  xdbg<<"LM\n";
1547  if (QNew <= Q) {
1548  isBetter = true;
1549  // we don't need the g vector anymore, so use this space
1550  // to calculate g-mu*h
1551  //double rho = (Q-QNew) / (0.5*h*(mu*h-g));
1552  g -= mu*h;
1553  double rho = (Q-QNew) / (-0.5*(h.transpose()*g)(0,0));
1554  mu *= std::max(1./3.,1.-std::pow(2.*rho-1.,3)); nu = 2.;
1555  xdbg<<"check1: "<<normInfGNew<<" <? "<<0.02*QNew<<std::endl;
1556  xdbg<<"check2: "<<Q-QNew<<" <? "<<0.02*QNew<<std::endl;
1557  if (std::min(normInfGNew,Q-QNew) < 0.02 * QNew) {
1558  ++count;
1559  if (count == 3) shouldSwitchMethod = true;
1560  } else {
1561  count = 0;
1562  }
1563  if (count != 3) {
1564  A = JNew.transpose() * JNew;
1565  A.EIGEN_diag() += mu;
1566  }
1567  } else {
1568  A.EIGEN_diag() += mu*(nu-1.); mu *= nu; nu *= 2.;
1569  count = 0;
1570  // MJ: try this?
1571  shouldSwitchMethod = (nu >= 32.);
1572  }
1573  xdbg<<"better = "<<isBetter<<std::endl;
1574  xdbg<<"switchmethod = "<<shouldSwitchMethod<<std::endl;
1575  xdbg<<"count = "<<count<<std::endl;
1576  }
1577 
1578  y = JNew.transpose()*(JNew*h) + (JNew-J).transpose()*fNew;
1579  double hy = (h.transpose()*y)(0,0);
1580  xdbg<<"hy = "<<hy<<std::endl;
1581  if (hy > 0.) {
1582  v = H*h;
1583  xdbg<<"v = "<<v.transpose()<<std::endl;
1584  xdbg<<"y = "<<y.transpose()<<std::endl;
1585  double hv = (h.transpose()*v)(0,0);
1586  xdbg<<"hv = "<<hv<<std::endl;
1587  H -= (1./hv) * (v * v.transpose());
1588  xdbg<<"H -> "<<H<<std::endl;
1589  H += (1./hy) * (y * y.transpose());
1590  xdbg<<"H -> "<<H<<std::endl;
1591  }
1592 
1593  if (isBetter) {
1594  xdbg<<"better"<<std::endl;
1595  x = xNew; f = fNew; Q = QNew; J = JNew; g = gNew;
1596  normInfF = f.TMV_normInf(); normInfG = normInfGNew;
1597  CHECKF(normInfF);
1598  CHECKG(normInfG);
1599  }
1600  if (shouldSwitchMethod) {
1601  if (shouldUseQuasiNewton) {
1602  xdbg<<"switch to LM\n";
1603  A = J.transpose() * J;
1604  //mu = _tau * A.diag().normInf();
1605  A.EIGEN_diag() += mu;
1606  shouldUseQuasiNewton = false;
1607  count = 0;
1608  } else {
1609  xdbg<<"switch to quasinewton\n";
1610  delta = std::max(1.5*_minStep*(x.norm()+_minStep),0.2*normH);
1611  shouldUseQuasiNewton = true;
1612  }
1613  }
1614  }
1615  dbg<<"Maximum iterations exceeded in Hybrid method\n";
1616  SHOWFAILFG;
1617  return false;
1618  }
1619 
1620  bool NLSolver::solve(DVector& x, DVector& f) const
1621  {
1622 #ifndef NOTHROW
1623  try {
1624 #endif
1625  switch (_method) {
1626  case HYBRID : return solveHybrid(x,f);
1627  case DOGLEG : return solveDogleg(x,f);
1628  default : dbg<<"Unknown method\n"; return false;
1629  }
1630 #ifndef NOTHROW
1631  } catch (...) {
1632  dbg<<"Error encountered in NLSolver::Solve\n";
1633  }
1634 #endif
1635  return false;
1636  }
1637 
1639  {
1640  const double eps = std::numeric_limits<double>::epsilon();
1641  if (!_pJ.get()) {
1642  throw std::runtime_error(
1643  "J not set before calling getCovariance");
1644  }
1645  DMatrix& J = *_pJ;
1646  // This might have changed between solve and getCovariance:
1647  // And we need to set the threshold to sqrt(eps) rather than eps
1648  if (_shouldUseSvd) {
1649  Eigen::JacobiSVD<DMatrix> SV_Solver_J(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
1650  const DVector& svd_s = SV_Solver_J.singularValues();
1651  int kmax = svd_s.size();
1652  while(svd_s(kmax-1) < eps * svd_s(0)) --kmax;
1653  dbg<<"In NLSolver::getCovariance:\n";
1654  dbg<<"Using kmax = "<<kmax<<" size = "<<svd_s.size()<<std::endl;
1655  // (JtJ)^-1 = ( (USVt)t (USVt) )^-1
1656  // = ( V St Ut U S Vt )^-1
1657  // = ( V S^2 Vt )^-1
1658  // = V S^-2 Vt
1659  const DMatrix& svd_v = SV_Solver_J.matrixV();
1660  DVector sm2 = svd_s.array().square().inverse().matrix();
1661  sm2.TMV_subVector(kmax, svd_s.size()).setZero();
1662  cov = svd_v * sm2.asDiagonal() * svd_v.transpose();
1663  } else {
1664  Eigen::ColPivHouseholderQR<DMatrix> QR_Solver_J(J);
1665  // (JtJ)^-1 = ( (QR)t (QR) )^-1
1666  // = ( Rt Qt Q R ) ^-1
1667  // = ( Rt R )^-1
1668  // = R^-1 Rt^-1
1669  cov.setIdentity();
1670  QR_Solver_J.matrixQR().triangularView<Eigen::Upper>().transpose().solveInPlace(cov);
1671  QR_Solver_J.matrixQR().triangularView<Eigen::Upper>().solveInPlace(cov);
1672  }
1673  }
1674 
1676  {
1677  if (!_pJ.get()) {
1678  throw std::runtime_error(
1679  "J not set before calling getInverseCovariance");
1680  }
1681  DMatrix& J = *_pJ;
1682  invCov = J.transpose() * J;
1683  }
1684 
1685 #endif
1686 
1687 }}}}
int y
virtual void getCovariance(DMatrix &cov) const
Definition: NLSolver.cc:1638
#define CHECKG(normInfG)
Definition: NLSolver.cc:1279
virtual void calculateF(const DVector &x, DVector &f) const =0
#define xdbg
Definition: NLSolver.cc:38
bool solveDogleg(DVector &x, DVector &f) const
Definition: NLSolver.cc:1307
virtual bool solve(DVector &x, DVector &f) const
Definition: NLSolver.cc:1620
bool solveHybrid(DVector &x, DVector &f) const
Definition: NLSolver.cc:1426
Matrix alpha
#define CHECKF(normInfF)
Definition: NLSolver.cc:1269
#define TMV_normSq()
Definition: MyMatrix.h:186
virtual void calculateJ(const DVector &x, const DVector &f, DMatrix &j) const
Definition: NLSolver.cc:1201
#define CHECKSTEP(normH)
Definition: NLSolver.cc:1295
#define TMV_maxAbsElement()
Definition: MyMatrix.h:188
int x
#define dbg
Definition: NLSolver.cc:37
tuple m
Definition: lsstimport.py:48
#define SHOWFAILFG
Definition: NLSolver.cc:1289
virtual bool testJ(const DVector &, DVector &, std::ostream *os=0, double relerr=0.) const
Definition: NLSolver.cc:1224
virtual void getInverseCovariance(DMatrix &invcov) const
Definition: NLSolver.cc:1675
afw::table::Key< double > b
Vector beta
#define xxdbg
Definition: NLSolver.cc:39