testGaussianConditional.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 
21 #include <gtsam/base/Matrix.h>
23 #include <gtsam/inference/Symbol.h>
29 
30 
31 #include <iostream>
32 #include <sstream>
33 #include <vector>
34 
35 using namespace gtsam;
36 using namespace std;
39 
40 static const double tol = 1e-5;
41 
42 static Matrix R = (Matrix(2, 2) <<
43  -12.1244, -5.1962,
44  0., 4.6904).finished();
45 
46 using Dims = std::vector<Eigen::Index>;
47 
48 /* ************************************************************************* */
50 {
51  Matrix S1 = (Matrix(2, 2) <<
52  -5.2786, -8.6603,
53  5.0254, 5.5432).finished();
54  Matrix S2 = (Matrix(2, 2) <<
55  -10.5573, -5.9385,
56  5.5737, 3.0153).finished();
57  Matrix S3 = (Matrix(2, 2) <<
58  -11.3820, -7.2581,
59  -3.0153, -3.5635).finished();
60 
61  Vector d = Vector2(1.0, 2.0);
63 
64  vector<pair<Key, Matrix> > terms = {{1, R}, {3, S1}, {5, S2}, {7, S3}};
65 
66  GaussianConditional actual(terms, 1, d, s);
67 
69  EXPECT(assert_equal(Key(1), *it));
70  EXPECT(assert_equal(R, actual.R()));
71  ++ it;
72  EXPECT(it == actual.endFrontals());
73 
74  it = actual.beginParents();
75  EXPECT(assert_equal(Key(3), *it));
76  EXPECT(assert_equal(S1, actual.S(it)));
77 
78  ++ it;
79  EXPECT(assert_equal(Key(5), *it));
80  EXPECT(assert_equal(S2, actual.S(it)));
81 
82  ++ it;
83  EXPECT(assert_equal(Key(7), *it));
84  EXPECT(assert_equal(S3, actual.S(it)));
85 
86  ++it;
87  EXPECT(it == actual.endParents());
88 
89  EXPECT(assert_equal(d, actual.d()));
90  EXPECT(assert_equal(*s, *actual.get_model()));
91 
92  // test copy constructor
93  GaussianConditional copied(actual);
94  EXPECT(assert_equal(d, copied.d()));
95  EXPECT(assert_equal(*s, *copied.get_model()));
96  EXPECT(assert_equal(R, copied.R()));
97 }
98 
99 /* ************************************************************************* */
101 {
102  // create a conditional gaussian node
103  Matrix A1(2,2);
104  A1(0,0) = 1 ; A1(1,0) = 2;
105  A1(0,1) = 3 ; A1(1,1) = 4;
106 
107  Matrix A2(2,2);
108  A2(0,0) = 6 ; A2(1,0) = 0.2;
109  A2(0,1) = 8 ; A2(1,1) = 0.4;
110 
111  Matrix R(2,2);
112  R(0,0) = 0.1 ; R(1,0) = 0.3;
113  R(0,1) = 0.0 ; R(1,1) = 0.34;
114 
116 
117  Vector d = Vector2(0.2, 0.5);
118 
120  expected(1, d, R, 2, A1, 10, A2, model),
121  actual(1, d, R, 2, A1, 10, A2, model);
122 
123  EXPECT( expected.equals(actual) );
124 }
125 
126 /* ************************************************************************* */
127 namespace density {
128 static const Key key = 77;
129 static constexpr double sigma = 3.0;
130 static const auto unitPrior =
131  GaussianConditional(key, Vector1::Constant(5), I_1x1),
133  key, Vector1::Constant(5), I_1x1,
135 } // namespace density
136 
137 /* ************************************************************************* */
138 // Check that the evaluate function matches direct calculation with R.
140  // Let's evaluate at the mean
142 
143  // We get the Hessian matrix, which has noise model applied!
144  const Matrix invSigma = density::unitPrior.information();
145 
146  // A Gaussian density ~ exp (-0.5*(Rx-d)'*(Rx-d))
147  // which at the mean is 1.0! So, the only thing we need to calculate is
148  // the normalization constant 1.0/sqrt((2*pi*Sigma).det()).
149  // The covariance matrix inv(Sigma) = R'*R, so the determinant is
150  const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
151  const double actual = density::unitPrior.evaluate(mean);
152  EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
153 
154  using density::key;
155  using density::sigma;
156 
157  // Check Invariants at the mean and a different value
158  for (auto vv : {mean, VectorValues{{key, Vector1(4)}}}) {
161  HybridValues{vv, {}, {}}));
162  }
163 
164  // Let's numerically integrate and see that we integrate to 1.0.
165  double integral = 0.0;
166  // Loop from -5*sigma to 5*sigma in 0.1*sigma steps:
167  for (double x = -5.0 * sigma; x <= 5.0 * sigma; x += 0.1 * sigma) {
168  VectorValues xValues;
169  xValues.insert(key, mean.at(key) + Vector1(x));
170  const double density = density::unitPrior.evaluate(xValues);
171  integral += 0.1 * sigma * density;
172  }
173  EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-9);
174 }
175 
176 /* ************************************************************************* */
177 // Check the evaluate with non-unit noise.
179  // See comments in test above.
181  const Matrix R = density::widerPrior.R();
182  const Matrix invSigma = density::widerPrior.information();
183  const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
184  const double actual = density::widerPrior.evaluate(mean);
185  EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
186 
187  using density::key;
188  using density::sigma;
189 
190  // Check Invariants at the mean and a different value
191  for (auto vv : {mean, VectorValues{{key, Vector1(4)}}}) {
194  HybridValues{vv, {}, {}}));
195  }
196 
197  // Let's numerically integrate and see that we integrate to 1.0.
198  double integral = 0.0;
199  // Loop from -5*sigma to 5*sigma in 0.1*sigma steps:
200  for (double x = -5.0 * sigma; x <= 5.0 * sigma; x += 0.1 * sigma) {
201  VectorValues xValues;
202  xValues.insert(key, mean.at(key) + Vector1(x));
203  const double density = density::widerPrior.evaluate(xValues);
204  integral += 0.1 * sigma * density;
205  }
206  EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-5);
207 }
208 
209 /* ************************************************************************* */
211 {
212  //expected solution
213  Vector expectedX(2);
214  expectedX(0) = 20-3-11 ; expectedX(1) = 40-7-15;
215 
216  // create a conditional Gaussian node
217  Matrix R = (Matrix(2, 2) << 1., 0.,
218  0., 1.).finished();
219 
220  Matrix A1 = (Matrix(2, 2) << 1., 2.,
221  3., 4.).finished();
222 
223  Matrix A2 = (Matrix(2, 2) << 5., 6.,
224  7., 8.).finished();
225 
226  Vector d(2); d << 20.0, 40.0;
227 
228  GaussianConditional cg(1, d, R, 2, A1, 10, A2);
229 
230  Vector sx1(2); sx1 << 1.0, 1.0;
231  Vector sl1(2); sl1 << 1.0, 1.0;
232 
233  VectorValues expected = {{1, expectedX}, {2, sx1}, {10, sl1}};
234 
235  VectorValues solution = {{2, sx1}, // parents
236  {10, sl1}};
237  solution.insert(cg.solve(solution));
238 
239  EXPECT(assert_equal(expected, solution, tol));
240 }
241 
242 /* ************************************************************************* */
243 TEST( GaussianConditional, solve_simple )
244 {
245  // 2 variables, frontal has dim=4
246  VerticalBlockMatrix blockMatrix(Dims{4, 2, 1}, 4);
247  blockMatrix.matrix() <<
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;
252 
253  // solve system as a non-multifrontal version first
254  GaussianConditional cg(KeyVector{1,2}, 1, blockMatrix);
255 
256  // partial solution
257  Vector sx1 = Vector2(9.0, 10.0);
258 
259  // elimination order: 1, 2
260  VectorValues actual = {{2, sx1}}; // parent
261 
263  {2, sx1}, {1, (Vector(4) << -3.1, -3.4, -11.9, -13.2).finished()}};
264 
265  // verify indices/size
266  EXPECT_LONGS_EQUAL(2, (long)cg.size());
267  EXPECT_LONGS_EQUAL(4, (long)cg.rows());
268 
269  // solve and verify
270  actual.insert(cg.solve(actual));
271  EXPECT(assert_equal(expected, actual, tol));
272 }
273 
274 /* ************************************************************************* */
275 TEST( GaussianConditional, solve_multifrontal )
276 {
277  // create full system, 3 variables, 2 frontals, all 2 dim
278  VerticalBlockMatrix blockMatrix(Dims{2, 2, 2, 1}, 4);
279  blockMatrix.matrix() <<
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;
284 
285  // 3 variables, all dim=2
286  GaussianConditional cg(KeyVector{1, 2, 10}, 2, blockMatrix);
287 
288  EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.d()));
289 
290  // partial solution
291  Vector sl1 = Vector2(9.0, 10.0);
292 
293  // elimination order; _x_, _x1_, _l1_
294  VectorValues actual = {{10, sl1}}; // parent
295 
297  {1, Vector2(-3.1, -3.4)}, {2, Vector2(-11.9, -13.2)}, {10, sl1}};
298 
299  // verify indices/size
300  EXPECT_LONGS_EQUAL(3, (long)cg.size());
301  EXPECT_LONGS_EQUAL(4, (long)cg.rows());
302 
303  // solve and verify
304  actual.insert(cg.solve(actual));
305  EXPECT(assert_equal(expected, actual, tol));
306 
307 }
308 
309 /* ************************************************************************* */
310 TEST( GaussianConditional, solveTranspose ) {
316  Matrix R11 = (Matrix(1, 1) << 1.0).finished(), S12 = (Matrix(1, 1) << 1.0).finished();
317  Matrix R22 = (Matrix(1, 1) << 1.0).finished();
318  Vector d1(1), d2(1);
319  d1(0) = 9;
320  d2(0) = 5;
321 
322  // define nodes and specify in reverse topological sort (i.e. parents last)
323  GaussianBayesNet cbn;
324  cbn.emplace_shared<GaussianConditional>(1, d1, R11, 2, S12);
325  cbn.emplace_shared<GaussianConditional>(1, d2, R22);
326 
327  // x=R'*y, y=inv(R')*x
328  // 2 = 1 2
329  // 5 1 1 3
330 
331  VectorValues x = {{1, (Vector(1) << 2.).finished()},
332  {2, (Vector(1) << 5.).finished()}},
333  y = {{1, (Vector(1) << 2.).finished()},
334  {2, (Vector(1) << 3.).finished()}};
335 
336  // test functional version
337  VectorValues actual = cbn.backSubstituteTranspose(x);
338  CHECK(assert_equal(y, actual));
339 }
340 
341 /* ************************************************************************* */
342 TEST( GaussianConditional, information ) {
343 
344  // Create R matrix
345  Matrix R(4,4); R <<
346  1, 2, 3, 4,
347  0, 5, 6, 7,
348  0, 0, 8, 9,
349  0, 0, 0, 10;
350 
351  // Create conditional
352  GaussianConditional conditional(0, Vector::Zero(4), R);
353 
354  // Expected information matrix (using permuted R)
355  Matrix IExpected = R.transpose() * R;
356 
357  // Actual information matrix (conditional should permute R)
358  Matrix IActual = conditional.information();
359  EXPECT(assert_equal(IExpected, IActual));
360 }
361 
362 /* ************************************************************************* */
363 TEST( GaussianConditional, isGaussianFactor ) {
364 
365  // Create R matrix
366  Matrix R(4,4); R <<
367  1, 2, 3, 4,
368  0, 5, 6, 7,
369  0, 0, 8, 9,
370  0, 0, 0, 10;
371 
372  // Create a conditional
373  GaussianConditional conditional(0, Vector::Zero(4), R);
374 
375  // Expected information matrix computed by conditional
376  Matrix IExpected = conditional.information();
377 
378  // Expected information matrix computed by a factor
379  JacobianFactor jf = conditional;
380  Matrix IActual = jf.information();
381 
382  EXPECT(assert_equal(IExpected, IActual));
383 }
384 
385 /* ************************************************************************* */
386 // Test FromMeanAndStddev named constructors
387 TEST(GaussianConditional, FromMeanAndStddev) {
388  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
389  Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished();
390  const Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6);
391  const double sigma = 3;
392 
393  VectorValues values{{X(0), x0}, {X(1), x1}, {X(2), x2}};
394 
395  auto conditional1 =
397  Vector2 e1 = (x0 - (A1 * x1 + b)) / sigma;
398  double expected1 = 0.5 * e1.dot(e1);
399  EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9);
400 
401  auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2,
402  X(2), b, sigma);
403  Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma;
404  double expected2 = 0.5 * e2.dot(e2);
405  EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9);
406 
407  // Check Invariants for both conditionals
408  for (auto conditional : {conditional1, conditional2}) {
411  HybridValues{values, {}, {}}));
412  }
413 }
414 
415 /* ************************************************************************* */
416 // Test likelihood method (conversion to JacobianFactor)
417 TEST(GaussianConditional, likelihood) {
418  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
419  const Vector2 b(20, 40), x0(1, 2);
420  const double sigma = 0.01;
421 
422  // |x0 - A1 x1 - b|^2
423  auto conditional =
425 
426  VectorValues frontalValues;
427  frontalValues.insert(X(0), x0);
428  auto actual1 = conditional.likelihood(frontalValues);
429  CHECK(actual1);
430 
431  // |(-A1) x1 - (b - x0)|^2
432  JacobianFactor expected(X(1), -A1, b - x0,
434  EXPECT(assert_equal(expected, *actual1, tol));
435 
436  // Check single vector version
437  auto actual2 = conditional.likelihood(x0);
438  CHECK(actual2);
439  EXPECT(assert_equal(expected, *actual2, tol));
440 }
441 
442 /* ************************************************************************* */
443 // Test sampling
445  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
446  const Vector2 b(20, 40), x1(3, 4);
447  const double sigma = 0.01;
448 
450  auto actual1 = density.sample();
451  EXPECT_LONGS_EQUAL(1, actual1.size());
452  EXPECT(assert_equal(b, actual1[X(0)], 50 * sigma));
453 
454  VectorValues given;
455  given.insert(X(1), x1);
456 
457  auto conditional =
459  auto actual2 = conditional.sample(given);
460  EXPECT_LONGS_EQUAL(1, actual2.size());
461  EXPECT(assert_equal(A1 * x1 + b, actual2[X(0)], 50 * sigma));
462 
463  // Use a specific random generator
464  std::mt19937_64 rng(4242);
465  auto actual3 = conditional.sample(given, &rng);
466  EXPECT_LONGS_EQUAL(1, actual2.size());
467  // regression is not repeatable across platforms/versions :-(
468  // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
469 }
470 
471 /* ************************************************************************* */
473  // Create univariate standard gaussian conditional
474  auto stdGaussian =
475  GaussianConditional::FromMeanAndStddev(X(0), Vector1::Zero(), 1.0);
477  values.insert(X(0), Vector1::Zero());
478  double logProbability = stdGaussian.logProbability(values);
479 
480  // Regression.
481  // These values were computed by hand for a univariate standard gaussian.
482  EXPECT_DOUBLES_EQUAL(-0.9189385332046727, logProbability, 1e-9);
483  EXPECT_DOUBLES_EQUAL(0.3989422804014327, exp(logProbability), 1e-9);
484  EXPECT_DOUBLES_EQUAL(stdGaussian(values), exp(logProbability), 1e-9);
485 }
486 
487 /* ************************************************************************* */
488 // Similar test for multivariate gaussian but with sigma 2.0
489 TEST(GaussianConditional, NegLogConstant) {
490  double sigma = 2.0;
491  auto conditional = GaussianConditional::FromMeanAndStddev(X(0), Vector3::Zero(), sigma);
492  VectorValues x;
493  x.insert(X(0), Vector3::Zero());
494  Matrix3 Sigma = I_3x3 * sigma * sigma;
495  double expectedNegLogConstant =
496  -log(1 / sqrt((2 * M_PI * Sigma).determinant()));
497 
498  EXPECT_DOUBLES_EQUAL(expectedNegLogConstant, conditional.negLogConstant(),
499  1e-9);
500 }
501 
502 /* ************************************************************************* */
504  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
505  Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished();
506  const Vector2 b(20, 40);
507  const double sigma = 3;
508 
509  GaussianConditional conditional(X(0), b, Matrix2::Identity(),
511 
512  // Test printing for no parents.
513  std::string expected =
514  "GaussianConditional p(x0)\n"
515  " R = [ 1 0 ]\n"
516  " [ 0 1 ]\n"
517  " d = [ 20 40 ]\n"
518  " mean: 1 elements\n"
519  " x0: 20 40\n"
520  " logNormalizationConstant: -4.0351\n"
521  "isotropic dim=2 sigma=3\n";
522  EXPECT(assert_print_equal(expected, conditional, "GaussianConditional"));
523 
524  auto conditional1 =
526 
527  // Test printing for single parent.
528  std::string expected1 =
529  "GaussianConditional p(x0 | x1)\n"
530  " R = [ 1 0 ]\n"
531  " [ 0 1 ]\n"
532  " S[x1] = [ -1 -2 ]\n"
533  " [ -3 -4 ]\n"
534  " d = [ 20 40 ]\n"
535  " logNormalizationConstant: -4.0351\n"
536  "isotropic dim=2 sigma=3\n";
537  EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional"));
538 
539  // Test printing for multiple parents.
540  auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, Y(0), A2,
541  Y(1), b, sigma);
542  std::string expected2 =
543  "GaussianConditional p(x0 | y0 y1)\n"
544  " R = [ 1 0 ]\n"
545  " [ 0 1 ]\n"
546  " S[y0] = [ -1 -2 ]\n"
547  " [ -3 -4 ]\n"
548  " S[y1] = [ -5 -6 ]\n"
549  " [ -7 -8 ]\n"
550  " d = [ 20 40 ]\n"
551  " logNormalizationConstant: -4.0351\n"
552  "isotropic dim=2 sigma=3\n";
553  EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional"));
554 }
555 
556 /* ************************************************************************* */
557 int main() {
558  TestResult tr;
559  return TestRegistry::runAllTests(tr);
560 }
561 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
S1
static double S1[]
Definition: shichi.c:61
gtsam::Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: Vector.h:41
gtsam::HybridValues
Definition: HybridValues.h:37
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
GaussianConditional.h
Conditional Gaussian Base class.
gtsam::GaussianConditional::S
constABlock S() const
Definition: GaussianConditional.h:225
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
TestHarness.h
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
Definition: JacobianFactor.h:292
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:352
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
log
const EIGEN_DEVICE_FUNC LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
X
#define X
Definition: icosphere.cpp:20
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
vv
static const VectorValues vv
Definition: testHybridGaussianConditional.cpp:44
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
main
int main()
Definition: testGaussianConditional.cpp:557
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
determinant
void determinant(const MatrixType &m)
Definition: determinant.cpp:14
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::Conditional::endParents
FACTOR::const_iterator endParents() const
Definition: Conditional.h:189
gtsam::Dims
std::vector< Key > Dims
Definition: HessianFactor.cpp:41
gtsam::GaussianConditional::R
constABlock R() const
Definition: GaussianConditional.h:222
gtsam::symbol_shorthand::X
Key X(std::uint64_t j)
Definition: inference/Symbol.h:171
density::unitPrior
static const auto unitPrior
Definition: testGaussianConditional.cpp:130
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::JacobianFactor::information
Matrix information() const override
Definition: JacobianFactor.cpp:505
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::Conditional::endFrontals
FACTOR::const_iterator endFrontals() const
Definition: Conditional.h:183
VerticalBlockMatrix.h
A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and GaussianConditional.
gtsam::Symmetric
Symmetric group.
Definition: testGroup.cpp:27
gtsam::Conditional< JacobianFactor, GaussianConditional >::CheckInvariants
static bool CheckInvariants(const GaussianConditional &conditional, const VALUES &x)
Definition: Conditional-inst.h:69
gtsam::GaussianConditional
Definition: GaussianConditional.h:40
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
A2
static const double A2[]
Definition: expn.h:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
x0
static Symbol x0('x', 0)
density
Definition: testGaussianConditional.cpp:127
gtsam::GaussianConditional::d
const constBVector d() const
Definition: GaussianConditional.h:231
Symbol.h
gtsam::symbol_shorthand::Y
Key Y(std::uint64_t j)
Definition: inference/Symbol.h:172
GaussianDensity.h
A Gaussian Density.
gtsam::Conditional::beginFrontals
FACTOR::const_iterator beginFrontals() const
Definition: Conditional.h:180
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
Vector2
Definition: test_operator_overloading.cpp:18
gtsam::GaussianBayesNet::backSubstituteTranspose
VectorValues backSubstituteTranspose(const VectorValues &gx) const
Definition: GaussianBayesNet.cpp:145
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
gtsam::equals
Definition: Testable.h:112
TestResult
Definition: TestResult.h:26
y
Scalar * y
Definition: level1_cplx_impl.h:124
JacobianFactor.h
test_docs.d2
d2
Definition: test_docs.py:29
gtsam::Conditional::beginParents
FACTOR::const_iterator beginParents() const
Definition: Conditional.h:186
gtsam::b
const G & b
Definition: Group.h:79
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:83
gtsam
traits
Definition: SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
constructor
Definition: init.h:200
gtsam::mean
Point3 mean(const CONTAINER &points)
mean
Definition: Point3.h:70
R
static Matrix R
Definition: testGaussianConditional.cpp:42
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Definition: VerticalBlockMatrix.h:188
std
Definition: BFloat16.h:88
A1
static const double A1[]
Definition: expn.h:6
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
density::sigma
static constexpr double sigma
Definition: testGaussianConditional.cpp:129
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::tol
const G double tol
Definition: Group.h:79
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
density::widerPrior
static const auto widerPrior
Definition: testGaussianConditional.cpp:132
HybridValues.h
gtsam::GaussianConditional::solve
VectorValues solve(const VectorValues &parents) const
Definition: GaussianConditional.cpp:218
density::key
static const Key key
Definition: testGaussianConditional.cpp:128
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35
gtsam::GaussianConditional::FromMeanAndStddev
static GaussianConditional FromMeanAndStddev(Key key, const Vector &mu, double sigma)
Construct from mean mu and standard deviation sigma.
Definition: GaussianConditional.cpp:66
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::GaussianDensity::FromMeanAndStddev
static GaussianDensity FromMeanAndStddev(Key key, const Vector &mean, double sigma)
Construct using a mean and standard deviation.
Definition: GaussianDensity.cpp:29


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:27