1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
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
|
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// This code initially comes from MINPACK whose original authors are:
// Copyright Jorge More - Argonne National Laboratory
// Copyright Burt Garbow - Argonne National Laboratory
// Copyright Ken Hillstrom - Argonne National Laboratory
//
// This Source Code Form is subject to the terms of the Minpack license
// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
#ifndef EIGEN_LMPAR_H
#define EIGEN_LMPAR_H
namespace Eigen {
namespace internal {
template <typename QRSolver, typename VectorType>
void lmpar2(
const QRSolver &qr,
const VectorType &diag,
const VectorType &qtb,
typename VectorType::Scalar m_delta,
typename VectorType::Scalar &par,
VectorType &x)
{
using std::sqrt;
using std::abs;
typedef typename QRSolver::MatrixType MatrixType;
typedef typename QRSolver::Scalar Scalar;
typedef typename QRSolver::Index Index;
/* Local variables */
Index j;
Scalar fp;
Scalar parc, parl;
Index iter;
Scalar temp, paru;
Scalar gnorm;
Scalar dxnorm;
// Make a copy of the triangular factor.
// This copy is modified during call the qrsolv
MatrixType s;
s = qr.matrixR();
/* Function Body */
const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
const Index n = qr.matrixR().cols();
eigen_assert(n==diag.size());
eigen_assert(n==qtb.size());
VectorType wa1, wa2;
/* compute and store in x the gauss-newton direction. if the */
/* jacobian is rank-deficient, obtain a least squares solution. */
// const Index rank = qr.nonzeroPivots(); // exactly double(0.)
const Index rank = qr.rank(); // use a threshold
wa1 = qtb;
wa1.tail(n-rank).setZero();
//FIXME There is no solve in place for sparse triangularView
wa1.head(rank) = s.topLeftCorner(rank,rank).template triangularView<Upper>().solve(qtb.head(rank));
x = qr.colsPermutation()*wa1;
/* initialize the iteration counter. */
/* evaluate the function at the origin, and test */
/* for acceptance of the gauss-newton direction. */
iter = 0;
wa2 = diag.cwiseProduct(x);
dxnorm = wa2.blueNorm();
fp = dxnorm - m_delta;
if (fp <= Scalar(0.1) * m_delta) {
par = 0;
return;
}
/* if the jacobian is not rank deficient, the newton */
/* step provides a lower bound, parl, for the zero of */
/* the function. otherwise set this bound to zero. */
parl = 0.;
if (rank==n) {
wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm;
s.topLeftCorner(n,n).transpose().template triangularView<Lower>().solveInPlace(wa1);
temp = wa1.blueNorm();
parl = fp / m_delta / temp / temp;
}
/* calculate an upper bound, paru, for the zero of the function. */
for (j = 0; j < n; ++j)
wa1[j] = s.col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)];
gnorm = wa1.stableNorm();
paru = gnorm / m_delta;
if (paru == 0.)
paru = dwarf / (std::min)(m_delta,Scalar(0.1));
/* if the input par lies outside of the interval (parl,paru), */
/* set par to the closer endpoint. */
par = (std::max)(par,parl);
par = (std::min)(par,paru);
if (par == 0.)
par = gnorm / dxnorm;
/* beginning of an iteration. */
while (true) {
++iter;
/* evaluate the function at the current value of par. */
if (par == 0.)
par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
wa1 = sqrt(par)* diag;
VectorType sdiag(n);
lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag);
wa2 = diag.cwiseProduct(x);
dxnorm = wa2.blueNorm();
temp = fp;
fp = dxnorm - m_delta;
/* if the function is small enough, accept the current value */
/* of par. also test for the exceptional cases where parl */
/* is zero or the number of iterations has reached 10. */
if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
break;
/* compute the newton correction. */
wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm);
// we could almost use this here, but the diagonal is outside qr, in sdiag[]
for (j = 0; j < n; ++j) {
wa1[j] /= sdiag[j];
temp = wa1[j];
for (Index i = j+1; i < n; ++i)
wa1[i] -= s.coeff(i,j) * temp;
}
temp = wa1.blueNorm();
parc = fp / m_delta / temp / temp;
/* depending on the sign of the function, update parl or paru. */
if (fp > 0.)
parl = (std::max)(parl,par);
if (fp < 0.)
paru = (std::min)(paru,par);
/* compute an improved estimate for par. */
par = (std::max)(parl,par+parc);
}
if (iter == 0)
par = 0.;
return;
}
} // end namespace internal
} // end namespace Eigen
#endif // EIGEN_LMPAR_H
|
