From 88e3330d279cf0c8616d5fb8e72ced0cbb6f18bd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Jan 2022 14:02:10 -0500 Subject: [PATCH 1/8] add space after commas --- gtsam/inference/BayesTree-inst.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 9b937fefb5..b341c1d5aa 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -95,7 +95,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTree::dot(std::ostream& s, sharedClique clique, - const KeyFormatter& indexFormatter, + const KeyFormatter& keyFormatter, int parentnum) const { static int num = 0; bool first = true; @@ -104,10 +104,10 @@ namespace gtsam { std::string parent = out.str(); parent += "[label=\""; - for (Key index : clique->conditional_->frontals()) { - if (!first) parent += ","; + for (Key key : clique->conditional_->frontals()) { + if (!first) parent += ", "; first = false; - parent += indexFormatter(index); + parent += keyFormatter(key); } if (clique->parent()) { @@ -116,10 +116,10 @@ namespace gtsam { } first = true; - for (Key sep : clique->conditional_->parents()) { - if (!first) parent += ","; + for (Key parentKey : clique->conditional_->parents()) { + if (!first) parent += ", "; first = false; - parent += indexFormatter(sep); + parent += keyFormatter(parentKey); } parent += "\"];\n"; s << parent; @@ -127,7 +127,7 @@ namespace gtsam { for (sharedClique c : clique->children) { num++; - dot(s, c, indexFormatter, parentnum); + dot(s, c, keyFormatter, parentnum); } } From 1d8a4563c994505aa6105ade46400e140f0ef291 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Jan 2022 14:02:29 -0500 Subject: [PATCH 2/8] use const & --- gtsam/slam/dataset.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 0684063de2..fc020b05b5 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -208,15 +208,15 @@ std::map parseVariables(const std::string &filename, /* ************************************************************************* */ // Interpret noise parameters according to flags -static SharedNoiseModel -createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat, - KernelFunctionType kernelFunctionType) { +static SharedNoiseModel createNoiseModel( + const Vector6 &v, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { if (noiseFormat == NoiseFormatAUTO) { // Try to guess covariance matrix layout - if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && // + if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && // v(3) != 0.0 && v(4) == 0.0 && v(5) == 0.0) { noiseFormat = NoiseFormatGRAPH; - } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && // + } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && // v(3) != 0.0 && v(4) == 0.0 && v(5) != 0.0) { noiseFormat = NoiseFormatCOV; } else { From 1cc67754d335c506868df7d9d2fb05f16a3265dd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Jan 2022 14:02:58 -0500 Subject: [PATCH 3/8] fix argument name --- gtsam/nonlinear/nonlinear.i | 4 +- python/gtsam/tests/test_GaussianBayesNet.py | 53 +++++++++++++++++++++ 2 files changed, 55 insertions(+), 2 deletions(-) create mode 100644 python/gtsam/tests/test_GaussianBayesNet.py diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index eedf421bc7..055fbd75b6 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -98,11 +98,11 @@ class NonlinearFactorGraph { string dot( const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& formatting = GraphvizFormatting()); + const GraphvizFormatting& writer = GraphvizFormatting()); void saveGraph( const string& s, const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& formatting = GraphvizFormatting()) const; + const GraphvizFormatting& writer = GraphvizFormatting()) const; // enabling serialization functionality void serialize() const; diff --git a/python/gtsam/tests/test_GaussianBayesNet.py b/python/gtsam/tests/test_GaussianBayesNet.py new file mode 100644 index 0000000000..8cdbec0af7 --- /dev/null +++ b/python/gtsam/tests/test_GaussianBayesNet.py @@ -0,0 +1,53 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Linear Factor Graphs. +Author: Frank Dellaert & Gerry Chen +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +import numpy as np +from gtsam import GaussianBayesNet, GaussianConditional +from gtsam.utils.test_case import GtsamTestCase + +# some keys +_x_ = 11 +_y_ = 22 +_z_ = 33 + + +def smallBayesNet(): + """Create a small Bayes Net for testing""" + bayesNet = GaussianBayesNet() + I_1x1 = np.eye(1, dtype=float) + bayesNet.push_back(GaussianConditional( + _x_, [9.0], I_1x1, _y_, I_1x1)) + bayesNet.push_back(GaussianConditional(_y_, [5.0], I_1x1)) + return bayesNet + + +class TestGaussianBayesNet(GtsamTestCase): + """Tests for Gaussian Bayes nets.""" + + def test_matrix(self): + """Test matrix method""" + R, d = smallBayesNet().matrix() # get matrix and RHS + R1 = np.array([ + [1.0, 1.0], + [0.0, 1.0]]) + d1 = np.array([9.0, 5.0]) + np.testing.assert_equal(R, R1) + np.testing.assert_equal(d, d1) + + +if __name__ == '__main__': + unittest.main() From 8e2f8a87eb04d699a4723b871fb738f74f64f3af Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Jan 2022 14:09:08 -0500 Subject: [PATCH 4/8] Better wrap of load --- gtsam/slam/dataset.h | 14 ++++++++------ gtsam/slam/slam.i | 39 +++++++++++++++++++++++++-------------- 2 files changed, 33 insertions(+), 20 deletions(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index db5d7d76ad..1611d78f65 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -167,10 +167,11 @@ GTSAM_EXPORT GraphAndValues load2D( * @param kernelFunctionType whether to wrap the noise model in a robust kernel * @return graph and initial values */ -GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, - SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise = - false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // - KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); +GTSAM_EXPORT GraphAndValues +load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), + size_t maxIndex = 0, bool addNoise = false, bool smart = true, + NoiseFormat noiseFormat = NoiseFormatAUTO, // + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, @@ -185,8 +186,9 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, * @param kernelFunctionType whether to wrap the noise model in a robust kernel * @return graph and initial values */ -GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false, - KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); +GTSAM_EXPORT GraphAndValues +readG2o(const std::string& g2oFile, const bool is3D = false, + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** * @brief This function writes a g2o file from diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index e044dd2c19..2b25a43698 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -253,17 +253,27 @@ bool writeBAL(string filename, gtsam::SfmData& data); gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, - bool addNoise, bool smart); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, - bool addNoise); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model, int maxIndex); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); +enum NoiseFormat { + NoiseFormatG2O, + NoiseFormatTORO, + NoiseFormatGRAPH, + NoiseFormatCOV, + NoiseFormatAUTO +}; + +enum KernelFunctionType { + KernelFunctionTypeNONE, + KernelFunctionTypeHUBER, + KernelFunctionTypeTUKEY +}; + +std::pair load2D( + string filename, gtsam::noiseModel::Diagonal* model = nullptr, + size_t maxIndex = 0, bool addNoise = false, bool smart = true, + gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormatAUTO, + gtsam::KernelFunctionType kernelFunctionType = + gtsam::KernelFunctionTypeNONE); + void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); @@ -290,9 +300,10 @@ gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); -pair readG2o(string filename); -pair readG2o(string filename, - bool is3D); +pair readG2o( + string filename, const bool is3D = false, + gtsam::KernelFunctionType kernelFunctionType = + gtsam::KernelFunctionTypeNONE); void writeG2o(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& estimate, string filename); From 605a4432835e2230a4b699d6bbbe463a8b53700e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Jan 2022 14:09:30 -0500 Subject: [PATCH 5/8] Add rangeFactor typedefs --- gtsam/sam/sam.i | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i index 370e1c3ea7..7ba401f1e1 100644 --- a/gtsam/sam/sam.i +++ b/gtsam/sam/sam.i @@ -22,10 +22,19 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { void serialize() const; }; +// between points: +typedef gtsam::RangeFactor RangeFactor2; +typedef gtsam::RangeFactor RangeFactor3; + +// between pose and point: typedef gtsam::RangeFactor RangeFactor2D; -typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose2; + +// between poses: +typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose3; + +// more specialized types: typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; typedef gtsam::RangeFactor, gtsam::Point3> From 656a6952678631f4fe1b7660ef2e5b68b2e985ac Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Jan 2022 14:09:55 -0500 Subject: [PATCH 6/8] wrap more eliminate variants (and dot) --- gtsam/discrete/discrete.i | 5 ++++- gtsam/linear/linear.i | 10 +++++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 56e7248a3f..93979bba17 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -270,11 +270,14 @@ class DiscreteFactorGraph { gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering); gtsam::DiscreteBayesNet eliminateSequential(); + gtsam::DiscreteBayesNet eliminateSequential(gtsam::Ordering::OrderingType type); gtsam::DiscreteBayesNet eliminateSequential(const gtsam::Ordering& ordering); std::pair eliminatePartialSequential(const gtsam::Ordering& ordering); + gtsam::DiscreteBayesTree eliminateMultifrontal(); - gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering); + gtsam::DiscreteBayesTree eliminateMultifrontal(gtsam::Ordering::OrderingType type); + gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering); std::pair eliminatePartialMultifrontal(const gtsam::Ordering& ordering); diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index b079c3dd18..674287b87f 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -407,8 +407,10 @@ class GaussianFactorGraph { // Elimination and marginals gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type); gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type); gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); pair eliminatePartialSequential( const gtsam::Ordering& ordering); @@ -527,6 +529,7 @@ virtual class GaussianBayesNet { gtsam::VectorValues optimize() const; gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; + std::pair matrix() const; gtsam::VectorValues optimizeGradientSearch() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; gtsam::VectorValues gradientAtZero() const; @@ -556,7 +559,12 @@ virtual class GaussianBayesTree { size_t size() const; bool empty() const; size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; + + string dot(const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void saveGraph(string s, + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; gtsam::VectorValues optimize() const; gtsam::VectorValues optimizeGradientSearch() const; From 06fb94144b5621a67153155adcc5ffe2140bf22b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Jan 2022 15:10:32 -0500 Subject: [PATCH 7/8] Expose all ordering methods --- gtsam/inference/inference.i | 47 ++++++++++++++++++++++++++++++------- 1 file changed, 39 insertions(+), 8 deletions(-) diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index 5a661d5cf2..e7b074ec49 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -4,6 +4,12 @@ namespace gtsam { +// Headers for overloaded methods below, break hierarchy :-/ +#include +#include +#include +#include + #include // Default keyformatter @@ -98,10 +104,41 @@ class Ordering { Ordering(); Ordering(const gtsam::Ordering& other); - template + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering ColamdConstrainedLast( + const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast, + bool forceOrder = false); + + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering ColamdConstrainedFirst( + const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst, + bool forceOrder = false); + + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering Natural(const FACTOR_GRAPH& graph); + + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering Metis(const FACTOR_GRAPH& graph); + + template < + FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType, + const FACTOR_GRAPH& graph); + // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -135,12 +172,6 @@ class DotWriter { }; #include - -// Headers for overloaded methods below, break hierarchy :-/ -#include -#include -#include - class VariableIndex { // Standard Constructors and Named Constructors VariableIndex(); From 113fbf10fe3233886c80ef4bf13bd02457eaf62e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 08:36:31 -0500 Subject: [PATCH 8/8] Fix test --- .../discrete/tests/testDiscreteBayesTree.cpp | 22 +++++++++---------- python/gtsam/tests/test_GaussianBayesNet.py | 4 ++-- python/gtsam/tests/test_GraphvizFormatting.py | 6 ++--- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 26356be3d8..6635633a27 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -243,27 +243,27 @@ TEST(DiscreteBayesTree, Dot) { string actual = self.bayesTree->dot(); EXPECT(actual == "digraph G{\n" - "0[label=\"13,11,6,7\"];\n" + "0[label=\"13, 11, 6, 7\"];\n" "0->1\n" - "1[label=\"14 : 11,13\"];\n" + "1[label=\"14 : 11, 13\"];\n" "1->2\n" - "2[label=\"9,12 : 14\"];\n" + "2[label=\"9, 12 : 14\"];\n" "2->3\n" - "3[label=\"3 : 9,12\"];\n" + "3[label=\"3 : 9, 12\"];\n" "2->4\n" - "4[label=\"2 : 9,12\"];\n" + "4[label=\"2 : 9, 12\"];\n" "2->5\n" - "5[label=\"8 : 12,14\"];\n" + "5[label=\"8 : 12, 14\"];\n" "5->6\n" - "6[label=\"1 : 8,12\"];\n" + "6[label=\"1 : 8, 12\"];\n" "5->7\n" - "7[label=\"0 : 8,12\"];\n" + "7[label=\"0 : 8, 12\"];\n" "1->8\n" - "8[label=\"10 : 13,14\"];\n" + "8[label=\"10 : 13, 14\"];\n" "8->9\n" - "9[label=\"5 : 10,13\"];\n" + "9[label=\"5 : 10, 13\"];\n" "8->10\n" - "10[label=\"4 : 10,13\"];\n" + "10[label=\"4 : 10, 13\"];\n" "}"); } diff --git a/python/gtsam/tests/test_GaussianBayesNet.py b/python/gtsam/tests/test_GaussianBayesNet.py index 8cdbec0af7..e4d396cfe8 100644 --- a/python/gtsam/tests/test_GaussianBayesNet.py +++ b/python/gtsam/tests/test_GaussianBayesNet.py @@ -5,8 +5,8 @@ See LICENSE for the license information -Unit tests for Linear Factor Graphs. -Author: Frank Dellaert & Gerry Chen +Unit tests for Gaussian Bayes Nets. +Author: Frank Dellaert """ # pylint: disable=invalid-name, no-name-in-module, no-member diff --git a/python/gtsam/tests/test_GraphvizFormatting.py b/python/gtsam/tests/test_GraphvizFormatting.py index 5962366efa..ecdc23b450 100644 --- a/python/gtsam/tests/test_GraphvizFormatting.py +++ b/python/gtsam/tests/test_GraphvizFormatting.py @@ -78,7 +78,7 @@ def test_swapped_axes(self): graphviz_formatting.paperHorizontalAxis = gtsam.GraphvizFormatting.Axis.X graphviz_formatting.paperVerticalAxis = gtsam.GraphvizFormatting.Axis.Y self.assertEqual(self.graph.dot(self.values, - formatting=graphviz_formatting), + writer=graphviz_formatting), textwrap.dedent(expected_result)) def test_factor_points(self): @@ -100,7 +100,7 @@ def test_factor_points(self): graphviz_formatting.plotFactorPoints = False self.assertEqual(self.graph.dot(self.values, - formatting=graphviz_formatting), + writer=graphviz_formatting), textwrap.dedent(expected_result)) def test_width_height(self): @@ -127,7 +127,7 @@ def test_width_height(self): graphviz_formatting.figureHeightInches = 10 self.assertEqual(self.graph.dot(self.values, - formatting=graphviz_formatting), + writer=graphviz_formatting), textwrap.dedent(expected_result))