17faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This file is part of Eigen, a lightweight C++ template library 27faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// for linear algebra. 37faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 47faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> 57faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr> 67faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 77faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This code initially comes from MINPACK whose original authors are: 87faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright Jorge More - Argonne National Laboratory 97faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright Burt Garbow - Argonne National Laboratory 107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright Ken Hillstrom - Argonne National Laboratory 117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This Source Code Form is subject to the terms of the Minpack license 137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#ifndef EIGEN_LMQRSOLV_H 167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#define EIGEN_LMQRSOLV_H 177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeznamespace Eigen { 197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeznamespace internal { 217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate <typename Scalar,int Rows, int Cols, typename Index> 237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezvoid lmqrsolv( 247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Matrix<Scalar,Rows,Cols> &s, 257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const PermutationMatrix<Dynamic,Dynamic,Index> &iPerm, 267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Matrix<Scalar,Dynamic,1> &diag, 277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Matrix<Scalar,Dynamic,1> &qtb, 287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Matrix<Scalar,Dynamic,1> &x, 297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Matrix<Scalar,Dynamic,1> &sdiag) 307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* Local variables */ 337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index i, j, k, l; 347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar temp; 357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index n = s.cols(); 367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Matrix<Scalar,Dynamic,1> wa(n); 377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez JacobiRotation<Scalar> givens; 387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* Function Body */ 407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // the following will only change the lower triangular part of s, including 417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // the diagonal, though the diagonal is restored afterward 427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* copy r and (q transpose)*b to preserve input and initialize s. */ 447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* in particular, save the diagonal elements of r in x. */ 457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x = s.diagonal(); 467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa = qtb; 477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose(); 507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* eliminate the diagonal matrix d using a givens rotation. */ 517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (j = 0; j < n; ++j) { 527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* prepare the row of d to be eliminated, locating the */ 547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* diagonal element using p from the qr factorization. */ 557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez l = iPerm.indices()(j); 567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (diag[l] == 0.) 577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez break; 587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sdiag.tail(n-j).setZero(); 597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sdiag[j] = diag[l]; 607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* the transformations to eliminate the row of d */ 627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* modify only a single element of (q transpose)*b */ 637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* beyond the first n, which is initially zero. */ 647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar qtbpj = 0.; 657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (k = j; k < n; ++k) { 667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* determine a givens rotation which eliminates the */ 677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* appropriate element in the current row of d. */ 687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez givens.makeGivens(-s(k,k), sdiag[k]); 697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* compute the modified diagonal element of r and */ 717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* the modified element of ((q transpose)*b,0). */ 727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k]; 737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp = givens.c() * wa[k] + givens.s() * qtbpj; 747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; 757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa[k] = temp; 767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* accumulate the tranformation in the row of s. */ 787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (i = k+1; i<n; ++i) { 797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp = givens.c() * s(i,k) + givens.s() * sdiag[i]; 807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i]; 817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez s(i,k) = temp; 827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* solve the triangular system for z. if the system is */ 877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* singular, then obtain a least squares solution. */ 887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index nsing; 897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {} 907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa.tail(n-nsing).setZero(); 927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing)); 937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // restore 957faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sdiag = s.diagonal(); 967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez s.diagonal() = x; 977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 987faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* permute the components of z back to components of x. */ 997faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x = iPerm * wa; 1007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 1017faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1027faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate <typename Scalar, int _Options, typename Index> 1037faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezvoid lmqrsolv( 1047faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez SparseMatrix<Scalar,_Options,Index> &s, 1057faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const PermutationMatrix<Dynamic,Dynamic> &iPerm, 1067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Matrix<Scalar,Dynamic,1> &diag, 1077faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Matrix<Scalar,Dynamic,1> &qtb, 1087faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Matrix<Scalar,Dynamic,1> &x, 1097faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Matrix<Scalar,Dynamic,1> &sdiag) 1107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 1117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* Local variables */ 1127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef SparseMatrix<Scalar,RowMajor,Index> FactorType; 1137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index i, j, k, l; 1147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar temp; 1157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index n = s.cols(); 1167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Matrix<Scalar,Dynamic,1> wa(n); 1177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez JacobiRotation<Scalar> givens; 1187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* Function Body */ 1207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // the following will only change the lower triangular part of s, including 1217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // the diagonal, though the diagonal is restored afterward 1227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* copy r and (q transpose)*b to preserve input and initialize R. */ 1247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa = qtb; 1257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FactorType R(s); 1267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Eliminate the diagonal matrix d using a givens rotation 1277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (j = 0; j < n; ++j) 1287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Prepare the row of d to be eliminated, locating the 1307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // diagonal element using p from the qr factorization 1317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez l = iPerm.indices()(j); 1327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (diag(l) == Scalar(0)) 1337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez break; 1347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sdiag.tail(n-j).setZero(); 1357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sdiag[j] = diag[l]; 1367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // the transformations to eliminate the row of d 1377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // modify only a single element of (q transpose)*b 1387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // beyond the first n, which is initially zero. 1397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar qtbpj = 0; 1417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Browse the nonzero elements of row j of the upper triangular s 1427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (k = j; k < n; ++k) 1437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typename FactorType::InnerIterator itk(R,k); 1457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (; itk; ++itk){ 1467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (itk.index() < k) continue; 1477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez else break; 1487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //At this point, we have the diagonal element R(k,k) 1507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Determine a givens rotation which eliminates 1517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // the appropriate element in the current row of d 1527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez givens.makeGivens(-itk.value(), sdiag(k)); 1537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Compute the modified diagonal element of r and 1557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // the modified element of ((q transpose)*b,0). 1567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k); 1577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp = givens.c() * wa(k) + givens.s() * qtbpj; 1587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj; 1597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa(k) = temp; 1607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Accumulate the transformation in the remaining k row/column of R 1627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (++itk; itk; ++itk) 1637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez i = itk.index(); 1657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp = givens.c() * itk.value() + givens.s() * sdiag(i); 1667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i); 1677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez itk.valueRef() = temp; 1687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Solve the triangular system for z. If the system is 1737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // singular, then obtain a least squares solution 1747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index nsing; 1757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {} 1767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa.tail(n-nsing).setZero(); 1787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// x = wa; 1797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing)); 1807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sdiag = R.diagonal(); 1827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Permute the components of z back to components of x 1837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x = iPerm * wa; 1847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 1857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} // end namespace internal 1867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} // end namespace Eigen 1887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#endif // EIGEN_LMQRSOLV_H 190