22 using namespace gtsam;
26 assert(
x.size() == 2);
38 Vector actual = numericalGradient<Vector2>(
f,
x);
50 Matrix actual = numericalHessian<Vector2>(
f,
x);
57 assert(
x.size() == 2);
63 TEST(testNumericalDerivative, numericalHessian2) {
83 double x1 = 1,
x2 = 5;
86 Matrix actual11 = numericalHessian211<double, double>(
f3,
x1,
x2);
90 Matrix actual12 = numericalHessian212<double, double>(
f3,
x1,
x2);
94 Matrix actual22 = numericalHessian222<double, double>(
f3,
x1,
x2);
105 double f4(
double x,
double y,
double z) {
112 double x = 1,
y = 2,
z = 3;
114 Matrix actual11 = numericalHessian311<double, double, double>(
f4,
x,
y,
z);
118 Matrix actual12 = numericalHessian312<double, double, double>(
f4,
x,
y,
z);
122 Matrix actual13 = numericalHessian313<double, double, double>(
f4,
x,
y,
z);
126 Matrix actual22 = numericalHessian322<double, double, double>(
f4,
x,
y,
z);
130 Matrix actual23 = numericalHessian323<double, double, double>(
f4,
x,
y,
z);
134 Matrix actual33 = numericalHessian333<double, double, double>(
f4,
x,
y,
z);
139 Vector6
f6(
const double x1,
const double x2,
const double x3,
const double x4,
140 const double x5,
const double x6) {
147 const double x5,
const double x6) {
155 TEST(testNumericalDerivative, numeriDerivative61) {
156 double x1 = 1,
x2 = 2,
x3 = 3 ,
x4 = 4,
x5 = 5, x6 = 6;
160 double, double, double,
double>(
f6,
x1,
x2,
x3,
x4,
x5, x6);
164 Matrix expected61Dynamic = Matrix::Zero(6, 1);
165 expected61Dynamic(0, 0) =
cos(
x1);
175 TEST(testNumericalDerivative, numeriDerivative62) {
176 double x1 = 1,
x2 = 2,
x3 = 3 ,
x4 = 4,
x5 = 5, x6 = 6;
184 Matrix expected62Dynamic = Matrix::Zero(6, 1);
185 expected62Dynamic(1, 0) = -
sin(
x2);
187 double, double, double, double, 1>(
f6,
x1,
x2,
x3,
x4,
x5, x6);
194 TEST(testNumericalDerivative, numeriDerivative63) {
195 double x1 = 1,
x2 = 2,
x3 = 3 ,
x4 = 4,
x5 = 5, x6 = 6;
197 Matrix expected63 = (
Matrix(6, 1) << 0, 0, 2 *
x3, 0, 0, 0).finished();
203 Matrix expected63Dynamic = Matrix::Zero(6, 1);
204 expected63Dynamic(2, 0) = 2 *
x3;
205 Matrix61 actual63Dynamic =
214 TEST(testNumericalDerivative, numeriDerivative64) {
215 double x1 = 1,
x2 = 2,
x3 = 3 ,
x4 = 4,
x5 = 5, x6 = 6;
217 Matrix expected64 = (
Matrix(6, 1) << 0, 0, 0, 3 *
x4 *
x4, 0, 0).finished();
223 Matrix expected64Dynamic = Matrix::Zero(6, 1);
224 expected64Dynamic(3, 0) = 3 *
x4 *
x4;
225 Matrix61 actual64Dynamic =
234 TEST(testNumericalDerivative, numeriDerivative65) {
235 double x1 = 1,
x2 = 2,
x3 = 3 ,
x4 = 4,
x5 = 5, x6 = 6;
243 Matrix expected65Dynamic = Matrix::Zero(6, 1);
244 expected65Dynamic(4, 0) = 0.5 /
sqrt(
x5);
245 Matrix61 actual65Dynamic =
254 TEST(testNumericalDerivative, numeriDerivative66) {
255 double x1 = 1,
x2 = 2,
x3 = 3 ,
x4 = 4,
x5 = 5, x6 = 6;
263 Matrix expected66Dynamic = Matrix::Zero(6, 1);
264 expected66Dynamic(5, 0) =
cos(x6) +
sin(x6);
265 Matrix61 actual66Dynamic =