![]() |
Eigen-unsupported
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // This code initially comes from MINPACK whose original authors are: 00005 // Copyright Jorge More - Argonne National Laboratory 00006 // Copyright Burt Garbow - Argonne National Laboratory 00007 // Copyright Ken Hillstrom - Argonne National Laboratory 00008 // 00009 // This Source Code Form is subject to the terms of the Minpack license 00010 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 00011 00012 #ifndef EIGEN_LMPAR_H 00013 #define EIGEN_LMPAR_H 00014 00015 namespace Eigen { 00016 00017 namespace internal { 00018 00019 template <typename QRSolver, typename VectorType> 00020 void lmpar2( 00021 const QRSolver &qr, 00022 const VectorType &diag, 00023 const VectorType &qtb, 00024 typename VectorType::Scalar m_delta, 00025 typename VectorType::Scalar &par, 00026 VectorType &x) 00027 00028 { 00029 using std::sqrt; 00030 using std::abs; 00031 typedef typename QRSolver::MatrixType MatrixType; 00032 typedef typename QRSolver::Scalar Scalar; 00033 // typedef typename QRSolver::StorageIndex StorageIndex; 00034 00035 /* Local variables */ 00036 Index j; 00037 Scalar fp; 00038 Scalar parc, parl; 00039 Index iter; 00040 Scalar temp, paru; 00041 Scalar gnorm; 00042 Scalar dxnorm; 00043 00044 // Make a copy of the triangular factor. 00045 // This copy is modified during call the qrsolv 00046 MatrixType s; 00047 s = qr.matrixR(); 00048 00049 /* Function Body */ 00050 const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); 00051 const Index n = qr.matrixR().cols(); 00052 eigen_assert(n==diag.size()); 00053 eigen_assert(n==qtb.size()); 00054 00055 VectorType wa1, wa2; 00056 00057 /* compute and store in x the gauss-newton direction. if the */ 00058 /* jacobian is rank-deficient, obtain a least squares solution. */ 00059 00060 // const Index rank = qr.nonzeroPivots(); // exactly double(0.) 00061 const Index rank = qr.rank(); // use a threshold 00062 wa1 = qtb; 00063 wa1.tail(n-rank).setZero(); 00064 //FIXME There is no solve in place for sparse triangularView 00065 wa1.head(rank) = s.topLeftCorner(rank,rank).template triangularView<Upper>().solve(qtb.head(rank)); 00066 00067 x = qr.colsPermutation()*wa1; 00068 00069 /* initialize the iteration counter. */ 00070 /* evaluate the function at the origin, and test */ 00071 /* for acceptance of the gauss-newton direction. */ 00072 iter = 0; 00073 wa2 = diag.cwiseProduct(x); 00074 dxnorm = wa2.blueNorm(); 00075 fp = dxnorm - m_delta; 00076 if (fp <= Scalar(0.1) * m_delta) { 00077 par = 0; 00078 return; 00079 } 00080 00081 /* if the jacobian is not rank deficient, the newton */ 00082 /* step provides a lower bound, parl, for the zero of */ 00083 /* the function. otherwise set this bound to zero. */ 00084 parl = 0.; 00085 if (rank==n) { 00086 wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; 00087 s.topLeftCorner(n,n).transpose().template triangularView<Lower>().solveInPlace(wa1); 00088 temp = wa1.blueNorm(); 00089 parl = fp / m_delta / temp / temp; 00090 } 00091 00092 /* calculate an upper bound, paru, for the zero of the function. */ 00093 for (j = 0; j < n; ++j) 00094 wa1[j] = s.col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; 00095 00096 gnorm = wa1.stableNorm(); 00097 paru = gnorm / m_delta; 00098 if (paru == 0.) 00099 paru = dwarf / (std::min)(m_delta,Scalar(0.1)); 00100 00101 /* if the input par lies outside of the interval (parl,paru), */ 00102 /* set par to the closer endpoint. */ 00103 par = (std::max)(par,parl); 00104 par = (std::min)(par,paru); 00105 if (par == 0.) 00106 par = gnorm / dxnorm; 00107 00108 /* beginning of an iteration. */ 00109 while (true) { 00110 ++iter; 00111 00112 /* evaluate the function at the current value of par. */ 00113 if (par == 0.) 00114 par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 00115 wa1 = sqrt(par)* diag; 00116 00117 VectorType sdiag(n); 00118 lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag); 00119 00120 wa2 = diag.cwiseProduct(x); 00121 dxnorm = wa2.blueNorm(); 00122 temp = fp; 00123 fp = dxnorm - m_delta; 00124 00125 /* if the function is small enough, accept the current value */ 00126 /* of par. also test for the exceptional cases where parl */ 00127 /* is zero or the number of iterations has reached 10. */ 00128 if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 00129 break; 00130 00131 /* compute the newton correction. */ 00132 wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); 00133 // we could almost use this here, but the diagonal is outside qr, in sdiag[] 00134 for (j = 0; j < n; ++j) { 00135 wa1[j] /= sdiag[j]; 00136 temp = wa1[j]; 00137 for (Index i = j+1; i < n; ++i) 00138 wa1[i] -= s.coeff(i,j) * temp; 00139 } 00140 temp = wa1.blueNorm(); 00141 parc = fp / m_delta / temp / temp; 00142 00143 /* depending on the sign of the function, update parl or paru. */ 00144 if (fp > 0.) 00145 parl = (std::max)(parl,par); 00146 if (fp < 0.) 00147 paru = (std::min)(paru,par); 00148 00149 /* compute an improved estimate for par. */ 00150 par = (std::max)(parl,par+parc); 00151 } 00152 if (iter == 0) 00153 par = 0.; 00154 return; 00155 } 00156 } // end namespace internal 00157 00158 } // end namespace Eigen 00159 00160 #endif // EIGEN_LMPAR_H