// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
//
// This code initially comes from MINPACK whose original authors are:
// Copyright Jorge More - Argonne National Laboratory
// Copyright Burt Garbow - Argonne National Laboratory
// Copyright Ken Hillstrom - Argonne National Laboratory
//
// This Source Code Form is subject to the terms of the Minpack license
// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
#ifndef EIGEN_LMQRSOLV_H
#define EIGEN_LMQRSOLV_H
namespace Eigen {
namespace internal {
template <typename Scalar,int Rows, int Cols, typename PermIndex>
void lmqrsolv(
Matrix<Scalar,Rows,Cols> &s,
const PermutationMatrix<Dynamic,Dynamic,PermIndex> &iPerm,
const Matrix<Scalar,Dynamic,1> &diag,
const Matrix<Scalar,Dynamic,1> &qtb,
Matrix<Scalar,Dynamic,1> &x,
Matrix<Scalar,Dynamic,1> &sdiag)
{
/* Local variables */
Index i, j, k;
Scalar temp;
Index n = s.cols();
Matrix<Scalar,Dynamic,1> wa(n);
JacobiRotation<Scalar> givens;
/* Function Body */
// the following will only change the lower triangular part of s, including
// the diagonal, though the diagonal is restored afterward
/* copy r and (q transpose)*b to preserve input and initialize s. */
/* in particular, save the diagonal elements of r in x. */
x = s.diagonal();
wa = qtb;
s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
/* eliminate the diagonal matrix d using a givens rotation. */
for (j = 0; j < n; ++j) {
/* prepare the row of d to be eliminated, locating the */
/* diagonal element using p from the qr factorization. */
const PermIndex l = iPerm.indices()(j);
if (diag[l] == 0.)
break;
sdiag.tail(n-j).setZero();
sdiag[j] = diag[l];
/* the transformations to eliminate the row of d */
/* modify only a single element of (q transpose)*b */
/* beyond the first n, which is initially zero. */
Scalar qtbpj = 0.;
for (k = j; k < n; ++k) {
/* determine a givens rotation which eliminates the */
/* appropriate element in the current row of d. */
givens.makeGivens(-s(k,k), sdiag[k]);
/* compute the modified diagonal element of r and */
/* the modified element of ((q transpose)*b,0). */
s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
temp = givens.c() * wa[k] + givens.s() * qtbpj;
qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
wa[k] = temp;
/* accumulate the tranformation in the row of s. */
for (i = k+1; i<n; ++i) {
temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
s(i,k) = temp;
}
}
}
/* solve the triangular system for z. if the system is */
/* singular, then obtain a least squares solution. */
Index nsing;
for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
wa.tail(n-nsing).setZero();
s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
// restore
sdiag = s.diagonal();
s.diagonal() = x;
/* permute the components of z back to components of x. */
x = iPerm * wa;
}
template <typename Scalar, int _Options, typename Index>
void lmqrsolv(
SparseMatrix<Scalar,_Options,Index> &s,
const PermutationMatrix<Dynamic,Dynamic> &iPerm,
const Matrix<Scalar,Dynamic,1> &diag,
const Matrix<Scalar,Dynamic,1> &qtb,
Matrix<Scalar,Dynamic,1> &x,
Matrix<Scalar,Dynamic,1> &sdiag)
{
/* Local variables */
typedef SparseMatrix<Scalar,RowMajor,Index> FactorType;
Index i, j, k, l;
Scalar temp;
Index n = s.cols();
Matrix<Scalar,Dynamic,1> wa(n);
JacobiRotation<Scalar> givens;
/* Function Body */
// the following will only change the lower triangular part of s, including
// the diagonal, though the diagonal is restored afterward
/* copy r and (q transpose)*b to preserve input and initialize R. */
wa = qtb;
FactorType R(s);
// Eliminate the diagonal matrix d using a givens rotation
for (j = 0; j < n; ++j)
{
// Prepare the row of d to be eliminated, locating the
// diagonal element using p from the qr factorization
l = iPerm.indices()(j);
if (diag(l) == Scalar(0))
break;
sdiag.tail(n-j).setZero();
sdiag[j] = diag[l];
// the transformations to eliminate the row of d
// modify only a single element of (q transpose)*b
// beyond the first n, which is initially zero.
Scalar qtbpj = 0;
// Browse the nonzero elements of row j of the upper triangular s
for (k = j; k < n; ++k)
{
typename FactorType::InnerIterator itk(R,k);
for (; itk; ++itk){
if (itk.index() < k) continue;
else break;
}
//At this point, we have the diagonal element R(k,k)
// Determine a givens rotation which eliminates
// the appropriate element in the current row of d
givens.makeGivens(-itk.value(), sdiag(k));
// Compute the modified diagonal element of r and
// the modified element of ((q transpose)*b,0).
itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k);
temp = givens.c() * wa(k) + givens.s() * qtbpj;
qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj;
wa(k) = temp;
// Accumulate the transformation in the remaining k row/column of R
for (++itk; itk; ++itk)
{
i = itk.index();
temp = givens.c() * itk.value() + givens.s() * sdiag(i);
sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i);
itk.valueRef() = temp;
}
}
}
// Solve the triangular system for z. If the system is
// singular, then obtain a least squares solution
Index nsing;
for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {}
wa.tail(n-nsing).setZero();
// x = wa;
wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing));
sdiag = R.diagonal();
// Permute the components of z back to components of x
x = iPerm * wa;
}
} // end namespace internal
} // end namespace Eigen
#endif // EIGEN_LMQRSOLV_H