LMqrsolv.h
Go to the documentation of this file.
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 
18 namespace Eigen {
19 
20 namespace internal {
21 
22 template <typename Scalar,int Rows, int Cols, typename PermIndex>
23 void lmqrsolv(
27  const Matrix<Scalar,Dynamic,1> &qtb,
30 {
31  /* Local variables */
32  Index i, j, k;
33  Scalar temp;
34  Index n = s.cols();
37 
38  /* Function Body */
39  // the following will only change the lower triangular part of s, including
40  // the diagonal, though the diagonal is restored afterward
41 
42  /* copy r and (q transpose)*b to preserve input and initialize s. */
43  /* in particular, save the diagonal elements of r in x. */
44  x = s.diagonal();
45  wa = qtb;
46 
47 
48  s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
49  /* eliminate the diagonal matrix d using a givens rotation. */
50  for (j = 0; j < n; ++j) {
51 
52  /* prepare the row of d to be eliminated, locating the */
53  /* diagonal element using p from the qr factorization. */
54  const PermIndex l = iPerm.indices()(j);
55  if (diag[l] == 0.)
56  break;
57  sdiag.tail(n-j).setZero();
58  sdiag[j] = diag[l];
59 
60  /* the transformations to eliminate the row of d */
61  /* modify only a single element of (q transpose)*b */
62  /* beyond the first n, which is initially zero. */
63  Scalar qtbpj = 0.;
64  for (k = j; k < n; ++k) {
65  /* determine a givens rotation which eliminates the */
66  /* appropriate element in the current row of d. */
67  givens.makeGivens(-s(k,k), sdiag[k]);
68 
69  /* compute the modified diagonal element of r and */
70  /* the modified element of ((q transpose)*b,0). */
71  s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
72  temp = givens.c() * wa[k] + givens.s() * qtbpj;
73  qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
74  wa[k] = temp;
75 
76  /* accumulate the transformation in the row of s. */
77  for (i = k+1; i<n; ++i) {
78  temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
79  sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
80  s(i,k) = temp;
81  }
82  }
83  }
84 
85  /* solve the triangular system for z. if the system is */
86  /* singular, then obtain a least squares solution. */
87  Index nsing;
88  for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
89 
90  wa.tail(n-nsing).setZero();
91  s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
92 
93  // restore
94  sdiag = s.diagonal();
95  s.diagonal() = x;
96 
97  /* permute the components of z back to components of x. */
98  x = iPerm * wa;
99 }
100 
101 template <typename Scalar, int _Options, typename Index>
102 void lmqrsolv(
106  const Matrix<Scalar,Dynamic,1> &qtb,
109 {
110  /* Local variables */
111  typedef SparseMatrix<Scalar,RowMajor,Index> FactorType;
112  Index i, j, k, l;
113  Scalar temp;
114  Index n = s.cols();
116  JacobiRotation<Scalar> givens;
117 
118  /* Function Body */
119  // the following will only change the lower triangular part of s, including
120  // the diagonal, though the diagonal is restored afterward
121 
122  /* copy r and (q transpose)*b to preserve input and initialize R. */
123  wa = qtb;
124  FactorType R(s);
125  // Eliminate the diagonal matrix d using a givens rotation
126  for (j = 0; j < n; ++j)
127  {
128  // Prepare the row of d to be eliminated, locating the
129  // diagonal element using p from the qr factorization
130  l = iPerm.indices()(j);
131  if (diag(l) == Scalar(0))
132  break;
133  sdiag.tail(n-j).setZero();
134  sdiag[j] = diag[l];
135  // the transformations to eliminate the row of d
136  // modify only a single element of (q transpose)*b
137  // beyond the first n, which is initially zero.
138 
139  Scalar qtbpj = 0;
140  // Browse the nonzero elements of row j of the upper triangular s
141  for (k = j; k < n; ++k)
142  {
143  typename FactorType::InnerIterator itk(R,k);
144  for (; itk; ++itk){
145  if (itk.index() < k) continue;
146  else break;
147  }
148  //At this point, we have the diagonal element R(k,k)
149  // Determine a givens rotation which eliminates
150  // the appropriate element in the current row of d
151  givens.makeGivens(-itk.value(), sdiag(k));
152 
153  // Compute the modified diagonal element of r and
154  // the modified element of ((q transpose)*b,0).
155  itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k);
156  temp = givens.c() * wa(k) + givens.s() * qtbpj;
157  qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj;
158  wa(k) = temp;
159 
160  // Accumulate the transformation in the remaining k row/column of R
161  for (++itk; itk; ++itk)
162  {
163  i = itk.index();
164  temp = givens.c() * itk.value() + givens.s() * sdiag(i);
165  sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i);
166  itk.valueRef() = temp;
167  }
168  }
169  }
170 
171  // Solve the triangular system for z. If the system is
172  // singular, then obtain a least squares solution
173  Index nsing;
174  for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {}
175 
176  wa.tail(n-nsing).setZero();
177 // x = wa;
178  wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing));
179 
180  sdiag = R.diagonal();
181  // Permute the components of z back to components of x
182  x = iPerm * wa;
183 }
184 } // end namespace internal
185 
186 } // end namespace Eigen
187 
188 #endif // EIGEN_LMQRSOLV_H
Index cols() const
Definition: SparseMatrix.h:140
SCALAR Scalar
Definition: bench_gemm.cpp:46
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
A versatible sparse matrix representation.
Definition: SparseMatrix.h:96
Matrix diag(const std::vector< Matrix > &Hs)
Definition: Matrix.cpp:206
EIGEN_DEVICE_FUNC Scalar & c()
Definition: Jacobi.h:47
int n
Rot2 R(Rot2::fromAngle(0.1))
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Rotation given by a cosine-sine pair.
Permutation matrix.
static const Line3 l(Rot3(), 1, 1)
const IndicesType & indices() const
EIGEN_DEVICE_FUNC void makeGivens(const Scalar &p, const Scalar &q, Scalar *r=0)
Definition: Jacobi.h:162
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
RealScalar s
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)
Definition: LMqrsolv.h:23
EIGEN_DEVICE_FUNC Scalar & s()
Definition: Jacobi.h:49
The matrix class, also used for vectors and row-vectors.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:33