32 using namespace gtsam;
36 using Dims = std::vector<Eigen::Index>;
63 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
64 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
65 -25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000,
66 0.0, -25.0000, 0.0, 25.0000, 0.0, 0.0, 7.5000,
67 -100.0000, 0.0, 0.0, 0.0, 100.0000, 0.0, -20.0000,
68 0.0, -100.0000, 0.0, 0.0, 0.0, 100.0000, 10.0000,
69 25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500).finished()));
72 0, (
Matrix(4,2) << -1., 0.,
76 1, (
Matrix(4,4) << 1., 0., 0.00, 0.,
79 0., 0., 0.00, -1.).finished(),
80 (
Vector(4) << -0.2, 0.3, 0.2, -0.1).finished(),
81 noiseModel::Diagonal::Sigmas((
Vector(4) << 0.2, 0.2, 0.1, 0.1).finished()));
111 double expected_manual = 0.5 * (
f - 2.0 * dx[0].dot(
g) + dx[0].transpose() *
G.selfadjointView<
Eigen::Upper>() * dx[0]);
140 Matrix G22 = (
Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
159 Vector linearExpected(3); linearExpected <<
g1,
g2;
178 Matrix G22 = (
Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
179 Matrix G23 = (
Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished();
181 Matrix G33 = (
Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0).finished();
194 HessianFactor factor(0, 1, 2, G11, G12, G13,
g1, G22, G23,
g2, G33, g3,
f);
203 Vector linearExpected(6); linearExpected <<
g1,
g2, g3;
221 Matrix G22 = (
Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
222 Matrix G23 = (
Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished();
224 Matrix G33 = (
Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0).finished();
239 std::vector<Matrix> Gs;
240 Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
241 std::vector<Vector> gs;
242 gs.push_back(
g1); gs.push_back(
g2); gs.push_back(g3);
252 Vector linearExpected(6); linearExpected <<
g1,
g2, g3;
265 Matrix3 A01 = 3.0 * I_3x3;
268 Matrix3
A21 = 4.0 * I_3x3;
297 CHECK(expectedFactor);
300 const auto [actualConditional, actualHessian] =
302 actualConditional->setModel(
false,
Vector3(1,1,1));
305 #ifdef TEST_ERROR_EQUIVALENCE
310 actualHessian->augmentedInformation(), 1
e-9));
331 gfg.
add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0,
true));
332 gfg.
add(0,
A10, 1,
A11,
b1, noiseModel::Diagonal::Sigmas(s1,
true));
333 gfg.
add(1,
A21,
b2, noiseModel::Diagonal::Sigmas(s2,
true));
336 A0 <<
A10, Z_3x3, Z_3x3;
344 noiseModel::Diagonal::Sigmas(
sigmas,
true));
355 const auto [expectedConditional, expectedFactor] =
359 const auto [actualConditional, actualHessian] =
361 actualConditional->setModel(
false,
Vector3(1,1,1));
364 #ifdef TEST_ERROR_EQUIVALENCE
371 actualHessian->augmentedInformation(), 1
e-9));
408 vector<pair<Key, Matrix> > meas;
409 meas.push_back(make_pair(0, Ax2));
410 meas.push_back(make_pair(1, Al1x1));
417 std::pair<GaussianConditional::shared_ptr, HessianFactor::shared_ptr> actual_Chol =
421 double oldSigma = 0.0894427;
425 ).finished()/oldSigma;
427 -0.20, 0.00,-0.80, 0.00,
428 +0.00,-0.20,+0.00,-0.80
429 ).finished()/oldSigma;
435 double sigma = 0.2236;
438 1.00, 0.00, -1.00, 0.00,
439 0.00, 1.00, +0.00, -1.00
442 JacobianFactor expectedLF(1, Bl1x1,
b1, noiseModel::Isotropic::Sigma(2,1.0));
452 0.0, 11.1803399).finished();
455 0.0, -2.23606798).finished();
458 0.0, -8.94427191).finished();
462 std::make_shared<JacobianFactor>(0,
A0, 1,
A1, 2,
A2,
b,
model)};
468 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
469 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
470 -25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000,
471 0.0, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 3.5000,
472 -100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000,
473 0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000,
474 25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500).finished();
493 const auto [
A,
b] =
factor.jacobian();
518 Vector expectedGrad1 = (
Vector(2) << 4.5, 16.1).finished();
545 map<Key,Matrix> actualBD =
factor.hessianBlockDiagonal();
556 Matrix2
G =
A.transpose() *
A;