testGaussianBayesTree.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 
18 #include <CppUnitLite/TestHarness.h>
19 #include <gtsam/base/debug.h>
20 #include <gtsam/base/numericalDerivative.h>
21 #include <gtsam/inference/Symbol.h>
22 #include <gtsam/linear/GaussianBayesTree.h>
23 #include <gtsam/linear/GaussianConditional.h>
24 #include <gtsam/linear/GaussianJunctionTree.h>
25 
26 #include <iostream>
27 #include <vector>
28 
29 using namespace std::placeholders;
30 using namespace std;
31 using namespace gtsam;
32 
33 using Pairs = std::vector<std::pair<Key, Matrix>>;
34 
35 namespace {
36  const Key x1=1, x2=2, x3=3, x4=4;
37  const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
38  const GaussianFactorGraph chain = {
39  std::make_shared<JacobianFactor>(
40  x2, I_1x1, x1, I_1x1, (Vector(1) << 1.).finished(), chainNoise),
41  std::make_shared<JacobianFactor>(
42  x2, I_1x1, x3, I_1x1, (Vector(1) << 1.).finished(), chainNoise),
43  std::make_shared<JacobianFactor>(
44  x3, I_1x1, x4, I_1x1, (Vector(1) << 1.).finished(), chainNoise),
45  std::make_shared<JacobianFactor>(
46  x4, I_1x1, (Vector(1) << 1.).finished(), chainNoise)};
47  const Ordering chainOrdering {x2, x1, x3, x4};
48 
49  /* ************************************************************************* */
50  // Helper functions for below
51  GaussianBayesTreeClique::shared_ptr LeafClique(const GaussianConditional& conditional)
52  {
53  return std::make_shared<GaussianBayesTreeClique>(
54  std::make_shared<GaussianConditional>(conditional));
55  }
56 
57  typedef std::vector<GaussianBayesTreeClique::shared_ptr> Children;
58 
59  GaussianBayesTreeClique::shared_ptr MakeClique(
60  const GaussianConditional& conditional, const Children& children)
61  {
62  auto clique = LeafClique(conditional);
63  clique->children.assign(children.begin(), children.end());
64  for(Children::const_iterator child = children.begin(); child != children.end(); ++child)
65  (*child)->parent_ = clique;
66  return clique;
67  }
68 }
69 
70 /* ************************************************************************* */
84 TEST( GaussianBayesTree, eliminate )
85 {
86  GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering);
87 
88  Scatter scatter(chain);
89  EXPECT_LONGS_EQUAL(4, scatter.size());
90  EXPECT_LONGS_EQUAL(1, scatter.at(0).key);
91  EXPECT_LONGS_EQUAL(2, scatter.at(1).key);
92  EXPECT_LONGS_EQUAL(3, scatter.at(2).key);
93  EXPECT_LONGS_EQUAL(4, scatter.at(3).key);
94 
95  const Matrix two = (Matrix(1, 1) << 2.).finished();
96  const Matrix one = I_1x1;
97 
98  const GaussianConditional gc1(
99  std::map<Key, Matrix>{
100  {x3, (Matrix21() << 2., 0.).finished()},
101  {x4, (Matrix21() << 2., 2.).finished()},
102  },
103  2, Vector2(2., 2.));
104  const GaussianConditional gc2(
105  Pairs{
106  {x2, (Matrix21() << -2. * sqrt(2.), 0.).finished()},
107  {x1, (Matrix21() << -sqrt(2.), -sqrt(2.)).finished()},
108  {x3, (Matrix21() << -sqrt(2.), sqrt(2.)).finished()},
109  },
110  2, (Vector(2) << -2. * sqrt(2.), 0.).finished());
111 
112  GaussianBayesTree bayesTree_expected;
113  bayesTree_expected.insertRoot(MakeClique(gc1, Children{LeafClique(gc2)}));
114 
115  EXPECT(assert_equal(bayesTree_expected, bt));
116 }
117 
118 /* ************************************************************************* */
119 TEST( GaussianBayesTree, optimizeMultiFrontal )
120 {
121  VectorValues expected = {{x1, (Vector(1) << 0.).finished()},
122  {x2, (Vector(1) << 1.).finished()},
123  {x3, (Vector(1) << 0.).finished()},
124  {x4, (Vector(1) << 1.).finished()}};
125 
126  VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize();
127  EXPECT(assert_equal(expected,actual));
128 }
129 
130 /* ************************************************************************* */
131 TEST(GaussianBayesTree, complicatedMarginal) {
132  // Create the conditionals to go in the BayesTree
133  const GaussianConditional gc1(
134  Pairs{{11, (Matrix(3, 1) << 0.0971, 0, 0).finished()},
135  {12, (Matrix(3, 2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655)
136  .finished()}},
137  2, Vector3(0.2638, 0.1455, 0.1361));
138  const GaussianConditional gc2(
139  Pairs{
140  {9, (Matrix(3, 1) << 0.7952, 0, 0).finished()},
141  {10, (Matrix(3, 2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797)
142  .finished()},
143  {11, (Matrix(3, 1) << 0.6551, 0.1626, 0.1190).finished()},
144  {12, (Matrix(3, 2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513)
145  .finished()}},
146  2, Vector3(0.4314, 0.9106, 0.1818));
147  const GaussianConditional gc3(
148  Pairs{{7, (Matrix(3, 1) << 0.2551, 0, 0).finished()},
149  {8, (Matrix(3, 2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575)
150  .finished()},
151  {11, (Matrix(3, 1) << 0.8407, 0.2543, 0.8143).finished()}},
152  2, Vector3(0.3998, 0.2599, 0.8001));
153  const GaussianConditional gc4(
154  Pairs{{5, (Matrix(3, 1) << 0.2435, 0, 0).finished()},
155  {6, (Matrix(3, 2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0)
156  .finished()},
157  // NOTE the non-upper-triangular form
158  // here since this test was written when we had column permutations
159  // from LDL. The code still works currently (does not enfore
160  // upper-triangularity in this case) but this test will need to be
161  // redone if this stops working in the future
162  {7, (Matrix(3, 1) << 0.5853, 0.5497, 0.9172).finished()},
163  {8, (Matrix(3, 2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759)
164  .finished()}},
165  2, Vector3(0.8173, 0.8687, 0.0844));
166  const GaussianConditional gc5(
167  Pairs{{3, (Matrix(3, 1) << 0.0540, 0, 0).finished()},
168  {4, (Matrix(3, 2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371)
169  .finished()},
170  {6, (Matrix(3, 2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020)
171  .finished()}},
172  2, Vector3(0.9619, 0.0046, 0.7749));
173  const GaussianConditional gc6(
174  Pairs{{1, (Matrix(3, 1) << 0.2630, 0, 0).finished()},
175  {2, (Matrix(3, 2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524)
176  .finished()},
177  {5, (Matrix(3, 1) << 0.8258, 0.5383, 0.9961).finished()}},
178  2, Vector3(0.0782, 0.4427, 0.1067));
179 
180  // Create the bayes tree:
181  GaussianBayesTree bt;
182  bt.insertRoot(MakeClique(
183  gc1, Children{LeafClique(gc2),
184  MakeClique(gc3, Children{MakeClique(
185  gc4, Children{LeafClique(gc5),
186  LeafClique(gc6)})})}));
187 
188  // Marginal on 5
189  Matrix expectedCov = (Matrix(1,1) << 236.5166).finished();
190  //GaussianConditional actualJacobianChol = *bt.marginalFactor(5, EliminateCholesky);
191  GaussianConditional actualJacobianQR = *bt.marginalFactor(5, EliminateQR);
192  //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
193  LONGS_EQUAL(1, (long)actualJacobianQR.rows());
194  LONGS_EQUAL(1, (long)actualJacobianQR.size());
195  LONGS_EQUAL(5, (long)actualJacobianQR.keys()[0]);
196  Matrix actualA = actualJacobianQR.getA(actualJacobianQR.begin());
197  Matrix actualCov = (actualA.transpose() * actualA).inverse();
198  EXPECT(assert_equal(expectedCov, actualCov, 1e-1));
199 
200  // Marginal on 6
201 // expectedCov = (Matrix(2,2) <<
202 // 8471.2, 2886.2,
203 // 2886.2, 1015.8);
204  expectedCov = (Matrix(2,2) <<
205  1015.8, 2886.2,
206  2886.2, 8471.2).finished();
207  //actualJacobianChol = bt.marginalFactor(6, EliminateCholesky);
208  actualJacobianQR = *bt.marginalFactor(6, EliminateQR);
209  //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
210  LONGS_EQUAL(2, (long)actualJacobianQR.rows());
211  LONGS_EQUAL(1, (long)actualJacobianQR.size());
212  LONGS_EQUAL(6, (long)actualJacobianQR.keys()[0]);
213  actualA = actualJacobianQR.getA(actualJacobianQR.begin());
214  actualCov = (actualA.transpose() * actualA).inverse();
215  EXPECT(assert_equal(expectedCov, actualCov, 1e1));
216 }
217 
218 /* ************************************************************************* */
219 namespace {
220 double computeError(const GaussianBayesTree& gbt, const Vector10& values) {
221  pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian();
222  return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
223 }
224 }
225 
226 /* ************************************************************************* */
227 TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
228  const GaussianConditional gc1(
229  Pairs{{2, (Matrix(6, 2) << 31.0, 32.0, 0.0, 34.0, 0.0, 0.0, 0.0, 0.0, 0.0,
230  0.0, 0.0, 0.0)
231  .finished()},
232  {3, (Matrix(6, 2) << 35.0, 36.0, 37.0, 38.0, 41.0, 42.0, 0.0, 44.0,
233  0.0, 0.0, 0.0, 0.0)
234  .finished()},
235  {4, (Matrix(6, 2) << 0.0, 0.0, 0.0, 0.0, 45.0, 46.0, 47.0, 48.0,
236  51.0, 52.0, 0.0, 54.0)
237  .finished()}},
238  3, (Vector(6) << 29.0, 30.0, 39.0, 40.0, 49.0, 50.0).finished()),
239  gc2(Pairs{{0, (Matrix(4, 2) << 3.0, 4.0, 0.0, 6.0, 0.0, 0.0, 0.0, 0.0)
240  .finished()},
241  {1, (Matrix(4, 2) << 0.0, 0.0, 0.0, 0.0, 17.0, 18.0, 0.0, 20.0)
242  .finished()},
243  {2, (Matrix(4, 2) << 0.0, 0.0, 0.0, 0.0, 21.0, 22.0, 23.0, 24.0)
244  .finished()},
245  {3, (Matrix(4, 2) << 7.0, 8.0, 9.0, 10.0, 0.0, 0.0, 0.0, 0.0)
246  .finished()},
247  {4, (Matrix(4, 2) << 11.0, 12.0, 13.0, 14.0, 25.0, 26.0, 27.0,
248  28.0)
249  .finished()}},
250  2, (Vector(4) << 1.0, 2.0, 15.0, 16.0).finished());
251 
252  // Create an arbitrary Bayes Tree
253  GaussianBayesTree bt;
254  bt.insertRoot(MakeClique(gc1, Children{LeafClique(gc2)}));
255 
256  // Compute the Hessian numerically
257  Matrix hessian = numericalHessian<Vector10>(
258  std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
259 
260  // Compute the gradient numerically
261  Vector gradient = numericalGradient<Vector10>(
262  std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
263 
264  // Compute the gradient using dense matrices
265  Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
266  LONGS_EQUAL(11, (long)augmentedHessian.cols());
267  Vector denseMatrixGradient = -augmentedHessian.col(10).segment(0,10);
268  EXPECT(assert_equal(gradient, denseMatrixGradient, 1e-5));
269 
270  // Compute the steepest descent point
271  double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
272  Vector expected = gradient * step;
273 
274  // Known steepest descent point from Bayes' net version
275  VectorValues expectedFromBN{{0, Vector2(0.000129034, 0.000688183)},
276  {1, Vector2(0.0109679, 0.0253767)},
277  {2, Vector2(0.0680441, 0.114496)},
278  {3, Vector2(0.16125, 0.241294)},
279  {4, Vector2(0.300134, 0.423233)}};
280 
281  // Compute the steepest descent point with the dogleg function
282  VectorValues actual = bt.optimizeGradientSearch();
283 
284  // Check that points agree
285  KeyVector keys {0, 1, 2, 3, 4};
286  EXPECT(assert_equal(expected, actual.vector(keys), 1e-5));
287  EXPECT(assert_equal(expectedFromBN, actual, 1e-5));
288 
289  // Check that point causes a decrease in error
290  double origError = GaussianFactorGraph(bt).error(VectorValues::Zero(actual));
291  double newError = GaussianFactorGraph(bt).error(actual);
292  EXPECT(newError < origError);
293 }
294 
295 /* ************************************************************************* */
296 TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) {
297 
298  // create small factor graph
299  GaussianFactorGraph fg;
300  Key x1 = 2, x2 = 0, l1 = 1;
301  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
302  fg.emplace_shared<JacobianFactor>(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
303  fg.emplace_shared<JacobianFactor>(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2);
304  fg.emplace_shared<JacobianFactor>(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
305  fg.emplace_shared<JacobianFactor>(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
306 
307  // create corresponding Bayes tree:
308  std::shared_ptr<gtsam::GaussianBayesTree> bt = fg.eliminateMultifrontal();
309  Matrix H = fg.hessian().first;
310 
311  // test determinant
312  // NOTE: the hessian of the factor graph is H = R'R where R is the matrix encoded by the bayes tree,
313  // for this reason we have to take the sqrt
314  double expectedDeterminant = sqrt(H.determinant()); // determinant computed from full matrix
315  double actualDeterminant = bt->determinant();
316  EXPECT_DOUBLES_EQUAL(expectedDeterminant,actualDeterminant,expectedDeterminant*1e-6);// relative tolerance
317 }
318 
319 /* ************************************************************************* */
321 TEST(GaussianBayesTree, LogDeterminant) {
322  using symbol_shorthand::L;
323  using symbol_shorthand::X;
324 
325  // Create a factor graph that will result in
326  // a bayes tree with at least 2 nodes.
327  GaussianFactorGraph fg;
328  Key x1 = X(1), x2 = X(2), l1 = L(1);
329  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
330  fg.emplace_shared<JacobianFactor>(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2);
331  fg.emplace_shared<JacobianFactor>(x2, 10 * I_2x2, x1, -10 * I_2x2,
332  Vector2(2.0, -1.0), unit2);
333  fg.emplace_shared<JacobianFactor>(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
334  fg.emplace_shared<JacobianFactor>(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
335  fg.emplace_shared<JacobianFactor>(x3, 10 * I_2x2, x2, -10 * I_2x2,
336  Vector2(2.0, -1.0), unit2);
337  fg.emplace_shared<JacobianFactor>(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2);
338 
339  // create corresponding Bayes net and Bayes tree:
340  std::shared_ptr<gtsam::GaussianBayesNet> bn = fg.eliminateSequential();
341  std::shared_ptr<gtsam::GaussianBayesTree> bt = fg.eliminateMultifrontal();
342 
343  // Test logDeterminant
344  EXPECT_DOUBLES_EQUAL(bn->logDeterminant(), bt->logDeterminant(), 1e-9);
345 }
346 
347 /* ************************************************************************* */
348 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
349 /* ************************************************************************* */
gtsam::utils.visual_isam.step
def step(data, isam, result, truth, currPoseIndex)
Definition: visual_isam.py:82
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::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
debug.h
Global debugging flags.
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Factor::size
size_t size() const
Definition: Factor.h:159
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
leaf::values
leaf::MyValues values
TEST
TEST(GaussianBayesTree, eliminate)
Definition: testGaussianBayesTree.cpp:84
L
MatrixXd L
Definition: LLT_example.cpp:6
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.
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::GaussianFactorGraph::error
double error(const VectorValues &x) const
Definition: GaussianFactorGraph.cpp:71
H
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:12
gtsam::GaussianBayesTreeClique::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: GaussianBayesTree.h:41
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
inverse
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::BayesTree::insertRoot
void insertRoot(const sharedClique &subtree)
Definition: BayesTree-inst.h:299
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::EliminateableFactorGraph::eliminateSequential
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:29
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GaussianConditional.h
Conditional Gaussian Base class.
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam::GaussianFactorGraph::hessian
std::pair< Matrix, Vector > hessian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:264
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
gtsam
traits
Definition: chartTesting.h:28
main
int main()
Definition: testGaussianBayesTree.cpp:348
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
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::EliminateableFactorGraph::eliminateMultifrontal
std::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:88
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:170
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
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
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
x4
static string x4("x4")
gtsam::Factor::begin
const_iterator begin() const
Definition: Factor.h:145
X
#define X
Definition: icosphere.cpp:20
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
gtsam::JacobianFactor::getA
constABlock getA(const_iterator variable) const
Definition: JacobianFactor.h:301
gtsam::Pairs
std::vector< std::pair< Eigen::Index, Matrix > > Pairs
Definition: JacobianFactor.cpp:44

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