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