diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 2073164c3..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,6 +210,12 @@ namespace gtsam { for (auto&& factor : factors) product = (*factor) * product; gttoc(product); + // 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); + // sum out frontals, this is the factor on the separator gttic(sum); DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys); diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index bb23b7a83..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,9 +124,13 @@ TEST(DiscreteFactorGraph, test) { EXPECT(assert_equal(expectedConditional, *conditional)); // Check Factor - CHECK(newFactor); + CHECK(&newFactor); DecisionTreeFactor expectedFactor(B & A, "10 6 6 10"); - EXPECT(assert_equal(expectedFactor, *newFactor)); + // Normalize by max. + normalization = expectedFactor.max(expectedFactor.size()); + // Ensure normalization is correct. + expectedFactor = expectedFactor / *normalization; + EXPECT(assert_equal(expectedFactor, newFactor)); // Test using elimination tree const Ordering ordering{0, 1, 2}; 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/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) : 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/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); } 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); } diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py index 42dc807b0..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 @@ -216,5 +216,63 @@ 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 + chain_length = 400 + 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(chain_length)} + graph = DiscreteFactorGraph() + + # 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 = DecisionTreeFactor(X[chain_length-1], 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, chain_length)): + transition_conditional = DiscreteConditional(X[i], [X[i-1]], transition_cpt) + graph.push_back(transition_conditional) + + # 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) + + # Get the DiscreteConditional representing the marginal on the last factor + last_marginal = sum_product.at(chain_length - 1) + + # Ensure marginal probabilities are close to the stationary distribution + self.gtsamAssertEquals(expected, last_marginal) + if __name__ == "__main__": unittest.main()