1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2009 Thomas Capricelli <orzel (at) freehackers.org> 5 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam (at) inria.fr> 6 // 7 // This code initially comes from MINPACK whose original authors are: 8 // Copyright Jorge More - Argonne National Laboratory 9 // Copyright Burt Garbow - Argonne National Laboratory 10 // Copyright Ken Hillstrom - Argonne National Laboratory 11 // 12 // This Source Code Form is subject to the terms of the Minpack license 13 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 14 15 #ifndef EIGEN_LMQRSOLV_H 16 #define EIGEN_LMQRSOLV_H 17 18 namespace Eigen { 19 20 namespace internal { 21 22 template <typename Scalar,int Rows, int Cols, typename PermIndex> 23 void lmqrsolv( 24 Matrix<Scalar,Rows,Cols> &s, 25 const PermutationMatrix<Dynamic,Dynamic,PermIndex> &iPerm, 26 const Matrix<Scalar,Dynamic,1> &diag, 27 const Matrix<Scalar,Dynamic,1> &qtb, 28 Matrix<Scalar,Dynamic,1> &x, 29 Matrix<Scalar,Dynamic,1> &sdiag) 30 { 31 /* Local variables */ 32 Index i, j, k; 33 Scalar temp; 34 Index n = s.cols(); 35 Matrix<Scalar,Dynamic,1> wa(n); 36 JacobiRotation<Scalar> givens; 37 38 /* Function Body */ 39 // the following will only change the lower triangular part of s, including 40 // the diagonal, though the diagonal is restored afterward 41 42 /* copy r and (q transpose)*b to preserve input and initialize s. */ 43 /* in particular, save the diagonal elements of r in x. */ 44 x = s.diagonal(); 45 wa = qtb; 46 47 48 s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose(); 49 /* eliminate the diagonal matrix d using a givens rotation. */ 50 for (j = 0; j < n; ++j) { 51 52 /* prepare the row of d to be eliminated, locating the */ 53 /* diagonal element using p from the qr factorization. */ 54 const PermIndex l = iPerm.indices()(j); 55 if (diag[l] == 0.) 56 break; 57 sdiag.tail(n-j).setZero(); 58 sdiag[j] = diag[l]; 59 60 /* the transformations to eliminate the row of d */ 61 /* modify only a single element of (q transpose)*b */ 62 /* beyond the first n, which is initially zero. */ 63 Scalar qtbpj = 0.; 64 for (k = j; k < n; ++k) { 65 /* determine a givens rotation which eliminates the */ 66 /* appropriate element in the current row of d. */ 67 givens.makeGivens(-s(k,k), sdiag[k]); 68 69 /* compute the modified diagonal element of r and */ 70 /* the modified element of ((q transpose)*b,0). */ 71 s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k]; 72 temp = givens.c() * wa[k] + givens.s() * qtbpj; 73 qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; 74 wa[k] = temp; 75 76 /* accumulate the tranformation in the row of s. */ 77 for (i = k+1; i<n; ++i) { 78 temp = givens.c() * s(i,k) + givens.s() * sdiag[i]; 79 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i]; 80 s(i,k) = temp; 81 } 82 } 83 } 84 85 /* solve the triangular system for z. if the system is */ 86 /* singular, then obtain a least squares solution. */ 87 Index nsing; 88 for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {} 89 90 wa.tail(n-nsing).setZero(); 91 s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing)); 92 93 // restore 94 sdiag = s.diagonal(); 95 s.diagonal() = x; 96 97 /* permute the components of z back to components of x. */ 98 x = iPerm * wa; 99 } 100 101 template <typename Scalar, int _Options, typename Index> 102 void lmqrsolv( 103 SparseMatrix<Scalar,_Options,Index> &s, 104 const PermutationMatrix<Dynamic,Dynamic> &iPerm, 105 const Matrix<Scalar,Dynamic,1> &diag, 106 const Matrix<Scalar,Dynamic,1> &qtb, 107 Matrix<Scalar,Dynamic,1> &x, 108 Matrix<Scalar,Dynamic,1> &sdiag) 109 { 110 /* Local variables */ 111 typedef SparseMatrix<Scalar,RowMajor,Index> FactorType; 112 Index i, j, k, l; 113 Scalar temp; 114 Index n = s.cols(); 115 Matrix<Scalar,Dynamic,1> wa(n); 116 JacobiRotation<Scalar> givens; 117 118 /* Function Body */ 119 // the following will only change the lower triangular part of s, including 120 // the diagonal, though the diagonal is restored afterward 121 122 /* copy r and (q transpose)*b to preserve input and initialize R. */ 123 wa = qtb; 124 FactorType R(s); 125 // Eliminate the diagonal matrix d using a givens rotation 126 for (j = 0; j < n; ++j) 127 { 128 // Prepare the row of d to be eliminated, locating the 129 // diagonal element using p from the qr factorization 130 l = iPerm.indices()(j); 131 if (diag(l) == Scalar(0)) 132 break; 133 sdiag.tail(n-j).setZero(); 134 sdiag[j] = diag[l]; 135 // the transformations to eliminate the row of d 136 // modify only a single element of (q transpose)*b 137 // beyond the first n, which is initially zero. 138 139 Scalar qtbpj = 0; 140 // Browse the nonzero elements of row j of the upper triangular s 141 for (k = j; k < n; ++k) 142 { 143 typename FactorType::InnerIterator itk(R,k); 144 for (; itk; ++itk){ 145 if (itk.index() < k) continue; 146 else break; 147 } 148 //At this point, we have the diagonal element R(k,k) 149 // Determine a givens rotation which eliminates 150 // the appropriate element in the current row of d 151 givens.makeGivens(-itk.value(), sdiag(k)); 152 153 // Compute the modified diagonal element of r and 154 // the modified element of ((q transpose)*b,0). 155 itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k); 156 temp = givens.c() * wa(k) + givens.s() * qtbpj; 157 qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj; 158 wa(k) = temp; 159 160 // Accumulate the transformation in the remaining k row/column of R 161 for (++itk; itk; ++itk) 162 { 163 i = itk.index(); 164 temp = givens.c() * itk.value() + givens.s() * sdiag(i); 165 sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i); 166 itk.valueRef() = temp; 167 } 168 } 169 } 170 171 // Solve the triangular system for z. If the system is 172 // singular, then obtain a least squares solution 173 Index nsing; 174 for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {} 175 176 wa.tail(n-nsing).setZero(); 177 // x = wa; 178 wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing)); 179 180 sdiag = R.diagonal(); 181 // Permute the components of z back to components of x 182 x = iPerm * wa; 183 } 184 } // end namespace internal 185 186 } // end namespace Eigen 187 188 #endif // EIGEN_LMQRSOLV_H 189
