gtsam
3rdparty
Eigen
unsupported
Eigen
src
NonLinearOptimization
qrsolv.h
Go to the documentation of this file.
1
namespace
Eigen
{
2
3
namespace
internal
{
4
5
// TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt
6
template
<
typename
Scalar>
7
void
qrsolv
(
8
Matrix< Scalar, Dynamic, Dynamic >
&
s
,
9
// TODO : use a PermutationMatrix once lmpar is no more:
10
const
VectorXi &ipvt,
11
const
Matrix< Scalar, Dynamic, 1 >
&
diag
,
12
const
Matrix< Scalar, Dynamic, 1 >
&qtb,
13
Matrix< Scalar, Dynamic, 1 >
&
x
,
14
Matrix< Scalar, Dynamic, 1 >
&sdiag)
15
16
{
17
typedef
DenseIndex
Index
;
18
19
/* Local variables */
20
Index
i
,
j
, k,
l
;
21
Scalar
temp;
22
Index
n
=
s
.cols();
23
Matrix< Scalar, Dynamic, 1 >
wa(
n
);
24
JacobiRotation<Scalar>
givens;
25
26
/* Function Body */
27
// the following will only change the lower triangular part of s, including
28
// the diagonal, though the diagonal is restored afterward
29
30
/* copy r and (q transpose)*b to preserve input and initialize s. */
31
/* in particular, save the diagonal elements of r in x. */
32
x
=
s
.diagonal();
33
wa = qtb;
34
35
s
.topLeftCorner(
n
,
n
).template triangularView<StrictlyLower>() =
s
.topLeftCorner(
n
,
n
).transpose();
36
37
/* eliminate the diagonal matrix d using a givens rotation. */
38
for
(
j
= 0;
j
<
n
; ++
j
) {
39
40
/* prepare the row of d to be eliminated, locating the */
41
/* diagonal element using p from the qr factorization. */
42
l
= ipvt[
j
];
43
if
(
diag
[
l
] == 0.)
44
break
;
45
sdiag.tail(
n
-
j
).
setZero
();
46
sdiag[
j
] =
diag
[
l
];
47
48
/* the transformations to eliminate the row of d */
49
/* modify only a single element of (q transpose)*b */
50
/* beyond the first n, which is initially zero. */
51
Scalar
qtbpj = 0.;
52
for
(k =
j
; k <
n
; ++k) {
53
/* determine a givens rotation which eliminates the */
54
/* appropriate element in the current row of d. */
55
givens.
makeGivens
(-
s
(k,k), sdiag[k]);
56
57
/* compute the modified diagonal element of r and */
58
/* the modified element of ((q transpose)*b,0). */
59
s
(k,k) = givens.
c
() *
s
(k,k) + givens.
s
() * sdiag[k];
60
temp = givens.
c
() * wa[k] + givens.
s
() * qtbpj;
61
qtbpj = -givens.
s
() * wa[k] + givens.
c
() * qtbpj;
62
wa[k] = temp;
63
64
/* accumulate the transformation in the row of s. */
65
for
(
i
= k+1;
i
<
n
; ++
i
) {
66
temp = givens.
c
() *
s
(
i
,k) + givens.
s
() * sdiag[
i
];
67
sdiag[
i
] = -givens.
s
() *
s
(
i
,k) + givens.
c
() * sdiag[
i
];
68
s
(
i
,k) = temp;
69
}
70
}
71
}
72
73
/* solve the triangular system for z. if the system is */
74
/* singular, then obtain a least squares solution. */
75
Index
nsing;
76
for
(nsing=0; nsing<
n
&& sdiag[nsing]!=0; nsing++) {}
77
78
wa.tail(
n
-nsing).
setZero
();
79
s
.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
80
81
// restore
82
sdiag =
s
.diagonal();
83
s
.diagonal() =
x
;
84
85
/* permute the components of z back to components of x. */
86
for
(
j
= 0;
j
<
n
; ++
j
)
x
[ipvt[
j
]] = wa[
j
];
87
}
88
89
}
// end namespace internal
90
91
}
// end namespace Eigen
Eigen
Namespace containing all symbols from the Eigen library.
Definition:
jet.h:637
s
RealScalar s
Definition:
level1_cplx_impl.h:126
gtsam::diag
Matrix diag(const std::vector< Matrix > &Hs)
Definition:
Matrix.cpp:206
x
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
Definition:
gnuplot_common_settings.hh:12
Eigen::JacobiRotation
Rotation given by a cosine-sine pair.
Definition:
ForwardDeclarations.h:283
n
int n
Definition:
BiCGSTAB_simple.cpp:1
j
std::ptrdiff_t j
Definition:
tut_arithmetic_redux_minmax.cpp:2
l
static const Line3 l(Rot3(), 1, 1)
Eigen::internal::qrsolv
void qrsolv(Matrix< Scalar, Dynamic, Dynamic > &s, const VectorXi &ipvt, const Matrix< Scalar, Dynamic, 1 > &diag, const Matrix< Scalar, Dynamic, 1 > &qtb, Matrix< Scalar, Dynamic, 1 > &x, Matrix< Scalar, Dynamic, 1 > &sdiag)
Definition:
qrsolv.h:7
Eigen::JacobiRotation::c
EIGEN_DEVICE_FUNC Scalar & c()
Definition:
Jacobi.h:47
Eigen::DenseIndex
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition:
Meta.h:66
Eigen::Matrix< Scalar, Dynamic, Dynamic >
internal
Definition:
BandTriangularSolver.h:13
Eigen::PlainObjectBase::setZero
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Definition:
CwiseNullaryOp.h:562
Eigen::JacobiRotation::s
EIGEN_DEVICE_FUNC Scalar & s()
Definition:
Jacobi.h:49
Eigen::JacobiRotation::makeGivens
EIGEN_DEVICE_FUNC void makeGivens(const Scalar &p, const Scalar &q, Scalar *r=0)
i
int i
Definition:
BiCGSTAB_step_by_step.cpp:9
Scalar
SCALAR Scalar
Definition:
bench_gemm.cpp:46
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition:
Meta.h:74
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:50