gtsam
3rdparty
Eigen
unsupported
Eigen
src
NonLinearOptimization
fdjac1.h
Go to the documentation of this file.
1
namespace
Eigen
{
2
3
namespace
internal
{
4
5
template
<
typename
FunctorType,
typename
Scalar>
6
DenseIndex
fdjac1
(
7
const
FunctorType &
Functor
,
8
Matrix< Scalar, Dynamic, 1 >
&
x
,
9
Matrix< Scalar, Dynamic, 1 >
&fvec,
10
Matrix< Scalar, Dynamic, Dynamic >
&fjac,
11
DenseIndex
ml,
DenseIndex
mu
,
12
Scalar
epsfcn)
13
{
14
using
std::sqrt
;
15
using
std::abs
;
16
17
typedef
DenseIndex
Index
;
18
19
/* Local variables */
20
Scalar
h
;
21
Index
j
, k;
22
Scalar
eps, temp;
23
Index
msum;
24
int
iflag;
25
Index
start, length;
26
27
/* Function Body */
28
const
Scalar
epsmch =
NumTraits<Scalar>::epsilon
();
29
const
Index
n
=
x
.
size
();
30
eigen_assert
(fvec.size()==
n
);
31
Matrix< Scalar, Dynamic, 1 >
wa1(
n
);
32
Matrix< Scalar, Dynamic, 1 >
wa2(
n
);
33
34
eps =
sqrt
((
std::max
)(epsfcn,epsmch));
35
msum = ml +
mu
+ 1;
36
if
(msum >=
n
) {
37
/* computation of dense approximate jacobian. */
38
for
(
j
= 0;
j
<
n
; ++
j
) {
39
temp =
x
[
j
];
40
h
= eps *
abs
(temp);
41
if
(
h
== 0.)
42
h
= eps;
43
x
[
j
] = temp +
h
;
44
iflag =
Functor
(
x
, wa1);
45
if
(iflag < 0)
46
return
iflag;
47
x
[
j
] = temp;
48
fjac.col(
j
) = (wa1-fvec)/
h
;
49
}
50
51
}
else
{
52
/* computation of banded approximate jacobian. */
53
for
(k = 0; k < msum; ++k) {
54
for
(
j
= k; (msum<0) ? (
j
>
n
): (
j
<
n
);
j
+= msum) {
55
wa2[
j
] =
x
[
j
];
56
h
= eps *
abs
(wa2[
j
]);
57
if
(
h
== 0.)
h
= eps;
58
x
[
j
] = wa2[
j
] +
h
;
59
}
60
iflag =
Functor
(
x
, wa1);
61
if
(iflag < 0)
62
return
iflag;
63
for
(
j
= k; (msum<0) ? (
j
>
n
): (
j
<
n
);
j
+= msum) {
64
x
[
j
] = wa2[
j
];
65
h
= eps *
abs
(wa2[
j
]);
66
if
(
h
== 0.)
h
= eps;
67
fjac.col(
j
).
setZero
();
68
start = std::max<Index>(0,
j
-
mu
);
69
length = (
std::min
)(
n
-1,
j
+ml) - start + 1;
70
fjac.col(
j
).segment(start, length) = ( wa1.segment(start, length)-fvec.segment(start, length))/
h
;
71
}
72
}
73
}
74
return
0;
75
}
76
77
}
// end namespace internal
78
79
}
// end namespace Eigen
Eigen
Namespace containing all symbols from the Eigen library.
Definition:
jet.h:637
mu
double mu
Definition:
testBoundingConstraint.cpp:37
eigen_assert
#define eigen_assert(x)
Definition:
Macros.h:1037
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
h
const double h
Definition:
testSimpleHelicopter.cpp:19
n
int n
Definition:
BiCGSTAB_simple.cpp:1
epsilon
static double epsilon
Definition:
testRot3.cpp:37
Functor
Definition:
NonLinearOptimization.cpp:117
j
std::ptrdiff_t j
Definition:
tut_arithmetic_redux_minmax.cpp:2
Eigen::internal::fdjac1
DenseIndex fdjac1(const FunctorType &Functor, Matrix< Scalar, Dynamic, 1 > &x, Matrix< Scalar, Dynamic, 1 > &fvec, Matrix< Scalar, Dynamic, Dynamic > &fjac, DenseIndex ml, DenseIndex mu, Scalar epsfcn)
Definition:
fdjac1.h:6
anyset::size
size_t size() const
Definition:
pytypes.h:2220
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, 1 >
abs
#define abs(x)
Definition:
datatypes.h:17
internal
Definition:
BandTriangularSolver.h:13
Eigen::PlainObjectBase::setZero
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Definition:
CwiseNullaryOp.h:562
max
#define max(a, b)
Definition:
datatypes.h:20
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition:
jet.h:418
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:32