testGaussianBayesNet.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 <gtsam/linear/GaussianBayesNet.h>
19 #include <gtsam/linear/JacobianFactor.h>
20 #include <gtsam/linear/GaussianFactorGraph.h>
21 #include <gtsam/base/Testable.h>
22 #include <gtsam/base/numericalDerivative.h>
23 
24 #include <CppUnitLite/TestHarness.h>
25 #include <boost/tuple/tuple.hpp>
26 #include <boost/assign/list_of.hpp>
27 #include <boost/assign/std/list.hpp> // for operator +=
28 using namespace boost::assign;
29 
30 // STL/C++
31 #include <iostream>
32 #include <sstream>
33 
34 using namespace std;
35 using namespace gtsam;
36 
37 static const Key _x_ = 11, _y_ = 22, _z_ = 33;
38 
39 static GaussianBayesNet smallBayesNet =
40  list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1))(
41  GaussianConditional(_y_, Vector1::Constant(5), I_1x1));
42 
43 static GaussianBayesNet noisyBayesNet =
44  list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1,
45  noiseModel::Isotropic::Sigma(1, 2.0)))(
46  GaussianConditional(_y_, Vector1::Constant(5), I_1x1,
47  noiseModel::Isotropic::Sigma(1, 3.0)));
48 
49 /* ************************************************************************* */
50 TEST( GaussianBayesNet, Matrix )
51 {
52  Matrix R; Vector d;
53  boost::tie(R,d) = smallBayesNet.matrix(); // find matrix and RHS
54 
55  Matrix R1 = (Matrix2() <<
56  1.0, 1.0,
57  0.0, 1.0
58  ).finished();
59  Vector d1 = Vector2(9.0, 5.0);
60 
61  EXPECT(assert_equal(R,R1));
62  EXPECT(assert_equal(d,d1));
63 }
64 
65 /* ************************************************************************* */
66 TEST( GaussianBayesNet, NoisyMatrix )
67 {
68  Matrix R; Vector d;
69  boost::tie(R,d) = noisyBayesNet.matrix(); // find matrix and RHS
70 
71  Matrix R1 = (Matrix2() <<
72  0.5, 0.5,
73  0.0, 1./3.
74  ).finished();
75  Vector d1 = Vector2(9./2., 5./3.);
76 
77  EXPECT(assert_equal(R,R1));
78  EXPECT(assert_equal(d,d1));
79 }
80 
81 /* ************************************************************************* */
82 TEST(GaussianBayesNet, Optimize) {
83  VectorValues expected =
84  map_list_of<Key, Vector>(_x_, Vector1::Constant(4))(_y_, Vector1::Constant(5));
85  VectorValues actual = smallBayesNet.optimize();
86  EXPECT(assert_equal(expected, actual));
87 }
88 
89 /* ************************************************************************* */
90 TEST(GaussianBayesNet, NoisyOptimize) {
91  Matrix R;
92  Vector d;
93  boost::tie(R, d) = noisyBayesNet.matrix(); // find matrix and RHS
94  const Vector x = R.inverse() * d;
95  VectorValues expected = map_list_of<Key, Vector>(_x_, x.head(1))(_y_, x.tail(1));
96 
97  VectorValues actual = noisyBayesNet.optimize();
98  EXPECT(assert_equal(expected, actual));
99 }
100 
101 /* ************************************************************************* */
102 TEST( GaussianBayesNet, optimizeIncomplete )
103 {
104  static GaussianBayesNet incompleteBayesNet = list_of
105  (GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1));
106 
107  VectorValues solutionForMissing = map_list_of<Key, Vector>
108  (_y_, Vector1::Constant(5));
109 
110  VectorValues actual = incompleteBayesNet.optimize(solutionForMissing);
111 
112  VectorValues expected = map_list_of<Key, Vector>
113  (_x_, Vector1::Constant(4))
114  (_y_, Vector1::Constant(5));
115 
116  EXPECT(assert_equal(expected,actual));
117 }
118 
119 /* ************************************************************************* */
120 TEST( GaussianBayesNet, optimize3 )
121 {
122  // y = R*x, x=inv(R)*y
123  // 4 = 1 1 -1
124  // 5 1 5
125  // NOTE: we are supplying a new RHS here
126 
127  VectorValues expected = map_list_of<Key, Vector>
128  (_x_, Vector1::Constant(-1))
129  (_y_, Vector1::Constant(5));
130 
131  // Test different RHS version
132  VectorValues gx = map_list_of<Key, Vector>
133  (_x_, Vector1::Constant(4))
134  (_y_, Vector1::Constant(5));
135  VectorValues actual = smallBayesNet.backSubstitute(gx);
136  EXPECT(assert_equal(expected, actual));
137 }
138 
139 /* ************************************************************************* */
140 TEST(GaussianBayesNet, ordering)
141 {
142  Ordering expected;
143  expected += _x_, _y_;
144  const auto actual = noisyBayesNet.ordering();
145  EXPECT(assert_equal(expected, actual));
146 }
147 
148 /* ************************************************************************* */
149 TEST( GaussianBayesNet, MatrixStress )
150 {
151  GaussianBayesNet bn;
152  using GC = GaussianConditional;
153  bn.emplace_shared<GC>(_x_, Vector2(1, 2), 1 * I_2x2, _y_, 2 * I_2x2, _z_, 3 * I_2x2);
154  bn.emplace_shared<GC>(_y_, Vector2(3, 4), 4 * I_2x2, _z_, 5 * I_2x2);
155  bn.emplace_shared<GC>(_z_, Vector2(5, 6), 6 * I_2x2);
156 
157  const VectorValues expected = bn.optimize();
158  for (const auto& keys :
159  {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}),
160  KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}),
161  KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) {
162  const Ordering ordering(keys);
163  Matrix R;
164  Vector d;
165  boost::tie(R, d) = bn.matrix(ordering);
166  EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d));
167  }
168 }
169 
170 /* ************************************************************************* */
171 TEST( GaussianBayesNet, backSubstituteTranspose )
172 {
173  // x=R'*y, expected=inv(R')*x
174  // 2 = 1 2
175  // 5 1 1 3
176  VectorValues
177  x = map_list_of<Key, Vector>
178  (_x_, Vector1::Constant(2))
179  (_y_, Vector1::Constant(5)),
180  expected = map_list_of<Key, Vector>
181  (_x_, Vector1::Constant(2))
182  (_y_, Vector1::Constant(3));
183 
184  VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
185  EXPECT(assert_equal(expected, actual));
186 
187  const auto ordering = noisyBayesNet.ordering();
188  const Matrix R = smallBayesNet.matrix(ordering).first;
189  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
190  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
191 }
192 
193 /* ************************************************************************* */
194 TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
195 {
196  // x=R'*y, expected=inv(R')*x
197  // 2 = 1 2
198  // 5 1 1 3
199  VectorValues
200  x = map_list_of<Key, Vector>
201  (_x_, Vector1::Constant(2))
202  (_y_, Vector1::Constant(5)),
203  expected = map_list_of<Key, Vector>
204  (_x_, Vector1::Constant(4))
205  (_y_, Vector1::Constant(9));
206 
207  VectorValues actual = noisyBayesNet.backSubstituteTranspose(x);
208  EXPECT(assert_equal(expected, actual));
209 
210  const auto ordering = noisyBayesNet.ordering();
211  const Matrix R = noisyBayesNet.matrix(ordering).first;
212  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
213  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
214 }
215 
216 /* ************************************************************************* */
217 // Tests computing Determinant
218 TEST( GaussianBayesNet, DeterminantTest )
219 {
220  GaussianBayesNet cbn;
221  cbn += GaussianConditional(
222  0, Vector2(3.0, 4.0), (Matrix2() << 1.0, 3.0, 0.0, 4.0).finished(),
223  1, (Matrix2() << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
224 
225  cbn += GaussianConditional(
226  1, Vector2(5.0, 6.0), (Matrix2() << 1.0, 1.0, 0.0, 3.0).finished(),
227  2, (Matrix2() << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
228 
229  cbn += GaussianConditional(
230  3, Vector2(7.0, 8.0), (Matrix2() << 1.0, 1.0, 0.0, 5.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
231 
232  double expectedDeterminant = 60.0 / 64.0;
233  double actualDeterminant = cbn.determinant();
234 
235  EXPECT_DOUBLES_EQUAL( expectedDeterminant, actualDeterminant, 1e-9);
236 }
237 
238 /* ************************************************************************* */
239 namespace {
240  double computeError(const GaussianBayesNet& gbn, const Vector10& values)
241  {
242  pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian();
243  return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
244  }
245 }
246 
247 /* ************************************************************************* */
248 TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
249 
250  // Create an arbitrary Bayes Net
251  GaussianBayesNet gbn;
252  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
253  0, Vector2(1.0,2.0), (Matrix2() << 3.0,4.0,0.0,6.0).finished(),
254  3, (Matrix2() << 7.0,8.0,9.0,10.0).finished(),
255  4, (Matrix2() << 11.0,12.0,13.0,14.0).finished()));
256  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
257  1, Vector2(15.0,16.0), (Matrix2() << 17.0,18.0,0.0,20.0).finished(),
258  2, (Matrix2() << 21.0,22.0,23.0,24.0).finished(),
259  4, (Matrix2() << 25.0,26.0,27.0,28.0).finished()));
260  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
261  2, Vector2(29.0,30.0), (Matrix2() << 31.0,32.0,0.0,34.0).finished(),
262  3, (Matrix2() << 35.0,36.0,37.0,38.0).finished()));
263  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
264  3, Vector2(39.0,40.0), (Matrix2() << 41.0,42.0,0.0,44.0).finished(),
265  4, (Matrix2() << 45.0,46.0,47.0,48.0).finished()));
266  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
267  4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished()));
268 
269  // Compute the Hessian numerically
270  Matrix hessian = numericalHessian<Vector10>(
271  boost::bind(&computeError, gbn, _1), Vector10::Zero());
272 
273  // Compute the gradient numerically
274  Vector gradient = numericalGradient<Vector10>(
275  boost::bind(&computeError, gbn, _1), Vector10::Zero());
276 
277  // Compute the gradient using dense matrices
278  Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
279  LONGS_EQUAL(11, (long)augmentedHessian.cols());
280  Vector denseMatrixGradient = -augmentedHessian.col(10).segment(0,10);
281  EXPECT(assert_equal(gradient, denseMatrixGradient, 1e-5));
282 
283  // Compute the steepest descent point
284  double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
285  Vector expected = gradient * step;
286 
287  // Compute the steepest descent point with the dogleg function
288  VectorValues actual = gbn.optimizeGradientSearch();
289 
290  // Check that points agree
291  KeyVector keys {0, 1, 2, 3, 4};
292  Vector actualAsVector = actual.vector(keys);
293  EXPECT(assert_equal(expected, actualAsVector, 1e-5));
294 
295  // Check that point causes a decrease in error
296  double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(actual));
297  double newError = GaussianFactorGraph(gbn).error(actual);
298  EXPECT(newError < origError);
299 }
300 
301 /* ************************************************************************* */
302 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
303 /* ************************************************************************* */
test_docs.d
d
Definition: test_docs.py:28
gtsam.utils.visual_isam.step
def step(data, isam, result, truth, currPoseIndex)
Definition: visual_isam.py:82
gtsam::GaussianBayesNet::matrix
std::pair< Matrix, Vector > matrix(const Ordering &ordering) const
Definition: GaussianBayesNet.cpp:160
Testable.h
Concept check for values that can be used in unit tests.
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
ordering
static enum @843 ordering
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
R
Rot2 R(Rot2::fromAngle(0.1))
leaf::values
leaf::MyValues values
main
int main()
Definition: testGaussianBayesNet.cpp:302
gtsam::GaussianBayesNet::optimize
VectorValues optimize() const
Solve the GaussianBayesNet, i.e. return , by back-substitution.
Definition: GaussianBayesNet.cpp:41
std
Definition: Half.h:150
numericalDerivative.h
Some functions to compute numerical derivatives.
_y_
static const Key _y_
Definition: testGaussianBayesNet.cpp:37
gtsam::GaussianBayesNet::optimizeGradientSearch
VectorValues optimizeGradientSearch() const
Definition: GaussianBayesNet.cpp:62
TEST
TEST(GaussianBayesNet, Matrix)
Definition: testGaussianBayesNet.cpp:50
gtsam::GaussianBayesNet::ordering
Ordering ordering() const
Definition: GaussianBayesNet.cpp:142
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
_x_
static const Key _x_
Definition: testGaussianBayesNet.cpp:37
gtsam::GaussianFactorGraph::error
double error(const VectorValues &x) const
Definition: GaussianFactorGraph.h:145
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:172
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
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: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.)
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
R1
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
noisyBayesNet
static GaussianBayesNet noisyBayesNet
Definition: testGaussianBayesNet.cpp:43
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
traits
Definition: chartTesting.h:28
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
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
gtsam::GaussianBayesNet::backSubstitute
VectorValues backSubstitute(const VectorValues &gx) const
Definition: GaussianBayesNet.cpp:84
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:45
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:42
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::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::GaussianBayesNet::backSubstituteTranspose
VectorValues backSubstituteTranspose(const VectorValues &gx) const
Definition: GaussianBayesNet.cpp:100
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:173

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