35 using namespace gtsam;
40 static const double tol = 1
e-5;
44 0., 4.6904).finished();
46 using Dims = std::vector<Eigen::Index>;
53 5.0254, 5.5432).finished();
56 5.5737, 3.0153).finished();
59 -3.0153, -3.5635).finished();
64 vector<pair<Key, Matrix> > terms = {{1,
R}, {3,
S1}, {5,
S2}, {7,
S3}};
104 A1(0,0) = 1 ;
A1(1,0) = 2;
105 A1(0,1) = 3 ;
A1(1,1) = 4;
108 A2(0,0) = 6 ;
A2(1,0) = 0.2;
109 A2(0,1) = 8 ;
A2(1,1) = 0.4;
112 R(0,0) = 0.1 ;
R(1,0) = 0.3;
113 R(0,1) = 0.0 ;
R(1,1) = 0.34;
129 static constexpr
double sigma = 3.0;
133 key, Vector1::Constant(5), I_1x1,
165 double integral = 0.0;
198 double integral = 0.0;
214 expectedX(0) = 20-3-11 ; expectedX(1) = 40-7-15;
230 Vector sx1(2); sx1 << 1.0, 1.0;
231 Vector sl1(2); sl1 << 1.0, 1.0;
248 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
249 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
250 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
251 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4;
263 {2, sx1}, {1, (
Vector(4) << -3.1, -3.4, -11.9, -13.2).finished()}};
270 actual.
insert(cg.solve(actual));
280 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
281 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
282 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
283 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4;
304 actual.
insert(cg.solve(actual));
316 Matrix R11 = (
Matrix(1, 1) << 1.0).finished(), S12 = (
Matrix(1, 1) << 1.0).finished();
332 {2, (
Vector(1) << 5.).finished()}},
333 y = {{1, (
Vector(1) << 2.).finished()},
334 {2, (
Vector(1) << 3.).finished()}};
355 Matrix IExpected =
R.transpose() *
R;
391 const double sigma = 3;
398 double expected1 = 0.5 * e1.dot(e1);
404 double expected2 = 0.5 * e2.dot(e2);
408 for (
auto conditional : {conditional1, conditional2}) {
420 const double sigma = 0.01;
428 auto actual1 = conditional.likelihood(frontalValues);
437 auto actual2 = conditional.likelihood(
x0);
447 const double sigma = 0.01;
450 auto actual1 =
density.sample();
459 auto actual2 = conditional.sample(given);
464 std::mt19937_64
rng(4242);
465 auto actual3 = conditional.sample(given, &
rng);
478 double logProbability = stdGaussian.logProbability(
values);
493 x.insert(
X(0), Vector3::Zero());
495 double expectedNegLogConstant =
507 const double sigma = 3;
514 "GaussianConditional p(x0)\n"
518 " mean: 1 elements\n"
520 " logNormalizationConstant: -4.0351\n"
521 "isotropic dim=2 sigma=3\n";
528 std::string expected1 =
529 "GaussianConditional p(x0 | x1)\n"
532 " S[x1] = [ -1 -2 ]\n"
535 " logNormalizationConstant: -4.0351\n"
536 "isotropic dim=2 sigma=3\n";
542 std::string expected2 =
543 "GaussianConditional p(x0 | y0 y1)\n"
546 " S[y0] = [ -1 -2 ]\n"
548 " S[y1] = [ -5 -6 ]\n"
551 " logNormalizationConstant: -4.0351\n"
552 "isotropic dim=2 sigma=3\n";