25 using namespace gtsam;
32 using Terms = vector<pair<Key, Matrix> >;
33 const Terms terms{make_pair(5, I_3x3), make_pair(10, 2 * I_3x3),
34 make_pair(15, 3 * I_3x3)};
61 LinearEquality actual(terms[0].first, terms[0].second, terms[1].first,
62 terms[1].second,
b, 0);
73 LinearEquality actual(terms[0].first, terms[0].second, terms[1].first,
74 terms[1].second, terms[2].first, terms[2].second,
b,
89 (
Matrix(4, 4) << 1.57, 2.695, -1.1, -2.35, 2.695, 11.3125, -0.65, -10.225,
90 -1.1, -0.65, 1, 0.5, -2.35, -10.225, 0.5, 9.25)
92 (
Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725);
97 }
catch (
const std::runtime_error& exception) {
111 Vector expected_unwhitened(3);
112 expected_unwhitened << 2.0, 1.0, 0.0;
117 Vector expected_whitened(3);
118 expected_whitened = expected_unwhitened;
122 double expected_error = 0.0;
133 AExpected << simple::terms[0].second, simple::terms[1].second,
134 simple::terms[2].second;
136 Matrix augmentedJacobianExpected(3, 10);
137 augmentedJacobianExpected << AExpected, rhsExpected;
156 Matrix jacobianExpected(3, 9);
157 jacobianExpected << simple::terms[0].second, simple::terms[1].second,
158 simple::terms[2].second;
160 Matrix augmentedJacobianExpected(3, 10);
161 augmentedJacobianExpected << jacobianExpected, rhsExpected;
163 Matrix augmentedHessianExpected =
164 augmentedJacobianExpected.transpose() * simple::noise->R().transpose() *
165 simple::noise->R() * augmentedJacobianExpected;
186 c.insert(1, (
Vector(2) << 10., 20.).finished());
187 c.insert(2, (
Vector(2) << 30., 60.).finished());
196 expectedX.
insert(1, (
Vector(2) << -20., -40.).finished());
197 expectedX.
insert(2, (
Vector(2) << 20., 40.).finished());
205 expectedG.
insert(1, (
Vector(2) << 0.2, -0.1).finished());
206 expectedG.
insert(2, (
Vector(2) << -0.2, 0.1).finished());