![]() |
Eigen-unsupported
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> 00005 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr> 00006 // 00007 // This code initially comes from MINPACK whose original authors are: 00008 // Copyright Jorge More - Argonne National Laboratory 00009 // Copyright Burt Garbow - Argonne National Laboratory 00010 // Copyright Ken Hillstrom - Argonne National Laboratory 00011 // 00012 // This Source Code Form is subject to the terms of the Minpack license 00013 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 00014 00015 #ifndef EIGEN_LMQRSOLV_H 00016 #define EIGEN_LMQRSOLV_H 00017 00018 namespace Eigen { 00019 00020 namespace internal { 00021 00022 template <typename Scalar,int Rows, int Cols, typename PermIndex> 00023 void lmqrsolv( 00024 Matrix<Scalar,Rows,Cols> &s, 00025 const PermutationMatrix<Dynamic,Dynamic,PermIndex> &iPerm, 00026 const Matrix<Scalar,Dynamic,1> &diag, 00027 const Matrix<Scalar,Dynamic,1> &qtb, 00028 Matrix<Scalar,Dynamic,1> &x, 00029 Matrix<Scalar,Dynamic,1> &sdiag) 00030 { 00031 /* Local variables */ 00032 Index i, j, k; 00033 Scalar temp; 00034 Index n = s.cols(); 00035 Matrix<Scalar,Dynamic,1> wa(n); 00036 JacobiRotation<Scalar> givens; 00037 00038 /* Function Body */ 00039 // the following will only change the lower triangular part of s, including 00040 // the diagonal, though the diagonal is restored afterward 00041 00042 /* copy r and (q transpose)*b to preserve input and initialize s. */ 00043 /* in particular, save the diagonal elements of r in x. */ 00044 x = s.diagonal(); 00045 wa = qtb; 00046 00047 00048 s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose(); 00049 /* eliminate the diagonal matrix d using a givens rotation. */ 00050 for (j = 0; j < n; ++j) { 00051 00052 /* prepare the row of d to be eliminated, locating the */ 00053 /* diagonal element using p from the qr factorization. */ 00054 const PermIndex l = iPerm.indices()(j); 00055 if (diag[l] == 0.) 00056 break; 00057 sdiag.tail(n-j).setZero(); 00058 sdiag[j] = diag[l]; 00059 00060 /* the transformations to eliminate the row of d */ 00061 /* modify only a single element of (q transpose)*b */ 00062 /* beyond the first n, which is initially zero. */ 00063 Scalar qtbpj = 0.; 00064 for (k = j; k < n; ++k) { 00065 /* determine a givens rotation which eliminates the */ 00066 /* appropriate element in the current row of d. */ 00067 givens.makeGivens(-s(k,k), sdiag[k]); 00068 00069 /* compute the modified diagonal element of r and */ 00070 /* the modified element of ((q transpose)*b,0). */ 00071 s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k]; 00072 temp = givens.c() * wa[k] + givens.s() * qtbpj; 00073 qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; 00074 wa[k] = temp; 00075 00076 /* accumulate the tranformation in the row of s. */ 00077 for (i = k+1; i<n; ++i) { 00078 temp = givens.c() * s(i,k) + givens.s() * sdiag[i]; 00079 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i]; 00080 s(i,k) = temp; 00081 } 00082 } 00083 } 00084 00085 /* solve the triangular system for z. if the system is */ 00086 /* singular, then obtain a least squares solution. */ 00087 Index nsing; 00088 for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {} 00089 00090 wa.tail(n-nsing).setZero(); 00091 s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing)); 00092 00093 // restore 00094 sdiag = s.diagonal(); 00095 s.diagonal() = x; 00096 00097 /* permute the components of z back to components of x. */ 00098 x = iPerm * wa; 00099 } 00100 00101 template <typename Scalar, int _Options, typename Index> 00102 void lmqrsolv( 00103 SparseMatrix<Scalar,_Options,Index> &s, 00104 const PermutationMatrix<Dynamic,Dynamic> &iPerm, 00105 const Matrix<Scalar,Dynamic,1> &diag, 00106 const Matrix<Scalar,Dynamic,1> &qtb, 00107 Matrix<Scalar,Dynamic,1> &x, 00108 Matrix<Scalar,Dynamic,1> &sdiag) 00109 { 00110 /* Local variables */ 00111 typedef SparseMatrix<Scalar,RowMajor,Index> FactorType; 00112 Index i, j, k, l; 00113 Scalar temp; 00114 Index n = s.cols(); 00115 Matrix<Scalar,Dynamic,1> wa(n); 00116 JacobiRotation<Scalar> givens; 00117 00118 /* Function Body */ 00119 // the following will only change the lower triangular part of s, including 00120 // the diagonal, though the diagonal is restored afterward 00121 00122 /* copy r and (q transpose)*b to preserve input and initialize R. */ 00123 wa = qtb; 00124 FactorType R(s); 00125 // Eliminate the diagonal matrix d using a givens rotation 00126 for (j = 0; j < n; ++j) 00127 { 00128 // Prepare the row of d to be eliminated, locating the 00129 // diagonal element using p from the qr factorization 00130 l = iPerm.indices()(j); 00131 if (diag(l) == Scalar(0)) 00132 break; 00133 sdiag.tail(n-j).setZero(); 00134 sdiag[j] = diag[l]; 00135 // the transformations to eliminate the row of d 00136 // modify only a single element of (q transpose)*b 00137 // beyond the first n, which is initially zero. 00138 00139 Scalar qtbpj = 0; 00140 // Browse the nonzero elements of row j of the upper triangular s 00141 for (k = j; k < n; ++k) 00142 { 00143 typename FactorType::InnerIterator itk(R,k); 00144 for (; itk; ++itk){ 00145 if (itk.index() < k) continue; 00146 else break; 00147 } 00148 //At this point, we have the diagonal element R(k,k) 00149 // Determine a givens rotation which eliminates 00150 // the appropriate element in the current row of d 00151 givens.makeGivens(-itk.value(), sdiag(k)); 00152 00153 // Compute the modified diagonal element of r and 00154 // the modified element of ((q transpose)*b,0). 00155 itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k); 00156 temp = givens.c() * wa(k) + givens.s() * qtbpj; 00157 qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj; 00158 wa(k) = temp; 00159 00160 // Accumulate the transformation in the remaining k row/column of R 00161 for (++itk; itk; ++itk) 00162 { 00163 i = itk.index(); 00164 temp = givens.c() * itk.value() + givens.s() * sdiag(i); 00165 sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i); 00166 itk.valueRef() = temp; 00167 } 00168 } 00169 } 00170 00171 // Solve the triangular system for z. If the system is 00172 // singular, then obtain a least squares solution 00173 Index nsing; 00174 for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {} 00175 00176 wa.tail(n-nsing).setZero(); 00177 // x = wa; 00178 wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing)); 00179 00180 sdiag = R.diagonal(); 00181 // Permute the components of z back to components of x 00182 x = iPerm * wa; 00183 } 00184 } // end namespace internal 00185 00186 } // end namespace Eigen 00187 00188 #endif // EIGEN_LMQRSOLV_H