From 108357992c0f61d63665939f6fe948f667b4e5af Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 02:18:52 -0400 Subject: [PATCH] Optimize an incomplete BayesNet --- .cproject | 184 +++++++++--------- gtsam.h | 1 + gtsam/linear/GaussianBayesNet.cpp | 9 +- gtsam/linear/GaussianBayesNet.h | 8 +- .../tests/testGaussianBayesNetUnordered.cpp | 18 ++ 5 files changed, 127 insertions(+), 93 deletions(-) diff --git a/.cproject b/.cproject index 31d4d9143..5d6801d19 100644 --- a/.cproject +++ b/.cproject @@ -998,6 +998,102 @@ true true + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + make -j2 @@ -2123,94 +2219,6 @@ true true - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianFactorGraphUnordered.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - make -j2 diff --git a/gtsam.h b/gtsam.h index 05cf7ffb5..8d61487a5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1423,6 +1423,7 @@ virtual class GaussianBayesNet { void push_back(const gtsam::GaussianBayesNet& bayesNet); gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; gtsam::VectorValues optimizeGradientSearch() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; gtsam::VectorValues gradientAtZero() const; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 8b3dc0084..b3b8b9a41 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -42,7 +42,14 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianBayesNet::optimize() const { - VectorValues soln; + VectorValues soln; // no missing variables -> just create an empty vector + return optimize(soln); + } + + /* ************************************************************************* */ + VectorValues GaussianBayesNet::optimize( + const VectorValues& solutionForMissing) const { + VectorValues soln(solutionForMissing); // possibly empty // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) /** solve each node in turn in topological sort order (parents first)*/ BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 26b187369..69a70a5e4 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -68,12 +68,12 @@ namespace gtsam { /// @name Standard Interface /// @{ - /** - * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by - * back-substitution. - */ + /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution VectorValues optimize() const; + /// Version of optimize for incomplete BayesNet, needs solution for missing variables + VectorValues optimize(const VectorValues& solutionForMissing) const; + ///@} ///@name Linear Algebra diff --git a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp index ca2b4c3d2..608e9b1e1 100644 --- a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp @@ -69,6 +69,24 @@ TEST( GaussianBayesNet, optimize ) EXPECT(assert_equal(expected,actual)); } +/* ************************************************************************* */ +TEST( GaussianBayesNet, optimizeIncomplete ) +{ + static GaussianBayesNet incompleteBayesNet = list_of + (GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0))); + + VectorValues solutionForMissing = map_list_of + (_y_, (Vector(1) << 5.0)); + + VectorValues actual = incompleteBayesNet.optimize(solutionForMissing); + + VectorValues expected = map_list_of + (_x_, (Vector(1) << 4.0)) + (_y_, (Vector(1) << 5.0)); + + EXPECT(assert_equal(expected,actual)); +} + /* ************************************************************************* */ TEST( GaussianBayesNet, optimize3 ) {