From faf004347ba984c6b235c3264c6f5c1fe368291b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Mar 2021 15:58:12 -0500 Subject: [PATCH] Fix Matlab tests and add saveGraph method to GaussianBayesNet --- gtsam/gtsam.i | 2 ++ gtsam/linear/GaussianBayesNet.cpp | 25 ++++++++++++++++++++++--- gtsam/linear/GaussianBayesNet.h | 10 ++++++++++ matlab/+gtsam/plotBayesNet.m | 4 +++- matlab/+gtsam/plotBayesTree.m | 4 +++- matlab/gtsam_tests/testThinBayesTree.m | 2 +- matlab/gtsam_tests/testTriangulation.m | 10 +++++----- matlab/gtsam_tests/thinBayesTree.m | 3 ++- matlab/gtsam_tests/thinTreeBayesNet.m | 13 ++++++++----- 9 files changed, 56 insertions(+), 17 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 22c2cc17d..b8cc15af2 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1791,6 +1791,8 @@ virtual class GaussianBayesNet { gtsam::KeySet keys() const; bool exists(size_t idx) const; + void saveGraph(const string& s) const; + gtsam::GaussianConditional* front() const; gtsam::GaussianConditional* back() const; void push_back(gtsam::GaussianConditional* conditional); diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 04094d593..1e790d0f1 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -12,15 +12,16 @@ /** * @file GaussianBayesNet.cpp * @brief Chordal Bayes Net, the result of eliminating a factor graph - * @author Frank Dellaert + * @author Frank Dellaert, Varun Agrawal */ +#include +#include #include #include -#include -#include #include +#include using namespace std; using namespace gtsam; @@ -204,5 +205,23 @@ namespace gtsam { } /* ************************************************************************* */ + void GaussianBayesNet::saveGraph(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::ofstream of(s.c_str()); + of << "digraph G{\n"; + + for (auto conditional : boost::adaptors::reverse(*this)) { + typename GaussianConditional::Frontals frontals = conditional->frontals(); + Key me = frontals.front(); + typename GaussianConditional::Parents parents = conditional->parents(); + for (Key p : parents) + of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl; + } + + of << "}"; + of.close(); + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 3f6d69e91..06782c3cf 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -177,6 +177,16 @@ namespace gtsam { */ VectorValues backSubstituteTranspose(const VectorValues& gx) const; + /** + * @brief Save the GaussianBayesNet as an image. Requires `dot` to be + * installed. + * + * @param s The name of the figure. + * @param keyFormatter Formatter to use for styling keys in the graph. + */ + void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + /// @} private: diff --git a/matlab/+gtsam/plotBayesNet.m b/matlab/+gtsam/plotBayesNet.m index dea9b04ad..446ffffac 100644 --- a/matlab/+gtsam/plotBayesNet.m +++ b/matlab/+gtsam/plotBayesNet.m @@ -5,4 +5,6 @@ function plotBayesNet(bayesNet) bayesNet.saveGraph('/tmp/bayesNet.dot') !dot -Tpng -o /tmp/dotImage.png /tmp/bayesNet.dot dotImage=imread('/tmp/dotImage.png'); -imshow(dotImage) \ No newline at end of file +imshow(dotImage) + +end \ No newline at end of file diff --git a/matlab/+gtsam/plotBayesTree.m b/matlab/+gtsam/plotBayesTree.m index 94628e8a5..91d0925a9 100644 --- a/matlab/+gtsam/plotBayesTree.m +++ b/matlab/+gtsam/plotBayesTree.m @@ -5,4 +5,6 @@ function plotBayesTree(bayesTree) bayesTree.saveGraph('/tmp/bayesTree.dot') !dot -Tpng -o /tmp/dotImage.png /tmp/bayesTree.dot dotImage=imread('/tmp/dotImage.png'); -imshow(dotImage) \ No newline at end of file +imshow(dotImage) + +end \ No newline at end of file diff --git a/matlab/gtsam_tests/testThinBayesTree.m b/matlab/gtsam_tests/testThinBayesTree.m index 034fc38de..747852146 100644 --- a/matlab/gtsam_tests/testThinBayesTree.m +++ b/matlab/gtsam_tests/testThinBayesTree.m @@ -20,4 +20,4 @@ %% Run the tests import gtsam.* bayesTree = thinBayesTree(3,2); -EQUALITY('7 = bayesTree.size', 7, bayesTree.size); \ No newline at end of file +EQUALITY('7 = bayesTree.size', 4, bayesTree.size); \ No newline at end of file diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m index 7116d3838..c623e7b2b 100644 --- a/matlab/gtsam_tests/testTriangulation.m +++ b/matlab/gtsam_tests/testTriangulation.m @@ -44,15 +44,15 @@ optimize = true; rank_tol = 1e-9; triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); -CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); +CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9); %% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements = Point2Vector; -measurements.push_back(z1.retract([0.1;0.5])); -measurements.push_back(z2.retract([-0.2;0.3])); +measurements.push_back(z1 + [0.1;0.5]); +measurements.push_back(z2 + [-0.2;0.3]); triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); -CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2)); +CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-2); %% two Poses with Bundler Calibration bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480); @@ -67,4 +67,4 @@ measurements.push_back(z1); measurements.push_back(z2); triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize); -CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); +CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9); diff --git a/matlab/gtsam_tests/thinBayesTree.m b/matlab/gtsam_tests/thinBayesTree.m index 6c2f496c6..61f89cead 100644 --- a/matlab/gtsam_tests/thinBayesTree.m +++ b/matlab/gtsam_tests/thinBayesTree.m @@ -1,5 +1,6 @@ function bayesTree = thinBayesTree(depth, width) import gtsam.* bayesNet = thinTreeBayesNet(depth, width); - bayesTree = GaussianBayesTree(bayesNet); + fg = GaussianFactorGraph(bayesNet); + bayesTree = fg.eliminateMultifrontal(); end \ No newline at end of file diff --git a/matlab/gtsam_tests/thinTreeBayesNet.m b/matlab/gtsam_tests/thinTreeBayesNet.m index d993c3fd3..b91a83ba8 100644 --- a/matlab/gtsam_tests/thinTreeBayesNet.m +++ b/matlab/gtsam_tests/thinTreeBayesNet.m @@ -6,8 +6,9 @@ bayesNet = GaussianBayesNet; tree = thinTree(depth,width); % Add root to the Bayes net -gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1)); -bayesNet.push_front(gc); +model = noiseModel.Isotropic.Sigma(1, 3*rand(1)); +gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), model); +bayesNet.push_back(gc); n=tree.getNumberOfElements(); for i=2:n @@ -17,13 +18,15 @@ for i=2:n % Create and link the corresponding GaussianConditionals if tree.getW == 1 || di == 2 % Creation of single-parent GaussianConditional - gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), 5*rand(1)); + model = noiseModel.Isotropic.Sigma(1, 5*rand(1)); + gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), model); elseif tree.getW == 2 || di == 3 % GaussianConditionalj associated with the second parent - gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), 5*rand(1)); + model = noiseModel.Isotropic.Sigma(1, 5*rand(1)); + gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), model); end % Add conditional to the Bayes net - bayesNet.push_front(gc); + bayesNet.push_back(gc); end end \ No newline at end of file