src/LevenbergMarquardt/LMqrsolv.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
0005 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
0006 //
0007 // This code initially comes from MINPACK whose original authors are:
0008 // Copyright Jorge More - Argonne National Laboratory
0009 // Copyright Burt Garbow - Argonne National Laboratory
0010 // Copyright Ken Hillstrom - Argonne National Laboratory
0011 //
0012 // This Source Code Form is subject to the terms of the Minpack license
0013 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
0014
0015 #ifndef EIGEN_LMQRSOLV_H
0016 #define EIGEN_LMQRSOLV_H
0017
0018 namespace Eigen {
0019
0020 namespace internal {
0021
0022 template <typename Scalar,int Rows, int Cols, typename PermIndex>
0023 void lmqrsolv(
0024   Matrix<Scalar,Rows,Cols> &s,
0025   const PermutationMatrix<Dynamic,Dynamic,PermIndex> &iPerm,
0026   const Matrix<Scalar,Dynamic,1> &diag,
0027   const Matrix<Scalar,Dynamic,1> &qtb,
0028   Matrix<Scalar,Dynamic,1> &x,
0029   Matrix<Scalar,Dynamic,1> &sdiag)
0030 {
0031     /* Local variables */
0032     Index i, j, k;
0033     Scalar temp;
0034     Index n = s.cols();
0035     Matrix<Scalar,Dynamic,1>  wa(n);
0036     JacobiRotation<Scalar> givens;
0037
0038     /* Function Body */
0039     // the following will only change the lower triangular part of s, including
0040     // the diagonal, though the diagonal is restored afterward
0041
0042     /*     copy r and (q transpose)*b to preserve input and initialize s. */
0043     /*     in particular, save the diagonal elements of r in x. */
0044     x = s.diagonal();
0045     wa = qtb;
0046
0047
0048     s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
0049     /*     eliminate the diagonal matrix d using a givens rotation. */
0050     for (j = 0; j < n; ++j) {
0051
0052         /*        prepare the row of d to be eliminated, locating the */
0053         /*        diagonal element using p from the qr factorization. */
0054         const PermIndex l = iPerm.indices()(j);
0055         if (diag[l] == 0.)
0056             break;
0057         sdiag.tail(n-j).setZero();
0058         sdiag[j] = diag[l];
0059
0060         /*        the transformations to eliminate the row of d */
0061         /*        modify only a single element of (q transpose)*b */
0062         /*        beyond the first n, which is initially zero. */
0063         Scalar qtbpj = 0.;
0064         for (k = j; k < n; ++k) {
0065             /*           determine a givens rotation which eliminates the */
0066             /*           appropriate element in the current row of d. */
0067             givens.makeGivens(-s(k,k), sdiag[k]);
0068
0069             /*           compute the modified diagonal element of r and */
0070             /*           the modified element of ((q transpose)*b,0). */
0071             s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
0072             temp = givens.c() * wa[k] + givens.s() * qtbpj;
0073             qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
0074             wa[k] = temp;
0075
0076             /*           accumulate the transformation in the row of s. */
0077             for (i = k+1; i<n; ++i) {
0078                 temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
0079                 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
0080                 s(i,k) = temp;
0081             }
0082         }
0083     }
0084
0085     /*     solve the triangular system for z. if the system is */
0086     /*     singular, then obtain a least squares solution. */
0087     Index nsing;
0088     for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
0089
0090     wa.tail(n-nsing).setZero();
0091     s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
0092
0093     // restore
0094     sdiag = s.diagonal();
0095     s.diagonal() = x;
0096
0097     /* permute the components of z back to components of x. */
0098     x = iPerm * wa;
0099 }
0100
0101 template <typename Scalar, int _Options, typename Index>
0102 void lmqrsolv(
0103   SparseMatrix<Scalar,_Options,Index> &s,
0104   const PermutationMatrix<Dynamic,Dynamic> &iPerm,
0105   const Matrix<Scalar,Dynamic,1> &diag,
0106   const Matrix<Scalar,Dynamic,1> &qtb,
0107   Matrix<Scalar,Dynamic,1> &x,
0108   Matrix<Scalar,Dynamic,1> &sdiag)
0109 {
0110   /* Local variables */
0111   typedef SparseMatrix<Scalar,RowMajor,Index> FactorType;
0112     Index i, j, k, l;
0113     Scalar temp;
0114     Index n = s.cols();
0115     Matrix<Scalar,Dynamic,1>  wa(n);
0116     JacobiRotation<Scalar> givens;
0117
0118     /* Function Body */
0119     // the following will only change the lower triangular part of s, including
0120     // the diagonal, though the diagonal is restored afterward
0121
0122     /*     copy r and (q transpose)*b to preserve input and initialize R. */
0123     wa = qtb;
0124     FactorType R(s);
0125     // Eliminate the diagonal matrix d using a givens rotation
0126     for (j = 0; j < n; ++j)
0127     {
0128       // Prepare the row of d to be eliminated, locating the
0129       // diagonal element using p from the qr factorization
0130       l = iPerm.indices()(j);
0131       if (diag(l) == Scalar(0))
0132         break;
0133       sdiag.tail(n-j).setZero();
0134       sdiag[j] = diag[l];
0135       // the transformations to eliminate the row of d
0136       // modify only a single element of (q transpose)*b
0137       // beyond the first n, which is initially zero.
0138
0139       Scalar qtbpj = 0;
0140       // Browse the nonzero elements of row j of the upper triangular s
0141       for (k = j; k < n; ++k)
0142       {
0143         typename FactorType::InnerIterator itk(R,k);
0144         for (; itk; ++itk){
0145           if (itk.index() < k) continue;
0146           else break;
0147         }
0148         //At this point, we have the diagonal element R(k,k)
0149         // Determine a givens rotation which eliminates
0150         // the appropriate element in the current row of d
0151         givens.makeGivens(-itk.value(), sdiag(k));
0152
0153         // Compute the modified diagonal element of r and
0154         // the modified element of ((q transpose)*b,0).
0155         itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k);
0156         temp = givens.c() * wa(k) + givens.s() * qtbpj;
0157         qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj;
0158         wa(k) = temp;
0159
0160         // Accumulate the transformation in the remaining k row/column of R
0161         for (++itk; itk; ++itk)
0162         {
0163           i = itk.index();
0164           temp = givens.c() *  itk.value() + givens.s() * sdiag(i);
0165           sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i);
0166           itk.valueRef() = temp;
0167         }
0168       }
0169     }
0170
0171     // Solve the triangular system for z. If the system is
0172     // singular, then obtain a least squares solution
0173     Index nsing;
0174     for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {}
0175
0176     wa.tail(n-nsing).setZero();
0177 //     x = wa;
0178     wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing));
0179
0180     sdiag = R.diagonal();
0181     // Permute the components of z back to components of x
0182     x = iPerm * wa;
0183 }
0184 } // end namespace internal
0185
0186 } // end namespace Eigen
0187
0188 #endif // EIGEN_LMQRSOLV_H