1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
5// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
6//
7// This code initially comes from MINPACK whose original authors are:
8// Copyright Jorge More - Argonne National Laboratory
9// Copyright Burt Garbow - Argonne National Laboratory
10// Copyright Ken Hillstrom - Argonne National Laboratory
11//
12// This Source Code Form is subject to the terms of the Minpack license
13// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
14
15#ifndef EIGEN_LMQRSOLV_H
16#define EIGEN_LMQRSOLV_H
17
18namespace Eigen {
19
20namespace internal {
21
22template <typename Scalar,int Rows, int Cols, typename Index>
23void lmqrsolv(
24  Matrix<Scalar,Rows,Cols> &s,
25  const PermutationMatrix<Dynamic,Dynamic,Index> &iPerm,
26  const Matrix<Scalar,Dynamic,1> &diag,
27  const Matrix<Scalar,Dynamic,1> &qtb,
28  Matrix<Scalar,Dynamic,1> &x,
29  Matrix<Scalar,Dynamic,1> &sdiag)
30{
31
32    /* Local variables */
33    Index i, j, k, l;
34    Scalar temp;
35    Index n = s.cols();
36    Matrix<Scalar,Dynamic,1>  wa(n);
37    JacobiRotation<Scalar> givens;
38
39    /* Function Body */
40    // the following will only change the lower triangular part of s, including
41    // the diagonal, though the diagonal is restored afterward
42
43    /*     copy r and (q transpose)*b to preserve input and initialize s. */
44    /*     in particular, save the diagonal elements of r in x. */
45    x = s.diagonal();
46    wa = qtb;
47
48
49    s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
50    /*     eliminate the diagonal matrix d using a givens rotation. */
51    for (j = 0; j < n; ++j) {
52
53        /*        prepare the row of d to be eliminated, locating the */
54        /*        diagonal element using p from the qr factorization. */
55        l = iPerm.indices()(j);
56        if (diag[l] == 0.)
57            break;
58        sdiag.tail(n-j).setZero();
59        sdiag[j] = diag[l];
60
61        /*        the transformations to eliminate the row of d */
62        /*        modify only a single element of (q transpose)*b */
63        /*        beyond the first n, which is initially zero. */
64        Scalar qtbpj = 0.;
65        for (k = j; k < n; ++k) {
66            /*           determine a givens rotation which eliminates the */
67            /*           appropriate element in the current row of d. */
68            givens.makeGivens(-s(k,k), sdiag[k]);
69
70            /*           compute the modified diagonal element of r and */
71            /*           the modified element of ((q transpose)*b,0). */
72            s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
73            temp = givens.c() * wa[k] + givens.s() * qtbpj;
74            qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
75            wa[k] = temp;
76
77            /*           accumulate the tranformation in the row of s. */
78            for (i = k+1; i<n; ++i) {
79                temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
80                sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
81                s(i,k) = temp;
82            }
83        }
84    }
85
86    /*     solve the triangular system for z. if the system is */
87    /*     singular, then obtain a least squares solution. */
88    Index nsing;
89    for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
90
91    wa.tail(n-nsing).setZero();
92    s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
93
94    // restore
95    sdiag = s.diagonal();
96    s.diagonal() = x;
97
98    /* permute the components of z back to components of x. */
99    x = iPerm * wa;
100}
101
102template <typename Scalar, int _Options, typename Index>
103void lmqrsolv(
104  SparseMatrix<Scalar,_Options,Index> &s,
105  const PermutationMatrix<Dynamic,Dynamic> &iPerm,
106  const Matrix<Scalar,Dynamic,1> &diag,
107  const Matrix<Scalar,Dynamic,1> &qtb,
108  Matrix<Scalar,Dynamic,1> &x,
109  Matrix<Scalar,Dynamic,1> &sdiag)
110{
111  /* Local variables */
112  typedef SparseMatrix<Scalar,RowMajor,Index> FactorType;
113    Index i, j, k, l;
114    Scalar temp;
115    Index n = s.cols();
116    Matrix<Scalar,Dynamic,1>  wa(n);
117    JacobiRotation<Scalar> givens;
118
119    /* Function Body */
120    // the following will only change the lower triangular part of s, including
121    // the diagonal, though the diagonal is restored afterward
122
123    /*     copy r and (q transpose)*b to preserve input and initialize R. */
124    wa = qtb;
125    FactorType R(s);
126    // Eliminate the diagonal matrix d using a givens rotation
127    for (j = 0; j < n; ++j)
128    {
129      // Prepare the row of d to be eliminated, locating the
130      // diagonal element using p from the qr factorization
131      l = iPerm.indices()(j);
132      if (diag(l) == Scalar(0))
133        break;
134      sdiag.tail(n-j).setZero();
135      sdiag[j] = diag[l];
136      // the transformations to eliminate the row of d
137      // modify only a single element of (q transpose)*b
138      // beyond the first n, which is initially zero.
139
140      Scalar qtbpj = 0;
141      // Browse the nonzero elements of row j of the upper triangular s
142      for (k = j; k < n; ++k)
143      {
144        typename FactorType::InnerIterator itk(R,k);
145        for (; itk; ++itk){
146          if (itk.index() < k) continue;
147          else break;
148        }
149        //At this point, we have the diagonal element R(k,k)
150        // Determine a givens rotation which eliminates
151        // the appropriate element in the current row of d
152        givens.makeGivens(-itk.value(), sdiag(k));
153
154        // Compute the modified diagonal element of r and
155        // the modified element of ((q transpose)*b,0).
156        itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k);
157        temp = givens.c() * wa(k) + givens.s() * qtbpj;
158        qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj;
159        wa(k) = temp;
160
161        // Accumulate the transformation in the remaining k row/column of R
162        for (++itk; itk; ++itk)
163        {
164          i = itk.index();
165          temp = givens.c() *  itk.value() + givens.s() * sdiag(i);
166          sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i);
167          itk.valueRef() = temp;
168        }
169      }
170    }
171
172    // Solve the triangular system for z. If the system is
173    // singular, then obtain a least squares solution
174    Index nsing;
175    for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {}
176
177    wa.tail(n-nsing).setZero();
178//     x = wa;
179    wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing));
180
181    sdiag = R.diagonal();
182    // Permute the components of z back to components of x
183    x = iPerm * wa;
184}
185} // end namespace internal
186
187} // end namespace Eigen
188
189#endif // EIGEN_LMQRSOLV_H
190