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