From 0710a8a8939fd5fc21445a9df01824095ec8424b Mon Sep 17 00:00:00 2001 From: Kevin Date: Wed, 8 Feb 2023 14:15:32 -0500 Subject: [PATCH 1/9] Add normalization trick to sum-product. --- gtsam/discrete/DiscreteFactorGraph.cpp | 6 ++ .../gtsam/tests/test_DiscreteFactorGraph.py | 65 +++++++++++++++++++ 2 files changed, 71 insertions(+) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 2073164c3..7d07043b2 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -210,6 +210,12 @@ namespace gtsam { for (auto&& factor : factors) product = (*factor) * product; gttoc(product); + // Sum all the potentials by pretending all keys are frontal: + auto normalization = product.sum(product.size()); + + // Normalize the product factor to prevent underflow. + product = product / (*normalization); + // sum out frontals, this is the factor on the separator gttic(sum); DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys); diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py index 42dc807b0..6990a995e 100644 --- a/python/gtsam/tests/test_DiscreteFactorGraph.py +++ b/python/gtsam/tests/test_DiscreteFactorGraph.py @@ -216,5 +216,70 @@ class TestDiscreteFactorGraph(GtsamTestCase): self.assertEqual(vals, [desired_state]*num_obs) + def test_sumProduct_chain(self): + """ + Test for numerical underflow in EliminateDiscrete on long chains. + Adapted from the toy problem of @pcl15423 + Ref: https://github.com/borglab/gtsam/issues/1448 + """ + num_states = 3 + num_obs = 200 + desired_state = 1 + states = list(range(num_states)) + + # Helper function to mimic the behavior of gtbook.Variables discrete_series function + def make_key(character, index, cardinality): + symbol = Symbol(character, index) + key = symbol.key() + return (key, cardinality) + + X = {index: make_key("X", index, len(states)) for index in range(num_obs)} + Z = {index: make_key("Z", index, num_obs + 1) for index in range(num_obs)} + graph = DiscreteFactorGraph() + + # Mostly identity transition matrix + transitions = np.eye(num_states) + + # Needed otherwise mpe is always state 0? + transitions += 0.1/(num_states) + + transition_cpt = [] + for i in range(0, num_states): + transition_row = "/".join([str(x) for x in transitions[i]]) + transition_cpt.append(transition_row) + transition_cpt = " ".join(transition_cpt) + + for i in reversed(range(1, num_obs)): + transition_conditional = DiscreteConditional(X[i], [X[i-1]], transition_cpt) + graph.push_back(transition_conditional) + + # Contrived example such that the desired state gives measurements [0, num_obs) with equal probability + # but all other states always give measurement num_obs + obs = np.zeros((num_states, num_obs+1)) + obs[:,-1] = 1 + obs[desired_state,0: -1] = 1 + obs[desired_state,-1] = 0 + obs_cpt_list = [] + for i in range(0, num_states): + obs_row = "/".join([str(z) for z in obs[i]]) + obs_cpt_list.append(obs_row) + obs_cpt = " ".join(obs_cpt_list) + + # Contrived example where each measurement is its own index + for i in range(0, num_obs): + obs_conditional = DiscreteConditional(Z[i], [X[i]], obs_cpt) + factor = obs_conditional.likelihood(i) + graph.push_back(factor) + + mpe = graph.optimize() + vals = [mpe[X[i][0]] for i in range(num_obs)] + sum_product = graph.sumProduct() + + print("This should have 9 potential assignments", sum_product.at(0)) + + print("This should have 9 potential assignments", sum_product.at(138)) + + self.assertEqual(vals, [desired_state]*num_obs) + if __name__ == "__main__": unittest.main() From 29358f826b0d7f8d74a5d4c916b56c6364771648 Mon Sep 17 00:00:00 2001 From: Kevin Date: Thu, 9 Feb 2023 14:23:52 -0500 Subject: [PATCH 2/9] Patch discrete factor graph test to normalize expected result. --- gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index bb23b7a83..8226a81dd 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -119,6 +119,9 @@ TEST(DiscreteFactorGraph, test) { // Check Factor CHECK(newFactor); DecisionTreeFactor expectedFactor(B & A, "10 6 6 10"); + auto normalization = expectedFactor.sum(expectedFactor.size()); + // Ensure normalization is correct. + expectedFactor = expectedFactor / *normalization; EXPECT(assert_equal(expectedFactor, *newFactor)); // Test using elimination tree From 548509f28b40ca1112e79609cb974c44e5a2420e Mon Sep 17 00:00:00 2001 From: Kevin Date: Fri, 10 Feb 2023 17:02:27 -0500 Subject: [PATCH 3/9] First pass at underflow test for sum-product. --- .../gtsam/tests/test_DiscreteFactorGraph.py | 65 ++++++++++--------- 1 file changed, 33 insertions(+), 32 deletions(-) diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py index 6990a995e..13e84c5b8 100644 --- a/python/gtsam/tests/test_DiscreteFactorGraph.py +++ b/python/gtsam/tests/test_DiscreteFactorGraph.py @@ -223,7 +223,7 @@ class TestDiscreteFactorGraph(GtsamTestCase): Ref: https://github.com/borglab/gtsam/issues/1448 """ num_states = 3 - num_obs = 200 + chain_length = 400 desired_state = 1 states = list(range(num_states)) @@ -233,53 +233,54 @@ class TestDiscreteFactorGraph(GtsamTestCase): key = symbol.key() return (key, cardinality) - X = {index: make_key("X", index, len(states)) for index in range(num_obs)} - Z = {index: make_key("Z", index, num_obs + 1) for index in range(num_obs)} + X = {index: make_key("X", index, len(states)) for index in range(chain_length)} graph = DiscreteFactorGraph() - # Mostly identity transition matrix - transitions = np.eye(num_states) - - # Needed otherwise mpe is always state 0? + # Construct test transition matrix + transitions = np.diag([1.0, 0.5, 0.1]) transitions += 0.1/(num_states) + # Ensure that the transition matrix is Markov (columns sum to 1) + transitions /= np.sum(transitions, axis=0) + + # The stationary distribution is the eigenvector corresponding to eigenvalue 1 + eigvals, eigvecs = np.linalg.eig(transitions) + stationary_idx = np.where(np.isclose(eigvals, 1.0)) + stationary_dist = eigvecs[:, stationary_idx] + + # Ensure that the stationary distribution is positive and normalized + stationary_dist /= np.sum(stationary_dist) + expected = stationary_dist.flatten() + + # The transition matrix parsed by DiscreteConditional is a row-wise CPT + transitions = transitions.T transition_cpt = [] for i in range(0, num_states): transition_row = "/".join([str(x) for x in transitions[i]]) transition_cpt.append(transition_row) transition_cpt = " ".join(transition_cpt) - for i in reversed(range(1, num_obs)): + for i in reversed(range(1, chain_length)): transition_conditional = DiscreteConditional(X[i], [X[i-1]], transition_cpt) graph.push_back(transition_conditional) - # Contrived example such that the desired state gives measurements [0, num_obs) with equal probability - # but all other states always give measurement num_obs - obs = np.zeros((num_states, num_obs+1)) - obs[:,-1] = 1 - obs[desired_state,0: -1] = 1 - obs[desired_state,-1] = 0 - obs_cpt_list = [] - for i in range(0, num_states): - obs_row = "/".join([str(z) for z in obs[i]]) - obs_cpt_list.append(obs_row) - obs_cpt = " ".join(obs_cpt_list) + # Run sum product using natural ordering so the resulting Bayes net has the form: + # X_0 <- X_1 <- ... <- X_n + sum_product = graph.sumProduct(OrderingType.NATURAL) - # Contrived example where each measurement is its own index - for i in range(0, num_obs): - obs_conditional = DiscreteConditional(Z[i], [X[i]], obs_cpt) - factor = obs_conditional.likelihood(i) - graph.push_back(factor) + # Get the DiscreteConditional representing the marginal on the last factor + last_marginal = sum_product.at(chain_length - 1) - mpe = graph.optimize() - vals = [mpe[X[i][0]] for i in range(num_obs)] - sum_product = graph.sumProduct() + # Extract the actual marginal probabilities + assignment = DiscreteValues() + marginal_probs = [] + for i in range(num_states): + assignment[X[chain_length - 1][0]] = i + marginal_probs.append(last_marginal(assignment)) + marginal_probs = np.array(marginal_probs) - print("This should have 9 potential assignments", sum_product.at(0)) - - print("This should have 9 potential assignments", sum_product.at(138)) - - self.assertEqual(vals, [desired_state]*num_obs) + # Ensure marginal probabilities are close to the stationary distribution + self.gtsamAssertEquals(expected, marginal_probs) if __name__ == "__main__": unittest.main() From 70fa5681319330f33a7b0779c7a7d976a33e6d61 Mon Sep 17 00:00:00 2001 From: Kevin Date: Sat, 11 Feb 2023 19:59:16 -0500 Subject: [PATCH 4/9] Normalize products by max in discrete elimination. --- gtsam/discrete/DiscreteFactorGraph.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 7d07043b2..4ededbb8b 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -121,8 +121,8 @@ namespace gtsam { for (auto&& factor : factors) product = (*factor) * product; gttoc(product); - // Sum all the potentials by pretending all keys are frontal: - auto normalization = product.sum(product.size()); + // Max over all the potentials by pretending all keys are frontal: + auto normalization = product.max(product.size()); // Normalize the product factor to prevent underflow. product = product / (*normalization); @@ -210,8 +210,8 @@ namespace gtsam { for (auto&& factor : factors) product = (*factor) * product; gttoc(product); - // Sum all the potentials by pretending all keys are frontal: - auto normalization = product.sum(product.size()); + // Max over all the potentials by pretending all keys are frontal: + auto normalization = product.max(product.size()); // Normalize the product factor to prevent underflow. product = product / (*normalization); From 9fa2d30362bb6cf5a54b5de66268e4ce239251d2 Mon Sep 17 00:00:00 2001 From: Kevin Date: Sat, 11 Feb 2023 20:00:26 -0500 Subject: [PATCH 5/9] Test sum-product in discrete factor graph up to scale. --- gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 8226a81dd..bbce5e8ce 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -108,7 +108,14 @@ TEST(DiscreteFactorGraph, test) { // Test EliminateDiscrete const Ordering frontalKeys{0}; - const auto [conditional, newFactor] = EliminateDiscrete(graph, frontalKeys); + const auto [conditional, newFactorPtr] = EliminateDiscrete(graph, frontalKeys); + + DecisionTreeFactor newFactor = *newFactorPtr; + + // Normalize newFactor by max for comparison with expected + auto normalization = newFactor.max(newFactor.size()); + + newFactor = newFactor / *normalization; // Check Conditional CHECK(conditional); @@ -117,12 +124,13 @@ TEST(DiscreteFactorGraph, test) { EXPECT(assert_equal(expectedConditional, *conditional)); // Check Factor - CHECK(newFactor); + CHECK(&newFactor); DecisionTreeFactor expectedFactor(B & A, "10 6 6 10"); - auto normalization = expectedFactor.sum(expectedFactor.size()); + // Normalize by max. + normalization = expectedFactor.max(expectedFactor.size()); // Ensure normalization is correct. expectedFactor = expectedFactor / *normalization; - EXPECT(assert_equal(expectedFactor, *newFactor)); + EXPECT(assert_equal(expectedFactor, newFactor)); // Test using elimination tree const Ordering ordering{0, 1, 2}; From 92443f537861ec26075b25a08a7b2d2f30117b8c Mon Sep 17 00:00:00 2001 From: Kevin Date: Sat, 11 Feb 2023 20:04:43 -0500 Subject: [PATCH 6/9] Simply sum-product test in Python. --- python/gtsam/tests/test_DiscreteFactorGraph.py | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py index 13e84c5b8..d725ceac8 100644 --- a/python/gtsam/tests/test_DiscreteFactorGraph.py +++ b/python/gtsam/tests/test_DiscreteFactorGraph.py @@ -14,7 +14,7 @@ Author: Frank Dellaert import unittest import numpy as np -from gtsam import DiscreteConditional, DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, Symbol +from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, Symbol from gtsam.utils.test_case import GtsamTestCase OrderingType = Ordering.OrderingType @@ -250,7 +250,7 @@ class TestDiscreteFactorGraph(GtsamTestCase): # Ensure that the stationary distribution is positive and normalized stationary_dist /= np.sum(stationary_dist) - expected = stationary_dist.flatten() + expected = DecisionTreeFactor(X[chain_length-1], stationary_dist.flatten()) # The transition matrix parsed by DiscreteConditional is a row-wise CPT transitions = transitions.T @@ -271,16 +271,8 @@ class TestDiscreteFactorGraph(GtsamTestCase): # Get the DiscreteConditional representing the marginal on the last factor last_marginal = sum_product.at(chain_length - 1) - # Extract the actual marginal probabilities - assignment = DiscreteValues() - marginal_probs = [] - for i in range(num_states): - assignment[X[chain_length - 1][0]] = i - marginal_probs.append(last_marginal(assignment)) - marginal_probs = np.array(marginal_probs) - # Ensure marginal probabilities are close to the stationary distribution - self.gtsamAssertEquals(expected, marginal_probs) + self.gtsamAssertEquals(expected, last_marginal) if __name__ == "__main__": unittest.main() From 9e460d26ebe0c86845a5f902a6ca39b1155f9077 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Feb 2023 22:45:15 -0800 Subject: [PATCH 7/9] Fix TBB compilation issue --- gtsam/linear/VectorValues.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index cea76c7e8..482654471 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -130,7 +130,7 @@ namespace gtsam { GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const VectorValues& v) { // Change print depending on whether we are using TBB #ifdef GTSAM_USE_TBB - map sorted; + std::map sorted; for (const auto& [key,value] : v) { sorted.emplace(key, value); } From 87cfb2563593628b5dd5839cbb8082c023d26108 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Feb 2023 22:45:37 -0800 Subject: [PATCH 8/9] Fix whitespace --- gtsam/geometry/Pose3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d30ea38b5..6c91d7468 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -52,7 +52,7 @@ public: /// @{ /** Default constructor is origin */ - Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} + Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} /** Copy constructor */ Pose3(const Pose3& pose) : From 598582087f5a66e1d5466a72318e2a70e408883a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Feb 2023 22:46:16 -0800 Subject: [PATCH 9/9] Remove more redundant virtual destructors --- gtsam/geometry/PinholePose.h | 7 +------ gtsam/linear/GaussianFactor.h | 29 +++++++++++++++++++------- gtsam/linear/GaussianFactorGraph.h | 3 --- gtsam/nonlinear/NonlinearFactor.h | 3 --- gtsam/nonlinear/NonlinearFactorGraph.h | 3 --- gtsam/symbolic/SymbolicFactor.h | 2 -- 6 files changed, 23 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 92ae0f6fc..5cad3b6e7 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -59,17 +59,12 @@ public: /// @name Advanced Constructors /// @{ - explicit PinholeBaseK(const Vector &v) : - PinholeBase(v) { - } + explicit PinholeBaseK(const Vector& v) : PinholeBase(v) {} /// @} /// @name Standard Interface /// @{ - virtual ~PinholeBaseK() override { - } - /// return calibration virtual const CALIBRATION& calibration() const = 0; diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 3441861e2..c5133c6b2 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -42,6 +42,9 @@ namespace gtsam { typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef Factor Base; ///< Our base class + /// @name Standard Constructors + /// @{ + /** Default constructor creates empty factor */ GaussianFactor() {} @@ -50,19 +53,22 @@ namespace gtsam { template GaussianFactor(const CONTAINER& keys) : Base(keys) {} - /** Destructor */ - virtual ~GaussianFactor() override {} + /// @} + /// @name Testable + /// @{ - // Implementing Testable interface - - /// print + /// print with optional string void print( const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const override = 0; - /** Equals for testable */ + /// assert equality up to a tolerance virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0; + /// @} + /// @name Standard Interface + /// @{ + /** * In Gaussian factors, the error function returns either the negative log-likelihood, e.g., * 0.5*(A*x-b)'*D*(A*x-b) @@ -144,6 +150,10 @@ namespace gtsam { virtual void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const = 0; + /// @} + /// @name Operator interface + /// @{ + /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; @@ -156,12 +166,18 @@ namespace gtsam { /// Gradient wrt a key at any values virtual Vector gradient(Key key, const VectorValues& x) const = 0; + /// @} + /// @name Advanced Interface + /// @{ + // Determine position of a given key template static DenseIndex Slot(const CONTAINER& keys, Key key) { return std::find(keys.begin(), keys.end(), key) - keys.begin(); } + /// @} + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ @@ -171,7 +187,6 @@ namespace gtsam { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } #endif - }; // GaussianFactor /// traits diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 96122eb6b..6a7848f51 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -107,9 +107,6 @@ namespace gtsam { template GaussianFactorGraph(const FactorGraph& graph) : Base(graph) {} - /** Virtual destructor */ - virtual ~GaussianFactorGraph() override {} - /// @} /// @name Testable /// @{ diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index c2e246add..57c4a00eb 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -105,9 +105,6 @@ public: /// @name Standard Interface /// @{ - /** Destructor */ - virtual ~NonlinearFactor() override {} - /** * In nonlinear factors, the error function returns the negative log-likelihood * as a non-linear function of the values in a \class Values object. diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index d4d3ea488..bf56d835f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -78,9 +78,6 @@ namespace gtsam { template NonlinearFactorGraph(const FactorGraph& graph) : Base(graph) {} - /// Destructor - virtual ~NonlinearFactorGraph() override {} - /// @} /// @name Testable /// @{ diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index b4b9b6dab..190609ecb 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -79,8 +79,6 @@ namespace gtsam { /** Create symbolic version of any factor */ explicit SymbolicFactor(const Factor& factor) : Base(factor.keys()) {} - virtual ~SymbolicFactor() override {} - /// Copy this object as its actual derived type. SymbolicFactor::shared_ptr clone() const { return std::make_shared(*this); }