From 59df91d295bd7888a37b15168baf799f735b89e5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 3 Apr 2019 18:44:18 -0400 Subject: [PATCH 1/5] Added optional ordering argument when converting to Matrix/Vector --- gtsam/linear/GaussianBayesNet.cpp | 26 ++++++++++++++++++-------- gtsam/linear/GaussianBayesNet.h | 12 +++++++++++- gtsam/linear/VectorValues.cpp | 20 +++++++++++++------- gtsam/linear/VectorValues.h | 2 +- 4 files changed, 43 insertions(+), 17 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index c9ef922be..22f1918e4 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -138,23 +138,33 @@ namespace gtsam { //} /* ************************************************************************* */ - pair GaussianBayesNet::matrix() const { + Ordering GaussianBayesNet::ordering() const { GaussianFactorGraph factorGraph(*this); - KeySet keys = factorGraph.keys(); + auto keys = factorGraph.keys(); // add frontal keys in order Ordering ordering; - for (const sharedConditional& cg: *this) + for (const sharedConditional& cg : *this) if (cg) { - for (Key key: cg->frontals()) { + for (Key key : cg->frontals()) { ordering.push_back(key); keys.erase(key); } } // add remaining keys in case Bayes net is incomplete - for (Key key: keys) - ordering.push_back(key); - // return matrix and RHS - return factorGraph.jacobian(ordering); + for (Key key : keys) ordering.push_back(key); + return ordering; + } + + /* ************************************************************************* */ + pair GaussianBayesNet::matrix(boost::optional ordering) const { + if (ordering) { + // Convert to a GaussianFactorGraph and use its machinery + GaussianFactorGraph factorGraph(*this); + return factorGraph.jacobian(ordering); + } else { + // recursively call with default ordering + return matrix(this->ordering()); + } } ///* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 401583bbf..64627a276 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -74,6 +74,14 @@ namespace gtsam { /// Version of optimize for incomplete BayesNet, needs solution for missing variables VectorValues optimize(const VectorValues& solutionForMissing) const; + /** + * Return ordering corresponding to a topological sort. + * There are many topological sorts of a Bayes net. This one + * corresponds to the one that makes 'matrix' below upper-triangular. + * In case Bayes net is incomplete any non-frontal are added to the end. + */ + Ordering ordering() const; + ///@} ///@name Linear Algebra @@ -81,8 +89,10 @@ namespace gtsam { /** * Return (dense) upper-triangular matrix representation + * Will return upper-triangular matrix only when using 'ordering' above. + * In case Bayes net is incomplete zero columns are added to the end. */ - std::pair matrix() const; + std::pair matrix(boost::optional ordering = boost::none) const; /** * Optimize along the gradient direction, with a closed-form computation to perform the line diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index b037aad92..7ff773bd5 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -142,19 +142,25 @@ namespace gtsam { } /* ************************************************************************* */ - Vector VectorValues::vector() const - { + Vector VectorValues::vector(boost::optional ordering) const { // Count dimensions DenseIndex totalDim = 0; - for(const Vector& v: *this | map_values) - totalDim += v.size(); + for (const Vector& v : *this | map_values) totalDim += v.size(); // Copy vectors Vector result(totalDim); DenseIndex pos = 0; - for(const Vector& v: *this | map_values) { - result.segment(pos, v.size()) = v; - pos += v.size(); + if (ordering) { + for (const auto& key : *ordering) { + const auto& v = (*this)[key]; + result.segment(pos, v.size()) = v; + pos += v.size(); + } + } else { + for (const Vector& v : *this | map_values) { + result.segment(pos, v.size()) = v; + pos += v.size(); + } } return result; diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 39abe1b56..5360edeff 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -244,7 +244,7 @@ namespace gtsam { /// @{ /** Retrieve the entire solution as a single vector */ - Vector vector() const; + Vector vector(boost::optional ordering = boost::none) const; /** Access a vector that is a subset of relevant keys. */ template From ecaf415d1eaf2e647ba417acee262a67fdc34180 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 3 Apr 2019 18:45:16 -0400 Subject: [PATCH 2/5] Better tests on backSubstituteTranspose --- gtsam/linear/tests/testGaussianBayesNet.cpp | 28 +++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 13601844c..3bc5f3371 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -152,6 +152,34 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); + + const auto ordering = noisyBayesNet.ordering(); + const Matrix R = smallBayesNet.matrix(ordering).first; + const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); + EXPECT(assert_equal(expected_vector, actual.vector(ordering))); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, backSubstituteTransposeNoisy ) +{ + // x=R'*y, expected=inv(R')*x + // 2 = 1 2 + // 5 1 1 3 + VectorValues + x = map_list_of + (_x_, Vector1::Constant(2)) + (_y_, Vector1::Constant(5)), + expected = map_list_of + (_x_, Vector1::Constant(4)) + (_y_, Vector1::Constant(9)); + + VectorValues actual = noisyBayesNet.backSubstituteTranspose(x); + EXPECT(assert_equal(expected, actual)); + + const auto ordering = noisyBayesNet.ordering(); + const Matrix R = noisyBayesNet.matrix(ordering).first; + const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); + EXPECT(assert_equal(expected_vector, actual.vector(ordering))); } /* ************************************************************************* */ From c450222ff13404ae29427b9ab122e378e6037447 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 3 Apr 2019 20:16:37 -0400 Subject: [PATCH 3/5] test on ordering --- gtsam/linear/tests/testGaussianBayesNet.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 3bc5f3371..74f07b92e 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -136,6 +136,15 @@ TEST( GaussianBayesNet, optimize3 ) EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(GaussianBayesNet, ordering) +{ + Ordering expected; + expected += 0, 1; + const auto actual = noisyBayesNet.ordering(); + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST( GaussianBayesNet, backSubstituteTranspose ) { From 5a8363a775ef0d3feb668cf16353594f76bbf327 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 3 Apr 2019 20:17:18 -0400 Subject: [PATCH 4/5] Removed Ordering again -> templated vector method simply works --- gtsam/linear/VectorValues.cpp | 16 ++++------------ gtsam/linear/VectorValues.h | 2 +- 2 files changed, 5 insertions(+), 13 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 7ff773bd5..ca95313cf 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -142,7 +142,7 @@ namespace gtsam { } /* ************************************************************************* */ - Vector VectorValues::vector(boost::optional ordering) const { + Vector VectorValues::vector() const { // Count dimensions DenseIndex totalDim = 0; for (const Vector& v : *this | map_values) totalDim += v.size(); @@ -150,17 +150,9 @@ namespace gtsam { // Copy vectors Vector result(totalDim); DenseIndex pos = 0; - if (ordering) { - for (const auto& key : *ordering) { - const auto& v = (*this)[key]; - result.segment(pos, v.size()) = v; - pos += v.size(); - } - } else { - for (const Vector& v : *this | map_values) { - result.segment(pos, v.size()) = v; - pos += v.size(); - } + for (const Vector& v : *this | map_values) { + result.segment(pos, v.size()) = v; + pos += v.size(); } return result; diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 5360edeff..39abe1b56 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -244,7 +244,7 @@ namespace gtsam { /// @{ /** Retrieve the entire solution as a single vector */ - Vector vector(boost::optional ordering = boost::none) const; + Vector vector() const; /** Access a vector that is a subset of relevant keys. */ template From 3126979ad5d214d7eb31c4242c52bd2edafff50a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 3 Apr 2019 22:45:49 -0400 Subject: [PATCH 5/5] Fixed memory issue (passing temporary to optional reference) --- gtsam/linear/GaussianBayesNet.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 22f1918e4..87e0ded15 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -163,7 +163,8 @@ namespace gtsam { return factorGraph.jacobian(ordering); } else { // recursively call with default ordering - return matrix(this->ordering()); + const auto defaultOrdering = this->ordering(); + return matrix(defaultOrdering); } }