LMpar.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // This code initially comes from MINPACK whose original authors are:
5 // Copyright Jorge More - Argonne National Laboratory
6 // Copyright Burt Garbow - Argonne National Laboratory
7 // Copyright Ken Hillstrom - Argonne National Laboratory
8 //
9 // This Source Code Form is subject to the terms of the Minpack license
10 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
11 
12 #ifndef EIGEN_LMPAR_H
13 #define EIGEN_LMPAR_H
14 
15 namespace Eigen {
16 
17 namespace internal {
18 
19  template <typename QRSolver, typename VectorType>
20  void lmpar2(
21  const QRSolver &qr,
22  const VectorType &diag,
23  const VectorType &qtb,
24  typename VectorType::Scalar m_delta,
25  typename VectorType::Scalar &par,
26  VectorType &x)
27 
28  {
29  using std::sqrt;
30  using std::abs;
31  typedef typename QRSolver::MatrixType MatrixType;
32  typedef typename QRSolver::Scalar Scalar;
33 // typedef typename QRSolver::StorageIndex StorageIndex;
34 
35  /* Local variables */
36  Index j;
37  Scalar fp;
38  Scalar parc, parl;
39  Index iter;
40  Scalar temp, paru;
41  Scalar gnorm;
42  Scalar dxnorm;
43 
44  // Make a copy of the triangular factor.
45  // This copy is modified during call the qrsolv
46  MatrixType s;
47  s = qr.matrixR();
48 
49  /* Function Body */
50  const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
51  const Index n = qr.matrixR().cols();
52  eigen_assert(n==diag.size());
53  eigen_assert(n==qtb.size());
54 
55  VectorType wa1, wa2;
56 
57  /* compute and store in x the gauss-newton direction. if the */
58  /* jacobian is rank-deficient, obtain a least squares solution. */
59 
60  // const Index rank = qr.nonzeroPivots(); // exactly double(0.)
61  const Index rank = qr.rank(); // use a threshold
62  wa1 = qtb;
63  wa1.tail(n-rank).setZero();
64  //FIXME There is no solve in place for sparse triangularView
65  wa1.head(rank) = s.topLeftCorner(rank,rank).template triangularView<Upper>().solve(qtb.head(rank));
66 
67  x = qr.colsPermutation()*wa1;
68 
69  /* initialize the iteration counter. */
70  /* evaluate the function at the origin, and test */
71  /* for acceptance of the gauss-newton direction. */
72  iter = 0;
73  wa2 = diag.cwiseProduct(x);
74  dxnorm = wa2.blueNorm();
75  fp = dxnorm - m_delta;
76  if (fp <= Scalar(0.1) * m_delta) {
77  par = 0;
78  return;
79  }
80 
81  /* if the jacobian is not rank deficient, the newton */
82  /* step provides a lower bound, parl, for the zero of */
83  /* the function. otherwise set this bound to zero. */
84  parl = 0.;
85  if (rank==n) {
86  wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm;
87  s.topLeftCorner(n,n).transpose().template triangularView<Lower>().solveInPlace(wa1);
88  temp = wa1.blueNorm();
89  parl = fp / m_delta / temp / temp;
90  }
91 
92  /* calculate an upper bound, paru, for the zero of the function. */
93  for (j = 0; j < n; ++j)
94  wa1[j] = s.col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)];
95 
96  gnorm = wa1.stableNorm();
97  paru = gnorm / m_delta;
98  if (paru == 0.)
99  paru = dwarf / (std::min)(m_delta,Scalar(0.1));
100 
101  /* if the input par lies outside of the interval (parl,paru), */
102  /* set par to the closer endpoint. */
103  par = (std::max)(par,parl);
104  par = (std::min)(par,paru);
105  if (par == 0.)
106  par = gnorm / dxnorm;
107 
108  /* beginning of an iteration. */
109  while (true) {
110  ++iter;
111 
112  /* evaluate the function at the current value of par. */
113  if (par == 0.)
114  par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
115  wa1 = sqrt(par)* diag;
116 
117  VectorType sdiag(n);
118  lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag);
119 
120  wa2 = diag.cwiseProduct(x);
121  dxnorm = wa2.blueNorm();
122  temp = fp;
123  fp = dxnorm - m_delta;
124 
125  /* if the function is small enough, accept the current value */
126  /* of par. also test for the exceptional cases where parl */
127  /* is zero or the number of iterations has reached 10. */
128  if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
129  break;
130 
131  /* compute the newton correction. */
132  wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm);
133  // we could almost use this here, but the diagonal is outside qr, in sdiag[]
134  for (j = 0; j < n; ++j) {
135  wa1[j] /= sdiag[j];
136  temp = wa1[j];
137  for (Index i = j+1; i < n; ++i)
138  wa1[i] -= s.coeff(i,j) * temp;
139  }
140  temp = wa1.blueNorm();
141  parc = fp / m_delta / temp / temp;
142 
143  /* depending on the sign of the function, update parl or paru. */
144  if (fp > 0.)
145  parl = (std::max)(parl,par);
146  if (fp < 0.)
147  paru = (std::min)(paru,par);
148 
149  /* compute an improved estimate for par. */
150  par = (std::max)(parl,par+parc);
151  }
152  if (iter == 0)
153  par = 0.;
154  return;
155  }
156 } // end namespace internal
157 
158 } // end namespace Eigen
159 
160 #endif // EIGEN_LMPAR_H
SCALAR Scalar
Definition: bench_gemm.cpp:46
#define max(a, b)
Definition: datatypes.h:20
Matrix diag(const std::vector< Matrix > &Hs)
Definition: Matrix.cpp:206
#define min(a, b)
Definition: datatypes.h:19
void lmpar2(const QRSolver &qr, const VectorType &diag, const VectorType &qtb, typename VectorType::Scalar m_delta, typename VectorType::Scalar &par, VectorType &x)
Definition: LMpar.h:20
int n
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
MatrixXf MatrixType
iterator iter(handle obj)
Definition: pytypes.h:2273
HouseholderQR< MatrixXf > qr(A)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
#define eigen_assert(x)
Definition: Macros.h:1037
RealScalar s
void lmqrsolv(Matrix< Scalar, Rows, Cols > &s, const PermutationMatrix< Dynamic, Dynamic, PermIndex > &iPerm, const Matrix< Scalar, Dynamic, 1 > &diag, const Matrix< Scalar, Dynamic, 1 > &qtb, Matrix< Scalar, Dynamic, 1 > &x, Matrix< Scalar, Dynamic, 1 > &sdiag)
Definition: LMqrsolv.h:23
A triangularView< Lower >().adjoint().solveInPlace(B)
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
#define abs(x)
Definition: datatypes.h:17
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:33