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

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:38