testGaussianBayesNet.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2022, 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 
18 #include <gtsam/linear/GaussianBayesNet.h>
19 #include <gtsam/linear/GaussianDensity.h>
20 #include <gtsam/linear/JacobianFactor.h>
21 #include <gtsam/linear/GaussianFactorGraph.h>
22 #include <gtsam/base/Testable.h>
23 #include <gtsam/base/numericalDerivative.h>
24 #include <gtsam/inference/Symbol.h>
25 
26 #include <CppUnitLite/TestHarness.h>
27 
28 // STL/C++
29 #include <iostream>
30 #include <sstream>
31 
32 using namespace std::placeholders;
33 using namespace std;
34 using namespace gtsam;
35 using symbol_shorthand::X;
36 
37 static const Key _x_ = 11, _y_ = 22, _z_ = 33;
38 
39 static GaussianBayesNet smallBayesNet = {
40  std::make_shared<GaussianConditional>(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1),
41  std::make_shared<GaussianConditional>(_y_, Vector1::Constant(5), I_1x1)};
42 
43 static GaussianBayesNet noisyBayesNet = {
44  std::make_shared<GaussianConditional>(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1,
45  noiseModel::Isotropic::Sigma(1, 2.0)),
46  std::make_shared<GaussianConditional>(_y_, Vector1::Constant(5), I_1x1,
47  noiseModel::Isotropic::Sigma(1, 3.0))};
48 
49 /* ************************************************************************* */
50 TEST( GaussianBayesNet, Matrix )
51 {
52  const auto [R, d] = smallBayesNet.matrix(); // find matrix and RHS
53 
54  Matrix R1 = (Matrix2() <<
55  1.0, 1.0,
56  0.0, 1.0
57  ).finished();
58  Vector d1 = Vector2(9.0, 5.0);
59 
60  EXPECT(assert_equal(R,R1));
61  EXPECT(assert_equal(d,d1));
62 }
63 
64 /* ************************************************************************* */
65 // Check that the evaluate function matches direct calculation with R.
66 TEST(GaussianBayesNet, Evaluate1) {
67  // Let's evaluate at the mean
68  const VectorValues mean = smallBayesNet.optimize();
69 
70  // We get the matrix, which has noise model applied!
71  const Matrix R = smallBayesNet.matrix().first;
72  const Matrix invSigma = R.transpose() * R;
73 
74  // The Bayes net is a Gaussian density ~ exp (-0.5*(Rx-d)'*(Rx-d))
75  // which at the mean is 1.0! So, the only thing we need to calculate is
76  // the normalization constant 1.0/sqrt((2*pi*Sigma).det()).
77  // The covariance matrix inv(Sigma) = R'*R, so the determinant is
78  const double constant = sqrt((invSigma / (2 * M_PI)).determinant());
79  EXPECT_DOUBLES_EQUAL(log(constant),
80  smallBayesNet.at(0)->logNormalizationConstant() +
81  smallBayesNet.at(1)->logNormalizationConstant(),
82  1e-9);
83  const double actual = smallBayesNet.evaluate(mean);
84  EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9);
85 }
86 
87 // Check the evaluate with non-unit noise.
88 TEST(GaussianBayesNet, Evaluate2) {
89  // See comments in test above.
90  const VectorValues mean = noisyBayesNet.optimize();
91  const Matrix R = noisyBayesNet.matrix().first;
92  const Matrix invSigma = R.transpose() * R;
93  const double constant = sqrt((invSigma / (2 * M_PI)).determinant());
94  const double actual = noisyBayesNet.evaluate(mean);
95  EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9);
96 }
97 
98 /* ************************************************************************* */
99 TEST( GaussianBayesNet, NoisyMatrix )
100 {
101  const auto [R, d] = noisyBayesNet.matrix(); // find matrix and RHS
102 
103  Matrix R1 = (Matrix2() <<
104  0.5, 0.5,
105  0.0, 1./3.
106  ).finished();
107  Vector d1 = Vector2(9./2., 5./3.);
108 
109  EXPECT(assert_equal(R,R1));
110  EXPECT(assert_equal(d,d1));
111 }
112 
113 /* ************************************************************************* */
114 TEST(GaussianBayesNet, Optimize) {
115  const VectorValues expected{{_x_, Vector1::Constant(4)},
116  {_y_, Vector1::Constant(5)}};
117  const VectorValues actual = smallBayesNet.optimize();
118  EXPECT(assert_equal(expected, actual));
119  }
120 
121 /* ************************************************************************* */
122 TEST(GaussianBayesNet, NoisyOptimize) {
123  const auto [R, d] = noisyBayesNet.matrix(); // find matrix and RHS
124  const Vector x = R.inverse() * d;
125  const VectorValues expected{{_x_, x.head(1)}, {_y_, x.tail(1)}};
126 
127  VectorValues actual = noisyBayesNet.optimize();
128  EXPECT(assert_equal(expected, actual));
129 }
130 
131 /* ************************************************************************* */
132 TEST( GaussianBayesNet, optimizeIncomplete )
133 {
134  static GaussianBayesNet incompleteBayesNet;
135  incompleteBayesNet.emplace_shared<GaussianConditional>(
136  _x_, Vector1::Constant(9), I_1x1, _y_, I_1x1);
137 
138  VectorValues solutionForMissing { {_y_, Vector1::Constant(5)} };
139 
140  VectorValues actual = incompleteBayesNet.optimize(solutionForMissing);
141 
142  VectorValues expected{{_x_, Vector1::Constant(4)},
143  {_y_, Vector1::Constant(5)}};
144 
145  EXPECT(assert_equal(expected,actual));
146 }
147 
148 /* ************************************************************************* */
149 TEST( GaussianBayesNet, optimize3 )
150 {
151  // y = R*x, x=inv(R)*y
152  // 4 = 1 1 -1
153  // 5 1 5
154  // NOTE: we are supplying a new RHS here
155 
156  VectorValues expected { {_x_, Vector1::Constant(-1)},
157  {_y_, Vector1::Constant(5)} };
158 
159  // Test different RHS version
160  VectorValues gx{{_x_, Vector1::Constant(4)}, {_y_, Vector1::Constant(5)}};
161  VectorValues actual = smallBayesNet.backSubstitute(gx);
162  EXPECT(assert_equal(expected, actual));
163 }
164 
165 /* ************************************************************************* */
166 namespace sampling {
167 static Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
168 static const Vector2 mean(20, 40), b(10, 10);
169 static const double sigma = 0.01;
170 static const GaussianBayesNet gbn = {
171  GaussianConditional::sharedMeanAndStddev(X(0), A1, X(1), b, sigma),
172  GaussianDensity::sharedMeanAndStddev(X(1), mean, sigma)};
173 } // namespace sampling
174 
175 /* ************************************************************************* */
176 TEST(GaussianBayesNet, sample) {
177  using namespace sampling;
178 
179  auto actual = gbn.sample();
180  EXPECT_LONGS_EQUAL(2, actual.size());
181  EXPECT(assert_equal(mean, actual[X(1)], 50 * sigma));
182  EXPECT(assert_equal(A1 * mean + b, actual[X(0)], 50 * sigma));
183 
184  // Use a specific random generator
185  std::mt19937_64 rng(4242);
186  auto actual3 = gbn.sample(&rng);
187  EXPECT_LONGS_EQUAL(2, actual.size());
188  // regression is not repeatable across platforms/versions :-(
189  // EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5));
190  // EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5));
191 }
192 
193 /* ************************************************************************* */
194 // Do Monte Carlo integration of square deviation, should be equal to 9.0.
195 TEST(GaussianBayesNet, MonteCarloIntegration) {
196  GaussianBayesNet gbn;
197  gbn.push_back(noisyBayesNet.at(1));
198 
199  double sum = 0.0;
200  constexpr size_t N = 1000;
201  // loop for N samples:
202  for (size_t i = 0; i < N; i++) {
203  const auto X_i = gbn.sample();
204  sum += pow(X_i[_y_].x() - 5.0, 2.0);
205  }
206  // Expected is variance = 3*3
207  EXPECT_DOUBLES_EQUAL(9.0, sum / N, 0.5); // Pretty high.
208 }
209 
210 /* ************************************************************************* */
211 TEST(GaussianBayesNet, ordering)
212 {
213  const Ordering expected{_x_, _y_};
214  const auto actual = noisyBayesNet.ordering();
215  EXPECT(assert_equal(expected, actual));
216 }
217 
218 /* ************************************************************************* */
219 TEST( GaussianBayesNet, MatrixStress )
220 {
221  GaussianBayesNet bn;
222  using GC = GaussianConditional;
223  bn.emplace_shared<GC>(_x_, Vector2(1, 2), 1 * I_2x2, _y_, 2 * I_2x2, _z_, 3 * I_2x2);
224  bn.emplace_shared<GC>(_y_, Vector2(3, 4), 4 * I_2x2, _z_, 5 * I_2x2);
225  bn.emplace_shared<GC>(_z_, Vector2(5, 6), 6 * I_2x2);
226 
227  const VectorValues expected = bn.optimize();
228  for (const auto& keys :
229  {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}),
230  KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}),
231  KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) {
232  const Ordering ordering(keys);
233  const auto [R, d] = bn.matrix(ordering);
234  EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d));
235  }
236 }
237 
238 /* ************************************************************************* */
239 TEST( GaussianBayesNet, backSubstituteTranspose )
240 {
241  // x=R'*y, expected=inv(R')*x
242  // 2 = 1 2
243  // 5 1 1 3
244  const VectorValues x{{_x_, Vector1::Constant(2)},
245  {_y_, Vector1::Constant(5)}},
246  expected{{_x_, Vector1::Constant(2)}, {_y_, Vector1::Constant(3)}};
247 
248  VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
249  EXPECT(assert_equal(expected, actual));
250 
251  const auto ordering = noisyBayesNet.ordering();
252  const Matrix R = smallBayesNet.matrix(ordering).first;
253  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
254  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
255 }
256 
257 /* ************************************************************************* */
258 TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
259 {
260  // x=R'*y, expected=inv(R')*x
261  // 2 = 1 2
262  // 5 1 1 3
263  VectorValues x{{_x_, Vector1::Constant(2)}, {_y_, Vector1::Constant(5)}},
264  expected{{_x_, Vector1::Constant(4)}, {_y_, Vector1::Constant(9)}};
265 
266  VectorValues actual = noisyBayesNet.backSubstituteTranspose(x);
267  EXPECT(assert_equal(expected, actual));
268 
269  const auto ordering = noisyBayesNet.ordering();
270  const Matrix R = noisyBayesNet.matrix(ordering).first;
271  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
272  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
273 }
274 
275 /* ************************************************************************* */
276 // Tests computing Determinant
277 TEST( GaussianBayesNet, DeterminantTest )
278 {
279  GaussianBayesNet cbn;
280  cbn.emplace_shared<GaussianConditional>(
281  0, Vector2(3.0, 4.0), (Matrix2() << 1.0, 3.0, 0.0, 4.0).finished(),
282  1, (Matrix2() << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
283 
284  cbn.emplace_shared<GaussianConditional>(
285  1, Vector2(5.0, 6.0), (Matrix2() << 1.0, 1.0, 0.0, 3.0).finished(),
286  2, (Matrix2() << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
287 
288  cbn.emplace_shared<GaussianConditional>(
289  3, Vector2(7.0, 8.0), (Matrix2() << 1.0, 1.0, 0.0, 5.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
290 
291  double expectedDeterminant = 60.0 / 64.0;
292  double actualDeterminant = cbn.determinant();
293 
294  EXPECT_DOUBLES_EQUAL( expectedDeterminant, actualDeterminant, 1e-9);
295 }
296 
297 /* ************************************************************************* */
298 namespace {
299  double computeError(const GaussianBayesNet& gbn, const Vector10& values)
300  {
301  pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian();
302  return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
303  }
304 }
305 
306 /* ************************************************************************* */
307 TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
308 
309  // Create an arbitrary Bayes Net
310  GaussianBayesNet gbn;
311  gbn.emplace_shared<GaussianConditional>(
312  0, Vector2(1.0,2.0), (Matrix2() << 3.0,4.0,0.0,6.0).finished(),
313  3, (Matrix2() << 7.0,8.0,9.0,10.0).finished(),
314  4, (Matrix2() << 11.0,12.0,13.0,14.0).finished());
315  gbn.emplace_shared<GaussianConditional>(
316  1, Vector2(15.0,16.0), (Matrix2() << 17.0,18.0,0.0,20.0).finished(),
317  2, (Matrix2() << 21.0,22.0,23.0,24.0).finished(),
318  4, (Matrix2() << 25.0,26.0,27.0,28.0).finished());
319  gbn.emplace_shared<GaussianConditional>(
320  2, Vector2(29.0,30.0), (Matrix2() << 31.0,32.0,0.0,34.0).finished(),
321  3, (Matrix2() << 35.0,36.0,37.0,38.0).finished());
322  gbn.emplace_shared<GaussianConditional>(
323  3, Vector2(39.0,40.0), (Matrix2() << 41.0,42.0,0.0,44.0).finished(),
324  4, (Matrix2() << 45.0,46.0,47.0,48.0).finished());
325  gbn.emplace_shared<GaussianConditional>(
326  4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished());
327 
328  // Compute the Hessian numerically
329  Matrix hessian = numericalHessian<Vector10>(
330  std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
331 
332  // Compute the gradient numerically
333  Vector gradient = numericalGradient<Vector10>(
334  std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
335 
336  // Compute the gradient using dense matrices
337  Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
338  LONGS_EQUAL(11, (long)augmentedHessian.cols());
339  Vector denseMatrixGradient = -augmentedHessian.col(10).segment(0,10);
340  EXPECT(assert_equal(gradient, denseMatrixGradient, 1e-5));
341 
342  // Compute the steepest descent point
343  double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
344  Vector expected = gradient * step;
345 
346  // Compute the steepest descent point with the dogleg function
347  VectorValues actual = gbn.optimizeGradientSearch();
348 
349  // Check that points agree
350  KeyVector keys {0, 1, 2, 3, 4};
351  Vector actualAsVector = actual.vector(keys);
352  EXPECT(assert_equal(expected, actualAsVector, 1e-5));
353 
354  // Check that point causes a decrease in error
355  double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(actual));
356  double newError = GaussianFactorGraph(gbn).error(actual);
357  EXPECT(newError < origError);
358 }
359 
360 /* ************************************************************************* */
361 TEST(GaussianBayesNet, Dot) {
362  GaussianBayesNet fragment;
363  DotWriter writer;
364  writer.variablePositions.emplace(_x_, Vector2(10, 20));
365  writer.variablePositions.emplace(_y_, Vector2(50, 20));
366 
367  auto position = writer.variablePos(_x_);
368  CHECK(position);
369  EXPECT(assert_equal(Vector2(10, 20), *position, 1e-5));
370 
371  string actual = noisyBayesNet.dot(DefaultKeyFormatter, writer);
372  EXPECT(actual ==
373  "digraph {\n"
374  " size=\"5,5\";\n"
375  "\n"
376  " var11[label=\"11\", pos=\"10,20!\"];\n"
377  " var22[label=\"22\", pos=\"50,20!\"];\n"
378  "\n"
379  " var22->var11\n"
380  "}");
381 }
382 
383 /* ************************************************************************* */
384 int main() {
385  TestResult tr;
386  return TestRegistry::runAllTests(tr);
387 }
388 /* ************************************************************************* */
test_docs.d
d
Definition: test_docs.py:28
sampling::A1
static Matrix A1
Definition: testGaussianBayesNet.cpp:167
gtsam::utils.visual_isam.step
def step(data, isam, result, truth, currPoseIndex)
Definition: visual_isam.py:82
CHECK
#define CHECK(condition)
Definition: Test.h:108
Testable.h
Concept check for values that can be used in unit tests.
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::GaussianBayesNet::sample
VectorValues sample(std::mt19937_64 *rng) const
Definition: GaussianBayesNet.cpp:63
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
determinant
void determinant(const MatrixType &m)
Definition: determinant.cpp:14
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
M_PI
#define M_PI
Definition: main.h:106
sampling::b
static const Vector2 b(10, 10)
R
Rot2 R(Rot2::fromAngle(0.1))
leaf::values
leaf::MyValues values
gtsam::BayesNet::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition: BayesNet-inst.h:45
main
int main()
Definition: testGaussianBayesNet.cpp:384
sampling::gbn
static const GaussianBayesNet gbn
Definition: testGaussianBayesNet.cpp:170
std
Definition: BFloat16.h:88
gtsam::GaussianFactorGraph::augmentedHessian
Matrix augmentedHessian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:247
numericalDerivative.h
Some functions to compute numerical derivatives.
_y_
static const Key _y_
Definition: testGaussianBayesNet.cpp:37
gtsam::GaussianFactorGraph::error
double error(const VectorValues &x) const
Definition: GaussianFactorGraph.cpp:71
N
#define N
Definition: gksort.c:12
gtsam::DotWriter::variablePos
std::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
Definition: DotWriter.cpp:79
TEST
TEST(GaussianBayesNet, Matrix)
Definition: testGaussianBayesNet.cpp:50
log
EIGEN_DEVICE_FUNC const LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
gtsam::DefaultKeyFormatter
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
ordering
static enum @1107 ordering
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
_x_
static const Key _x_
Definition: testGaussianBayesNet.cpp:37
sampling::mean
static const Vector2 mean(20, 40)
GaussianDensity.h
A Gaussian Density.
gtsam::GaussianBayesNet::optimize
VectorValues optimize() const
Definition: GaussianBayesNet.cpp:44
gtsam::DotWriter::variablePositions
std::map< Key, Vector2 > variablePositions
Definition: DotWriter.h:48
smallBayesNet
static GaussianBayesNet smallBayesNet
Definition: testGaussianBayesNet.cpp:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
_z_
static const Key _z_
Definition: testGaussianBayesNet.cpp:37
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::GaussianBayesNet::backSubstituteTranspose
VectorValues backSubstituteTranspose(const VectorValues &gx) const
Definition: GaussianBayesNet.cpp:145
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
noisyBayesNet
static GaussianBayesNet noisyBayesNet
Definition: testGaussianBayesNet.cpp:43
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition: DotWriter.h:36
gtsam
traits
Definition: chartTesting.h:28
gtsam::GaussianBayesNet::matrix
std::pair< Matrix, Vector > matrix(const Ordering &ordering) const
Definition: GaussianBayesNet.cpp:205
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
gtsam::GaussianFactorGraph::jacobian
std::pair< Matrix, Vector > jacobian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:232
gtsam::GaussianBayesNet::evaluate
double evaluate(const VectorValues &x) const
Definition: GaussianBayesNet.cpp:124
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:343
gtsam::Rot2::inverse
Rot2 inverse() const
Definition: Rot2.h:113
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:170
gtsam::GaussianBayesNet::optimizeGradientSearch
VectorValues optimizeGradientSearch() const
Definition: GaussianBayesNet.cpp:89
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:169
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
ceres::pow
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::GaussianBayesNet::backSubstitute
VectorValues backSubstitute(const VectorValues &gx) const
Definition: GaussianBayesNet.cpp:129
X
#define X
Definition: icosphere.cpp:20
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::internal::position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:25
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:08