34 using namespace gtsam;
38 if (get<0>(
a) == get<0>(
b) && get<1>(
a) == get<1>(
b) &&
39 get<2>(
a) == get<2>(
b))
return true;
41 cout <<
"not equal:" << endl;
42 cout <<
"\texpected: "
43 "(" << get<0>(
a) <<
", " << get<1>(
a) <<
") = " << get<2>(
a) << endl;
45 "(" << get<0>(
b) <<
", " << get<1>(
b) <<
") = " << get<2>(
b) << endl;
66 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 6., 7., 8., 7., 8., 7., 8.,
67 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 3., 4., 5., 6., 7., 7.,
68 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5.,
69 1., -5., -5., 5., 5., -1., 1.5).finished();
81 make_shared<JacobianFactor>(0, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
82 auto f2 = make_shared<JacobianFactor>(0, -10 * I_2x2, 1, 10 * I_2x2,
84 auto f3 = make_shared<JacobianFactor>(0, -5 * I_2x2, 2, 5 * I_2x2,
122 4., 6.,32.).finished();
129 const Key x123 = 0, x45 = 1;
130 gfg.
add(x123, (
Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
132 gfg.
add(x123, (
Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
133 x45, (
Matrix(2, 2) << 11, 12, 14, 15.).finished(),
145 for (
int i = 0;
i < 16;
i++) {
161 Matrix A00 = (
Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished();
171 Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16;
180 const auto [actualA, actualb] = gfg.
jacobian();
187 const auto [actualL, actualEta] = gfg.
hessian();
193 expectLdiagonal.
insert(0,
Vector3(1 + 25 + 81, 4 + 36 + 100, 9 + 49));
283 const auto [AtA, eta] = gfg.
hessian();
287 expectedAtA << 125, 0, -25, 0, -100, 0,
288 0, 125, 0, -25, 0, -100,
289 -25, 0, 50, 0, -25, 0,
290 0, -25, 0, 50, 0, -25,
291 -100, 0, -25, 0, 225, 0,
292 0, -100, 0, -25, 0, 225;
320 400 * I_2x2,
Vector2(1.0, 1.0), 3.0);
329 const auto [AtA, eta] = gfg.
hessian();
331 X << 1, 2, 3, 4, 5, 6;
333 Y << -450, -450, 300, 400, 2950, 3450;
355 const auto [AtA, eta] = gfg.
hessian();
377 blockMatrix.
matrix() << 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0,
378 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4;
396 jacFactor0->getA(jacFactor0->begin()) *= 7.;
421 Vector d = infoMatrix.diagonal();
442 noiseModel::Isotropic::Sigma(1, 1.0));
479 }
catch (
const exception& exc) {
480 std::string expected_exception_message =
"An inference algorithm was called with inconsistent "
483 "factor graph, ordering, or variable index were "
486 "other, or a full elimination routine was called with "
489 "does not include all of the variables.\n"
490 "Leftover keys after elimination: 2, x3.";
491 EXPECT(expected_exception_message == exc.what());
496 for (
size_t i = 0;
i < 1000;
i++) {
503 }
catch (
const exception& exc) {
504 std::string expected_exception_message =
"An inference algorithm was called with inconsistent "
507 "factor graph, ordering, or variable index were "
510 "other, or a full elimination routine was called with "
513 "does not include all of the variables.\n"
514 "Leftover keys after elimination: 2, 3, 4, 5, ... (total 999 keys).";
515 EXPECT(expected_exception_message == exc.what());