$treeview $search $mathjax
Eigen-unsupported
3.2.5
$projectbrief
|
$projectbrief
|
$searchbox |
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 Index> 00023 void lmqrsolv( 00024 Matrix<Scalar,Rows,Cols> &s, 00025 const PermutationMatrix<Dynamic,Dynamic,Index> &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 00032 /* Local variables */ 00033 Index i, j, k, l; 00034 Scalar temp; 00035 Index n = s.cols(); 00036 Matrix<Scalar,Dynamic,1> wa(n); 00037 JacobiRotation<Scalar> givens; 00038 00039 /* Function Body */ 00040 // the following will only change the lower triangular part of s, including 00041 // the diagonal, though the diagonal is restored afterward 00042 00043 /* copy r and (q transpose)*b to preserve input and initialize s. */ 00044 /* in particular, save the diagonal elements of r in x. */ 00045 x = s.diagonal(); 00046 wa = qtb; 00047 00048 00049 s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose(); 00050 /* eliminate the diagonal matrix d using a givens rotation. */ 00051 for (j = 0; j < n; ++j) { 00052 00053 /* prepare the row of d to be eliminated, locating the */ 00054 /* diagonal element using p from the qr factorization. */ 00055 l = iPerm.indices()(j); 00056 if (diag[l] == 0.) 00057 break; 00058 sdiag.tail(n-j).setZero(); 00059 sdiag[j] = diag[l]; 00060 00061 /* the transformations to eliminate the row of d */ 00062 /* modify only a single element of (q transpose)*b */ 00063 /* beyond the first n, which is initially zero. */ 00064 Scalar qtbpj = 0.; 00065 for (k = j; k < n; ++k) { 00066 /* determine a givens rotation which eliminates the */ 00067 /* appropriate element in the current row of d. */ 00068 givens.makeGivens(-s(k,k), sdiag[k]); 00069 00070 /* compute the modified diagonal element of r and */ 00071 /* the modified element of ((q transpose)*b,0). */ 00072 s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k]; 00073 temp = givens.c() * wa[k] + givens.s() * qtbpj; 00074 qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; 00075 wa[k] = temp; 00076 00077 /* accumulate the tranformation in the row of s. */ 00078 for (i = k+1; i<n; ++i) { 00079 temp = givens.c() * s(i,k) + givens.s() * sdiag[i]; 00080 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i]; 00081 s(i,k) = temp; 00082 } 00083 } 00084 } 00085 00086 /* solve the triangular system for z. if the system is */ 00087 /* singular, then obtain a least squares solution. */ 00088 Index nsing; 00089 for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {} 00090 00091 wa.tail(n-nsing).setZero(); 00092 s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing)); 00093 00094 // restore 00095 sdiag = s.diagonal(); 00096 s.diagonal() = x; 00097 00098 /* permute the components of z back to components of x. */ 00099 x = iPerm * wa; 00100 } 00101 00102 template <typename Scalar, int _Options, typename Index> 00103 void lmqrsolv( 00104 SparseMatrix<Scalar,_Options,Index> &s, 00105 const PermutationMatrix<Dynamic,Dynamic> &iPerm, 00106 const Matrix<Scalar,Dynamic,1> &diag, 00107 const Matrix<Scalar,Dynamic,1> &qtb, 00108 Matrix<Scalar,Dynamic,1> &x, 00109 Matrix<Scalar,Dynamic,1> &sdiag) 00110 { 00111 /* Local variables */ 00112 typedef SparseMatrix<Scalar,RowMajor,Index> FactorType; 00113 Index i, j, k, l; 00114 Scalar temp; 00115 Index n = s.cols(); 00116 Matrix<Scalar,Dynamic,1> wa(n); 00117 JacobiRotation<Scalar> givens; 00118 00119 /* Function Body */ 00120 // the following will only change the lower triangular part of s, including 00121 // the diagonal, though the diagonal is restored afterward 00122 00123 /* copy r and (q transpose)*b to preserve input and initialize R. */ 00124 wa = qtb; 00125 FactorType R(s); 00126 // Eliminate the diagonal matrix d using a givens rotation 00127 for (j = 0; j < n; ++j) 00128 { 00129 // Prepare the row of d to be eliminated, locating the 00130 // diagonal element using p from the qr factorization 00131 l = iPerm.indices()(j); 00132 if (diag(l) == Scalar(0)) 00133 break; 00134 sdiag.tail(n-j).setZero(); 00135 sdiag[j] = diag[l]; 00136 // the transformations to eliminate the row of d 00137 // modify only a single element of (q transpose)*b 00138 // beyond the first n, which is initially zero. 00139 00140 Scalar qtbpj = 0; 00141 // Browse the nonzero elements of row j of the upper triangular s 00142 for (k = j; k < n; ++k) 00143 { 00144 typename FactorType::InnerIterator itk(R,k); 00145 for (; itk; ++itk){ 00146 if (itk.index() < k) continue; 00147 else break; 00148 } 00149 //At this point, we have the diagonal element R(k,k) 00150 // Determine a givens rotation which eliminates 00151 // the appropriate element in the current row of d 00152 givens.makeGivens(-itk.value(), sdiag(k)); 00153 00154 // Compute the modified diagonal element of r and 00155 // the modified element of ((q transpose)*b,0). 00156 itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k); 00157 temp = givens.c() * wa(k) + givens.s() * qtbpj; 00158 qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj; 00159 wa(k) = temp; 00160 00161 // Accumulate the transformation in the remaining k row/column of R 00162 for (++itk; itk; ++itk) 00163 { 00164 i = itk.index(); 00165 temp = givens.c() * itk.value() + givens.s() * sdiag(i); 00166 sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i); 00167 itk.valueRef() = temp; 00168 } 00169 } 00170 } 00171 00172 // Solve the triangular system for z. If the system is 00173 // singular, then obtain a least squares solution 00174 Index nsing; 00175 for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {} 00176 00177 wa.tail(n-nsing).setZero(); 00178 // x = wa; 00179 wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing)); 00180 00181 sdiag = R.diagonal(); 00182 // Permute the components of z back to components of x 00183 x = iPerm * wa; 00184 } 00185 } // end namespace internal 00186 00187 } // end namespace Eigen 00188 00189 #endif // EIGEN_LMQRSOLV_H