![]() |
Eigen-unsupported
3.3.3
|
00001 namespace Eigen { 00002 00003 namespace internal { 00004 00005 template <typename Scalar> 00006 void lmpar( 00007 Matrix< Scalar, Dynamic, Dynamic > &r, 00008 const VectorXi &ipvt, 00009 const Matrix< Scalar, Dynamic, 1 > &diag, 00010 const Matrix< Scalar, Dynamic, 1 > &qtb, 00011 Scalar delta, 00012 Scalar &par, 00013 Matrix< Scalar, Dynamic, 1 > &x) 00014 { 00015 using std::abs; 00016 using std::sqrt; 00017 typedef DenseIndex Index; 00018 00019 /* Local variables */ 00020 Index i, j, l; 00021 Scalar fp; 00022 Scalar parc, parl; 00023 Index iter; 00024 Scalar temp, paru; 00025 Scalar gnorm; 00026 Scalar dxnorm; 00027 00028 00029 /* Function Body */ 00030 const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); 00031 const Index n = r.cols(); 00032 eigen_assert(n==diag.size()); 00033 eigen_assert(n==qtb.size()); 00034 eigen_assert(n==x.size()); 00035 00036 Matrix< Scalar, Dynamic, 1 > wa1, wa2; 00037 00038 /* compute and store in x the gauss-newton direction. if the */ 00039 /* jacobian is rank-deficient, obtain a least squares solution. */ 00040 Index nsing = n-1; 00041 wa1 = qtb; 00042 for (j = 0; j < n; ++j) { 00043 if (r(j,j) == 0. && nsing == n-1) 00044 nsing = j - 1; 00045 if (nsing < n-1) 00046 wa1[j] = 0.; 00047 } 00048 for (j = nsing; j>=0; --j) { 00049 wa1[j] /= r(j,j); 00050 temp = wa1[j]; 00051 for (i = 0; i < j ; ++i) 00052 wa1[i] -= r(i,j) * temp; 00053 } 00054 00055 for (j = 0; j < n; ++j) 00056 x[ipvt[j]] = wa1[j]; 00057 00058 /* initialize the iteration counter. */ 00059 /* evaluate the function at the origin, and test */ 00060 /* for acceptance of the gauss-newton direction. */ 00061 iter = 0; 00062 wa2 = diag.cwiseProduct(x); 00063 dxnorm = wa2.blueNorm(); 00064 fp = dxnorm - delta; 00065 if (fp <= Scalar(0.1) * delta) { 00066 par = 0; 00067 return; 00068 } 00069 00070 /* if the jacobian is not rank deficient, the newton */ 00071 /* step provides a lower bound, parl, for the zero of */ 00072 /* the function. otherwise set this bound to zero. */ 00073 parl = 0.; 00074 if (nsing >= n-1) { 00075 for (j = 0; j < n; ++j) { 00076 l = ipvt[j]; 00077 wa1[j] = diag[l] * (wa2[l] / dxnorm); 00078 } 00079 // it's actually a triangularView.solveInplace(), though in a weird 00080 // way: 00081 for (j = 0; j < n; ++j) { 00082 Scalar sum = 0.; 00083 for (i = 0; i < j; ++i) 00084 sum += r(i,j) * wa1[i]; 00085 wa1[j] = (wa1[j] - sum) / r(j,j); 00086 } 00087 temp = wa1.blueNorm(); 00088 parl = fp / delta / temp / temp; 00089 } 00090 00091 /* calculate an upper bound, paru, for the zero of the function. */ 00092 for (j = 0; j < n; ++j) 00093 wa1[j] = r.col(j).head(j+1).dot(qtb.head(j+1)) / diag[ipvt[j]]; 00094 00095 gnorm = wa1.stableNorm(); 00096 paru = gnorm / delta; 00097 if (paru == 0.) 00098 paru = dwarf / (std::min)(delta,Scalar(0.1)); 00099 00100 /* if the input par lies outside of the interval (parl,paru), */ 00101 /* set par to the closer endpoint. */ 00102 par = (std::max)(par,parl); 00103 par = (std::min)(par,paru); 00104 if (par == 0.) 00105 par = gnorm / dxnorm; 00106 00107 /* beginning of an iteration. */ 00108 while (true) { 00109 ++iter; 00110 00111 /* evaluate the function at the current value of par. */ 00112 if (par == 0.) 00113 par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 00114 wa1 = sqrt(par)* diag; 00115 00116 Matrix< Scalar, Dynamic, 1 > sdiag(n); 00117 qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag); 00118 00119 wa2 = diag.cwiseProduct(x); 00120 dxnorm = wa2.blueNorm(); 00121 temp = fp; 00122 fp = dxnorm - delta; 00123 00124 /* if the function is small enough, accept the current value */ 00125 /* of par. also test for the exceptional cases where parl */ 00126 /* is zero or the number of iterations has reached 10. */ 00127 if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 00128 break; 00129 00130 /* compute the newton correction. */ 00131 for (j = 0; j < n; ++j) { 00132 l = ipvt[j]; 00133 wa1[j] = diag[l] * (wa2[l] / dxnorm); 00134 } 00135 for (j = 0; j < n; ++j) { 00136 wa1[j] /= sdiag[j]; 00137 temp = wa1[j]; 00138 for (i = j+1; i < n; ++i) 00139 wa1[i] -= r(i,j) * temp; 00140 } 00141 temp = wa1.blueNorm(); 00142 parc = fp / delta / temp / temp; 00143 00144 /* depending on the sign of the function, update parl or paru. */ 00145 if (fp > 0.) 00146 parl = (std::max)(parl,par); 00147 if (fp < 0.) 00148 paru = (std::min)(paru,par); 00149 00150 /* compute an improved estimate for par. */ 00151 /* Computing MAX */ 00152 par = (std::max)(parl,par+parc); 00153 00154 /* end of an iteration. */ 00155 } 00156 00157 /* termination. */ 00158 if (iter == 0) 00159 par = 0.; 00160 return; 00161 } 00162 00163 template <typename Scalar> 00164 void lmpar2( 00165 const ColPivHouseholderQR<Matrix< Scalar, Dynamic, Dynamic> > &qr, 00166 const Matrix< Scalar, Dynamic, 1 > &diag, 00167 const Matrix< Scalar, Dynamic, 1 > &qtb, 00168 Scalar delta, 00169 Scalar &par, 00170 Matrix< Scalar, Dynamic, 1 > &x) 00171 00172 { 00173 using std::sqrt; 00174 using std::abs; 00175 typedef DenseIndex Index; 00176 00177 /* Local variables */ 00178 Index j; 00179 Scalar fp; 00180 Scalar parc, parl; 00181 Index iter; 00182 Scalar temp, paru; 00183 Scalar gnorm; 00184 Scalar dxnorm; 00185 00186 00187 /* Function Body */ 00188 const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); 00189 const Index n = qr.matrixQR().cols(); 00190 eigen_assert(n==diag.size()); 00191 eigen_assert(n==qtb.size()); 00192 00193 Matrix< Scalar, Dynamic, 1 > wa1, wa2; 00194 00195 /* compute and store in x the gauss-newton direction. if the */ 00196 /* jacobian is rank-deficient, obtain a least squares solution. */ 00197 00198 // const Index rank = qr.nonzeroPivots(); // exactly double(0.) 00199 const Index rank = qr.rank(); // use a threshold 00200 wa1 = qtb; 00201 wa1.tail(n-rank).setZero(); 00202 qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank)); 00203 00204 x = qr.colsPermutation()*wa1; 00205 00206 /* initialize the iteration counter. */ 00207 /* evaluate the function at the origin, and test */ 00208 /* for acceptance of the gauss-newton direction. */ 00209 iter = 0; 00210 wa2 = diag.cwiseProduct(x); 00211 dxnorm = wa2.blueNorm(); 00212 fp = dxnorm - delta; 00213 if (fp <= Scalar(0.1) * delta) { 00214 par = 0; 00215 return; 00216 } 00217 00218 /* if the jacobian is not rank deficient, the newton */ 00219 /* step provides a lower bound, parl, for the zero of */ 00220 /* the function. otherwise set this bound to zero. */ 00221 parl = 0.; 00222 if (rank==n) { 00223 wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; 00224 qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); 00225 temp = wa1.blueNorm(); 00226 parl = fp / delta / temp / temp; 00227 } 00228 00229 /* calculate an upper bound, paru, for the zero of the function. */ 00230 for (j = 0; j < n; ++j) 00231 wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; 00232 00233 gnorm = wa1.stableNorm(); 00234 paru = gnorm / delta; 00235 if (paru == 0.) 00236 paru = dwarf / (std::min)(delta,Scalar(0.1)); 00237 00238 /* if the input par lies outside of the interval (parl,paru), */ 00239 /* set par to the closer endpoint. */ 00240 par = (std::max)(par,parl); 00241 par = (std::min)(par,paru); 00242 if (par == 0.) 00243 par = gnorm / dxnorm; 00244 00245 /* beginning of an iteration. */ 00246 Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR(); 00247 while (true) { 00248 ++iter; 00249 00250 /* evaluate the function at the current value of par. */ 00251 if (par == 0.) 00252 par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 00253 wa1 = sqrt(par)* diag; 00254 00255 Matrix< Scalar, Dynamic, 1 > sdiag(n); 00256 qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag); 00257 00258 wa2 = diag.cwiseProduct(x); 00259 dxnorm = wa2.blueNorm(); 00260 temp = fp; 00261 fp = dxnorm - delta; 00262 00263 /* if the function is small enough, accept the current value */ 00264 /* of par. also test for the exceptional cases where parl */ 00265 /* is zero or the number of iterations has reached 10. */ 00266 if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 00267 break; 00268 00269 /* compute the newton correction. */ 00270 wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); 00271 // we could almost use this here, but the diagonal is outside qr, in sdiag[] 00272 // qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); 00273 for (j = 0; j < n; ++j) { 00274 wa1[j] /= sdiag[j]; 00275 temp = wa1[j]; 00276 for (Index i = j+1; i < n; ++i) 00277 wa1[i] -= s(i,j) * temp; 00278 } 00279 temp = wa1.blueNorm(); 00280 parc = fp / delta / temp / temp; 00281 00282 /* depending on the sign of the function, update parl or paru. */ 00283 if (fp > 0.) 00284 parl = (std::max)(parl,par); 00285 if (fp < 0.) 00286 paru = (std::min)(paru,par); 00287 00288 /* compute an improved estimate for par. */ 00289 par = (std::max)(parl,par+parc); 00290 } 00291 if (iter == 0) 00292 par = 0.; 00293 return; 00294 } 00295 00296 } // end namespace internal 00297 00298 } // end namespace Eigen