diff options
Diffstat (limited to 'unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h')
| -rw-r--r-- | unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h | 189 |
1 files changed, 189 insertions, 0 deletions
diff --git a/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h b/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h new file mode 100644 index 000000000..f5290dee4 --- /dev/null +++ b/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h @@ -0,0 +1,189 @@ +// 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 Index> +void lmqrsolv( + Matrix<Scalar,Rows,Cols> &s, + const PermutationMatrix<Dynamic,Dynamic,Index> &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, 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 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. */ + 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 |