gtsam
3rdparty
Eigen
unsupported
Eigen
src
NonLinearOptimization
dogleg.h
Go to the documentation of this file.
1
namespace
Eigen
{
2
3
namespace
internal
{
4
5
template
<
typename
Scalar>
6
void
dogleg
(
7
const
Matrix< Scalar, Dynamic, Dynamic >
&qrfac,
8
const
Matrix< Scalar, Dynamic, 1 >
&
diag
,
9
const
Matrix< Scalar, Dynamic, 1 >
&qtb,
10
Scalar
delta
,
11
Matrix< Scalar, Dynamic, 1 >
&
x
)
12
{
13
using
std::abs
;
14
using
std::sqrt
;
15
16
typedef
DenseIndex
Index
;
17
18
/* Local variables */
19
Index
i
,
j
;
20
Scalar
sum, temp,
alpha
, bnorm;
21
Scalar
gnorm, qnorm;
22
Scalar
sgnorm;
23
24
/* Function Body */
25
const
Scalar
epsmch =
NumTraits<Scalar>::epsilon
();
26
const
Index
n
= qrfac.
cols
();
27
eigen_assert
(
n
==qtb.size());
28
eigen_assert
(
n
==
x
.
size
());
29
eigen_assert
(
n
==
diag
.size());
30
Matrix< Scalar, Dynamic, 1 >
wa1(
n
), wa2(
n
);
31
32
/* first, calculate the gauss-newton direction. */
33
for
(
j
=
n
-1;
j
>=0; --
j
) {
34
temp = qrfac(
j
,
j
);
35
if
(temp == 0.) {
36
temp = epsmch * qrfac.col(
j
).head(
j
+1).maxCoeff();
37
if
(temp == 0.)
38
temp = epsmch;
39
}
40
if
(
j
==
n
-1)
41
x
[
j
] = qtb[
j
] / temp;
42
else
43
x
[
j
] = (qtb[
j
] - qrfac.row(
j
).tail(
n
-
j
-1).dot(
x
.tail(
n
-
j
-1))) / temp;
44
}
45
46
/* test whether the gauss-newton direction is acceptable. */
47
qnorm =
diag
.cwiseProduct(
x
).stableNorm();
48
if
(qnorm <=
delta
)
49
return
;
50
51
// TODO : this path is not tested by Eigen unit tests
52
53
/* the gauss-newton direction is not acceptable. */
54
/* next, calculate the scaled gradient direction. */
55
56
wa1.fill(0.);
57
for
(
j
= 0;
j
<
n
; ++
j
) {
58
wa1.tail(
n
-
j
) += qrfac.row(
j
).tail(
n
-
j
) * qtb[
j
];
59
wa1[
j
] /=
diag
[
j
];
60
}
61
62
/* calculate the norm of the scaled gradient and test for */
63
/* the special case in which the scaled gradient is zero. */
64
gnorm = wa1.stableNorm();
65
sgnorm = 0.;
66
alpha
=
delta
/ qnorm;
67
if
(gnorm == 0.)
68
goto
algo_end;
69
70
/* calculate the point along the scaled gradient */
71
/* at which the quadratic is minimized. */
72
wa1.array() /= (
diag
*gnorm).
array
();
73
// TODO : once unit tests cover this part,:
74
// wa2 = qrfac.template triangularView<Upper>() * wa1;
75
for
(
j
= 0;
j
<
n
; ++
j
) {
76
sum = 0.;
77
for
(
i
=
j
;
i
<
n
; ++
i
) {
78
sum += qrfac(
j
,
i
) * wa1[
i
];
79
}
80
wa2[
j
] = sum;
81
}
82
temp = wa2.stableNorm();
83
sgnorm = gnorm / temp / temp;
84
85
/* test whether the scaled gradient direction is acceptable. */
86
alpha
= 0.;
87
if
(sgnorm >=
delta
)
88
goto
algo_end;
89
90
/* the scaled gradient direction is not acceptable. */
91
/* finally, calculate the point along the dogleg */
92
/* at which the quadratic is minimized. */
93
bnorm = qtb.stableNorm();
94
temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm /
delta
);
95
temp = temp -
delta
/ qnorm *
numext::abs2
(sgnorm /
delta
) +
sqrt
(
numext::abs2
(temp -
delta
/ qnorm) + (1.-
numext::abs2
(
delta
/ qnorm)) * (1.-
numext::abs2
(sgnorm /
delta
)));
96
alpha
=
delta
/ qnorm * (1. -
numext::abs2
(sgnorm /
delta
)) / temp;
97
algo_end:
98
99
/* form appropriate convex combination of the gauss-newton */
100
/* direction and the scaled gradient direction. */
101
temp = (1.-
alpha
) * (
std::min
)(sgnorm,
delta
);
102
x
= temp * wa1 +
alpha
*
x
;
103
}
104
105
}
// end namespace internal
106
107
}
// end namespace Eigen
Eigen
Namespace containing all symbols from the Eigen library.
Definition:
jet.h:637
array
int array[24]
Definition:
Map_general_stride.cpp:1
alpha
RealScalar alpha
Definition:
level1_cplx_impl.h:147
eigen_assert
#define eigen_assert(x)
Definition:
Macros.h:1037
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::internal::dogleg
void dogleg(const Matrix< Scalar, Dynamic, Dynamic > &qrfac, const Matrix< Scalar, Dynamic, 1 > &diag, const Matrix< Scalar, Dynamic, 1 > &qtb, Scalar delta, Matrix< Scalar, Dynamic, 1 > &x)
Definition:
dogleg.h:6
n
int n
Definition:
BiCGSTAB_simple.cpp:1
epsilon
static double epsilon
Definition:
testRot3.cpp:37
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition:
PlanarManipulatorExample.py:45
j
std::ptrdiff_t j
Definition:
tut_arithmetic_redux_minmax.cpp:2
anyset::size
size_t size() const
Definition:
pytypes.h:2220
Eigen::numext::abs2
EIGEN_DEVICE_FUNC bool abs2(bool x)
Definition:
Eigen/src/Core/MathFunctions.h:1294
Eigen::PlainObjectBase::cols
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
Definition:
PlainObjectBase.h:145
Eigen::DenseIndex
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition:
Meta.h:66
min
#define min(a, b)
Definition:
datatypes.h:19
Eigen::Matrix< Scalar, Dynamic, Dynamic >
abs
#define abs(x)
Definition:
datatypes.h:17
internal
Definition:
BandTriangularSolver.h:13
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition:
jet.h:418
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:32:26