1 namespace Eigen { 2 3 namespace internal { 4 5 template <typename Scalar> 6 void lmpar( 7 Matrix< Scalar, Dynamic, Dynamic > &r, 8 const VectorXi &ipvt, 9 const Matrix< Scalar, Dynamic, 1 > &diag, 10 const Matrix< Scalar, Dynamic, 1 > &qtb, 11 Scalar delta, 12 Scalar &par, 13 Matrix< Scalar, Dynamic, 1 > &x) 14 { 15 using std::abs; 16 using std::sqrt; 17 typedef DenseIndex Index; 18 19 /* Local variables */ 20 Index i, j, l; 21 Scalar fp; 22 Scalar parc, parl; 23 Index iter; 24 Scalar temp, paru; 25 Scalar gnorm; 26 Scalar dxnorm; 27 28 29 /* Function Body */ 30 const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); 31 const Index n = r.cols(); 32 eigen_assert(n==diag.size()); 33 eigen_assert(n==qtb.size()); 34 eigen_assert(n==x.size()); 35 36 Matrix< Scalar, Dynamic, 1 > wa1, wa2; 37 38 /* compute and store in x the gauss-newton direction. if the */ 39 /* jacobian is rank-deficient, obtain a least squares solution. */ 40 Index nsing = n-1; 41 wa1 = qtb; 42 for (j = 0; j < n; ++j) { 43 if (r(j,j) == 0. && nsing == n-1) 44 nsing = j - 1; 45 if (nsing < n-1) 46 wa1[j] = 0.; 47 } 48 for (j = nsing; j>=0; --j) { 49 wa1[j] /= r(j,j); 50 temp = wa1[j]; 51 for (i = 0; i < j ; ++i) 52 wa1[i] -= r(i,j) * temp; 53 } 54 55 for (j = 0; j < n; ++j) 56 x[ipvt[j]] = wa1[j]; 57 58 /* initialize the iteration counter. */ 59 /* evaluate the function at the origin, and test */ 60 /* for acceptance of the gauss-newton direction. */ 61 iter = 0; 62 wa2 = diag.cwiseProduct(x); 63 dxnorm = wa2.blueNorm(); 64 fp = dxnorm - delta; 65 if (fp <= Scalar(0.1) * delta) { 66 par = 0; 67 return; 68 } 69 70 /* if the jacobian is not rank deficient, the newton */ 71 /* step provides a lower bound, parl, for the zero of */ 72 /* the function. otherwise set this bound to zero. */ 73 parl = 0.; 74 if (nsing >= n-1) { 75 for (j = 0; j < n; ++j) { 76 l = ipvt[j]; 77 wa1[j] = diag[l] * (wa2[l] / dxnorm); 78 } 79 // it's actually a triangularView.solveInplace(), though in a weird 80 // way: 81 for (j = 0; j < n; ++j) { 82 Scalar sum = 0.; 83 for (i = 0; i < j; ++i) 84 sum += r(i,j) * wa1[i]; 85 wa1[j] = (wa1[j] - sum) / r(j,j); 86 } 87 temp = wa1.blueNorm(); 88 parl = fp / delta / temp / temp; 89 } 90 91 /* calculate an upper bound, paru, for the zero of the function. */ 92 for (j = 0; j < n; ++j) 93 wa1[j] = r.col(j).head(j+1).dot(qtb.head(j+1)) / diag[ipvt[j]]; 94 95 gnorm = wa1.stableNorm(); 96 paru = gnorm / delta; 97 if (paru == 0.) 98 paru = dwarf / (std::min)(delta,Scalar(0.1)); 99 100 /* if the input par lies outside of the interval (parl,paru), */ 101 /* set par to the closer endpoint. */ 102 par = (std::max)(par,parl); 103 par = (std::min)(par,paru); 104 if (par == 0.) 105 par = gnorm / dxnorm; 106 107 /* beginning of an iteration. */ 108 while (true) { 109 ++iter; 110 111 /* evaluate the function at the current value of par. */ 112 if (par == 0.) 113 par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 114 wa1 = sqrt(par)* diag; 115 116 Matrix< Scalar, Dynamic, 1 > sdiag(n); 117 qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag); 118 119 wa2 = diag.cwiseProduct(x); 120 dxnorm = wa2.blueNorm(); 121 temp = fp; 122 fp = dxnorm - delta; 123 124 /* if the function is small enough, accept the current value */ 125 /* of par. also test for the exceptional cases where parl */ 126 /* is zero or the number of iterations has reached 10. */ 127 if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 128 break; 129 130 /* compute the newton correction. */ 131 for (j = 0; j < n; ++j) { 132 l = ipvt[j]; 133 wa1[j] = diag[l] * (wa2[l] / dxnorm); 134 } 135 for (j = 0; j < n; ++j) { 136 wa1[j] /= sdiag[j]; 137 temp = wa1[j]; 138 for (i = j+1; i < n; ++i) 139 wa1[i] -= r(i,j) * temp; 140 } 141 temp = wa1.blueNorm(); 142 parc = fp / delta / temp / temp; 143 144 /* depending on the sign of the function, update parl or paru. */ 145 if (fp > 0.) 146 parl = (std::max)(parl,par); 147 if (fp < 0.) 148 paru = (std::min)(paru,par); 149 150 /* compute an improved estimate for par. */ 151 /* Computing MAX */ 152 par = (std::max)(parl,par+parc); 153 154 /* end of an iteration. */ 155 } 156 157 /* termination. */ 158 if (iter == 0) 159 par = 0.; 160 return; 161 } 162 163 template <typename Scalar> 164 void lmpar2( 165 const ColPivHouseholderQR<Matrix< Scalar, Dynamic, Dynamic> > &qr, 166 const Matrix< Scalar, Dynamic, 1 > &diag, 167 const Matrix< Scalar, Dynamic, 1 > &qtb, 168 Scalar delta, 169 Scalar &par, 170 Matrix< Scalar, Dynamic, 1 > &x) 171 172 { 173 using std::sqrt; 174 using std::abs; 175 typedef DenseIndex Index; 176 177 /* Local variables */ 178 Index j; 179 Scalar fp; 180 Scalar parc, parl; 181 Index iter; 182 Scalar temp, paru; 183 Scalar gnorm; 184 Scalar dxnorm; 185 186 187 /* Function Body */ 188 const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); 189 const Index n = qr.matrixQR().cols(); 190 eigen_assert(n==diag.size()); 191 eigen_assert(n==qtb.size()); 192 193 Matrix< Scalar, Dynamic, 1 > wa1, wa2; 194 195 /* compute and store in x the gauss-newton direction. if the */ 196 /* jacobian is rank-deficient, obtain a least squares solution. */ 197 198 // const Index rank = qr.nonzeroPivots(); // exactly double(0.) 199 const Index rank = qr.rank(); // use a threshold 200 wa1 = qtb; 201 wa1.tail(n-rank).setZero(); 202 qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank)); 203 204 x = qr.colsPermutation()*wa1; 205 206 /* initialize the iteration counter. */ 207 /* evaluate the function at the origin, and test */ 208 /* for acceptance of the gauss-newton direction. */ 209 iter = 0; 210 wa2 = diag.cwiseProduct(x); 211 dxnorm = wa2.blueNorm(); 212 fp = dxnorm - delta; 213 if (fp <= Scalar(0.1) * delta) { 214 par = 0; 215 return; 216 } 217 218 /* if the jacobian is not rank deficient, the newton */ 219 /* step provides a lower bound, parl, for the zero of */ 220 /* the function. otherwise set this bound to zero. */ 221 parl = 0.; 222 if (rank==n) { 223 wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; 224 qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); 225 temp = wa1.blueNorm(); 226 parl = fp / delta / temp / temp; 227 } 228 229 /* calculate an upper bound, paru, for the zero of the function. */ 230 for (j = 0; j < n; ++j) 231 wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; 232 233 gnorm = wa1.stableNorm(); 234 paru = gnorm / delta; 235 if (paru == 0.) 236 paru = dwarf / (std::min)(delta,Scalar(0.1)); 237 238 /* if the input par lies outside of the interval (parl,paru), */ 239 /* set par to the closer endpoint. */ 240 par = (std::max)(par,parl); 241 par = (std::min)(par,paru); 242 if (par == 0.) 243 par = gnorm / dxnorm; 244 245 /* beginning of an iteration. */ 246 Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR(); 247 while (true) { 248 ++iter; 249 250 /* evaluate the function at the current value of par. */ 251 if (par == 0.) 252 par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 253 wa1 = sqrt(par)* diag; 254 255 Matrix< Scalar, Dynamic, 1 > sdiag(n); 256 qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag); 257 258 wa2 = diag.cwiseProduct(x); 259 dxnorm = wa2.blueNorm(); 260 temp = fp; 261 fp = dxnorm - delta; 262 263 /* if the function is small enough, accept the current value */ 264 /* of par. also test for the exceptional cases where parl */ 265 /* is zero or the number of iterations has reached 10. */ 266 if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 267 break; 268 269 /* compute the newton correction. */ 270 wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); 271 // we could almost use this here, but the diagonal is outside qr, in sdiag[] 272 // qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); 273 for (j = 0; j < n; ++j) { 274 wa1[j] /= sdiag[j]; 275 temp = wa1[j]; 276 for (Index i = j+1; i < n; ++i) 277 wa1[i] -= s(i,j) * temp; 278 } 279 temp = wa1.blueNorm(); 280 parc = fp / delta / temp / temp; 281 282 /* depending on the sign of the function, update parl or paru. */ 283 if (fp > 0.) 284 parl = (std::max)(parl,par); 285 if (fp < 0.) 286 paru = (std::min)(paru,par); 287 288 /* compute an improved estimate for par. */ 289 par = (std::max)(parl,par+parc); 290 } 291 if (iter == 0) 292 par = 0.; 293 return; 294 } 295 296 } // end namespace internal 297 298 } // end namespace Eigen 299
