diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index b4e8fc842c..089d732040 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -197,11 +197,25 @@ jobs: echo "GTSAM_BUILD_UNSTABLE=OFF" >> $GITHUB_ENV echo "GTSAM 'unstable' will not be built." - - name: Set Swap Space + - name: Create swap (Linux only) if: runner.os == 'Linux' - uses: pierotofy/set-swap-space@master - with: - swap-size-gb: 8 + shell: bash + run: | + set -euo pipefail + SWAP=/mnt/swapfile + sudo swapoff $SWAP 2>/dev/null || true + sudo rm -f $SWAP + + for SIZE in 8 4 2 1; do + if sudo fallocate -l ${SIZE}G $SWAP; then + sudo chmod 600 $SWAP + sudo mkswap $SWAP + sudo swapon $SWAP && break + fi + sudo rm -f $SWAP + done + + swapon --show - name: Build & Test run: | diff --git a/gtsam/discrete/DiscreteValues.cpp b/gtsam/discrete/DiscreteValues.cpp index 2bcc15fc4a..0cdc7a3b4e 100644 --- a/gtsam/discrete/DiscreteValues.cpp +++ b/gtsam/discrete/DiscreteValues.cpp @@ -144,12 +144,6 @@ string DiscreteValues::html(const KeyFormatter& keyFormatter, return ss.str(); } -/* ************************************************************************ */ -void PrintDiscreteValues(const DiscreteValues& values, const std::string& s, - const KeyFormatter& keyFormatter) { - values.print(s, keyFormatter); -} - string markdown(const DiscreteValues& values, const KeyFormatter& keyFormatter, const DiscreteValues::Names& names) { return values.markdown(keyFormatter, names); diff --git a/gtsam/discrete/DiscreteValues.h b/gtsam/discrete/DiscreteValues.h index 46d865a0b9..0644e0c16b 100644 --- a/gtsam/discrete/DiscreteValues.h +++ b/gtsam/discrete/DiscreteValues.h @@ -188,11 +188,6 @@ inline std::vector cartesianProduct(const DiscreteKeys& keys) { return DiscreteValues::CartesianProduct(keys); } -/// Free version of print for wrapper -void GTSAM_EXPORT -PrintDiscreteValues(const DiscreteValues& values, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /// Free version of markdown. std::string GTSAM_EXPORT markdown(const DiscreteValues& values, diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 54d00f82ac..24b080681c 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -23,10 +23,6 @@ class DiscreteKeys { std::vector cartesianProduct( const gtsam::DiscreteKeys& keys); -void PrintDiscreteValues( - const gtsam::DiscreteValues& values, const std::string& s = "", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); - string markdown( const gtsam::DiscreteValues& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index e0402969dd..795872d768 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include @@ -290,30 +291,31 @@ TEST(DiscreteBayesTree, Dot) { std::string actual = self.bayesTree->dot(); // print actual: if (debug) std::cout << actual << std::endl; - EXPECT(actual == - "digraph G{\n" - "0[label=\"13, 11, 6, 7\"];\n" - "0->1\n" - "1[label=\"14 : 11, 13\"];\n" - "1->2\n" - "2[label=\"9, 12 : 14\"];\n" - "2->3\n" - "3[label=\"3 : 9, 12\"];\n" - "2->4\n" - "4[label=\"2 : 9, 12\"];\n" - "2->5\n" - "5[label=\"8 : 12, 14\"];\n" - "5->6\n" - "6[label=\"1 : 8, 12\"];\n" - "5->7\n" - "7[label=\"0 : 8, 12\"];\n" - "1->8\n" - "8[label=\"10 : 13, 14\"];\n" - "8->9\n" - "9[label=\"5 : 10, 13\"];\n" - "8->10\n" - "10[label=\"4 : 10, 13\"];\n" - "}"); + std::string expected = + R"(digraph G{ +13[label="13, 11, 6, 7"]; +13->14 +14[label="14 : 11, 13"]; +14->9 +9[label="9, 12 : 14"]; +9->3 +3[label="3 : 9, 12"]; +9->2 +2[label="2 : 9, 12"]; +9->8 +8[label="8 : 12, 14"]; +8->1 +1[label="1 : 8, 12"]; +8->0 +0[label="0 : 8, 12"]; +14->10 +10[label="10 : 13, 14"]; +10->5 +5[label="5 : 10, 13"]; +10->4 +4[label="4 : 10, 13"]; +})"; + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 7fffb06d3b..d398b8b04c 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -112,13 +112,13 @@ bool HybridConditional::equals(const HybridFactor &other, double tol) const { } /* ************************************************************************ */ -double HybridConditional::error(const HybridValues &values) const { +double HybridConditional::error(const HybridValues &hybridValues) const { if (auto gc = asGaussian()) { - return gc->error(values.continuous()); + return gc->error(hybridValues.continuous()); } else if (auto gm = asHybrid()) { - return gm->error(values); + return gm->error(hybridValues); } else if (auto dc = asDiscrete()) { - return dc->error(values.discrete()); + return dc->error(hybridValues.discrete()); } else throw std::runtime_error( "HybridConditional::error: conditional type not handled"); @@ -126,11 +126,11 @@ double HybridConditional::error(const HybridValues &values) const { /* ************************************************************************ */ AlgebraicDecisionTree HybridConditional::errorTree( - const VectorValues &values) const { + const VectorValues &continuousValues) const { if (auto gc = asGaussian()) { - return {gc->error(values)}; // NOTE: a "constant" tree + return {gc->error(continuousValues)}; // NOTE: a "constant" tree } else if (auto gm = asHybrid()) { - return gm->errorTree(values); + return gm->errorTree(continuousValues); } else if (auto dc = asDiscrete()) { return dc->errorTree(); } else diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 45b00969b2..d3aa5ade43 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -182,7 +182,7 @@ class GTSAM_EXPORT HybridConditional std::shared_ptr inner() const { return inner_; } /// Return the error of the underlying conditional. - double error(const HybridValues& values) const override; + double error(const HybridValues& hybridValues) const override; /** * @brief Compute error of the HybridConditional as a tree. @@ -192,7 +192,7 @@ class GTSAM_EXPORT HybridConditional * as the conditionals involved, and leaf values as the error. */ AlgebraicDecisionTree errorTree( - const VectorValues& values) const override; + const VectorValues& continuousValues) const override; /// Return the log-probability (or density) of the underlying conditional. double logProbability(const HybridValues& values) const override; diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 543e09c6f0..ee7c620cee 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -43,21 +43,22 @@ class GTSAM_EXPORT HybridEliminationTree /// @{ /** - * Build the elimination tree of a factor graph using pre-computed column + * Construct the elimination tree of a factor graph using pre-computed column * structure. * @param factorGraph The factor graph for which to build the elimination tree * @param structure The set of factors involving each variable. If this is * not precomputed, you can call the Create(const FactorGraph&) * named constructor instead. - * @return The elimination tree + * @param order The ordering of the variables. */ HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order); - /** Build the elimination tree of a factor graph. Note that this has to + /** Construct the elimination tree of a factor graph. Note that this has to * compute the column structure as a VariableIndex, so if you already have * this precomputed, use the other constructor instead. * @param factorGraph The factor graph for which to build the elimination tree + * @param order The ordering of the variables. */ HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const Ordering& order); diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 4147420bd1..7611024991 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -135,7 +135,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { /// Compute tree of linear errors. virtual AlgebraicDecisionTree errorTree( - const VectorValues &values) const = 0; + const VectorValues &continuousValues) const = 0; /// Restrict the factor to the given discrete values. virtual std::shared_ptr restrict( @@ -162,4 +162,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { template <> struct traits : public Testable {}; +// For wrapper: +using AlgebraicDecisionTreeKey = AlgebraicDecisionTree; + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 616e015f63..8c7c38cd9d 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -193,16 +194,44 @@ AlgebraicDecisionTree HybridGaussianFactor::errorTree( } /* *******************************************************************************/ -double HybridGaussianFactor::error(const HybridValues& values) const { +double HybridGaussianFactor::error(const HybridValues& hybridValues) const { // Directly index to get the component, no need to build the whole tree. - const GaussianFactorValuePair pair = factors_(values.discrete()); - return PotentiallyPrunedComponentError(pair, values.continuous()); + const GaussianFactorValuePair pair = factors_(hybridValues.discrete()); + return PotentiallyPrunedComponentError(pair, hybridValues.continuous()); } /* ************************************************************************ */ std::shared_ptr HybridGaussianFactor::restrict( - const DiscreteValues& assignment) const { - throw std::runtime_error("HybridGaussianFactor::restrict not implemented"); + const DiscreteValues& assignment) const { + FactorValuePairs restrictedTree = this->factors_; // Start with the original tree + + const DiscreteKeys& currentFactorDiscreteKeys = this->discreteKeys(); + DiscreteKeys newFactorDiscreteKeys; // For the new, restricted factor + + // Iterate over the discrete keys of the current factor + for (const DiscreteKey& factor_dk_pair : currentFactorDiscreteKeys) { + const Key& key = factor_dk_pair.first; + + // Check if this key is specified in the assignment + auto assignment_it = assignment.find(key); + + if (assignment_it != assignment.end()) { + // Key is in assignment: restrict the tree by choosing the branch + size_t assigned_value = assignment_it->second; + restrictedTree = restrictedTree.choose(key, assigned_value); + // This key is now fixed, so it's not a discrete key for the new factor + } + else { + // Key is not in assignment: it remains a discrete key for the new factor + newFactorDiscreteKeys.push_back(factor_dk_pair); + } + } + + // Create and return the new HybridGaussianFactor. + // Its constructor will derive continuous keys from the GaussianFactor + // shared_ptrs within the restrictedTree. + return std::make_shared(newFactorDiscreteKeys, + restrictedTree); } /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 0bf38effa4..8bde1d64c1 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -144,7 +144,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * @brief Compute the log-likelihood, including the log-normalizing constant. * @return double */ - double error(const HybridValues &values) const override; + double error(const HybridValues &hybridValues) const override; /// Getter for GaussianFactor decision tree const FactorValuePairs &factors() const { return factors_; } diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index 17b4649fc3..cb7984a4cb 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -57,15 +57,7 @@ class GTSAM_EXPORT HybridJunctionTree typedef HybridJunctionTree This; ///< This class typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class - /** - * Build the elimination tree of a factor graph using precomputed column - * structure. - * @param factorGraph The factor graph for which to build the elimination tree - * @param structure The set of factors involving each variable. If this is - * not precomputed, you can call the Create(const FactorGraph&) - * named constructor instead. - * @return The elimination tree - */ + /// Construct the junction tree from an elimination tree HybridJunctionTree(const HybridEliminationTree& eliminationTree); }; diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 9b07730730..583c74b25d 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -117,8 +117,8 @@ double HybridNonlinearFactor::error( } /* *******************************************************************************/ -double HybridNonlinearFactor::error(const HybridValues& values) const { - return error(values.nonlinear(), values.discrete()); +double HybridNonlinearFactor::error(const HybridValues& hybridValues) const { + return error(hybridValues.nonlinear(), hybridValues.discrete()); } /* *******************************************************************************/ @@ -138,6 +138,7 @@ void HybridNonlinearFactor::print(const std::string& s, auto [factor, val] = v; if (factor) { RedirectCout rd; + std::cout << "(" << val << ") "; factor->print("", keyFormatter); return rd.str(); } else { diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 9fe08a3644..08a5df6bcf 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -32,7 +32,7 @@ namespace gtsam { /// Alias for a NoiseModelFactor shared pointer and double scalar pair. using NonlinearFactorValuePair = - std::pair; + std::pair; /** * @brief Implementation of a discrete-conditioned hybrid factor. @@ -72,14 +72,16 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { /// Decision tree of nonlinear factors indexed by discrete keys. FactorValuePairs factors_; - /// HybridFactor method implementation. Should not be used. + /// HybridFactor method implementation. Should not be used for this class. + /// This version is for linear/Gaussian continuous values. + /// Use the overload taking gtsam::Values for nonlinear values. AlgebraicDecisionTree errorTree( - const VectorValues& continuousValues) const override { + const VectorValues& continuousValues) const override { throw std::runtime_error( - "HybridNonlinearFactor::error does not take VectorValues."); + "HybridNonlinearFactor::errorTree: use errorTree(gtsam::Values) for nonlinear values."); } - public: +public: /// @name Constructors /// @{ @@ -89,14 +91,14 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { /** * @brief Construct a new HybridNonlinearFactor on a single discrete key, * providing the factors for each mode m as a vector of factors ϕ_m(x). - * The value ϕ(x,m) for the factor is simply ϕ_m(x). + * The value ϕ(x,m) for the factor is simply ϕ_m(x) (i.e. scalar part is 0.0). * * @param discreteKey The discrete key for the "mode", indexing components. - * @param factors Vector of gaussian factors, one for each mode. + * @param factors Vector of nonlinear factors, one for each mode. */ HybridNonlinearFactor( - const DiscreteKey& discreteKey, - const std::vector& factors); + const DiscreteKey& discreteKey, + const std::vector& factors); /** * @brief Construct a new HybridNonlinearFactor on a single discrete key, @@ -105,7 +107,7 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { * The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m. * * @param discreteKey The discrete key for the "mode", indexing components. - * @param pairs Vector of gaussian factor-scalar pairs, one per mode. + * @param pairs Vector of nonlinear factor-scalar pairs, one per mode. */ HybridNonlinearFactor(const DiscreteKey& discreteKey, const std::vector& pairs); @@ -122,22 +124,25 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { HybridNonlinearFactor(const DiscreteKeys& discreteKeys, const FactorValuePairs& factors); + /// @} + /// @name Standard Interface + /// @{ + /** * @brief Compute error of the HybridNonlinearFactor as a tree. * - * @param continuousValues The continuous values for which to compute the - * error. - * @return AlgebraicDecisionTree A decision tree with the same keys - * as the factor, and leaf values as the error. + * @param continuousValues The continuous gtsam::Values for which to compute the error. + * @return AlgebraicDecisionTree A decision tree with the same discrete + * keys as the factor, and leaf values as the error for each continuous component. */ AlgebraicDecisionTree errorTree(const Values& continuousValues) const; /** * @brief Compute error of factor given both continuous and discrete values. * - * @param continuousValues The continuous Values. - * @param discreteValues The discrete Values. - * @return double The error of this factor. + * @param continuousValues The continuous gtsam::Values. + * @param assignment The discrete Values (assignment for discrete keys). + * @return double The error of this factor for the given assignment. */ double error(const Values& continuousValues, const DiscreteValues& assignment) const; @@ -145,54 +150,59 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { /** * @brief Compute error of factor given hybrid values. * - * @param values The continuous Values and the discrete assignment. + * @param values The HybridValues containing continuous (gtsam::Values) and + * discrete assignments. * @return double The error of this factor. */ - double error(const HybridValues& values) const override; + double error(const HybridValues& hybridValues) const override; /** * @brief Get the dimension of the factor (number of rows on linearization). - * Returns the dimension of the first component factor. + * Returns the dimension of the first component factor found in the tree. + * Assumes all component factors have the same dimension. * @return size_t */ size_t dim() const; - /// @} - /// @name Testable - /// @{ - - /// print to stdout - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override; - - /// Check equality - bool equals(const HybridFactor& other, double tol = 1e-9) const override; - - /// @} - /// @name Standard API - /// @{ - /// Getter for NonlinearFactor decision tree const FactorValuePairs& factors() const { return factors_; } /// Linearize specific nonlinear factors based on the assignment in /// discreteValues. GaussianFactor::shared_ptr linearize(const Values& continuousValues, - const DiscreteValues& assignment) const; + const DiscreteValues& assignment) const; /// Linearize all the continuous factors to get a HybridGaussianFactor. std::shared_ptr linearize( - const Values& continuousValues) const; + const Values& continuousValues) const; /// Prune this factor based on the discrete probabilities. + /// @param discreteProbs A DecisionTreeFactor representing P(M) or P(M|...). + /// Entries with probability 0 (or very small) in discreteProbs will lead to + /// pruning of corresponding branches in this factor. HybridNonlinearFactor::shared_ptr prune( const DecisionTreeFactor& discreteProbs) const; /// Restrict the factor to the given discrete values. + /// If all discrete keys in this factor are assigned, the result will be + /// a NonlinearFactor (wrapped in a Factor::shared_ptr). + /// Otherwise, it will be a new HybridNonlinearFactor over the remaining + /// unassigned discrete keys. std::shared_ptr restrict( const DiscreteValues& assignment) const override; /// @} + /// @name Testable + /// @{ + + /// print to stdout + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// Check equality + bool equals(const HybridFactor& other, double tol = 1e-9) const override; + + /// @} private: /// Helper struct to assist private constructor below. @@ -200,6 +210,18 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { // Private constructor using ConstructorHelper above. HybridNonlinearFactor(const ConstructorHelper& helper); + +#if GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + // Serialize base class + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + // Serialize derived class members + ar& BOOST_SERIALIZATION_NVP(factors_); + } +#endif }; // traits diff --git a/gtsam/hybrid/HybridNonlinearISAM.h b/gtsam/hybrid/HybridNonlinearISAM.h index 686a083276..c9bc499ef5 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.h +++ b/gtsam/hybrid/HybridNonlinearISAM.h @@ -114,10 +114,6 @@ class GTSAM_EXPORT HybridNonlinearISAM { void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /// @} - /// @name Advanced Interface - /// @{ - /** Add new factors along with their initial linearization points */ void update(const HybridNonlinearFactorGraph& newFactors, const Values& initialValues, diff --git a/gtsam/hybrid/doc/HybridBayesNet.ipynb b/gtsam/hybrid/doc/HybridBayesNet.ipynb new file mode 100644 index 0000000000..c9e0ad7999 --- /dev/null +++ b/gtsam/hybrid/doc/HybridBayesNet.ipynb @@ -0,0 +1,696 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridBayesNet" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `HybridBayesNet` represents a directed graphical model (Bayes Net) specifically designed for hybrid systems. It is a collection of `gtsam.HybridConditional` objects, ordered according to an elimination sequence.\n", + "\n", + "It extends `gtsam.BayesNet` and allows representing the joint probability distribution $P(X, M)$ over continuous variables $X$ and discrete variables $M$ as a product of conditional probabilities:\n", + "$$\n", + "P(X, M) = \\prod_i P(\\text{Frontal}_i | \\text{Parents}_i)\n", + "$$\n", + "where each conditional $P(\\text{Frontal}_i | \\text{Parents}_i)$ is stored as a `HybridConditional`. This structure allows for representing complex dependencies, such as continuous variables conditioned on discrete modes ($P(X|M)$) alongside purely discrete ($P(M)$) or purely continuous ($P(X)$) relationships.\n", + "\n", + "`HybridBayesNet` objects are typically obtained by eliminating a `HybridGaussianFactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "import graphviz\n", + "\n", + "from gtsam import (\n", + " HybridConditional,\n", + " GaussianConditional,\n", + " DiscreteConditional,\n", + " HybridGaussianConditional,\n", + " HybridGaussianFactorGraph,\n", + " HybridGaussianFactor,\n", + " JacobianFactor,\n", + " DecisionTreeFactor,\n", + " Ordering,\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating a HybridBayesNet\n", + "\n", + "While they can be constructed manually by adding `HybridConditional`s, they are more commonly obtained via elimination of a `HybridGaussianFactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Manually Constructed HybridBayesNet:\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "var7205759403792793600->var8646911284551352320\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352320->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# --- Method 1: Manual Construction ---\n", + "hbn_manual = gtsam.HybridBayesNet()\n", + "\n", + "# P(D0)\n", + "dk0 = (D(0), 2)\n", + "cond_d0 = DiscreteConditional(dk0, [], \"7/3\") # P(D0=0)=0.7\n", + "hbn_manual.push_back(HybridConditional(cond_d0))\n", + "\n", + "# P(X0 | D0)\n", + "dk0_parent = (D(0), 2)\n", + " # Mode 0: P(X0 | D0=0) = N(0, 1)\n", + "gc0 = GaussianConditional(X(0), np.zeros(1), np.eye(1), gtsam.noiseModel.Unit.Create(1))\n", + " # Mode 1: P(X0 | D0=1) = N(5, 4)\n", + "gc1 = GaussianConditional(X(0), np.array([2.5]), np.eye(1)*0.5, gtsam.noiseModel.Isotropic.Sigma(1,2.0))\n", + "cond_x0_d0 = HybridGaussianConditional(dk0_parent, [gc0, gc1])\n", + "hbn_manual.push_back(HybridConditional(cond_x0_d0))\n", + "\n", + "# P(X1 | X0)\n", + "cond_x1_x0 = GaussianConditional(X(1), np.array([0.0]), np.eye(1), # d, R=I\n", + " X(0), np.eye(1), # Parent X0, S=I\n", + " gtsam.noiseModel.Isotropic.Sigma(1, 1.0)) # N(X1; X0, I)\n", + "hbn_manual.push_back(HybridConditional(cond_x1_x0))\n", + "\n", + "print(\"Manually Constructed HybridBayesNet:\")\n", + "# hbn_manual.print()\n", + "graphviz.Source(hbn_manual.dot())" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "f11254b8", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Original HybridGaussianFactorGraph for Elimination:\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor2\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# --- Method 2: From Elimination ---\n", + "hgfg = HybridGaussianFactorGraph()\n", + "# P(D0) = 70/30\n", + "hgfg.add(DecisionTreeFactor([dk0], \"0.7 0.3\"))\n", + "# P(X0|D0) = mixture N(0,1); N(5,4)\n", + "# Factor version: 0.5*|X0-0|^2/1 + C0 ; 0.5*|X0-5|^2/4 + C1\n", + "factor_gf0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n", + "factor_gf1 = JacobianFactor(X(0), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 2.0))\n", + "# Store -log(prior) for D0 in the hybrid factor (optional, could keep separate)\n", + "logP_D0_0 = -np.log(0.7)\n", + "logP_D0_1 = -np.log(0.3)\n", + "hgfg.add(HybridGaussianFactor(dk0, [(factor_gf0, logP_D0_0), (factor_gf1, logP_D0_1)]))\n", + "# P(X1|X0) = N(X0, 1)\n", + "hgfg.add(JacobianFactor(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0)))\n", + "\n", + "print(\"\\nOriginal HybridGaussianFactorGraph for Elimination:\")\n", + "# hgfg.print()\n", + "graphviz.Source(hgfg.dot())" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "07db8de1", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "HybridBayesNet from Elimination:\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "var7205759403792793600->var8646911284551352320\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352320->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Note: Using HybridOrdering(hgfg) is generally recommended: \n", + "# it returns a Colamd constrained ordering where the discrete keys are\n", + "# eliminated after the continuous keys.\n", + "ordering = gtsam.HybridOrdering(hgfg)\n", + "\n", + "hbn_elim, _ = hgfg.eliminatePartialSequential(ordering)\n", + "print(\"\\nHybridBayesNet from Elimination:\")\n", + "# hbn_elim.print()\n", + "graphviz.Source(hbn_elim.dot())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Operations on HybridBayesNet\n", + "\n", + "`HybridBayesNet` allows evaluating the joint probability, sampling, optimizing (finding the MAP state), and extracting marginal or conditional distributions." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "LogProbability P(X0=0.1, X1=0.2, D0=0): -2.160749387689685\n", + "Probability P(X0=0.1, X1=0.2, D0=0): 0.11523873018620859\n", + "\n", + "Sampled HybridValues:\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x0: 6.29382\n", + " x1: 6.6918\n", + " Discrete: (d0, 1)\n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "MAP Solution (Optimize):\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x0: 0\n", + " x1: 0\n", + " Discrete: (d0, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "MPE Discrete Assignment:\n", + "DiscreteValues{7205759403792793600: 0}\n" + ] + } + ], + "source": [ + "# Use the Bayes Net from elimination for consistency\n", + "hbn = hbn_elim\n", + "\n", + "# --- Evaluation ---\n", + "values = gtsam.HybridValues()\n", + "values.insert(D(0), 0)\n", + "values.insert(X(0), np.array([0.1]))\n", + "values.insert(X(1), np.array([0.2]))\n", + "\n", + "log_prob = hbn.logProbability(values)\n", + "prob = hbn.evaluate(values) # Same as exp(log_prob)\n", + "print(f\"\\nLogProbability P(X0=0.1, X1=0.2, D0=0): {log_prob}\")\n", + "print(f\"Probability P(X0=0.1, X1=0.2, D0=0): {prob}\")\n", + "\n", + "# --- Sampling ---\n", + "full_sample = hbn.sample()\n", + "print(\"\\nSampled HybridValues:\")\n", + "full_sample.print()\n", + "\n", + "# --- Optimization (Finding MAP state) ---\n", + "# Computes MPE for discrete, then optimizes continuous given MPE\n", + "map_solution = hbn.optimize()\n", + "print(\"\\nMAP Solution (Optimize):\")\n", + "map_solution.print()\n", + "\n", + "# --- MPE (Most Probable Explanation for Discrete Variables) ---\n", + "mpe_assignment = hbn.mpe()\n", + "print(\"\\nMPE Discrete Assignment:\")\n", + "print(mpe_assignment) # Should match discrete part of map_solution" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "d8e3e0ee", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Optimized Continuous Solution for D0=1:\n", + "VectorValues: 2 elements\n", + " x0: 5\n", + " x1: 5\n" + ] + } + ], + "source": [ + "# --- Optimize Continuous given specific Discrete Assignment ---\n", + "dv = gtsam.DiscreteValues()\n", + "dv[D(0)] = 1\n", + "cont_solution_d0_eq_1 = hbn.optimize(dv)\n", + "print(\"\\nOptimized Continuous Solution for D0=1:\")\n", + "cont_solution_d0_eq_1.print()\n" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "758c1790", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Discrete Marginal P(M):\n", + "DiscreteBayesNet\n", + " \n", + "size: 1\n", + "conditional 0: P( d0 ):\n", + " f[ (d0,2), ]\n", + "(d0, 0) | 0.731343 | 0\n", + "(d0, 1) | 0.268657 | 1\n", + "number of nnzs: 2\n", + "\n", + "\n", + "Gaussian Conditional P(X | D0=0):\n", + "\n", + "size: 2\n", + "conditional 0: p(x1 | x0)\n", + " R = [ 1 ]\n", + " S[x0] = [ -1 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.918939\n", + " No noise model\n", + "conditional 1: p(x0)\n", + " R = [ 1 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x0: 0\n", + " logNormalizationConstant: -0.918939\n", + " No noise model\n" + ] + } + ], + "source": [ + "# --- Extract Marginal/Conditional Distributions ---\n", + "# Get P(M) = P(D0)\n", + "discrete_marginal_bn = hbn.discreteMarginal()\n", + "print(\"\\nDiscrete Marginal P(M):\")\n", + "discrete_marginal_bn.print()\n", + "\n", + "# Get P(X | M=m) = P(X0, X1 | D0=0)\n", + "dv[D(0)] = 0\n", + "gaussian_conditional_bn = hbn.choose(dv)\n", + "print(\"\\nGaussian Conditional P(X | D0=0):\")\n", + "gaussian_conditional_bn.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Advanced Operations (`errorTree`, `discretePosterior`, `prune`)" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Error Tree (Unnormalized Log Posterior Log P'(M|x)) for x0=0.5, x1=1.0:\n", + "AlgebraicDecisionTreeKey\n", + " Choice(d0) \n", + "AlgebraicDecisionTreeKey\n", + " 0 Leaf 0.56287232\n", + "AlgebraicDecisionTreeKey\n", + " 1 Leaf 4.663718\n", + "\n", + "Discrete Posterior Tree P(M|x) for x0=0.5, x1=1.0:\n", + "AlgebraicDecisionTreeKey\n", + " Choice(d0) \n", + "AlgebraicDecisionTreeKey\n", + " 0 Leaf 0.98371106\n", + "AlgebraicDecisionTreeKey\n", + " 1 Leaf 0.016288942\n" + ] + } + ], + "source": [ + "# --- Error Tree (Log P'(M|x) = log P(x|M) + log P(M)) ---\n", + "# Evaluate unnormalized log posterior of discrete modes given continuous values\n", + "cont_values_for_error = gtsam.VectorValues()\n", + "cont_values_for_error.insert(X(0), np.array([0.5]))\n", + "cont_values_for_error.insert(X(1), np.array([1.0]))\n", + "\n", + "error_tree = hbn.errorTree(cont_values_for_error)\n", + "print(\"\\nError Tree (Unnormalized Log Posterior Log P'(M|x)) for x0=0.5, x1=1.0:\")\n", + "error_tree.print()\n", + "\n", + "# --- Discrete Posterior P(M|x) ---\n", + "# Normalized version of exp(-errorTree)\n", + "posterior_tree = hbn.discretePosterior(cont_values_for_error)\n", + "print(\"\\nDiscrete Posterior Tree P(M|x) for x0=0.5, x1=1.0:\")\n", + "posterior_tree.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "9bec1c66", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Pruned HybridBayesNet (max_leaves=1):\n", + "HybridBayesNet\n", + " \n", + "size: 2\n", + "conditional 0: p(x1 | x0)\n", + " R = [ 1 ]\n", + " S[x0] = [ -1 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.918939\n", + " No noise model\n", + "conditional 1: p(x0)\n", + " R = [ 1 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x0: 0\n", + " logNormalizationConstant: -0.918939\n", + " No noise model\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352320->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# --- Pruning ---\n", + "# Reduces complexity by removing low-probability discrete branches\n", + "max_leaves = 1 # Force pruning to the most likely mode\n", + "pruned_hbn = hbn.prune(max_leaves, marginalThreshold=0.8)\n", + "\n", + "print(f\"\\nPruned HybridBayesNet (max_leaves={max_leaves}):\")\n", + "pruned_hbn.print()\n", + "# Visualize the pruned Bayes Net\n", + "graphviz.Source(pruned_hbn.dot())" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridBayesTree.ipynb b/gtsam/hybrid/doc/HybridBayesTree.ipynb new file mode 100644 index 0000000000..921ebddbd8 --- /dev/null +++ b/gtsam/hybrid/doc/HybridBayesTree.ipynb @@ -0,0 +1,449 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridBayesTree" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `HybridBayesTree` is the hybrid equivalent of a `gtsam.GaussianBayesTree`. It represents the result of **multifrontal** variable elimination on a `HybridGaussianFactorGraph`.\n", + "\n", + "Like a standard Bayes tree, it's a tree structure where each node is a 'clique'. However, in a `HybridBayesTree`, each clique contains a `gtsam.HybridConditional` representing $P(F_k | S_k)$, where $F_k$ are the frontal variables eliminated in that clique, and $S_k$ are the separator variables shared with the parent clique.\n", + "\n", + "In **hybrid** Bayes trees discrete variables can only occur as frontal variables if the separator is entirely discrete, i.e., we will never have the situation that a discrete variable is conditioned on a continuous variable.\n", + "\n", + "The joint probability distribution $P(X, M)$ encoded by the tree is the product of all clique conditionals:\n", + "$$\n", + "P(X, M) = \\prod_k P(F_k : S_k)\n", + "$$\n", + "Compared to a `HybridBayesNet` (from sequential elimination), a `HybridBayesTree` is (a) tree-structured, (b) often has a shallower structure. Both are advantageous for inference tasks like marginalization and incremental updates, especially in sparse problems common in robotics (SLAM)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import (\n", + " HybridGaussianFactorGraph, \n", + " JacobianFactor, DecisionTreeFactor, HybridGaussianFactor,\n", + " DiscreteValues\n", + ")\n", + "from gtsam.symbol_shorthand import X, D\n", + "\n", + "import graphviz" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating a HybridBayesTree (via Elimination)\n", + "\n", + "`HybridBayesTree` objects are obtained by performing **multifrontal** elimination on a `HybridGaussianFactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original HybridGaussianFactorGraph:\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor3\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Elimination Ordering: Position 0: x0, x1, d0\n", + "\n", + "\n", + "Resulting HybridBayesTree:\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "8646911284551352321\n", + "\n", + "x1 : d0\n", + "\n", + "\n", + "\n", + "7205759403792793600->8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "8646911284551352320\n", + "\n", + "x0 : x1\n", + "\n", + "\n", + "\n", + "8646911284551352321->8646911284551352320\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# --- Create a HybridGaussianFactorGraph ---\n", + "hgfg = HybridGaussianFactorGraph()\n", + "dk0 = (D(0), 2) # Binary discrete variable\n", + "\n", + "# Prior on D0: P(D0=0)=0.6, P(D0=1)=0.4\n", + "prior_d0 = DecisionTreeFactor([dk0], \"0.6 0.4\")\n", + "hgfg.add(prior_d0)\n", + "\n", + "# Prior on X0: P(X0) = N(0, 1)\n", + "prior_x0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n", + "hgfg.add(prior_x0)\n", + "\n", + "# Conditional measurement on X1: P(X1 | D0)\n", + "# Mode 0: P(X1 | D0=0) = N(1, 0.25)\n", + "gf0 = JacobianFactor(X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, 0.5))\n", + "# Mode 1: P(X1 | D0=1) = N(5, 1.0)\n", + "gf1 = JacobianFactor(X(1), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n", + "meas_x1_d0 = HybridGaussianFactor(dk0, [gf0, gf1])\n", + "hgfg.add(meas_x1_d0)\n", + "\n", + "# Factor connecting X0 and X1: P(X1 | X0) = N(X0+1, 0.1)\n", + "odom_x0_x1 = JacobianFactor(X(0), -np.eye(1), X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, np.sqrt(0.1)))\n", + "hgfg.add(odom_x0_x1)\n", + "\n", + "print(\"Original HybridGaussianFactorGraph:\")\n", + "# hgfg.print()\n", + "display(graphviz.Source(hgfg.dot()))\n", + "\n", + "# --- Elimination ---\n", + "# Use default hybrid ordering (continuous first, then discrete)\n", + "ordering = gtsam.HybridOrdering(hgfg)\n", + "print(f\"\\nElimination Ordering: {ordering}\")\n", + "\n", + "# Perform multifrontal elimination\n", + "hybrid_bayes_tree = hgfg.eliminateMultifrontal(ordering)\n", + "\n", + "print(\"\\nResulting HybridBayesTree:\")\n", + "# hybrid_bayes_tree.print()\n", + "display(graphviz.Source(hybrid_bayes_tree.dot()))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Operations on HybridBayesTree\n", + "\n", + "Similar to `HybridBayesNet`, the tree can be used for optimization, evaluation, and extracting specific conditional distributions." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "MAP Solution (Optimize):\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x0: 0\n", + " x1: 1\n", + " Discrete: (d0, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "MPE Discrete Assignment:\n", + "DiscreteValues{7205759403792793600: 0}\n" + ] + } + ], + "source": [ + "hbt = hybrid_bayes_tree # Use the tree from elimination\n", + "\n", + "# --- Optimization (Finding MAP state) ---\n", + "# Computes MPE for discrete, then optimizes continuous given MPE\n", + "map_solution = hbt.optimize()\n", + "print(\"\\nMAP Solution (Optimize):\")\n", + "map_solution.print()\n", + "\n", + "# --- MPE (Most Probable Explanation for Discrete Variables) ---\n", + "mpe_assignment = hbt.mpe()\n", + "print(\"\\nMPE Discrete Assignment:\")\n", + "print(mpe_assignment)" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "389541c5", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Optimized Continuous Solution for D0=0:\n", + "VectorValues: 2 elements\n", + " x0: 0\n", + " x1: 1\n", + "\n", + "Optimized Continuous Solution for D0=1:\n", + "VectorValues: 2 elements\n", + " x0: 1.90476\n", + " x1: 3.09524\n", + "\n", + "Total Error at MAP solution: 0.023412453920796855\n" + ] + } + ], + "source": [ + "# --- Optimize Continuous given specific Discrete Assignment ---\n", + "dv0 = DiscreteValues()\n", + "dv0[D(0)] = 0\n", + "cont_solution_d0_eq_0 = hbt.optimize(dv0)\n", + "print(\"\\nOptimized Continuous Solution for D0=0:\")\n", + "cont_solution_d0_eq_0.print()\n", + "\n", + "dv1 = DiscreteValues()\n", + "dv1[D(0)] = 1\n", + "cont_solution_d0_eq_1 = hbt.optimize(dv1)\n", + "print(\"\\nOptimized Continuous Solution for D0=1:\")\n", + "cont_solution_d0_eq_1.print()\n", + "\n", + "# --- Evaluation ---\n", + "# Evaluate error (sum of errors of clique conditionals)\n", + "total_error = hbt.error(map_solution)\n", + "print(f\"\\nTotal Error at MAP solution: {total_error}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "c1bd1d19", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "GaussianBayesTree for D0=0:\n", + ": cliques: 3, variables: 2\n", + "- p()\n", + " R = Empty (0x0)\n", + " d = Empty (0x1)\n", + " mean: 0 elements\n", + " logNormalizationConstant: -0\n", + " No noise model\n", + "| - p(x1)\n", + " R = [ 2.21565 ]\n", + " d = [ 2.21565 ]\n", + " mean: 1 elements\n", + " x1: 1\n", + " logNormalizationConstant: -0.123394\n", + " No noise model\n", + "| | - p(x0 | x1)\n", + " R = [ 3.31662 ]\n", + " S[x1] = [ -3.01511 ]\n", + " d = [ -3.01511 ]\n", + " logNormalizationConstant: 0.280009\n", + " No noise model\n" + ] + } + ], + "source": [ + "# --- Extract Conditional Distributions ---\n", + "# Choose a specific GaussianBayesTree for a discrete assignment\n", + "gaussian_bayes_tree_d0_eq_0 = hbt.choose(dv0)\n", + "print(\"\\nGaussianBayesTree for D0=0:\")\n", + "gaussian_bayes_tree_d0_eq_0.print()\n", + "# display(graphviz.Source(gaussian_bayes_tree_d0_eq_0.dot()))" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridConditional.ipynb b/gtsam/hybrid/doc/HybridConditional.ipynb new file mode 100644 index 0000000000..8e03cdc47a --- /dev/null +++ b/gtsam/hybrid/doc/HybridConditional.ipynb @@ -0,0 +1,403 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridConditional" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridConditional` acts as a **type-erased wrapper** for different kinds of conditional distributions that can appear in a `HybridBayesNet` or `HybridBayesTree`. It allows these containers to hold conditionals resulting from eliminating different types of variables (discrete, continuous, or mixtures) without needing to be templated on the specific conditional type.\n", + "\n", + "A `HybridConditional` object internally holds a shared pointer to one of the following concrete conditional types:\n", + "* `gtsam.GaussianConditional`\n", + "* `gtsam.DiscreteConditional`\n", + "* `gtsam.HybridGaussianConditional`\n", + "\n", + "It inherits from `HybridFactor` and `Conditional`, providing access to both factor-like properties (keys) and conditional-like properties (frontals, parents).\n", + "\n", + "```mermaid\n", + "graph TD\n", + " HybridConditional --> HybridFactor\n", + " HybridConditional --> Conditional\n", + " HybridConditional -- Holds shared pointer to --> GaussianConditional\n", + " HybridConditional -- Holds shared pointer to --> DiscreteConditional\n", + " HybridConditional -- Holds shared pointer to --> HybridGaussianConditional\n", + "```" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import (\n", + " GaussianConditional,\n", + " DiscreteConditional,\n", + " HybridConditional,\n", + " HybridGaussianConditional,\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization\n", + "\n", + "A `HybridConditional` is created by wrapping a shared pointer to one of the concrete conditional types. These concrete conditionals are usually obtained from factor graph elimination." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "HybridConditional from GaussianConditional:\n", + "Hybrid Conditional\n", + "p(x0 | x1)\n", + " R = [ 2 ]\n", + " S[x1] = [ 0.5 ]\n", + " d = [ 1 ]\n", + " logNormalizationConstant: 0.467356\n", + "isotropic dim=1 sigma=0.5\n", + "\n", + "HybridConditional from DiscreteConditional:\n", + "Hybrid Conditional\n", + " P( d0 | d1 ):\n", + " Choice(d1) \n", + " 0 Choice(d0) \n", + " 0 0 Leaf 0.8\n", + " 0 1 Leaf 0.2\n", + " 1 Choice(d0) \n", + " 1 0 Leaf 0.2\n", + " 1 1 Leaf 0.8\n", + "\n", + "\n", + "HybridConditional from HybridGaussianConditional:\n", + "Hybrid Conditional\n", + " P( x2 | d2)\n", + " Discrete Keys = (d2, 2), \n", + " logNormalizationConstant: 0.467356\n", + "\n", + " Choice(d2) \n", + " 0 Leaf p(x2)\n", + " R = [ 1 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x2: 0\n", + " logNormalizationConstant: -0.918939\n", + " Noise model: unit (1) \n", + "\n", + " 1 Leaf p(x2)\n", + " R = [ 2 ]\n", + " d = [ 10 ]\n", + " mean: 1 elements\n", + " x2: 5\n", + " logNormalizationConstant: 0.467356\n", + "isotropic dim=1 sigma=0.5\n", + "\n" + ] + } + ], + "source": [ + "# --- Create concrete conditionals (examples) ---\n", + "# 1. GaussianConditional P(X0 | X1)\n", + "gc = GaussianConditional(X(0), np.array([1.0]), np.eye(1)*2.0, # d, R\n", + " X(1), np.array([[0.5]]), # Parent, S\n", + " gtsam.noiseModel.Diagonal.Sigmas([0.5])) # sigma=0.5 -> prec=4 -> R=2\n", + "\n", + "# 2. DiscreteConditional P(D0 | D1) (D0, D1 binary)\n", + "dk0 = (D(0), 2)\n", + "dk1 = (D(1), 2)\n", + "dc = DiscreteConditional(dk0, [dk1], \"4/1 1/4\") # P(D0|D1=0) = 80/20, P(D0|D1=1) = 20/80\n", + "\n", + "# 3. HybridGaussianConditional P(X2 | D2) (X2 1D, D2 binary)\n", + "dk2 = (D(2), 2)\n", + "# Mode 0: P(X2 | D2=0) = N(0, 1) -> R=1, d=0\n", + "hgc_gc0 = GaussianConditional(X(2), np.zeros(1), np.eye(1), gtsam.noiseModel.Unit.Create(1))\n", + "# Mode 1: P(X2 | D2=1) = N(5, 0.25) -> R=2, d=10\n", + "hgc_gc1 = GaussianConditional(X(2), np.array([10.0]), np.eye(1)*2.0, gtsam.noiseModel.Isotropic.Sigma(1,0.5))\n", + "# This constructor takes vector of conditionals directly if parents match\n", + "hgc = HybridGaussianConditional(dk2, [hgc_gc0, hgc_gc1])\n", + "\n", + "# --- Wrap them into HybridConditionals ---\n", + "hybrid_cond_g = HybridConditional(gc)\n", + "hybrid_cond_d = HybridConditional(dc)\n", + "hybrid_cond_h = HybridConditional(hgc)\n", + "\n", + "print(\"HybridConditional from GaussianConditional:\")\n", + "hybrid_cond_g.print()\n", + "print(\"\\nHybridConditional from DiscreteConditional:\")\n", + "hybrid_cond_d.print()\n", + "print(\"\\nHybridConditional from HybridGaussianConditional:\")\n", + "hybrid_cond_h.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Accessing Information and Inner Type\n", + "\n", + "You can access keys, frontals, and parents like any conditional. You can also check the underlying type and attempt to cast back to the concrete type." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "--- Inspecting HybridConditional from Gaussian ---\n", + "Keys: [8646911284551352320, 8646911284551352321]\n", + "Frontals: 1\n", + "Parents: 1\n", + "Is Continuous? True\n", + "Is Discrete? False\n", + "Is Hybrid? False\n", + "Successfully cast back to GaussianConditional:\n", + "GaussianConditional p(x0 | x1)\n", + " R = [ 2 ]\n", + " S[x1] = [ 0.5 ]\n", + " d = [ 1 ]\n", + " logNormalizationConstant: 0.467356\n", + "isotropic dim=1 sigma=0.5\n", + "Cast back to DiscreteConditional successful? False\n", + "\n", + "--- Inspecting HybridConditional from Hybrid ---\n", + "Keys: [8646911284551352322, 7205759403792793602]\n", + "Frontals: 1\n", + "Parents: 1\n", + "Continuous Keys: [8646911284551352322]\n", + "Discrete Keys: \n", + "d2 2\n", + "\n", + "Is Continuous? False\n", + "Is Discrete? False\n", + "Is Hybrid? True\n", + "Successfully cast back to HybridGaussianConditional.\n" + ] + } + ], + "source": [ + "print(\"\\n--- Inspecting HybridConditional from Gaussian ---\")\n", + "print(f\"Keys: {hybrid_cond_g.keys()}\")\n", + "print(f\"Frontals: {hybrid_cond_g.nrFrontals()}\")\n", + "print(f\"Parents: {hybrid_cond_g.nrParents()}\")\n", + "print(f\"Is Continuous? {hybrid_cond_g.isContinuous()}\") # True\n", + "print(f\"Is Discrete? {hybrid_cond_g.isDiscrete()}\") # False\n", + "print(f\"Is Hybrid? {hybrid_cond_g.isHybrid()}\") # False\n", + "\n", + "# Try casting back\n", + "inner_gaussian = hybrid_cond_g.asGaussian()\n", + "if inner_gaussian:\n", + " print(\"Successfully cast back to GaussianConditional:\")\n", + " inner_gaussian.print()\n", + "else:\n", + " print(\"Failed to cast back to GaussianConditional.\")\n", + "\n", + "inner_discrete = hybrid_cond_g.asDiscrete()\n", + "print(f\"Cast back to DiscreteConditional successful? {inner_discrete is not None}\")\n", + "\n", + "print(\"\\n--- Inspecting HybridConditional from Hybrid ---\")\n", + "print(f\"Keys: {hybrid_cond_h.keys()}\")\n", + "print(f\"Frontals: {hybrid_cond_h.nrFrontals()}\")\n", + "print(f\"Parents: {hybrid_cond_h.nrParents()}\") # Contains continuous AND discrete parents\n", + "print(f\"Continuous Keys: {hybrid_cond_h.continuousKeys()}\")\n", + "print(f\"Discrete Keys: {hybrid_cond_h.discreteKeys()}\")\n", + "print(f\"Is Continuous? {hybrid_cond_h.isContinuous()}\") # False\n", + "print(f\"Is Discrete? {hybrid_cond_h.isDiscrete()}\") # False\n", + "print(f\"Is Hybrid? {hybrid_cond_h.isHybrid()}\") # True\n", + "\n", + "# Try casting back\n", + "inner_hybrid = hybrid_cond_h.asHybrid()\n", + "if inner_hybrid:\n", + " print(\"Successfully cast back to HybridGaussianConditional.\")\n", + "else:\n", + " print(\"Failed to cast back to HybridGaussianConditional.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Evaluation (`error`, `logProbability`, `evaluate`)\n", + "\n", + "These methods delegate to the underlying concrete conditional's implementation. They require a `HybridValues` object containing assignments for all involved variables (frontal and parents)." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Gaussian HybridConditional P(X0=2|X1=1):\n", + " Error: 24.5\n", + " LogProbability: -24.032644172084783\n", + " Probability: 3.6538881633458336e-11\n", + "\n", + "Discrete HybridConditional P(D0=1|D1=0):\n", + " Error: 1.6094379124341003\n", + " LogProbability: -1.6094379124341003\n", + " Probability: 0.2\n", + "\n", + "Hybrid Gaussian HybridConditional P(X2=4.5|D2=1):\n", + " Error: 2.0\n", + " LogProbability: -1.5326441720847823\n", + " Probability: 0.21596386605275217\n" + ] + } + ], + "source": [ + "# --- Evaluate the Gaussian Conditional P(X0 | X1) ---\n", + "vals_g = gtsam.HybridValues()\n", + "vals_g.insert(X(0), np.array([2.0])) # Frontal\n", + "vals_g.insert(X(1), np.array([1.0])) # Parent\n", + "\n", + "err_g = hybrid_cond_g.error(vals_g)\n", + "log_prob_g = hybrid_cond_g.logProbability(vals_g)\n", + "prob_g = hybrid_cond_g.evaluate(vals_g) # Equivalent to exp(logProbability)\n", + "\n", + "print(f\"\\nGaussian HybridConditional P(X0=2|X1=1):\")\n", + "print(f\" Error: {err_g}\")\n", + "print(f\" LogProbability: {log_prob_g}\")\n", + "print(f\" Probability: {prob_g}\")\n", + "\n", + "# --- Evaluate the Discrete Conditional P(D0 | D1) ---\n", + "vals_d = gtsam.HybridValues()\n", + "vals_d.insert(D(0), 1) # Frontal = 1\n", + "vals_d.insert(D(1), 0) # Parent = 0\n", + "\n", + "err_d = hybrid_cond_d.error(vals_d) # -log(P(D0=1|D1=0)) = -log(0.2)\n", + "log_prob_d = hybrid_cond_d.logProbability(vals_d) # log(0.2)\n", + "prob_d = hybrid_cond_d.evaluate(vals_d) # 0.2\n", + "\n", + "print(f\"\\nDiscrete HybridConditional P(D0=1|D1=0):\")\n", + "print(f\" Error: {err_d}\")\n", + "print(f\" LogProbability: {log_prob_d}\")\n", + "print(f\" Probability: {prob_d}\")\n", + "\n", + "# --- Evaluate the Hybrid Gaussian Conditional P(X2 | D2) ---\n", + "vals_h = gtsam.HybridValues()\n", + "vals_h.insert(X(2), np.array([4.5])) # Frontal\n", + "vals_h.insert(D(2), 1) # Parent (selects mode 1: N(5, 0.25))\n", + "\n", + "err_h = hybrid_cond_h.error(vals_h)\n", + "log_prob_h = hybrid_cond_h.logProbability(vals_h)\n", + "prob_h = hybrid_cond_h.evaluate(vals_h)\n", + "\n", + "print(f\"\\nHybrid Gaussian HybridConditional P(X2=4.5|D2=1):\")\n", + "print(f\" Error: {err_h}\")\n", + "print(f\" LogProbability: {log_prob_h}\")\n", + "print(f\" Probability: {prob_h}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Restriction (`restrict`)\n", + "\n", + "The `restrict` method allows fixing the discrete parent variables, potentially simplifying the conditional (e.g., a `HybridGaussianConditional` might become a `GaussianConditional`)." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Restricted HybridConditional (D2=1):p(x2)\n", + " R = [ 2 ]\n", + " d = [ 10 ]\n", + " mean: 1 elements\n", + " x2: 5\n", + " logNormalizationConstant: 0.467356\n", + "isotropic dim=1 sigma=0.5\n" + ] + } + ], + "source": [ + "# Restrict the HybridGaussianConditional P(X2 | D2)\n", + "assignment = gtsam.DiscreteValues()\n", + "assignment[D(2)] = 1 # Fix D2 to mode 1\n", + "\n", + "restricted_factor = hybrid_cond_h.restrict(assignment)\n", + "\n", + "restricted_factor.print(\"\\nRestricted HybridConditional (D2=1):\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridEliminationTree.ipynb b/gtsam/hybrid/doc/HybridEliminationTree.ipynb new file mode 100644 index 0000000000..728a4713ca --- /dev/null +++ b/gtsam/hybrid/doc/HybridEliminationTree.ipynb @@ -0,0 +1,316 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridEliminationTree" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `HybridEliminationTree` is the hybrid equivalent of `gtsam.GaussianEliminationTree`. It represents the structure of **sequential** variable elimination performed on a `HybridGaussianFactorGraph` given a specific `Ordering`.\n", + "\n", + "Each node in the tree corresponds to a variable being eliminated. The factors associated with that variable (from the original graph) are stored at that node. The tree structure reflects the dependencies created during sequential elimination: the parent of a node `j` is the variable `i` appearing earliest in the ordering such that the conditional $P(j | ...)$ resulting from eliminating `j` involves `i`.\n", + "\n", + "Eliminating a `HybridEliminationTree` yields a `HybridBayesNet`.\n", + "\n", + "While fundamental to sequential elimination, direct manipulation of `HybridEliminationTree` objects is less common than using the `eliminateSequential` method on a `HybridGaussianFactorGraph`, which uses this structure internally. It primarily serves to understand the computational flow of sequential elimination in the hybrid context." + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "import graphviz\n", + "\n", + "from gtsam import (\n", + " HybridGaussianFactorGraph,\n", + " Ordering,\n", + " HybridEliminationTree,\n", + " JacobianFactor,\n", + " DecisionTreeFactor,\n", + " HybridGaussianFactor,\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating a HybridEliminationTree\n", + "\n", + "It is constructed from a `HybridGaussianFactorGraph` and an `Ordering`." + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original HybridGaussianFactorGraph:\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor3\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 15, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# --- Create a HybridGaussianFactorGraph (same as HybridBayesTree example) ---\n", + "hgfg = HybridGaussianFactorGraph()\n", + "dk0 = (D(0), 2) # Binary discrete variable\n", + "\n", + "# Prior on D0: P(D0=0)=0.6, P(D0=1)=0.4\n", + "prior_d0 = DecisionTreeFactor([dk0], \"0.6 0.4\")\n", + "hgfg.add(prior_d0) # Factor 0\n", + "\n", + "# Prior on X0: P(X0) = N(0, 1)\n", + "prior_x0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n", + "hgfg.add(prior_x0) # Factor 1\n", + "\n", + "# Conditional measurement on X1: P(X1 | D0)\n", + "# Mode 0: P(X1 | D0=0) = N(1, 0.25)\n", + "gf0 = JacobianFactor(X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, 0.5))\n", + "# Mode 1: P(X1 | D0=1) = N(5, 1.0)\n", + "gf1 = JacobianFactor(X(1), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n", + "meas_x1_d0 = HybridGaussianFactor(dk0, [gf0, gf1])\n", + "hgfg.add(meas_x1_d0) # Factor 2\n", + "\n", + "# Factor connecting X0 and X1: P(X1 | X0) = N(X0+1, 0.1)\n", + "odom_x0_x1 = JacobianFactor(X(0), -np.eye(1), X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, np.sqrt(0.1)))\n", + "hgfg.add(odom_x0_x1) # Factor 3\n", + "\n", + "print(\"Original HybridGaussianFactorGraph:\")\n", + "# hgfg.print()\n", + "graphviz.Source(hgfg.dot())" + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "id": "26adbd08", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Elimination Ordering: Position 0: x0, x1, d0\n", + "\n", + "\n", + "Resulting HybridEliminationTree:\n", + "-(d0)\n", + "- f[ (d0,2), ]\n", + " Choice(d0) \n", + " 0 Leaf 0.6\n", + " 1 Leaf 0.4\n", + "| -(x1)\n", + "| -\n", + "Hybrid [x1; d0]{\n", + " Choice(d0) \n", + " 0 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + "isotropic dim=1 sigma=0.5\n", + "scalar: 0\n", + "\n", + " 1 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 5 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + "}\n", + "| | -(x0)\n", + "| | -\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "| | -\n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + "isotropic dim=1 sigma=0.316228\n" + ] + } + ], + "source": [ + "\n", + "# --- Define an Ordering ---\n", + "# Eliminate continuous first, then discrete (matches default HybridOrdering)\n", + "ordering = Ordering([X(0), X(1), D(0)])\n", + "print(f\"\\nElimination Ordering: {ordering}\")\n", + "\n", + "# --- Construct the Elimination Tree ---\n", + "het = HybridEliminationTree(hgfg, ordering)\n", + "print(\"\\nResulting HybridEliminationTree:\")\n", + "# Printing shows the tree structure and factors stored at each node\n", + "het.print()" + ] + }, + { + "cell_type": "markdown", + "id": "85db36bd", + "metadata": {}, + "source": [ + "The primary use for elimination trees is in sequential elimination, yielding a `HybridBayesNet`. This is typically done via the `eliminateSequential` method of the factor graph itself." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridFactor.ipynb b/gtsam/hybrid/doc/HybridFactor.ipynb new file mode 100644 index 0000000000..c38fd294b2 --- /dev/null +++ b/gtsam/hybrid/doc/HybridFactor.ipynb @@ -0,0 +1,249 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridFactor` is the abstract base class for all factors involved in hybrid inference problems within GTSAM. It extends the standard `gtsam.Factor` interface to accommodate factors that may involve both discrete and continuous variables.\n", + "\n", + "Key features:\n", + "* Manages separate sets of continuous (`gtsam.Key`) and discrete (`gtsam.DiscreteKey`) variable keys.\n", + "* Provides methods to check the type of factor (discrete-only, continuous-only, or truly hybrid).\n", + "* Defines virtual methods like `errorTree` and `restrict` that derived classes must implement based on their specific nature.\n", + "\n", + "* [`gtsam.HybridGaussianFactor`](HybridGaussianFactor.ipynb)\n", + "* [`gtsam.HybridGaussianConditional`](HybridGaussianConditional.ipynb) (inherits from HybridGaussianFactor)\n", + "* [`gtsam.HybridNonlinearFactor`](HybridNonlinearFactor.ipynb)\n", + "* [`gtsam.HybridConditional`](HybridConditional.ipynb) (wrapper class)" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import HybridGaussianFactor, JacobianFactor, DecisionTreeFactor\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Basic Interface\n", + "\n", + "All hybrid factors provide methods to access their continuous and discrete keys and check their category." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Is Discrete? False\n", + "Is Continuous? False\n", + "Is Hybrid? True\n", + "Number of Continuous Keys: 1\n", + "Continuous Keys: [8646911284551352320]\n", + "Discrete Keys: \n", + "d0 2\n", + "\n", + "All Keys (Factor base): [8646911284551352320, 7205759403792793600]\n", + "\n", + "Hybrid Factor Example (HybridGaussianFactor):\n", + "Hybrid [x0; d0]{\n", + " Choice(d0) \n", + " 0 Leaf :\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + " 1 Leaf :\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + "}\n" + ] + } + ], + "source": [ + "# Example using a derived class (HybridGaussianFactor)\n", + "dk = (D(0), 2) # Discrete key D0 with cardinality 2\n", + "gf0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Unit.Create(1))\n", + "gf1 = JacobianFactor(X(0), np.eye(1), np.ones(1), gtsam.noiseModel.Unit.Create(1))\n", + "hybrid_factor = HybridGaussianFactor(dk, [gf0, gf1])\n", + "\n", + "# Access methods inherited from HybridFactor\n", + "print(f\"Is Discrete? {hybrid_factor.isDiscrete()}\")\n", + "print(f\"Is Continuous? {hybrid_factor.isContinuous()}\")\n", + "print(f\"Is Hybrid? {hybrid_factor.isHybrid()}\")\n", + "print(f\"Number of Continuous Keys: {hybrid_factor.nrContinuous()}\")\n", + "print(f\"Continuous Keys: {hybrid_factor.continuousKeys()}\")\n", + "print(f\"Discrete Keys: {hybrid_factor.discreteKeys()}\") # List of (key, card) pairs\n", + "print(f\"All Keys (Factor base): {hybrid_factor.keys()}\") # Combined keys\n", + "\n", + "hybrid_factor.print(\"\\nHybrid Factor Example (HybridGaussianFactor):\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Virtual Methods (`errorTree`, `restrict`)\n", + "\n", + "Derived classes implement methods to handle the interaction between discrete and continuous parts.\n", + "\n", + "* `errorTree(VectorValues)`: Calculates the error for the continuous part, returning an `AlgebraicDecisionTree` keyed by the discrete variables.\n", + "* `restrict(DiscreteValues)`: Creates a new factor (typically continuous-only) by fixing the discrete variables to a specific assignment." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Error Tree (AlgebraicDecisionTree):\n", + "ErrorTree\n", + " Leaf 0.125\n" + ] + } + ], + "source": [ + "# --- errorTree ---\n", + "# Needs continuous values\n", + "continuous_values = gtsam.VectorValues()\n", + "continuous_values.insert(X(0), np.array([0.5]))\n", + "continuous_values.insert(X(1), np.array([1.5]))\n", + "\n", + "adt = hybrid_factor.errorTree(continuous_values)\n", + "print(\"\\nError Tree (AlgebraicDecisionTree):\")\n", + "adt.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "f21e8197", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Restricted Factor (Assignment D0=0):\n", + "Continuous [x0]{\n", + " Leaf :\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + "}\n", + "\n", + "Restricted Factor (Assignment D0=1):\n", + "Continuous [x0]{\n", + " Leaf :\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + "}\n" + ] + } + ], + "source": [ + "# --- restrict ---\n", + "# Needs a discrete assignment\n", + "assignment = gtsam.DiscreteValues()\n", + "assignment[D(0)] = 0 # Choose the first Gaussian factor component\n", + "\n", + "restricted_factor = hybrid_factor.restrict(assignment)\n", + "# The result is typically a shared_ptr[Factor], often a GaussianFactor\n", + "restricted_factor.print(\"\\nRestricted Factor (Assignment D0=0):\")\n", + "\n", + "# Try restricting with the other assignment\n", + "assignment[D(0)] = 1\n", + "restricted_factor_1 = hybrid_factor.restrict(assignment)\n", + "restricted_factor_1.print(\"\\nRestricted Factor (Assignment D0=1):\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridFactorGraph.ipynb b/gtsam/hybrid/doc/HybridFactorGraph.ipynb new file mode 100644 index 0000000000..5e3bf7375f --- /dev/null +++ b/gtsam/hybrid/doc/HybridFactorGraph.ipynb @@ -0,0 +1,422 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridFactorGraph" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridFactorGraph` is a `gtsam.FactorGraph` specifically designed to hold `gtsam.Factor` types that can represent hybrid discrete-continuous systems. While it can store any factor type (like its base class), it provides convenience methods for working with hybrid systems.\n", + "\n", + "It serves as a base class for more specialized hybrid factor graphs:\n", + "* `gtsam.HybridGaussianFactorGraph`: Holds factors that are either Gaussian, discrete, or hybrid Gaussian (linear conditional Gaussians dependent on discrete modes). Result of linearizing a `HybridNonlinearFactorGraph`.\n", + "* `gtsam.HybridNonlinearFactorGraph`: Holds factors that can be nonlinear, discrete, or hybrid nonlinear (nonlinear functions dependent on discrete modes).\n", + "\n", + "This notebook focuses on the base `HybridFactorGraph` interface. See the derived classes for specific elimination and usage examples." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "import graphviz\n", + "\n", + "from gtsam import (\n", + " PriorFactorPose2,\n", + " BetweenFactorPose2,\n", + " Pose2,\n", + " DecisionTreeFactor,\n", + " HybridFactorGraph,\n", + " HybridGaussianFactor,\n", + " JacobianFactor,\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization and Adding Factors" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "HybridFactorGraph size: 4\n", + "\n", + "Graph Contents: \n", + "size: 4\n", + "factor 0: PriorFactor on x0\n", + " prior mean: (0, 0, 0)\n", + " noise model: unit (3) \n", + "factor 1: f[ (d0,2), (d1,2), ]\n", + " Choice(d1) \n", + " 0 Choice(d0) \n", + " 0 0 Leaf 0.1\n", + " 0 1 Leaf 0.2\n", + " 1 Choice(d0) \n", + " 1 0 Leaf 0.9\n", + " 1 1 Leaf 0.8\n", + "factor 2: \n", + "Hybrid [x5; d2]{\n", + " Choice(d2) \n", + " 0 Leaf :\n", + " A[x5] = [\n", + "\t2\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + " 1 Leaf :\n", + " A[x5] = [\n", + "\t0.5\n", + "]\n", + " b = [ 1 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + "}\n", + "factor 3: \n", + " A[x6] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n" + ] + } + ], + "source": [ + "# Create an empty HybridFactorGraph\n", + "hfg = HybridFactorGraph()\n", + "\n", + "# --- Create diverse factors ---\n", + "# Nonlinear (can be added, but usage depends on derived graph type)\n", + "prior_nl = PriorFactorPose2(X(0), Pose2(0,0,0), gtsam.noiseModel.Unit.Create(3))\n", + "between_nl = BetweenFactorPose2(X(0), X(1), Pose2(1,0,0), gtsam.noiseModel.Unit.Create(3))\n", + "\n", + "# Discrete\n", + "dk1 = (D(0), 2)\n", + "dk2 = (D(1), 2)\n", + "discrete_f = DecisionTreeFactor([dk1, dk2], \"0.1 0.9 0.2 0.8\")\n", + "\n", + "# Hybrid Gaussian\n", + "dk_h = (D(2), 2) # Hybrid mode key\n", + "gf0 = JacobianFactor(X(5), np.eye(1)*2, np.zeros(1), gtsam.noiseModel.Unit.Create(1))\n", + "gf1 = JacobianFactor(X(5), np.eye(1)*0.5, np.ones(1), gtsam.noiseModel.Unit.Create(1))\n", + "hybrid_gf = HybridGaussianFactor(dk_h, [gf0, gf1])\n", + "\n", + "# Add factors to the graph\n", + "hfg.push_back(prior_nl) # Index 0\n", + "hfg.push_back(discrete_f) # Index 1\n", + "hfg.push_back(hybrid_gf) # Index 2\n", + "# Can also add standard Gaussian factors if needed\n", + "gaussian_f = JacobianFactor(X(6), np.eye(1), np.zeros(1), gtsam.noiseModel.Unit.Create(1))\n", + "hfg.add(gaussian_f) # Index 3\n", + "\n", + "print(f\"HybridFactorGraph size: {hfg.size()}\")\n", + "hfg.print(\"\\nGraph Contents:\")" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "fceac58c", + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793601\n", + "\n", + "d1\n", + "\n", + "\n", + "\n", + "var7205759403792793601--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793602\n", + "\n", + "d2\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793602--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352325\n", + "\n", + "x5\n", + "\n", + "\n", + "\n", + "var8646911284551352325--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326\n", + "\n", + "x6\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326--factor3\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# We can visualize the graph using Graphviz\n", + "graphviz.Source(hfg.dot())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Inspecting Keys\n", + "\n", + "`HybridFactorGraph` provides methods to easily retrieve sets of discrete and continuous keys present in the graph." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Discrete KeySet: d0d1d2\n", + "Continuous KeySet: x0x5x6\n", + "All Keys (including nonlinear): d0d1d2x0x5x6\n" + ] + } + ], + "source": [ + "discrete_keyset = hfg.discreteKeySet() # Returns gtsam.KeySet\n", + "print(f\"Discrete KeySet: {discrete_keyset}\")\n", + "\n", + "continuous_keyset = hfg.continuousKeySet() # Returns gtsam.KeySet\n", + "print(f\"Continuous KeySet: {continuous_keyset}\")\n", + "\n", + "all_keys = hfg.keys() # Inherited from FactorGraph base class\n", + "print(f\"All Keys (including nonlinear): {all_keys}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Error Calculation\n", + "\n", + "The `error` method sums the errors of all contained factors, assuming appropriate `Values` (or `HybridValues`) are provided. The exact interpretation depends on the factor types (nonlinear error, Gaussian negative log-likelihood, etc.)." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Evaluating error with HybridValues:\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x5: 0.1\n", + " x6: 0.2\n", + " Discrete: (d0, 0)(d1, 1)(d2, 0)\n", + " Nonlinear\n", + "Values with 2 values:\n", + "Value x0: (gtsam::Pose2)\n", + "(0.1, 0, 0)\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(1.1, 0, 0)\n", + "\n", + "\n", + "Total graph error: 0.15036051565782632\n" + ] + } + ], + "source": [ + "# Requires HybridValues to evaluate all factor types\n", + "hybrid_values = gtsam.HybridValues()\n", + "\n", + "# Add Nonlinear values\n", + "nl_values = gtsam.Values()\n", + "nl_values.insert(X(0), Pose2(0.1, 0, 0)) # Small error for prior\n", + "nl_values.insert(X(1), Pose2(1.1, 0, 0)) # Matches between factor\n", + "hybrid_values.insert(nl_values)\n", + "\n", + "# Add Discrete values\n", + "disc_values = gtsam.DiscreteValues()\n", + "disc_values[D(0)] = 0\n", + "disc_values[D(1)] = 1\n", + "disc_values[D(2)] = 0 # Selects gf0 in the hybrid factor\n", + "hybrid_values.insert(disc_values)\n", + "\n", + "# Add Continuous Vector values\n", + "vec_values = gtsam.VectorValues()\n", + "vec_values.insert(X(5), np.array([0.1])) # Value for hybrid_gf (component 0)\n", + "vec_values.insert(X(6), np.array([0.2])) # Value for gaussian_f\n", + "hybrid_values.insert(vec_values)\n", + "\n", + "print(\"\\nEvaluating error with HybridValues:\")\n", + "hybrid_values.print()\n", + "\n", + "try:\n", + " total_error = hfg.error(hybrid_values)\n", + " print(f\"\\nTotal graph error: {total_error}\")\n", + " # Note: Error interpretation mixes nonlinear, discrete (-log(prob)), Gaussian\n", + "except Exception as e:\n", + " print(f\"\\nError calculating graph error: {e}\")\n", + " print(\"Ensure HybridValues contains assignments for all keys in all factors.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Further Operations\n", + "\n", + "Operations like elimination (`eliminateSequential`, `eliminateMultifrontal`) are typically performed on the more specific derived classes ([`HybridGaussianFactorGraph`](HybridGaussianFactorGraph.ipynb), [`HybridNonlinearFactorGraph`](HybridNonlinearFactorGraph.ipynb)). See their documentation for details." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridGaussianConditional.ipynb b/gtsam/hybrid/doc/HybridGaussianConditional.ipynb new file mode 100644 index 0000000000..aa75b1df50 --- /dev/null +++ b/gtsam/hybrid/doc/HybridGaussianConditional.ipynb @@ -0,0 +1,277 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridGaussianConditional" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `HybridGaussianConditional` represents a conditional probability distribution $P(X | M, Z)$ where the frontal variables $X$ are continuous, and the parent variables include both discrete variables $M$ and potentially other continuous variables $Z$. It inherits from `HybridGaussianFactor` and `Conditional`.\n", + "\n", + "Essentially, it's a collection of `GaussianConditional`s, where the choice of which `GaussianConditional` applies depends on the assignment $m$ of the discrete parent variables $M$.\n", + "$$\n", + "P(X | M=m, Z) = P_m(X | Z)\n", + "$$\n", + "where $P_m(X | Z)$ is a standard `GaussianConditional`.\n", + "\n", + "Internally, it uses a `DecisionTree` to store the different conditional components, indexed by the discrete parent `DiscreteKey`s $M$. Note that unlike `HybridGaussianFactor`, the scalar value stored alongside the factor in the base class represents the *negative log normalization constant* for that branch, ensuring each $P_m(X|Z)$ integrates to 1.\n", + "\n", + "These conditionals are the standard result of eliminating continuous variables when discrete variables are present in the separator during hybrid elimination." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import HybridGaussianConditional, GaussianConditional\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Can be initialized with one or more discrete parent keys and the corresponding `GaussianConditional` components." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "HybridGaussianConditional P(X0 | D0):\n", + "HybridGaussianConditional\n", + "\n", + " P( x0 | d0)\n", + " Discrete Keys = (d0, 2), \n", + " logNormalizationConstant: -0.918939\n", + "\n", + " Choice(d0) \n", + " 0 Leaf p(x0)\n", + " R = [ 1 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x0: 0\n", + " logNormalizationConstant: -0.918939\n", + " Noise model: unit (1) \n", + "\n", + " 1 Leaf p(x0)\n", + " R = [ 0.5 ]\n", + " d = [ 2.5 ]\n", + " mean: 1 elements\n", + " x0: 5\n", + " logNormalizationConstant: -2.30523\n", + "isotropic dim=1 sigma=2\n", + "\n" + ] + } + ], + "source": [ + "# --- Example: Single Discrete Parent P(X0 | D0) ---\n", + "dk0 = (D(0), 2) # Binary discrete parent D0\n", + "\n", + "# Gaussian conditional for mode D0=0: P(X0|D0=0) = N(0, 1)\n", + "# R=1, d=0\n", + "gc0 = GaussianConditional(X(0), np.zeros(1), np.eye(1), gtsam.noiseModel.Unit.Create(1))\n", + "\n", + "# Gaussian conditional for mode D0=1: P(X0|D0=1) = N(5, 4=2^2)\n", + "# R=1/2=0.5, d = R*mean = 0.5*5 = 2.5\n", + "# Need noise model with sigma=2.0\n", + "noise1 = gtsam.noiseModel.Isotropic.Sigma(1, 2.0)\n", + "gc1 = GaussianConditional(X(0), np.array([2.5]), np.eye(1)*0.5, noise1)\n", + "\n", + "hgc1 = HybridGaussianConditional(dk0, [gc0, gc1])\n", + "print(\"HybridGaussianConditional P(X0 | D0):\")\n", + "hgc1.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Accessing Components and Properties\n", + "\n", + "You can retrieve the underlying decision tree of conditionals and select a specific `GaussianConditional` based on a discrete assignment." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Selected GaussianConditional for D0=0:\n", + "GaussianConditional p(x0)\n", + " R = [ 1 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x0: 0\n", + " logNormalizationConstant: -0.918939\n", + " Noise model: unit (1) \n", + "\n", + "Selected GaussianConditional for D0=1\n", + ": (GaussianConditional p(x0)\n", + " R = [ 0.5 ]\n", + " d = [ 2.5 ]\n", + " mean: 1 elements\n", + " x0: 5\n", + " logNormalizationConstant: -2.30523\n", + "isotropic dim=1 sigma=2\n", + ", 1.3862943611198908)\n", + "Discrete Parents (part of HybridFactor): \n", + "d0 2\n", + "\n", + "Continuous Parents: []\n" + ] + } + ], + "source": [ + "# Using hgc1 from Example 1: P(X0 | D0) with modes N(0,1) and N(5,4)\n", + "# Get the GaussianConditional for a specific assignment using choose() or ()\n", + "assignment0 = gtsam.DiscreteValues()\n", + "assignment0[D(0)] = 0\n", + "selected_gc0 = hgc1.choose(assignment0) # or hgc1(assignment0)\n", + "print(\"\\nSelected GaussianConditional for D0=0:\")\n", + "selected_gc0.print() # Should be N(0, 1)\n", + "\n", + "assignment1 = gtsam.DiscreteValues()\n", + "assignment1[D(0)] = 1\n", + "selected_gc1 = hgc1(assignment1)\n", + "print(f\"\\nSelected GaussianConditional for D0=1\\n: {selected_gc1}\") # Should be N(5, 4)\n", + "\n", + "# Access conditional properties\n", + "print(f\"Discrete Parents (part of HybridFactor): {hgc1.discreteKeys()}\")\n", + "print(f\"Continuous Parents: {hgc1.continuousParents()}\") # Convenience method" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Evaluation (`logProbability`, `evaluate`)\n", + "\n", + "These methods evaluate the probability density $P_m(X | Z)$ corresponding to the selected discrete mode $m$. They require a `HybridValues` object containing assignments for all involved variables (frontal $X$, discrete parents $M$, continuous parents $Z$)." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "P(X0=0.1 | D0=0): LogProb=-0.9239385332046728, Prob=0.39695254747701175\n", + "P(X0=4.0 | D0=1): LogProb=-2.3364828943245635, Prob=0.0966670292007123\n", + "Error(X0=0.1|D0=0): 0.005000000000000001 (Should be approx -log_prob0 - negLogConstant(gc0))\n", + " Check: 0.0050000000000000044\n" + ] + } + ], + "source": [ + "# Using hgc1: P(X0 | D0) with modes N(0,1) and N(5,4)\n", + "\n", + "# --- Evaluate mode 0: P(X0=0.1 | D0=0) ---\n", + "hybrid_vals0 = gtsam.HybridValues()\n", + "hybrid_vals0.insert(X(0), np.array([0.1])) # Frontal value\n", + "hybrid_vals0.insert(D(0), 0) # Discrete parent assignment\n", + "\n", + "log_prob0 = hgc1.logProbability(hybrid_vals0)\n", + "prob0 = hgc1.evaluate(hybrid_vals0)\n", + "# Expected: log density of N(0.1; 0, 1)\n", + "print(f\"\\nP(X0=0.1 | D0=0): LogProb={log_prob0}, Prob={prob0}\")\n", + "\n", + "# --- Evaluate mode 1: P(X0=4.0 | D0=1) ---\n", + "hybrid_vals1 = gtsam.HybridValues()\n", + "hybrid_vals1.insert(X(0), np.array([4.0])) # Frontal value\n", + "hybrid_vals1.insert(D(0), 1) # Discrete parent assignment\n", + "\n", + "log_prob1 = hgc1.logProbability(hybrid_vals1)\n", + "prob1 = hgc1.evaluate(hybrid_vals1)\n", + "# Expected: log density of N(4.0; 5, 4)\n", + "print(f\"P(X0=4.0 | D0=1): LogProb={log_prob1}, Prob={prob1}\")\n", + "\n", + "# Note: The error() method inherited from HybridGaussianFactor gives\n", + "# the Gaussian negative log-likelihood *plus* the neg log normalization constant\n", + "# stored in the tree.\n", + "# error = -log P(x|m,z) - log(NormConstant)\n", + "err0 = hgc1.error(hybrid_vals0)\n", + "print(f\"Error(X0=0.1|D0=0): {err0} (Should be approx -log_prob0 - negLogConstant(gc0))\")\n", + "neg_log_const_0 = gc0.negLogConstant() # Should be ~0.9189\n", + "print(f\" Check: {-log_prob0 - neg_log_const_0}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridGaussianFactor.ipynb b/gtsam/hybrid/doc/HybridGaussianFactor.ipynb new file mode 100644 index 0000000000..f6ccac777c --- /dev/null +++ b/gtsam/hybrid/doc/HybridGaussianFactor.ipynb @@ -0,0 +1,378 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridGaussianFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `HybridGaussianFactor` represents a factor where the underlying probability distribution is Gaussian, but *which* Gaussian distribution applies depends on the assignment of a set of discrete variables. It inherits from `HybridFactor`.\n", + "\n", + "Mathematically, it represents a factor $\\phi(X, M)$ over continuous variables $X$ and discrete variables $M$, defined as:\n", + "$$\n", + "\\phi(X, M=m) \\propto \\exp(-E_m - \\frac12 ||A_m X - b_m||^2)\n", + "$$\n", + "where for each discrete assignment $m$, we have a standard Gaussian factor defined by $A_m$, $b_m$ (and an associated noise model, implicitly defining the Mahalanobis distance), plus an optional scalar energy term $E_m$.\n", + "\n", + "Internally, it uses a `DecisionTree` to store the different Gaussian factor components (`GaussianFactor::shared_ptr`) and their associated scalar energies (`double`), indexed by the `DiscreteKey`s.\n", + "\n", + "These factors typically arise from:\n", + "1. Linearizing a `HybridNonlinearFactor`.\n", + "2. Representing sensor models with discrete operating modes (e.g., a sensor that works differently in rain vs. sun).\n", + "3. As intermediate results during hybrid elimination." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import HybridGaussianFactor, JacobianFactor, DiscreteKeys, DiscreteValues\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Can be initialized with one or more discrete keys and the corresponding Gaussian factor components." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "HybridGaussianFactor (factors only):\n", + "HybridGaussianFactor\n", + "\n", + "Hybrid [x0; d0]{\n", + " Choice(d0) \n", + " 0 Leaf :\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + " 1 Leaf :\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 5 ]\n", + "isotropic dim=1 sigma=2\n", + "scalar: 0\n", + "\n", + "}\n" + ] + } + ], + "source": [ + "# --- Example 1: Single Discrete Key ---\n", + "dk0 = (D(0), 2) # Binary key D0\n", + "\n", + "# Gaussian factor for mode D0=0: N(X(0); mean=0, sigma=1) -> ||x0 - 0||^2 / 2\n", + "gf0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n", + "# Gaussian factor for mode D0=1: N(X(0); mean=5, sigma=2) -> ||x0 - 5||^2 / (2*4)\n", + "gf1 = JacobianFactor(X(0), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 2.0))\n", + "\n", + "# Option A: Just factors (scalar energy E_m defaults to 0)\n", + "hgf_a = HybridGaussianFactor(dk0, [gf0, gf1])\n", + "print(\"HybridGaussianFactor (factors only):\")\n", + "hgf_a.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "65a3f460", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "HybridGaussianFactor (factors + scalars):\n", + "HybridGaussianFactor\n", + "\n", + "Hybrid [x0; d0]{\n", + " Choice(d0) \n", + " 0 Leaf :\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "scalar: 0.356675\n", + "\n", + " 1 Leaf :\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 5 ]\n", + "isotropic dim=1 sigma=2\n", + "scalar: 1.20397\n", + "\n", + "}\n" + ] + } + ], + "source": [ + "# Option B: Factors with scalar energies E_m\n", + "# Example: Add -log P(D0=0)= -log(0.7) and -log P(D0=1)=-log(0.3)\n", + "scalar0 = -np.log(0.7)\n", + "scalar1 = -np.log(0.3)\n", + "hgf_b = HybridGaussianFactor(dk0, [(gf0, scalar0), (gf1, scalar1)])\n", + "print(\"\\nHybridGaussianFactor (factors + scalars):\")\n", + "hgf_b.print()" + ] + }, + { + "cell_type": "markdown", + "id": "3a9d47a1", + "metadata": {}, + "source": [ + "In C++ you can create HybridGaussianFactors with multiple discrete keys as well, but that capability is not yet exposed in the wrapper." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Accessing Components\n", + "\n", + "You can retrieve the underlying decision tree structure and select a specific Gaussian factor component based on a discrete assignment." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor/Scalar pair for D0=0:\n", + "Factor:\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "Scalar: 0.35667494393873245\n", + "\n", + "Factor/Scalar pair for D0=1:\n", + "Factor:\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 5 ]\n", + "isotropic dim=1 sigma=2\n", + "Scalar: 1.2039728043259361\n" + ] + } + ], + "source": [ + "# Using hgf_b from Example 1B\n", + "\n", + "# Get the factor/scalar pair for a specific assignment\n", + "assignment0 = gtsam.DiscreteValues()\n", + "assignment0[D(0)] = 0\n", + "factor_pair0 = hgf_b(assignment0) # Calls operator()\n", + "print(\"\\nFactor/Scalar pair for D0=0:\")\n", + "factor_pair0[0].print(\"Factor:\") # GaussianFactor component\n", + "print(f\"Scalar: {factor_pair0[1]}\") # Scalar energy term\n", + "\n", + "assignment1 = gtsam.DiscreteValues()\n", + "assignment1[D(0)] = 1\n", + "factor_pair1 = hgf_b(assignment1)\n", + "print(\"\\nFactor/Scalar pair for D0=1:\")\n", + "factor_pair1[0].print(\"Factor:\")\n", + "print(f\"Scalar: {factor_pair1[1]}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Error Calculation (`error`, `errorTree`)\n", + "\n", + "* `error(HybridValues)`: Calculates the total error $E_m + \\frac{1}{2} ||A_m X - b_m||^2$ for the specified continuous values $X$ and discrete assignment $M=m$.\n", + "* `errorTree(VectorValues)`: Calculates the Gaussian error term $\\frac{1}{2} ||A_m X - b_m||^2$ for the given continuous values $X$ across *all* discrete assignments $m$, returning the results as an `AlgebraicDecisionTree` indexed by the discrete keys $M$. It also adds the scalar energy $E_m$ to each leaf." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Error for X0=0.1, D0=0: 0.36167494393873245 (Expected ~ 0.36167494393873245)\n", + "Error for X0=4.0, D0=1: 1.3289728043259361 (Expected ~ 1.3289728043259361)\n", + "\n", + "Error Tree for X0=1.0:\n", + "ErrorTree\n", + " Choice(d0) \n", + "ErrorTree\n", + " 0 Leaf 0.85667494\n", + "ErrorTree\n", + " 1 Leaf 3.2039728\n", + "(Expected Leaf 0 ~ 0.8566749439387324)\n", + "(Expected Leaf 1 ~ 3.203972804325936)\n" + ] + } + ], + "source": [ + "# Using hgf_b = HybridGaussianFactor(dk0, [(gf0, scalar0), (gf1, scalar1)])\n", + "# gf0: N(X(0); 0, 1), scalar0 = -log(0.7)\n", + "# gf1: N(X(0); 5, 2), scalar1 = -log(0.3)\n", + "\n", + "# --- error(HybridValues) ---\n", + "hybrid_vals0 = gtsam.HybridValues()\n", + "hybrid_vals0.insert(X(0), np.array([0.1])) # Continuous value\n", + "hybrid_vals0.insert(D(0), 0) # Discrete assignment\n", + "\n", + "err0 = hgf_b.error(hybrid_vals0)\n", + "# Expected: scalar0 + 0.5 * ||0.1 - 0||^2 / 1^2\n", + "print(f\"\\nError for X0=0.1, D0=0: {err0} (Expected ~ {-np.log(0.7) + 0.5*0.1**2})\")\n", + "\n", + "hybrid_vals1 = gtsam.HybridValues()\n", + "hybrid_vals1.insert(X(0), np.array([4.0])) # Continuous value\n", + "hybrid_vals1.insert(D(0), 1) # Discrete assignment\n", + "\n", + "err1 = hgf_b.error(hybrid_vals1)\n", + "# Expected: scalar1 + 0.5 * ||4.0 - 5.0||^2 / 2^2\n", + "print(f\"Error for X0=4.0, D0=1: {err1} (Expected ~ {-np.log(0.3) + 0.5*(-1.0)**2 / 4.0})\")\n", + "\n", + "# --- errorTree(VectorValues) ---\n", + "cont_vals = gtsam.VectorValues()\n", + "cont_vals.insert(X(0), np.array([1.0])) # Evaluate Gaussian part at X0=1.0\n", + "\n", + "adt = hgf_b.errorTree(cont_vals)\n", + "# Leaf 0 (D0=0): scalar0 + 0.5 * ||1.0 - 0||^2 / 1^2 = scalar0 + 0.5\n", + "# Leaf 1 (D0=1): scalar1 + 0.5 * ||1.0 - 5.0||^2 / 2^2 = scalar1 + 0.5 * 16 / 4 = scalar1 + 2.0\n", + "print(\"\\nError Tree for X0=1.0:\")\n", + "adt.print()\n", + "print(f\"(Expected Leaf 0 ~ {-np.log(0.7) + 0.5})\")\n", + "print(f\"(Expected Leaf 1 ~ {-np.log(0.3) + 2.0})\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Restriction (`restrict`)\n", + "\n", + "Fixes the discrete variables according to a given assignment, returning the corresponding `GaussianFactor` component (as a `gtsam.Factor::shared_ptr`)." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Restricted Factor for D0=1:\n", + "Continuous [x0]{\n", + " Leaf :\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 5 ]\n", + "isotropic dim=1 sigma=2\n", + "scalar: 1.20397\n", + "\n", + "}\n" + ] + } + ], + "source": [ + "# Using hgf_b\n", + "restricted_factor_ptr = hgf_b.restrict(assignment1)# Choose mode 1\n", + "\n", + "if restricted_factor_ptr:\n", + " restricted_factor_ptr.print(\"\\nRestricted Factor for D0=1:\")\n", + " # Should be equivalent to gf1: N(X(0); 5, 2)\n", + "else:\n", + " print(\"\\nRestriction failed.\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridGaussianFactorGraph.ipynb b/gtsam/hybrid/doc/HybridGaussianFactorGraph.ipynb new file mode 100644 index 0000000000..b5f78fd486 --- /dev/null +++ b/gtsam/hybrid/doc/HybridGaussianFactorGraph.ipynb @@ -0,0 +1,663 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridGaussianFactorGraph" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridGaussianFactorGraph` is a specialized `HybridFactorGraph` designed to represent linearized hybrid systems or systems that are inherently Conditional Linear Gaussian (CLG). It primarily contains:\n", + "\n", + "* `gtsam.GaussianFactor` (or derived types like `JacobianFactor`, `HessianFactor`)\n", + "* `gtsam.DiscreteFactor` (or derived types like `DecisionTreeFactor`, `TableFactor`)\n", + "* `gtsam.HybridGaussianFactor` (including `HybridGaussianConditional`)\n", + "\n", + "This graph type is typically the result of linearizing a `HybridNonlinearFactorGraph`. It supports specialized elimination methods (`eliminateSequential`, `eliminateMultifrontal` via `EliminateableFactorGraph` base) that correctly handle the mix of discrete and continuous variables, producing a `HybridBayesNet` or `HybridBayesTree`." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "import graphviz\n", + "\n", + "from gtsam import (\n", + " JacobianFactor,\n", + " DecisionTreeFactor,\n", + " HybridGaussianFactorGraph,\n", + " HybridGaussianFactor,\n", + " GaussianConditional,\n", + " HybridValues,\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization and Adding Factors" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Hybrid Gaussian Factor Graph: \n", + "size: 3\n", + "Factor 0\n", + "GaussianFactor:\n", + "\n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "\n", + "Factor 1\n", + "DiscreteFactor:\n", + " f[ (d0,2), ]\n", + " Choice(d0) \n", + " 0 Leaf 0.7\n", + " 1 Leaf 0.3\n", + "\n", + "Factor 2\n", + "HybridGaussianFactor:\n", + "Hybrid [x1; d1]{\n", + " Choice(d1) \n", + " 0 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + " 1 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 2 ]\n", + "isotropic dim=1 sigma=0.5\n", + "scalar: 0\n", + "\n", + "}\n", + "\n" + ] + } + ], + "source": [ + "hgfg = HybridGaussianFactorGraph()\n", + "\n", + "# Gaussian Factor (e.g., prior on X0)\n", + "prior_model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "prior_factor = JacobianFactor(X(0), -np.eye(1), np.zeros(1), prior_model)\n", + "hgfg.add(prior_factor)\n", + "\n", + "# Discrete Factor (e.g., prior on D0)\n", + "dk0 = (D(0), 2) # D0 has 2 states\n", + "discrete_prior = DecisionTreeFactor([dk0], \"0.7 0.3\") # P(D0=0)=0.7, P(D0=1)=0.3\n", + "hgfg.add(discrete_prior)\n", + "\n", + "# Hybrid Gaussian Factor (e.g., measurement on X1 depends on D0)\n", + "dk1 = (D(1), 2)\n", + "# Measurement model 0: N(X1; 0, 1)\n", + "gf0 = JacobianFactor(X(1), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n", + "# Measurement model 1: N(X1; 2, 0.5)\n", + "gf1 = JacobianFactor(X(1), np.eye(1), np.array([2.0]), gtsam.noiseModel.Isotropic.Sigma(1, 0.5))\n", + "hybrid_meas = HybridGaussianFactor(dk1, [gf0, gf1])\n", + "hgfg.add(hybrid_meas)\n", + "\n", + "hgfg.print(\"\\nHybrid Gaussian Factor Graph:\")" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "dbdda9c1", + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793601\n", + "\n", + "d1\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793601--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor2\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# We can visualize the graph using Graphviz\n", + "graphviz.Source(hgfg.dot())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Key Inspection and Choosing a Gaussian Subgraph\n", + "\n", + "Besides the standard `keys()`, `discreteKeys()`, etc., you can `choose` a specific `GaussianFactorGraph` corresponding to a discrete assignment." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Discrete Keys: d0d1\n", + "Continuous Keys: x0x1\n", + "\n", + "Chosen GaussianFactorGraph for D0=0, D1=1:\n", + "\n", + "size: 2\n", + "factor 0: \n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "factor 1: \n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 2 ]\n", + "isotropic dim=1 sigma=0.5\n" + ] + } + ], + "source": [ + "print(f\"\\nDiscrete Keys: {hgfg.discreteKeySet()}\")\n", + "print(f\"Continuous Keys: {hgfg.continuousKeySet()}\")\n", + "\n", + "# Choose the GaussianFactorGraph for a specific assignment\n", + "assignment = gtsam.DiscreteValues()\n", + "assignment[D(0)] = 0\n", + "assignment[D(1)] = 1 # Selects gf1 from hybrid_meas\n", + "\n", + "gaussian_subgraph = hgfg.choose(assignment)\n", + "\n", + "print(f\"\\nChosen GaussianFactorGraph for D0=0, D1=1:\")\n", + "gaussian_subgraph.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "f7956872", + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor1\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Note: The discrete factor is ignored in the chosen graph.\n", + "# The HybridGaussianFactor contributes its gf1 component.\n", + "# The prior on X0 is included.\n", + "graphviz.Source(gaussian_subgraph.dot())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Computing Errors (`error`, `errorTree`, `probPrime`, `discretePosterior`)\n", + "\n", + "Several methods exist to evaluate the graph under different assumptions." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Total Hybrid Error (Gaussian+Discrete): 0.38167494393873247\n", + "\n", + "Error Tree (Gaussian factor errors):\n", + "ErrorTree\n", + " Choice(d1) \n", + "ErrorTree\n", + " 0 Choice(d0) \n", + "ErrorTree\n", + " 0 0 Leaf 2.5666749\n", + "ErrorTree\n", + " 0 1 Leaf 3.4139728\n", + "ErrorTree\n", + " 1 Choice(d0) \n", + "ErrorTree\n", + " 1 0 Leaf 0.38167494\n", + "ErrorTree\n", + " 1 1 Leaf 1.2289728\n" + ] + } + ], + "source": [ + "# --- Error methods ---\n", + "hybrid_values = HybridValues()\n", + "hybrid_values.insert(X(0), np.array([0.1]))\n", + "hybrid_values.insert(X(1), np.array([2.1])) # Close to mean of gf1\n", + "hybrid_values.insert(assignment) # Use D0=0, D1=1\n", + "\n", + "# Sum of errors (Gaussian parts) + neg log prob (Discrete parts)\n", + "total_error = hgfg.error(hybrid_values)\n", + "print(f\"\\nTotal Hybrid Error (Gaussian+Discrete): {total_error}\") # Should be low\n", + "\n", + "# Error Tree (only considers Gaussian factors, returns ADT over discrete keys)\n", + "continuous_values = hybrid_values.continuous()\n", + "error_tree = hgfg.errorTree(continuous_values)\n", + "print(\"\\nError Tree (Gaussian factor errors):\")\n", + "error_tree.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "4d31e67b", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Probability Prime (unnormalized density): 0.6827169384198328\n", + "\n", + "Discrete Posterior Tree P(M | X=x):\n", + "ErrorTree\n", + " Choice(d1) \n", + "ErrorTree\n", + " 0 Choice(d0) \n", + "ErrorTree\n", + " 0 0 Leaf 0.070773923\n", + "ErrorTree\n", + " 0 1 Leaf 0.030331681\n", + "ErrorTree\n", + " 1 Choice(d0) \n", + "ErrorTree\n", + " 1 0 Leaf 0.62922608\n", + "ErrorTree\n", + " 1 1 Leaf 0.26966832\n" + ] + } + ], + "source": [ + "# probPrime (unnormalized posterior density for a full hybrid assignment)\n", + "prob_prime = hgfg.probPrime(hybrid_values)\n", + "print(f\"\\nProbability Prime (unnormalized density): {prob_prime}\") # exp(-error)\n", + "\n", + "# discretePosterior (P(M | X=x), needs only continuous values)\n", + "discrete_posterior_tree = hgfg.discretePosterior(continuous_values)\n", + "print(\"\\nDiscrete Posterior Tree P(M | X=x):\")\n", + "discrete_posterior_tree.print() # ADT representing normalized discrete posterior" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Elimination (`eliminateSequential`, `eliminateMultifrontal`)\n", + "\n", + "Elimination methods handle the hybrid nature, producing `HybridBayesNet` or `HybridBayesTree`. Discrete keys are typically eliminated *after* continuous keys by default using `HybridOrdering`." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Default Hybrid Ordering: Position 0: x0, x1, d0, d1\n", + "\n", + "\n", + "Resulting HybridBayesNet:\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "var7205759403792793601\n", + "\n", + "d1\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var7205759403792793601->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 9, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Default ordering eliminates continuous then discrete\n", + "ordering = gtsam.HybridOrdering(hgfg)\n", + "print(f\"\\nDefault Hybrid Ordering: {ordering}\") # Expect X(0), X(1), D(0), D(1)...\n", + "\n", + "# Sequential elimination\n", + "hybrid_bayes_net: gtsam.HybridBayesNet\n", + "hybrid_bayes_net, remaining_factors = hgfg.eliminatePartialSequential(ordering)\n", + "print(\"\\nResulting HybridBayesNet:\")\n", + "graphviz.Source(hybrid_bayes_net.dot())" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "0340b339", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Resulting HybridBayesTree:\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "7205759403792793601\n", + "\n", + "d1\n", + "\n", + "\n", + "\n", + "8646911284551352321\n", + "\n", + "x1 : d1\n", + "\n", + "\n", + "\n", + "7205759403792793601->8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Multifrontal elimination (often preferred)\n", + "hybrid_bayes_tree = hgfg.eliminateMultifrontal(ordering)\n", + "print(\"\\nResulting HybridBayesTree:\")\n", + "graphviz.Source(hybrid_bayes_tree.dot())" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridGaussianISAM.ipynb b/gtsam/hybrid/doc/HybridGaussianISAM.ipynb new file mode 100644 index 0000000000..05d66ebf32 --- /dev/null +++ b/gtsam/hybrid/doc/HybridGaussianISAM.ipynb @@ -0,0 +1,427 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridGaussianISAM" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridGaussianISAM` implements the Incremental Smoothing and Mapping (ISAM) algorithm for **hybrid** factor graphs, specifically `HybridGaussianFactorGraph`s. It inherits from `gtsam.ISAM`, meaning it maintains an underlying `HybridBayesTree` representing the smoothed posterior distribution $P(X, M | Z)$ over continuous variables $X$, discrete variables $M$, given measurements $Z$.\n", + "\n", + "The key feature is the `update` method, which efficiently incorporates new factors (measurements) into the existing `HybridBayesTree` without re-processing the entire history. This involves:\n", + "1. Identifying the portion of the Bayes tree affected by the new factors.\n", + "2. Removing the affected cliques (orphans).\n", + "3. Re-eliminating the variables in the orphaned cliques along with the new factors.\n", + "4. Merging the newly created Bayes sub-tree back into the main tree.\n", + "\n", + "It provides an incremental solution for problems involving both continuous and discrete variables, where the underlying system dynamics are linear or have been linearized (resulting in a `HybridGaussianFactorGraph`)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import (\n", + " HybridGaussianISAM, HybridGaussianFactorGraph, HybridBayesTree,\n", + " JacobianFactor, DecisionTreeFactor, HybridGaussianFactor,\n", + " DiscreteValues, VectorValues, HybridValues, Ordering\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization\n", + "\n", + "Can be initialized empty or from an existing `HybridBayesTree`." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Empty HybridGaussianISAM created.\n", + "\n", + "HybridGaussianISAM from initial HybridBayesTree:\n", + "HybridBayesTree\n", + ": cliques: 2, variables: 2\n", + "HybridBayesTree\n", + "-p(x0)\n", + " R = [ 1 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x0: 0\n", + " logNormalizationConstant: -0.918939\n", + " No noise model\n", + "HybridBayesTree\n", + "- P( d0 ):\n", + " f[ (d0,2), ]\n", + "(d0, 0) | 0.6 | 0\n", + "(d0, 1) | 0.4 | 1\n", + "number of nnzs: 2\n", + "\n" + ] + } + ], + "source": [ + "# 1. Empty ISAM\n", + "hisam1 = gtsam.HybridGaussianISAM()\n", + "print(\"Empty HybridGaussianISAM created.\")\n", + "\n", + "# 2. From existing HybridBayesTree\n", + "# Create a minimal initial graph and Bayes tree P(D0), P(X0)\n", + "initial_graph = gtsam.HybridGaussianFactorGraph()\n", + "dk0 = (D(0), 2)\n", + "initial_graph.add(DecisionTreeFactor([dk0], \"0.6 0.4\")) # P(D0)\n", + "initial_graph.add(JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Unit.Create(1))) # P(X0)\n", + "ordering = gtsam.Ordering([X(0), D(0)])\n", + "initial_hbt = initial_graph.eliminateMultifrontal(ordering)\n", + "\n", + "hisam2 = gtsam.HybridGaussianISAM(initial_hbt)\n", + "print(\"\\nHybridGaussianISAM from initial HybridBayesTree:\")\n", + "hisam2.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Incremental Updates\n", + "\n", + "The `update` method takes a `HybridGaussianFactorGraph` containing new factors to be added." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Adding Update 1 Factors:\n", + "\n", + "size: 2\n", + "Factor 0\n", + "GaussianFactor:\n", + "\n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + "isotropic dim=1 sigma=0.316228\n", + "\n", + "Factor 1\n", + "HybridGaussianFactor:\n", + "Hybrid [x1; d0]{\n", + " Choice(d0) \n", + " 0 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + "isotropic dim=1 sigma=0.5\n", + "scalar: 0\n", + "\n", + " 1 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 5 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + "}\n", + "\n", + "\n", + "ISAM state after Update 1:\n", + "HybridBayesTree\n", + ": cliques: 3, variables: 3\n", + "HybridBayesTree\n", + "- P( d0 ):\n", + " f[ (d0,2), ]\n", + "(d0, 0) | 0.976859 | 0\n", + "(d0, 1) | 0.0231405 | 1\n", + "number of nnzs: 2\n", + "\n", + "HybridBayesTree\n", + "| - P( x1 | d0)\n", + " Discrete Keys = (d0, 2), \n", + " logNormalizationConstant: -0.123394\n", + "\n", + " Choice(d0) \n", + " 0 Leaf p(x1)\n", + " R = [ 2.21565 ]\n", + " d = [ 2.21565 ]\n", + " mean: 1 elements\n", + " x1: 1\n", + " logNormalizationConstant: -0.123394\n", + " No noise model\n", + "\n", + " 1 Leaf p(x1)\n", + " R = [ 1.3817 ]\n", + " d = [ 4.27669 ]\n", + " mean: 1 elements\n", + " x1: 3.09524\n", + " logNormalizationConstant: -0.595625\n", + " No noise model\n", + "\n", + "HybridBayesTree\n", + "| | -p(x0 | x1)\n", + " R = [ 3.31662 ]\n", + " S[x1] = [ -3.01511 ]\n", + " d = [ -3.01511 ]\n", + " logNormalizationConstant: 0.280009\n", + " No noise model\n", + "\n", + "Adding Update 2 Factors:\n", + "\n", + "size: 1\n", + "Factor 0\n", + "GaussianFactor:\n", + "\n", + " A[x1] = [\n", + "\t-1\n", + "]\n", + " A[x2] = [\n", + "\t1\n", + "]\n", + " b = [ 2 ]\n", + " Noise model: unit (1) \n", + "\n", + "\n", + "ISAM state after Update 2:\n", + "HybridBayesTree\n", + ": cliques: 3, variables: 4\n", + "HybridBayesTree\n", + "- P( d0 ):\n", + " f[ (d0,2), ]\n", + "(d0, 0) | 0.976859 | 0\n", + "(d0, 1) | 0.0231405 | 1\n", + "number of nnzs: 2\n", + "\n", + "HybridBayesTree\n", + "| - P( x1 x2 | d0)\n", + " Discrete Keys = (d0, 2), \n", + " logNormalizationConstant: -1.04233\n", + "\n", + " Choice(d0) \n", + " 0 Leaf p(x1 x2 )\n", + " R = [ 2.43086 -0.411377 ]\n", + " [ 0 0.911465 ]\n", + " d = [ 1.19673 2.7344 ]\n", + " mean: 2 elements\n", + " x1: 1\n", + " x2: 3\n", + " logNormalizationConstant: -1.04233\n", + " No noise model\n", + "\n", + " 1 Leaf p(x1 x2 )\n", + " R = [ 1.70561 -0.586302 ]\n", + " [ 0 0.810093 ]\n", + " d = [ 2.29191 4.12761 ]\n", + " mean: 2 elements\n", + " x1: 3.09524\n", + " x2: 5.09524\n", + " logNormalizationConstant: -1.51456\n", + " No noise model\n", + "\n", + "HybridBayesTree\n", + "| | -p(x0 | x1)\n", + " R = [ 3.31662 ]\n", + " S[x1] = [ -3.01511 ]\n", + " d = [ -3.01511 ]\n", + " logNormalizationConstant: 0.280009\n", + " No noise model\n" + ] + } + ], + "source": [ + "# Start with hisam2 from above\n", + "hisam = hisam2\n", + "\n", + "# --- Update 1: Add factors connecting X0, X1, D0 ---\n", + "update1_graph = gtsam.HybridGaussianFactorGraph()\n", + "# Add P(X1 | X0) = N(X0+1, 0.1)\n", + "update1_graph.add(JacobianFactor(X(0), -np.eye(1), X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, np.sqrt(0.1))))\n", + "# Add P(X1 | D0) = mixture N(1, 0.25); N(5, 1.0)\n", + "gf0 = JacobianFactor(X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, 0.5))\n", + "gf1 = JacobianFactor(X(1), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n", + "update1_graph.add(HybridGaussianFactor(dk0, [gf0, gf1]))\n", + "\n", + "print(\"\\nAdding Update 1 Factors:\")\n", + "update1_graph.print()\n", + "\n", + "hisam.update(update1_graph)\n", + "print(\"\\nISAM state after Update 1:\")\n", + "hisam.print()\n", + "\n", + "# --- Update 2: Add factor connecting X1, X2 ---\n", + "update2_graph = gtsam.HybridGaussianFactorGraph()\n", + "update2_graph.add(JacobianFactor(X(1), -np.eye(1), X(2), np.eye(1), np.array([2.0]), gtsam.noiseModel.Isotropic.Sigma(1, 1.0)))\n", + "\n", + "print(\"\\nAdding Update 2 Factors:\")\n", + "update2_graph.print()\n", + "\n", + "hisam.update(update2_graph)\n", + "print(\"\\nISAM state after Update 2:\")\n", + "hisam.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Solution and Marginals\n", + "\n", + "After updates, the underlying `HybridBayesTree` can be used to obtain the current MAP estimate or calculate marginals, similar to the batch case." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Current MAP Solution from ISAM:\n", + "HybridValues: \n", + " Continuous: 3 elements\n", + " x0: 2.67796e-16\n", + " x1: 1\n", + " x2: 3\n", + " Discrete: (d0, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n" + ] + } + ], + "source": [ + "# Get the current MAP estimate from the ISAM object\n", + "# ISAM inherits optimize() from HybridBayesTree\n", + "current_map_solution = hisam.optimize()\n", + "print(\"\\nCurrent MAP Solution from ISAM:\")\n", + "current_map_solution.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "9cc5afed", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "MPE Assignment: DiscreteValues{7205759403792793600: 0}\n", + "\n", + "GaussianBayesTree for MPE assignment:\n", + ": cliques: 3, variables: 3\n", + "- p()\n", + " R = Empty (0x0)\n", + " d = Empty (0x1)\n", + " mean: 0 elements\n", + " logNormalizationConstant: -0\n", + " No noise model\n", + "| - p(x1 x2 )\n", + " R = [ 2.43086 -0.411377 ]\n", + " [ 0 0.911465 ]\n", + " d = [ 1.19673 2.7344 ]\n", + " mean: 2 elements\n", + " x1: 1\n", + " x2: 3\n", + " logNormalizationConstant: -1.04233\n", + " No noise model\n", + "| | - p(x0 | x1)\n", + " R = [ 3.31662 ]\n", + " S[x1] = [ -3.01511 ]\n", + " d = [ -3.01511 ]\n", + " logNormalizationConstant: 0.280009\n", + " No noise model\n" + ] + } + ], + "source": [ + "# Access the underlying HybridBayesTree methods\n", + "# Get a specific GaussianBayesTree for an MPE assignment\n", + "mpe = hisam.mpe()\n", + "print(\"\\nMPE Assignment:\", mpe)\n", + "gbt_mpe = hisam.choose(mpe)\n", + "print(\"\\nGaussianBayesTree for MPE assignment:\")\n", + "gbt_mpe.print()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridGaussianProductFactor.ipynb b/gtsam/hybrid/doc/HybridGaussianProductFactor.ipynb new file mode 100644 index 0000000000..d449af93d9 --- /dev/null +++ b/gtsam/hybrid/doc/HybridGaussianProductFactor.ipynb @@ -0,0 +1,48 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridGaussianProductFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridGaussianProductFactor` is a specialized `DecisionTree` used internally during hybrid elimination. It represents a collection of `GaussianFactorGraph`s, each associated with a specific assignment of a set of discrete variables, along with a scalar value accumulated for that assignment.\n", + "\n", + "Specifically, it's a `DecisionTree`, where `GaussianFactorGraphValuePair` is `std::pair`.\n", + "\n", + "Its primary role is to hold the intermediate results when eliminating a continuous variable that has discrete variables in its separator/conditional parents. When eliminating such a variable, the resulting factor on the separator involves:\n", + "1. A set of purely continuous factors (a `GaussianFactorGraph`).\n", + "2. A scalar term (representing accumulated constants or log-likelihoods).\n", + "This pair depends on the assignment of the discrete separator variables. The `HybridGaussianProductFactor` stores these pairs in a decision tree structure indexed by the discrete keys.\n", + "\n", + "While crucial internally for elimination algorithms like `EliminateHybrid`, direct user interaction with `HybridGaussianProductFactor` is less common compared to `HybridGaussianFactor` or `HybridGaussianConditional`. It can be thought of as the precursor structure from which a `HybridGaussianFactor` or `HybridGaussianConditional` is formed after elimination." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridJunctionTree.ipynb b/gtsam/hybrid/doc/HybridJunctionTree.ipynb new file mode 100644 index 0000000000..5a038e94b5 --- /dev/null +++ b/gtsam/hybrid/doc/HybridJunctionTree.ipynb @@ -0,0 +1,263 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridJunctionTree" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `HybridJunctionTree` is the hybrid equivalent of `gtsam.GaussianJunctionTree`. It represents an intermediate stage in **multifrontal** variable elimination on a `HybridGaussianFactorGraph`.\n", + "\n", + "It is a tree structure where each node (a \"cluster\" or \"clique\") contains:\n", + "1. A set of frontal variables (those being eliminated at this node).\n", + "2. A set of separator variables (those shared with the parent clique).\n", + "3. The original factors from the `HybridGaussianFactorGraph` that involve *only* the frontal and separator variables of this clique and its descendants in the elimination tree.\n", + "\n", + "Key differences:\n", + "* **vs. HybridEliminationTree:** A junction tree node can eliminate multiple variables (frontals) at once, whereas an elimination tree node corresponds to a single variable elimination.\n", + "* **vs. HybridBayesTree:** A junction tree node stores the *original factors* before elimination, while a Bayes tree node stores the *result* of eliminating those factors (a `HybridConditional`).\n", + "\n", + "Like `HybridEliminationTree`, the `HybridJunctionTree` is primarily an internal data structure used by the `eliminateMultifrontal` process. Its structure directly informs the structure of the resulting `HybridBayesTree`. Direct manipulation in Python is uncommon." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import (\n", + " HybridGaussianFactorGraph,\n", + " Ordering,\n", + " HybridEliminationTree,\n", + " HybridJunctionTree,\n", + " JacobianFactor,\n", + " DecisionTreeFactor,\n", + " HybridGaussianFactor,\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating a HybridJunctionTree\n", + "\n", + "It is constructed from a `HybridEliminationTree`. The process typically involves:\n", + "1. Create `HybridGaussianFactorGraph`.\n", + "2. Determine an `Ordering`.\n", + "3. Build the `HybridEliminationTree`.\n", + "4. Build the `HybridJunctionTree` from the `HybridEliminationTree`.\n", + "\n", + "This process is encapsulated within `HybridGaussianFactorGraph.eliminateMultifrontal`." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original HybridGaussianFactorGraph:\n", + "\n", + "size: 4\n", + "Factor 0\n", + "DiscreteFactor:\n", + " f[ (d0,2), ]\n", + " Choice(d0) \n", + " 0 Leaf 0.6\n", + " 1 Leaf 0.4\n", + "\n", + "Factor 1\n", + "GaussianFactor:\n", + "\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "\n", + "Factor 2\n", + "HybridGaussianFactor:\n", + "Hybrid [x1; d0]{\n", + " Choice(d0) \n", + " 0 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + "isotropic dim=1 sigma=0.5\n", + "scalar: 0\n", + "\n", + " 1 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 5 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + "}\n", + "\n", + "Factor 3\n", + "GaussianFactor:\n", + "\n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + "isotropic dim=1 sigma=0.316228\n", + "\n", + "\n", + "Elimination Ordering: Position 0: x0, x1, d0\n", + "\n", + "\n", + "HybridEliminationTree:\n", + "-(d0)\n", + "- f[ (d0,2), ]\n", + " Choice(d0) \n", + " 0 Leaf 0.6\n", + " 1 Leaf 0.4\n", + "| -(x1)\n", + "| -\n", + "Hybrid [x1; d0]{\n", + " Choice(d0) \n", + " 0 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + "isotropic dim=1 sigma=0.5\n", + "scalar: 0\n", + "\n", + " 1 Leaf :\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 5 ]\n", + " Noise model: unit (1) \n", + "scalar: 0\n", + "\n", + "}\n", + "| | -(x0)\n", + "| | -\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "| | -\n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 1 ]\n", + "isotropic dim=1 sigma=0.316228\n", + "\n", + "Resulting HybridJunctionTree:\n", + "- (4) d0 \n", + "| - (4) x1 \n", + "| | - (4) x0 \n" + ] + } + ], + "source": [ + "# --- Create a HybridGaussianFactorGraph (same as HBT/HET examples) ---\n", + "hgfg = HybridGaussianFactorGraph()\n", + "dk0 = (D(0), 2)\n", + "prior_d0 = DecisionTreeFactor([dk0], \"0.6 0.4\"); hgfg.add(prior_d0) # F0\n", + "prior_x0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0)); hgfg.add(prior_x0) # F1\n", + "gf0 = JacobianFactor(X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, 0.5))\n", + "gf1 = JacobianFactor(X(1), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n", + "meas_x1_d0 = HybridGaussianFactor(dk0, [gf0, gf1]); hgfg.add(meas_x1_d0) # F2\n", + "odom_x0_x1 = JacobianFactor(X(0), -np.eye(1), X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, np.sqrt(0.1))); hgfg.add(odom_x0_x1) # F3\n", + "print(\"Original HybridGaussianFactorGraph:\")\n", + "hgfg.print()\n", + "\n", + "# --- Ordering and Elimination Tree ---\n", + "ordering = gtsam.Ordering([X(0), X(1), D(0)])\n", + "print(f\"\\nElimination Ordering: {ordering}\")\n", + "het = HybridEliminationTree(hgfg, ordering)\n", + "print(\"\\nHybridEliminationTree:\")\n", + "het.print()\n", + "\n", + "# --- Construct the Junction Tree ---\n", + "hjt = HybridJunctionTree(het)\n", + "print(\"\\nResulting HybridJunctionTree:\")\n", + "# Printing shows cliques, separators, and the *original factor indices* stored within each clique.\n", + "hjt.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Elimination\n", + "\n", + "The primary purpose of the junction tree is to structure the multifrontal elimination process, which ultimately yields a `HybridBayesTree`. The `eliminate` method of the junction tree performs this step, but it's usually accessed via `HybridGaussianFactorGraph.eliminateMultifrontal`." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridNonlinearFactor.ipynb b/gtsam/hybrid/doc/HybridNonlinearFactor.ipynb new file mode 100644 index 0000000000..769ba7dec3 --- /dev/null +++ b/gtsam/hybrid/doc/HybridNonlinearFactor.ipynb @@ -0,0 +1,380 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridNonlinearFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `HybridNonlinearFactor` represents a factor in a **nonlinear** hybrid optimization problem. Similar to `HybridGaussianFactor`, it represents a factor whose behavior depends on the assignment of discrete variables, but here the underlying components are `gtsam.NonlinearFactor`s (specifically, `NoiseModelFactor`s). It inherits from `HybridFactor`.\n", + "\n", + "Mathematically, it represents a factor $\\phi(X, M)$ whose value for a specific discrete assignment $M=m$ is proportional to the likelihood of the continuous variables $X$ under that mode, potentially weighted:\n", + "$$\n", + "\\phi(X, M=m) \\propto \\exp\\left(-E_m - \\frac{1}{2} ||h_m(X) - z_m||^2_{\\Sigma_m}\\right)\n", + "$$\n", + "This corresponds to an error (negative log-likelihood) for mode $m$ of:\n", + "$$\n", + "\\text{error}(X, M=m) = E_m + \\frac{1}{2} ||h_m(X) - z_m||^2_{\\Sigma_m}\n", + "$$\n", + "where for each discrete assignment $m$, we have:\n", + "* A nonlinear measurement function $h_m(X)$.\n", + "* A measurement $z_m$.\n", + "* A noise model $\\Sigma_m$ defining the squared Mahalanobis distance $|| \\cdot ||^2_{\\Sigma_m}$.\n", + "* An optional scalar term $E_m$. This term is added to the negative log-likelihood derived from the `NoiseModelFactor`. It can be used, for example, to incorporate prior probabilities $P(m)$ for different modes by setting $E_m = -\\log P(m)$ (plus any constant, as only differences in $E_m$ matter for optimization). A smaller $E_m$ makes mode $m$ relatively more likely, all else being equal.\n", + "\n", + "Internally, it uses a `DecisionTree` to store the different nonlinear factor components (`NoiseModelFactor::shared_ptr`) and their associated scalar terms (`double`), indexed by the `DiscreteKey`s.\n", + "\n", + "```mermaid\n", + "graph TD\n", + " HybridNonlinearFactor --> HybridFactor\n", + " HybridNonlinearFactor -- Uses --> DecisionTree[\"DecisionTree\"]\n", + " DecisionTree -- Stores Leaf Values --> NonlinearFactorValuePair\n", + " NonlinearFactorValuePair -- Contains --> ComponentFactor[\"NoiseModelFactor::shared_ptr
(embodies hm, zm, Σm)\"]\n", + " NonlinearFactorValuePair -- Contains --> ScalarTerm[\"double
(the Em scalar term)\"]" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import (\n", + " HybridNonlinearFactor,\n", + " BetweenFactorPose2,\n", + " Values,\n", + " DiscreteValues,\n", + " HybridValues,\n", + " Pose2)\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Initialize with discrete keys and corresponding `NoiseModelFactor` components, optionally with scalar energies." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "HybridNonlinearFactor (factors only):\n", + "HybridNonlinearFactor\n", + " Hybrid [x0 x1; d0]\n", + "HybridNonlinearFactor\n", + " Choice(d0) \n", + " 0 Leaf (0) BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.5; 0.5; 0.174532925];\n", + "\n", + " 1 Leaf (0) BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];\n", + "\n" + ] + } + ], + "source": [ + "# --- Example: Mode-Dependent Odometry ---\n", + "dk0 = (D(0), 2) # Binary mode: Slippery (0) or Grippy (1)\n", + "\n", + "# Nonlinear factors (BetweenFactorPose2) for each mode\n", + "# Mode 0 (Slippery): Larger noise\n", + "noise0 = gtsam.noiseModel.Diagonal.Sigmas([0.5, 0.5, np.radians(10)])\n", + "odom0 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise0)\n", + "\n", + "# Mode 1 (Grippy): Smaller noise\n", + "noise1 = gtsam.noiseModel.Diagonal.Sigmas([0.1, 0.1, np.radians(1)])\n", + "odom1 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise1)\n", + "\n", + "# Option A: Just factors (scalar energy E_m = 0)\n", + "hnlf_a = HybridNonlinearFactor(dk0, [odom0, odom1])\n", + "print(\"HybridNonlinearFactor (factors only):\")\n", + "hnlf_a.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "624e20b8", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "HybridNonlinearFactor (factors + scalars):\n", + "HybridNonlinearFactor\n", + " Hybrid [x0 x1; d0]\n", + "HybridNonlinearFactor\n", + " Choice(d0) \n", + " 0 Leaf (1.60943791) BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.5; 0.5; 0.174532925];\n", + "\n", + " 1 Leaf (0.223143551) BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];\n", + "\n" + ] + } + ], + "source": [ + "# Option B: Factors + scalar energies (e.g., prior on mode)\n", + "# P(Slippery)=0.2 -> E0 = -log(0.2)\n", + "# P(Grippy)=0.8 -> E1 = -log(0.8)\n", + "scalar0 = -np.log(0.2)\n", + "scalar1 = -np.log(0.8)\n", + "hnlf_b = HybridNonlinearFactor(dk0, [(odom0, scalar0), (odom1, scalar1)])\n", + "print(\"\\nHybridNonlinearFactor (factors + scalars):\")\n", + "hnlf_b.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Accessing Components and Error Calculation\n", + "\n", + "Similar to [`HybridGaussianFactor`](HybridGaussianFactor.ipynb), you can access the underlying decision tree and calculate errors." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Error for D0=0 (Slip): 1.6694379124341003\n", + "Error using HybridValues (Slip): 1.6694379124341003\n", + "\n", + "Error for D0=1 (Grippy): 3.2231435513142106\n", + "\n", + "Error Tree Choice(d0) \n", + "Error Tree 0 Leaf 1.6694379\n", + "Error Tree 1 Leaf 3.2231436\n" + ] + } + ], + "source": [ + "# --- Error Calculation ---\n", + "# Requires continuous Values and discrete DiscreteValues (or HybridValues)\n", + "\n", + "# Continuous values\n", + "cont_vals = Values()\n", + "cont_vals.insert(X(0), Pose2(0, 0, 0))\n", + "cont_vals.insert(X(1), Pose2(1.1, 0.1, np.radians(2))) # Slightly off\n", + "\n", + "# Discrete assignment\n", + "assignment0 = DiscreteValues()\n", + "assignment0[D(0)] = 0 # Slip mode\n", + "\n", + "# Method 1: error(Values, DiscreteValues)\n", + "err0 = hnlf_b.error(cont_vals, assignment0)\n", + "# Expected: scalar0 + 0.5 * || odom0.evaluateError(x0, x1) ||^2\n", + "print(f\"\\nError for D0=0 (Slip): {err0}\")\n", + "\n", + "# Method 2: error(HybridValues)\n", + "hybrid_vals0 = HybridValues(cv=gtsam.VectorValues(), dv=assignment0, v=cont_vals)\n", + "err0_hv = hnlf_b.error(hybrid_vals0)\n", + "print(f\"Error using HybridValues (Slip): {err0_hv}\")\n", + "\n", + "# Check Grippy mode\n", + "assignment1 = DiscreteValues()\n", + "assignment1[D(0)] = 1 # Grippy mode\n", + "err1 = hnlf_b.error(cont_vals, assignment1)\n", + "print(f\"\\nError for D0=1 (Grippy): {err1}\\n\") # Should be much lower due to smaller noise\n", + "\n", + "# --- errorTree(Values) ---\n", + "# Calculates nonlinear error for *all* discrete modes given continuous values\n", + "adt = hnlf_b.errorTree(cont_vals)\n", + "adt.print(\"Error Tree\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Linearization\n", + "\n", + "A key function is `linearize`, which converts the `HybridNonlinearFactor` into a `HybridGaussianFactor` at a given linearization point (continuous `Values`)." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Linearized HybridGaussianFactor:\n", + "HybridGaussianFactor\n", + "\n", + "Hybrid [x0 x1; d0]{\n", + " Choice(d0) \n", + " 0 Leaf :\n", + " A[x0] = [\n", + "\t-2, -0, -0;\n", + "\t0, -2, -2;\n", + "\t-0, -0, -5.72957795\n", + "]\n", + " A[x1] = [\n", + "\t2, 0, 0;\n", + "\t0, 2, 0;\n", + "\t0, 0, 5.72957795\n", + "]\n", + " b = [ -0 -0 -0 ]\n", + " No noise model\n", + "scalar: 1.23431728\n", + "\n", + " 1 Leaf :\n", + " A[x0] = [\n", + "\t-10, -0, -0;\n", + "\t0, -10, -10;\n", + "\t-0, -0, -57.2957795\n", + "]\n", + " A[x1] = [\n", + "\t10, 0, 0;\n", + "\t0, 10, 0;\n", + "\t0, 0, 57.2957795\n", + "]\n", + " b = [ -0 -0 -0 ]\n", + " No noise model\n", + "scalar: -5.673438\n", + "\n", + "}\n" + ] + } + ], + "source": [ + "# Using hnlf_b\n", + "linearization_point = Values()\n", + "linearization_point.insert(X(0), Pose2(0, 0, 0))\n", + "linearization_point.insert(X(1), Pose2(1.0, 0, 0)) # Linearize at the expected mean\n", + "\n", + "# Linearize the whole factor (all modes)\n", + "hybrid_gaussian_factor = hnlf_b.linearize(linearization_point)\n", + "print(\"\\nLinearized HybridGaussianFactor:\")\n", + "hybrid_gaussian_factor.print()\n", + "# Note: The Gaussian components will be JacobianFactors evaluated at the linearization point.\n", + "# The scalar energies E_m are carried over.\n", + "\n", + "# Linearize only for a specific mode (useful internally, maybe not direct API)\n", + "# assignment = gtsam.DiscreteValues([(D(0), 0)]) # Slippery\n", + "# gaussian_factor_mode0 = hnlf_b.linearize(linearization_point, assignment) # Hypothetical API\n", + "# print(\"\\nLinearized GaussianFactor for Mode 0:\")\n", + "# gaussian_factor_mode0.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Restriction (`restrict`)\n", + "\n", + "Fixes the discrete variables, returning the corresponding `NoiseModelFactor` component." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Restricted Factor for D0=1 (Grippy):BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];\n" + ] + } + ], + "source": [ + "# Using hnlf_b\n", + "# Create an empty DiscreteValues and assign D(0) = 1 (Grippy mode)\n", + "assignment = DiscreteValues()\n", + "assignment[D(0)] = 1\n", + "restricted_factor_ptr = hnlf_b.restrict(assignment)\n", + "\n", + "restricted_factor_ptr.print(\"\\nRestricted Factor for D0=1 (Grippy):\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridNonlinearFactorGraph.ipynb b/gtsam/hybrid/doc/HybridNonlinearFactorGraph.ipynb new file mode 100644 index 0000000000..0064540c69 --- /dev/null +++ b/gtsam/hybrid/doc/HybridNonlinearFactorGraph.ipynb @@ -0,0 +1,417 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridNonlinearFactorGraph" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridNonlinearFactorGraph` is a `HybridFactorGraph` specifically for representing **nonlinear** hybrid optimization problems. It can contain:\n", + "\n", + "* `gtsam.NonlinearFactor` (including derived types like `PriorFactor`, `BetweenFactor`)\n", + "* `gtsam.DiscreteFactor` (including `DecisionTreeFactor`, `TableFactor`)\n", + "* `gtsam.HybridNonlinearFactor`\n", + "\n", + "This is the graph type you would typically build to model a system with both continuous states (potentially on manifolds) and discrete modes or decisions, where the relationships are nonlinear.\n", + "\n", + "The primary operation performed on a `HybridNonlinearFactorGraph` is `linearize`, which converts it into a `HybridGaussianFactorGraph` suitable for inference using methods like `eliminateSequential` or `eliminateMultifrontal`." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import (\n", + " HybridNonlinearFactorGraph, HybridNonlinearFactor,\n", + " PriorFactorPose2, BetweenFactorPose2, Pose2, Point3,\n", + " DecisionTreeFactor, Values, DiscreteValues, HybridValues,\n", + " HybridGaussianFactorGraph\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization and Adding Factors" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "HybridNonlinearFactorGraph Contents:\n", + "HybridNonlinearFactorGraph\n", + " \n", + "size: 3\n", + "factor 0: PriorFactor on x0\n", + " prior mean: (0, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.05];\n", + "\n", + "factor 1: f[ (d0,2), ]\n", + " Choice(d0) \n", + " 0 Leaf 0.8\n", + " 1 Leaf 0.2\n", + "\n", + "factor 2: Hybrid [x0 x1; d0]\n", + "HybridNonlinearFactor\n", + " Choice(d0) \n", + " 0 Leaf (0) BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];\n", + "\n", + " 1 Leaf (0) BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.5; 0.5; 0.174532925];\n", + "\n", + "\n" + ] + } + ], + "source": [ + "hnfg = HybridNonlinearFactorGraph()\n", + "\n", + "# 1. Add a Nonlinear Factor (Prior)\n", + "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n", + "hnfg.add(PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise))\n", + "\n", + "# 2. Add a Discrete Factor (Prior on mode)\n", + "dk0 = (D(0), 2) # Binary mode\n", + "hnfg.add(DecisionTreeFactor([dk0], \"0.8 0.2\")) # P(D0=0)=0.8\n", + "\n", + "# 3. Add a HybridNonlinearFactor (Mode-dependent odometry)\n", + "# Mode 0 (Grippy): Smaller noise\n", + "noise0 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, np.radians(1)))\n", + "odom0 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise0)\n", + "# Mode 1 (Slippery): Larger noise\n", + "noise1 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.5, 0.5, np.radians(10)))\n", + "odom1 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise1)\n", + "# Assume equal probability for modes within this factor's context\n", + "hybrid_odom = HybridNonlinearFactor(dk0, [odom0, odom1])\n", + "hnfg.add(hybrid_odom)\n", + "\n", + "print(\"HybridNonlinearFactorGraph Contents:\")\n", + "hnfg.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Error Calculation (`error`, `errorTree`, `discretePosterior`)\n", + "\n", + "Evaluates the combined error (nonlinear factors) and negative log probability (discrete factors) of the graph." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Evaluating error with HybridValues:\n", + "HybridValues: \n", + " Continuous: 0 elements\n", + " Discrete: (d0, 0)\n", + " Nonlinear\n", + "Values with 2 values:\n", + "Value x0: (gtsam::Pose2)\n", + "(0.05, 0.05, 0.0174532925)\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(1, -0.05, 0)\n", + "\n", + "\n", + "Total graph error: 1.8480600597872188\n" + ] + } + ], + "source": [ + "# Requires HybridValues for evaluation\n", + "hybrid_values = HybridValues()\n", + "\n", + "# Continuous/Nonlinear Values\n", + "nl_vals = Values()\n", + "nl_vals.insert(X(0), Pose2(0.05, 0.05, np.radians(1))) # Near prior mean\n", + "nl_vals.insert(X(1), Pose2(1.0, -0.05, np.radians(0))) # Near odom mean\n", + "hybrid_values.insert(nl_vals)\n", + "\n", + "# Discrete Values\n", + "disc_vals = DiscreteValues()\n", + "disc_vals[D(0)] = 0 # Select Grippy mode (lower error)\n", + "hybrid_values.insert(disc_vals)\n", + "\n", + "print(\"\\nEvaluating error with HybridValues:\")\n", + "hybrid_values.print()\n", + "\n", + "# --- error(HybridValues) ---\n", + "total_error = hnfg.error(hybrid_values)\n", + "print(f\"\\nTotal graph error: {total_error}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "65ca9727", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Error Tree (Nonlinear errors across modes):\n", + "ErrorTree\n", + " Choice(d0) \n", + "ErrorTree\n", + " 0 Leaf 1.8480601\n", + "ErrorTree\n", + " 1 Leaf 1.9579211\n", + "\n", + "Discrete Posterior P(M | X=x):\n", + "ErrorTree\n", + " Choice(d0) \n", + "ErrorTree\n", + " 0 Leaf 0.52743767\n", + "ErrorTree\n", + " 1 Leaf 0.47256233\n" + ] + } + ], + "source": [ + "# --- errorTree(Values) ---\n", + "# Calculates error for Nonlinear & HybridNonlinear factors across all discrete modes\n", + "# Ignores purely DiscreteFactors for this calculation.\n", + "cont_values = hybrid_values.nonlinear()\n", + "error_tree = hnfg.errorTree(cont_values)\n", + "print(\"\\nError Tree (Nonlinear errors across modes):\")\n", + "error_tree.print()\n", + "\n", + "# --- discretePosterior(Values) ---\n", + "# Calculates P(M | X=x_cont), normalizing exp(-errorTree)\n", + "# Includes contributions from purely discrete factors as well.\n", + "discrete_posterior = hnfg.discretePosterior(cont_values)\n", + "print(\"\\nDiscrete Posterior P(M | X=x):\")\n", + "discrete_posterior.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Linearization\n", + "\n", + "The primary operation is `linearize`, which converts the graph to a `HybridGaussianFactorGraph` at a given linearization point (continuous `Values`)." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Linearizing at:\n", + "Values with 2 values:\n", + "Value x0: (gtsam::Pose2)\n", + "(0, 0, 0)\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(1, 0, 0)\n", + "\n", + "\n", + "Resulting HybridGaussianFactorGraph:\n", + "\n", + "size: 3\n", + "Factor 0\n", + "GaussianFactor:\n", + "\n", + " A[x0] = [\n", + "\t10, 0, 0;\n", + "\t0, 10, 0;\n", + "\t0, 0, 20\n", + "]\n", + " b = [ 0 0 0 ]\n", + " No noise model\n", + "\n", + "Factor 1\n", + "DiscreteFactor:\n", + " f[ (d0,2), ]\n", + " Choice(d0) \n", + " 0 Leaf 0.8\n", + " 1 Leaf 0.2\n", + "\n", + "Factor 2\n", + "HybridGaussianFactor:\n", + "Hybrid [x0 x1; d0]{\n", + " Choice(d0) \n", + " 0 Leaf :\n", + " A[x0] = [\n", + "\t-10, -0, -0;\n", + "\t0, -10, -10;\n", + "\t-0, -0, -57.2957795\n", + "]\n", + " A[x1] = [\n", + "\t10, 0, 0;\n", + "\t0, 10, 0;\n", + "\t0, 0, 57.2957795\n", + "]\n", + " b = [ -0 -0 -0 ]\n", + " No noise model\n", + "scalar: -5.89658155\n", + "\n", + " 1 Leaf :\n", + " A[x0] = [\n", + "\t-2, -0, -0;\n", + "\t0, -2, -2;\n", + "\t-0, -0, -5.72957795\n", + "]\n", + " A[x1] = [\n", + "\t2, 0, 0;\n", + "\t0, 2, 0;\n", + "\t0, 0, 5.72957795\n", + "]\n", + " b = [ -0 -0 -0 ]\n", + " No noise model\n", + "scalar: -0.375120634\n", + "\n", + "}\n", + "\n" + ] + } + ], + "source": [ + "# Linearization point (often the current estimate)\n", + "linearization_point = Values()\n", + "linearization_point.insert(X(0), Pose2(0, 0, 0))\n", + "linearization_point.insert(X(1), Pose2(1, 0, 0))\n", + "\n", + "print(\"\\nLinearizing at:\")\n", + "linearization_point.print()\n", + "\n", + "# Perform linearization\n", + "hgfg = hnfg.linearize(linearization_point)\n", + "\n", + "print(\"\\nResulting HybridGaussianFactorGraph:\")\n", + "hgfg.print()\n", + "# Note: NonlinearFactors become JacobianFactors.\n", + "# HybridNonlinearFactors become HybridGaussianFactors.\n", + "# DiscreteFactors remain DiscreteFactors." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Restriction (`restrict`)\n", + "\n", + "Applies `restrict` to all factors in the graph for a given discrete assignment." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Restricted HybridNonlinearFactorGraph (D0=0):\n", + "HybridNonlinearFactorGraph\n", + " \n", + "size: 2\n", + "factor 0: PriorFactor on x0\n", + " prior mean: (0, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.05];\n", + "\n", + "factor 1: BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];\n", + "\n" + ] + } + ], + "source": [ + "# Restrict the graph to the 'Grippy' mode (D0=0)\n", + "assignment = DiscreteValues()\n", + "assignment[D(0)] = 0\n", + "restricted_hnfg = hnfg.restrict(assignment)\n", + "\n", + "print(\"\\nRestricted HybridNonlinearFactorGraph (D0=0):\")\n", + "restricted_hnfg.print()\n", + "# Note: The HybridNonlinearFactor is replaced by its D0=0 component (odom0).\n", + "# The DiscreteFactor is removed as its variable is assigned." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridNonlinearISAM.ipynb b/gtsam/hybrid/doc/HybridNonlinearISAM.ipynb new file mode 100644 index 0000000000..a7bfe46aa3 --- /dev/null +++ b/gtsam/hybrid/doc/HybridNonlinearISAM.ipynb @@ -0,0 +1,388 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridNonlinearISAM" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridNonlinearISAM` provides an interface for performing incremental nonlinear optimization for hybrid systems using ISAM (Incremental Smoothing and Mapping). It manages an underlying `HybridGaussianISAM` object and handles the relinearization process.\n", + "\n", + "Key concepts:\n", + "* Maintains a full nonlinear factor graph (`HybridNonlinearFactorGraph`) of all factors added so far.\n", + "* Keeps track of the current linearization point (`Values`) and discrete assignment (`DiscreteValues`).\n", + "* Uses an internal `HybridGaussianISAM` object to store the incrementally updated linearized representation (`HybridBayesTree`).\n", + "* The `update` method linearizes new nonlinear factors, incorporates them into the `HybridGaussianISAM`, and updates the estimate.\n", + "* Periodically (controlled by `reorderInterval`), it can perform a full relinearization and reordering of the underlying `HybridGaussianISAM` to maintain accuracy and efficiency." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "import graphviz\n", + "\n", + "from gtsam import (\n", + " HybridNonlinearISAM,\n", + " HybridNonlinearFactorGraph,\n", + " HybridNonlinearFactor,\n", + " PriorFactorPose2,\n", + " BetweenFactorPose2,\n", + " Pose2,\n", + " Point3,\n", + " DecisionTreeFactor,\n", + " Values,\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initialized HybridNonlinearISAM with reorderInterval=0\n" + ] + } + ], + "source": [ + "# Initialize HybridNonlinearISAM\n", + "# reorderInterval=1 means re-linearize/re-order every update (like batch)\n", + "# Set higher (e.g., 10, 100) for true incremental behavior\n", + "reorder_interval = 0 # Set to 0 to disable reordering for this example\n", + "hisam = HybridNonlinearISAM(reorder_interval)\n", + "\n", + "print(f\"Initialized HybridNonlinearISAM with reorderInterval={hisam.reorderInterval()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Incremental Updates\n", + "\n", + "The `update` method takes new factors (`HybridNonlinearFactorGraph`) and initial estimates (`Values`) for any *new* variables introduced in those factors." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "--- Update 0 ---\n", + "ISAM state after update 0:\n", + " Current Lin Point Size: 1\n", + " Current Estimate 0:\n", + "Values with 1 values:\n", + "Value x0: (gtsam::Pose2)\n", + "(0, 1.73472e-18, -7.77621e-19)\n", + "\n", + "\n", + "--- Update 1 ---\n", + "ISAM state after update 1:\n", + " Current Lin Point Size: 2\n", + " Current Estimate 1:\n", + "Values with 2 values:\n", + "Value x0: (gtsam::Pose2)\n", + "(8.67362e-18, -1.73472e-18, 9.57103e-19)\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(1.00005, -6.64995e-07, -7.77621e-19)\n", + "\n" + ] + } + ], + "source": [ + "# --- Initial Step (Pose 0 and Mode Prior) ---\n", + "step0_graph = HybridNonlinearFactorGraph()\n", + "step0_values = Values()\n", + "\n", + "# Add prior on pose X0\n", + "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n", + "step0_graph.add(PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise))\n", + "step0_values.insert(X(0), Pose2(0.01, 0.01, 0.01)) # Initial estimate for X0\n", + "\n", + "# Add prior on discrete mode D0\n", + "dk0 = (D(0), 2) # Binary mode\n", + "step0_graph.add(DecisionTreeFactor([dk0], \"0.8 0.2\")) # P(D0=0)=0.8\n", + "\n", + "print(\"--- Update 0 ---\")\n", + "hisam.update(step0_graph, step0_values)\n", + "print(\"ISAM state after update 0:\")\n", + "# hisam.print() # Printing can be verbose\n", + "print(f\" Current Lin Point Size: {hisam.getLinearizationPoint().size()}\")\n", + "current_estimate_0 = hisam.estimate()\n", + "print(\" Current Estimate 0:\")\n", + "current_estimate_0.print()\n", + "\n", + "\n", + "# --- Second Step (Pose 1 and Hybrid Odometry) ---\n", + "step1_graph = gtsam.HybridNonlinearFactorGraph()\n", + "step1_values = gtsam.Values()\n", + "\n", + "# Add HybridNonlinearFactor for odometry X0->X1\n", + "# Mode 0 (Grippy): Smaller noise\n", + "noise0 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, np.radians(1)))\n", + "odom0 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise0)\n", + "# Mode 1 (Slippery): Larger noise\n", + "noise1 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.5, 0.5, np.radians(10)))\n", + "odom1 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise1)\n", + "hybrid_odom = HybridNonlinearFactor(dk0, [odom0, odom1])\n", + "step1_graph.add(hybrid_odom)\n", + "\n", + "# Provide initial estimate for the *new* variable X1\n", + "# Use odometry from the current estimate of X0\n", + "x0_estimate = current_estimate_0.atPose2(X(0))\n", + "x1_initial_guess = x0_estimate.compose(Pose2(1.0, 0, 0))\n", + "step1_values.insert(X(1), x1_initial_guess)\n", + "\n", + "print(\"\\n--- Update 1 ---\")\n", + "hisam.update(step1_graph, step1_values)\n", + "print(\"ISAM state after update 1:\")\n", + "print(f\" Current Lin Point Size: {hisam.getLinearizationPoint().size()}\")\n", + "current_estimate_1 = hisam.estimate()\n", + "print(\" Current Estimate 1:\")\n", + "current_estimate_1.print() # Should be close to (X0=(0,0,0), X1=(1,0,0), D0=0)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Accessing State and Estimates\n", + "\n", + "After updates, you can retrieve the current estimate, the linearization point, the underlying Bayes tree, and the accumulated factors." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Final Estimate:\n", + "Values with 2 values:\n", + "Value x0: (gtsam::Pose2)\n", + "(8.67362e-18, -1.73472e-18, 9.57103e-19)\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(1.00005, -6.64995e-07, -7.77621e-19)\n", + "\n", + "\n", + "Current Linearization Point (Values):\n", + "Values with 2 values:\n", + "Value x0: (gtsam::Pose2)\n", + "(0.01, 0.01, 0.01)\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(1, 9.57103e-19, -7.77621e-19)\n", + "\n", + "\n", + "Current Discrete Assignment:\n", + "DiscreteValues{7205759403792793600: 0}\n" + ] + } + ], + "source": [ + "# Get the final MAP estimate (calculates from current BayesTree)\n", + "final_estimate = hisam.estimate()\n", + "print(\"\\nFinal Estimate:\")\n", + "final_estimate.print()\n", + "\n", + "# Get the current linearization point (used for the last update)\n", + "lin_point = hisam.getLinearizationPoint()\n", + "print(\"\\nCurrent Linearization Point (Values):\")\n", + "lin_point.print()\n", + "\n", + "# Get the current discrete assignment (MPE from the last update)\n", + "assignment = hisam.assignment()\n", + "print(\"\\nCurrent Discrete Assignment:\")\n", + "print(assignment)" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "56fda45b", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Underlying HybridGaussianISAM (HybridBayesTree):\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "7205759403792793600\n", + "\n", + "d0\n", + "\n", + "\n", + "\n", + "8646911284551352320\n", + "\n", + "x0, x1 : d0\n", + "\n", + "\n", + "\n", + "7205759403792793600->8646911284551352320\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Get the underlying Gaussian ISAM object (HybridBayesTree)\n", + "gaussian_isam = hisam.bayesTree()\n", + "print(\"\\nUnderlying HybridGaussianISAM (HybridBayesTree):\")\n", + "# gaussian_isam.print()\n", + "graphviz.Source(gaussian_isam.dot()) # Visualize the BayesTree" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Relinearization and Reordering\n", + "\n", + "If `reorderInterval` is greater than 0, ISAM automatically triggers relinearization and reordering periodically. You can also trigger it manually." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Manually triggering relinearize...\n", + "Manual relinearization might be implicit or handled by ISAM2 patterns.\n", + "Typically happens during update() if counters trigger.\n", + "Reorder Counter: 0\n", + "Reorder Interval: 0\n" + ] + } + ], + "source": [ + "# Manually trigger relinearization (if needed, e.g., after large changes)\n", + "print(\"\\nManually triggering relinearize...\")\n", + "# This relinearizes *all* factors at the current estimate and rebuilds the Bayes tree\n", + "try:\n", + " # Note: Direct relinearize might not be exposed or might be handled differently\n", + " # in the nonlinear wrapper compared to ISAM2. Often done within update loop.\n", + " # hisam.reorderRelinearize() # Check if this method exists/works as expected\n", + " print(\"Manual relinearization might be implicit or handled by ISAM2 patterns.\")\n", + " print(\"Typically happens during update() if counters trigger.\")\n", + "except Exception as e:\n", + " print(f\"Could not call reorderRelinearize: {e}\")\n", + "\n", + "# Check counters\n", + "print(f\"Reorder Counter: {hisam.reorderCounter()}\")\n", + "print(f\"Reorder Interval: {hisam.reorderInterval()}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridSmoother.ipynb b/gtsam/hybrid/doc/HybridSmoother.ipynb new file mode 100644 index 0000000000..1f39edf72d --- /dev/null +++ b/gtsam/hybrid/doc/HybridSmoother.ipynb @@ -0,0 +1,408 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridSmoother" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridSmoother` implements an incremental fixed-lag smoother for hybrid systems. Unlike a full ISAM approach which keeps the entire history, a fixed-lag smoother marginalizes out older variables to maintain a bounded-size active window.\n", + "\n", + "It includes features for:\n", + "* Storing all factors (`HybridNonlinearFactorGraph`).\n", + "* Maintaining a linearization point (`Values`).\n", + "* Holding the current posterior (`HybridBayesNet`).\n", + "* Optionally removing \"dead modes\" based on a marginal probability threshold (`marginalThreshold_`).\n", + "* An `update` method to incorporate new factors.\n", + "* A `relinearize` method." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import (\n", + " HybridSmoother,\n", + " HybridNonlinearFactorGraph,\n", + " PriorFactorPose2, BetweenFactorPose2, Pose2, Point3,\n", + " DecisionTreeFactor, HybridNonlinearFactor,\n", + " Values\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Constructed optionally with a `marginalThreshold` for dead mode removal. It starts with an empty internal state." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initialized Smoother 1 (no threshold)\n", + "Initialized Smoother 2 (threshold=0.99)\n", + " Smoother 2 initial fixed values: DiscreteValues{}\n" + ] + } + ], + "source": [ + "# Initialize without dead mode removal\n", + "smoother1 = gtsam.HybridSmoother()\n", + "print(\"Initialized Smoother 1 (no threshold)\")\n", + "\n", + "# Initialize with dead mode removal threshold\n", + "threshold = 0.99\n", + "smoother2 = gtsam.HybridSmoother(marginalThreshold=threshold)\n", + "print(f\"Initialized Smoother 2 (threshold={threshold})\")\n", + "print(f\" Smoother 2 initial fixed values: {smoother2.fixedValues()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Update Steps\n", + "\n", + "The `update` method incorporates new nonlinear factors and initial estimates. It performs linearization, updates the internal `HybridBayesNet` posterior (by eliminating variables related to the new factors against the current posterior), and updates the linearization point. Pruning based on `maxNrLeaves` and `marginalThreshold` occurs during this step." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "--- Update 0 ---\n", + "Smoother state after update 0:\n", + " Lin Point Size: 1\n", + " Factors Size: 2\n", + " Posterior HybridBayesNet:\n", + "HybridBayesNet\n", + " \n", + "size: 1\n", + "conditional 0: p(x0)\n", + " R = [ 10 0 0 ]\n", + " [ 0 10 0 ]\n", + " [ 0 0 20 ]\n", + " d = [ 0 0 0 ]\n", + " mean: 1 elements\n", + " x0: 0 0 0\n", + " logNormalizationConstant: 4.84409\n", + " No noise model\n", + " Fixed Values: DiscreteValues{7205759403792793600: 0}\n", + "\n", + "--- Update 1 ---\n", + "Smoother state after update 1:\n", + " Lin Point Size: 2\n", + " Posterior HybridBayesNet:\n", + "HybridBayesNet\n", + " \n", + "size: 2\n", + "conditional 0: p(x0 | x1)\n", + " R = [ 14.1421 0 0 ]\n", + " [ 0 14.1421 7.07107 ]\n", + " [ 0 0 61.0967 ]\n", + " S[x1] = [ -7.07107 0 0 ]\n", + " [ 0 -7.07107 0 ]\n", + " [ 0 -0.818375 -53.7313 ]\n", + " d = [ 0 0 0 ]\n", + " logNormalizationConstant: 6.65396\n", + " No noise model\n", + "conditional 1: p(x1)\n", + " R = [ 7.07107 0 0 ]\n", + " [ 0 7.02355 -6.2607 ]\n", + " [ 0 0 18.8827 ]\n", + " d = [ 0 0 0 ]\n", + " mean: 1 elements\n", + " x1: 0 0 0\n", + " logNormalizationConstant: 4.08671\n", + " No noise model\n", + " Fixed Values: DiscreteValues{7205759403792793600: 0}\n" + ] + } + ], + "source": [ + "smoother = HybridSmoother(marginalThreshold=0.99)\n", + "\n", + "# --- Initial Step (Pose 0 and Mode Prior) ---\n", + "step0_graph = HybridNonlinearFactorGraph()\n", + "step0_values = Values()\n", + "dk0 = (D(0), 2)\n", + "\n", + "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n", + "step0_graph.add(PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise))\n", + "step0_values.insert(X(0), Pose2(0.0, 0.0, 0.0)) # Initial estimate for X0\n", + "\n", + "step0_graph.add(DecisionTreeFactor([dk0], \"0.995 0.005\")) # High prior on D0=0\n", + "\n", + "print(\"--- Update 0 ---\")\n", + "smoother.update(step0_graph, step0_values, maxNrLeaves=10)\n", + "print(\"Smoother state after update 0:\")\n", + "print(f\" Lin Point Size: {smoother.linearizationPoint().size()}\")\n", + "print(f\" Factors Size: {smoother.allFactors().size()}\")\n", + "print(\" Posterior HybridBayesNet:\")\n", + "smoother.hybridBayesNet().print()\n", + "print(f\" Fixed Values: {smoother.fixedValues()}\")\n", + "\n", + "\n", + "# --- Second Step (Pose 1 and Hybrid Odometry) ---\n", + "step1_graph = HybridNonlinearFactorGraph()\n", + "step1_values = Values()\n", + "\n", + "noise0 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, np.radians(1)))\n", + "odom0 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise0)\n", + "noise1 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.5, 0.5, np.radians(10)))\n", + "odom1 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise1)\n", + "hybrid_odom = HybridNonlinearFactor(dk0, [odom0, odom1])\n", + "step1_graph.add(hybrid_odom)\n", + "\n", + "x0_estimate = smoother.linearizationPoint().atPose2(X(0))\n", + "x1_initial_guess = x0_estimate.compose(Pose2(1.0, 0, 0))\n", + "\n", + "step1_values.insert(X(0), x0_estimate)\n", + "step1_values.insert(X(1), x1_initial_guess)\n", + "\n", + "print(\"\\n--- Update 1 ---\")\n", + "smoother.update(step1_graph, step1_values, maxNrLeaves=10)\n", + "print(\"Smoother state after update 1:\")\n", + "print(f\" Lin Point Size: {smoother.linearizationPoint().size()}\")\n", + "print(\" Posterior HybridBayesNet:\")\n", + "smoother.hybridBayesNet().print()\n", + "print(f\" Fixed Values: {smoother.fixedValues()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Accessing State and Optimization\n", + "\n", + "Allows accessing the current linearization point, the posterior `HybridBayesNet`, and optimizing the posterior." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Current Linearization Point:\n", + "Values with 2 values:\n", + "Value x0: (gtsam::Pose2)\n", + "(0, 0, 0)\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(1, 0, 0)\n", + "\n", + "\n", + "Current Posterior HybridBayesNet:\n", + "HybridBayesNet\n", + " \n", + "size: 2\n", + "conditional 0: p(x0 | x1)\n", + " R = [ 14.1421 0 0 ]\n", + " [ 0 14.1421 7.07107 ]\n", + " [ 0 0 61.0967 ]\n", + " S[x1] = [ -7.07107 0 0 ]\n", + " [ 0 -7.07107 0 ]\n", + " [ 0 -0.818375 -53.7313 ]\n", + " d = [ 0 0 0 ]\n", + " logNormalizationConstant: 6.65396\n", + " No noise model\n", + "conditional 1: p(x1)\n", + " R = [ 7.07107 0 0 ]\n", + " [ 0 7.02355 -6.2607 ]\n", + " [ 0 0 18.8827 ]\n", + " d = [ 0 0 0 ]\n", + " mean: 1 elements\n", + " x1: 0 0 0\n", + " logNormalizationConstant: 4.08671\n", + " No noise model\n", + "\n", + "Current Fixed Values: DiscreteValues{7205759403792793600: 0}\n", + "\n", + "MAP Solution from Smoother:\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x0: 0 0 0\n", + " x1: 0 0 0\n", + " Discrete: (d0, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n", + " Solution D(0): 0, Fixed D(0): 0\n" + ] + } + ], + "source": [ + "# Get the current linearization point\n", + "lin_point = smoother.linearizationPoint()\n", + "print(\"\\nCurrent Linearization Point:\")\n", + "lin_point.print()\n", + "\n", + "# Get the posterior Bayes Net\n", + "posterior_hbn = smoother.hybridBayesNet()\n", + "print(\"\\nCurrent Posterior HybridBayesNet:\")\n", + "posterior_hbn.print()\n", + "\n", + "# Get fixed values determined during updates\n", + "fixed_vals = smoother.fixedValues()\n", + "print(f\"\\nCurrent Fixed Values: {fixed_vals}\")\n", + "\n", + "# Optimize the current posterior Bayes Net (may implicitly use fixed_vals)\n", + "map_solution = smoother.optimize()\n", + "print(\"\\nMAP Solution from Smoother:\")\n", + "map_solution.print()\n", + "# Note: The solution should respect the fixed values.\n", + "if D(0) in fixed_vals:\n", + " print(f\" Solution D(0): {map_solution.discrete()[D(0)]}, Fixed D(0): {fixed_vals[D(0)]}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Relinearization\n", + "\n", + "The `relinearize` method rebuilds the posterior `HybridBayesNet` by relinearizing all stored factors (`allFactors_`) around the current linearization point." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Relinearizing...\n", + "Relinearization complete. Posterior HybridBayesNet:\n", + "HybridBayesNet\n", + " \n", + "size: 2\n", + "conditional 0: p(x1 | x0)\n", + " R = [ 10 0 0 ]\n", + " [ 0 10 0 ]\n", + " [ 0 0 57.2958 ]\n", + " S[x0] = [ -10 0 0 ]\n", + " [ 0 -10 -10 ]\n", + " [ 0 0 -57.2958 ]\n", + " d = [ 0 0 0 ]\n", + " logNormalizationConstant: 5.89658\n", + " No noise model\n", + "conditional 1: p(x0)\n", + " R = [ 10 0 0 ]\n", + " [ 0 10 0 ]\n", + " [ 0 0 20 ]\n", + " d = [ 0 0 0 ]\n", + " mean: 1 elements\n", + " x0: 0 0 0\n", + " logNormalizationConstant: 4.84409\n", + " No noise model\n", + "\n", + "MAP Solution after relinearization:\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x0: 0 0 0\n", + " x1: 0 0 0\n", + " Discrete: (d0, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n" + ] + } + ], + "source": [ + "print(\"\\nRelinearizing...\")\n", + "# This might be computationally expensive as it involves all factors\n", + "try:\n", + " smoother.relinearize()\n", + " print(\"Relinearization complete. Posterior HybridBayesNet:\")\n", + " smoother.hybridBayesNet().print()\n", + " # Optimize again after relinearization\n", + " map_solution_relinearized = smoother.optimize()\n", + " print(\"\\nMAP Solution after relinearization:\")\n", + " map_solution_relinearized.print()\n", + "except Exception as e:\n", + " print(f\"Relinearization failed: {e}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/doc/HybridValues.ipynb b/gtsam/hybrid/doc/HybridValues.ipynb new file mode 100644 index 0000000000..451df07213 --- /dev/null +++ b/gtsam/hybrid/doc/HybridValues.ipynb @@ -0,0 +1,349 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# HybridValues" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "`HybridValues` is a container class in GTSAM designed to hold results from hybrid inference. It stores three types of variable assignments simultaneously:\n", + "\n", + "1. **Continuous `VectorValues`**: Stores vector-valued assignments for continuous variables, typically used with Gaussian factors/conditionals (`gtsam.GaussianFactor`, `gtsam.GaussianConditional`).\n", + "2. **Discrete `DiscreteValues`**: Stores assignments (unsigned integers) for discrete variables (`gtsam.DiscreteKey`, `gtsam.DiscreteFactor`).\n", + "3. **Nonlinear `Values`**: Stores assignments for variables living on manifolds, used with nonlinear factors (`gtsam.NonlinearFactor`).\n", + "\n", + "It provides a unified way to represent the complete state (or solution) in a hybrid system." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "from gtsam import (\n", + " HybridValues, VectorValues, DiscreteValues, Values, Pose2\n", + ")\n", + "from gtsam.symbol_shorthand import X, D" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Initialization" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Empty HybridValues:\n", + "HybridValues: \n", + " Continuous: 0 elements\n", + " Discrete: \n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "HybridValues from VectorValues and DiscreteValues:\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x0: 1 2\n", + " x1: 3\n", + " Discrete: (d0, 1)(d1, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "HybridValues from all three types:\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x0: 1 2\n", + " x1: 3\n", + " Discrete: (d0, 1)(d1, 0)\n", + " Nonlinear\n", + "Values with 1 values:\n", + "Value x5: (class gtsam::Pose2)\n", + "(1, 2, 0.3)\n", + "\n" + ] + } + ], + "source": [ + "# 1. Default constructor (empty)\n", + "hybrid_values_empty = HybridValues()\n", + "print(\"Empty HybridValues:\")\n", + "hybrid_values_empty.print()\n", + "\n", + "# 2. From VectorValues and DiscreteValues\n", + "vec_vals = VectorValues()\n", + "vec_vals.insert(X(0), np.array([1.0, 2.0]))\n", + "vec_vals.insert(X(1), np.array([3.0]))\n", + "\n", + "disc_vals = DiscreteValues()\n", + "disc_vals[D(0)] = 1\n", + "disc_vals[D(1)] = 0\n", + "\n", + "hybrid_values_vd = HybridValues(vec_vals, disc_vals)\n", + "print(\"\\nHybridValues from VectorValues and DiscreteValues:\")\n", + "hybrid_values_vd.print()\n", + "\n", + "# 3. From VectorValues, DiscreteValues, and Nonlinear Values\n", + "nonlinear_vals = Values()\n", + "nonlinear_vals.insert(X(5), Pose2(1, 2, 0.3)) # Example nonlinear type\n", + "\n", + "hybrid_values_all = HybridValues(vec_vals, disc_vals, nonlinear_vals)\n", + "print(\"\\nHybridValues from all three types:\")\n", + "hybrid_values_all.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Accessing Values\n", + "\n", + "Methods are provided to access the underlying containers and check for key existence." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Accessed Continuous Values size: 2\n", + "Accessed Discrete Values size: 2\n", + "Accessed Nonlinear Values size: 1\n", + "\n", + "Exists Vector X(0)? True\n", + "Exists Discrete D(1)? True\n", + "Exists Nonlinear X(5)? True\n", + "Exists Vector D(0)? False\n", + "Exists X(0)? True\n", + "Exists D(0)? True\n", + "Exists X(5)? True\n", + "\n", + "Value at X(0): [1. 2.]\n", + "Value at D(0): 1\n", + "Value at X(5): (1, 2, 0.3)\n", + "\n" + ] + } + ], + "source": [ + "# Access underlying containers\n", + "cont_vals = hybrid_values_all.continuous()\n", + "disc_vals_acc = hybrid_values_all.discrete()\n", + "nonlin_vals_acc = hybrid_values_all.nonlinear()\n", + "\n", + "print(f\"\\nAccessed Continuous Values size: {cont_vals.size()}\")\n", + "print(f\"Accessed Discrete Values size: {len(disc_vals_acc)}\") # DiscreteValues acts like dict\n", + "print(f\"Accessed Nonlinear Values size: {nonlin_vals_acc.size()}\")\n", + "\n", + "# Check existence\n", + "print(f\"\\nExists Vector X(0)? {hybrid_values_all.existsVector(X(0))}\")\n", + "print(f\"Exists Discrete D(1)? {hybrid_values_all.existsDiscrete(D(1))}\")\n", + "print(f\"Exists Nonlinear X(5)? {hybrid_values_all.existsNonlinear(X(5))}\")\n", + "print(f\"Exists Vector D(0)? {hybrid_values_all.existsVector(D(0))}\") # False\n", + "\n", + "# exists() checks across all types (nonlinear checked first)\n", + "print(f\"Exists X(0)? {hybrid_values_all.exists(X(0))}\") # Checks VectorValues\n", + "print(f\"Exists D(0)? {hybrid_values_all.exists(D(0))}\") # Checks DiscreteValues\n", + "print(f\"Exists X(5)? {hybrid_values_all.exists(X(5))}\") # Checks Nonlinear Values\n", + "\n", + "# Access specific values\n", + "print(f\"\\nValue at X(0): {hybrid_values_all.at(X(0))}\")\n", + "print(f\"Value at D(0): {hybrid_values_all.discrete()[D(0)]}\")\n", + "print(f\"Value at X(5): {hybrid_values_all.nonlinear().atPose2(X(5))}\") # Use type-specific getter" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Modifying Values (Insert, Update, Retract)\n", + "\n", + "Values can be inserted individually or from other containers. `update` modifies existing keys, while `insert` adds new keys. `retract` applies a delta to the continuous VectorValues part." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "After individual inserts:\n", + "HybridValues: \n", + " Continuous: 1 elements\n", + " x10: 1\n", + " Discrete: (d10, 1)\n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "After container inserts:\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x10: 1\n", + " x12: 5\n", + " Discrete: (d10, 1)(d11, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "After update:\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x10: 99\n", + " x12: 5\n", + " Discrete: (d10, 2)(d11, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "After retract:\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x10: 99\n", + " x12: 5\n", + " Discrete: (d10, 2)(d11, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "Original (unchanged by retract):\n", + "HybridValues: \n", + " Continuous: 2 elements\n", + " x10: 99\n", + " x12: 5\n", + " Discrete: (d10, 2)(d11, 0)\n", + " Nonlinear\n", + "Values with 0 values:\n", + "\n", + "After insert_or_assign:\n", + "HybridValues: \n", + " Continuous: 3 elements\n", + " x10: 100\n", + " x12: 5\n", + " x13: 13\n", + " Discrete: (d10, 2)(d11, 0)(d12, 1)\n", + " Nonlinear\n", + "Values with 0 values:\n" + ] + } + ], + "source": [ + "hv = HybridValues()\n", + "\n", + "# Insert individual values\n", + "hv.insert(X(10), np.array([1.0])) # Vector value\n", + "hv.insert(D(10), 1) # Discrete value\n", + "\n", + "print(\"After individual inserts:\")\n", + "hv.print()\n", + "\n", + "# Insert from containers\n", + "new_vec = VectorValues()\n", + "new_vec.insert(X(12), np.array([5.0]))\n", + "new_disc = DiscreteValues()\n", + "new_disc[D(11)] = 0\n", + "\n", + "hv.insert(new_vec)\n", + "hv.insert(new_disc)\n", + "print(\"\\nAfter container inserts:\")\n", + "hv.print()\n", + "\n", + "# Update existing values\n", + "update_vec = VectorValues()\n", + "update_vec.insert(X(10), np.array([99.0]))\n", + "update_disc = DiscreteValues()\n", + "update_disc[D(10)] = 2\n", + "\n", + "hv.update(update_vec)\n", + "hv.update(update_disc)\n", + "print(\"\\nAfter update:\")\n", + "hv.print()\n", + "\n", + "# Retract (applies delta to VectorValues part)\n", + "delta = VectorValues()\n", + "delta.insert(X(10), np.array([-1.0]))\n", + "delta.insert(X(12), np.array([0.5]))\n", + "\n", + "hv_retracted = hv.retract(delta)\n", + "print(\"\\nAfter retract:\")\n", + "hv_retracted.print()\n", + "print(\"\\nOriginal (unchanged by retract):\")\n", + "hv.print() # Original is not modified\n", + "\n", + "# Insert or assign\n", + "# Replaces if exists, inserts if not.\n", + "hv.insert_or_assign(X(10), np.array([100.0])) # Overwrites X(10)\n", + "hv.insert_or_assign(D(12), 1) # Inserts D(12)\n", + "hv.insert_or_assign(X(13), np.array([13.0])) # Inserts X(13)\n", + "print(\"\\nAfter insert_or_assign:\")\n", + "hv.print()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index dbc9bf5fe0..d33b74347b 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -8,9 +8,12 @@ namespace gtsam { class HybridValues { gtsam::VectorValues continuous() const; gtsam::DiscreteValues discrete() const; + gtsam::Values& nonlinear() const; HybridValues(); HybridValues(const gtsam::VectorValues& cv, const gtsam::DiscreteValues& dv); + HybridValues(const gtsam::VectorValues& cv, const gtsam::DiscreteValues& dv, const gtsam::Values& v); + void print(string s = "HybridValues", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -25,16 +28,29 @@ class HybridValues { void insert(const gtsam::VectorValues& values); void insert(const gtsam::DiscreteValues& values); void insert(const gtsam::HybridValues& values); + void insert(const gtsam::Values& values); void update(const gtsam::VectorValues& values); void update(const gtsam::DiscreteValues& values); void update(const gtsam::HybridValues& values); + bool existsVector(gtsam::Key j); + bool existsDiscrete(gtsam::Key j); + bool existsNonlinear(gtsam::Key j); + bool exists(gtsam::Key j); + + gtsam::HybridValues retract(const gtsam::VectorValues& delta) const; + size_t& atDiscrete(gtsam::Key j); gtsam::Vector& at(gtsam::Key j); }; #include +class AlgebraicDecisionTreeKey { + void print(string s = "AlgebraicDecisionTreeKey\n", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; +}; virtual class HybridFactor : gtsam::Factor { void print(string s = "HybridFactor\n", const gtsam::KeyFormatter& keyFormatter = @@ -42,17 +58,30 @@ virtual class HybridFactor : gtsam::Factor { bool equals(const gtsam::HybridFactor& lf, double tol = 1e-9) const; // Standard interface: - double error(const gtsam::HybridValues& values) const; bool isDiscrete() const; bool isContinuous() const; bool isHybrid() const; size_t nrContinuous() const; gtsam::DiscreteKeys discreteKeys() const; gtsam::KeyVector continuousKeys() const; + double error(const gtsam::HybridValues& hybridValues) const; + gtsam::AlgebraicDecisionTreeKey errorTree(const gtsam::VectorValues &continuousValues); + gtsam::Factor restrict(const gtsam::DiscreteValues& assignment) const; }; #include -virtual class HybridConditional { +virtual class HybridConditional : gtsam::HybridFactor { + HybridConditional(); + HybridConditional(const gtsam::KeyVector& continuousKeys, + const gtsam::DiscreteKeys& discreteKeys, size_t nFrontals); + HybridConditional(const gtsam::KeyVector& continuousFrontals, + const gtsam::DiscreteKeys& discreteFrontals, + const gtsam::KeyVector& continuousParents, + const gtsam::DiscreteKeys& discreteParents); + HybridConditional(const gtsam::GaussianConditional::shared_ptr& continuousConditional); + HybridConditional(const gtsam::DiscreteConditional::shared_ptr& discreteConditional); + HybridConditional(const gtsam::HybridGaussianConditional::shared_ptr& hybridGaussianCond); + void print(string s = "Hybrid Conditional\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -65,11 +94,15 @@ virtual class HybridConditional { double logProbability(const gtsam::HybridValues& values) const; double evaluate(const gtsam::HybridValues& values) const; double operator()(const gtsam::HybridValues& values) const; + + bool isDiscrete() const; + bool isContinuous() const; + bool isHybrid() const; gtsam::HybridGaussianConditional* asHybrid() const; gtsam::GaussianConditional* asGaussian() const; gtsam::DiscreteConditional* asDiscrete() const; + gtsam::Factor* inner(); - double error(const gtsam::HybridValues& values) const; }; #include @@ -81,6 +114,8 @@ class HybridGaussianFactor : gtsam::HybridFactor { const gtsam::DiscreteKey& discreteKey, const std::vector>& factorPairs); + std::pair operator()( + const gtsam::DiscreteValues& assignment) const; void print(string s = "HybridGaussianFactor\n", const gtsam::KeyFormatter& keyFormatter = @@ -88,18 +123,41 @@ class HybridGaussianFactor : gtsam::HybridFactor { }; #include -class HybridGaussianConditional : gtsam::HybridFactor { +class HybridGaussianConditional : gtsam::HybridGaussianFactor { HybridGaussianConditional( const gtsam::DiscreteKeys& discreteParents, const gtsam::HybridGaussianConditional::Conditionals& conditionals); HybridGaussianConditional( const gtsam::DiscreteKey& discreteParent, const std::vector& conditionals); + HybridGaussianConditional( + const gtsam::DiscreteKey& discreteParent, size_t key, + const gtsam::Matrix& A, size_t parent, + const std::vector>& parameters); + HybridGaussianConditional( + const gtsam::DiscreteKey& discreteParent, size_t key, // + const gtsam::Matrix& A1, size_t parent1, const gtsam::Matrix& A2, + size_t parent2, + const std::vector>& parameters); + + // Standard API + gtsam::GaussianConditional::shared_ptr choose( + const gtsam::DiscreteValues &discreteValues) const; +// gtsam::GaussianConditional::shared_ptr operator()( +// const gtsam::DiscreteValues &discreteValues) const; + size_t nrComponents() const; + gtsam::KeyVector continuousParents() const; + double negLogConstant() const; gtsam::HybridGaussianFactor* likelihood( const gtsam::VectorValues& frontals) const; double logProbability(const gtsam::HybridValues& values) const; double evaluate(const gtsam::HybridValues& values) const; +// double operator()(const gtsam::HybridValues &values) const; + + HybridGaussianConditional::shared_ptr prune( + const gtsam::DiscreteConditional &discreteProbs) const; + bool pruned() const; void print(string s = "HybridGaussianConditional\n", const gtsam::KeyFormatter& keyFormatter = @@ -115,7 +173,7 @@ class HybridBayesTreeClique { // double evaluate(const gtsam::HybridValues& values) const; }; -class HybridBayesTree { +virtual class HybridBayesTree { HybridBayesTree(); void print(string s = "HybridBayesTree\n", const gtsam::KeyFormatter& keyFormatter = @@ -127,6 +185,11 @@ class HybridBayesTree { const HybridBayesTreeClique* operator[](size_t j) const; gtsam::HybridValues optimize() const; + gtsam::VectorValues optimize(const gtsam::DiscreteValues& assignment) const; + + gtsam::GaussianBayesTree choose(const gtsam::DiscreteValues& assignment) const; + double error(const gtsam::HybridValues& values) const; + gtsam::DiscreteValues mpe() const; string dot(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -138,6 +201,7 @@ class HybridBayesNet { void push_back(const gtsam::HybridGaussianConditional* s); void push_back(const gtsam::GaussianConditional* s); void push_back(const gtsam::DiscreteConditional* s); + void push_back(gtsam::HybridConditional::shared_ptr conditional); bool empty() const; size_t size() const; @@ -148,19 +212,32 @@ class HybridBayesNet { double logProbability(const gtsam::HybridValues& x) const; double evaluate(const gtsam::HybridValues& values) const; double error(const gtsam::HybridValues& values) const; + gtsam::AlgebraicDecisionTreeKey errorTree( + const gtsam::VectorValues& continuousValues) const; gtsam::HybridGaussianFactorGraph toFactorGraph( const gtsam::VectorValues& measurements) const; + gtsam::AlgebraicDecisionTreeKey discretePosterior( + const gtsam::VectorValues &continuousValues) const; gtsam::DiscreteBayesNet discreteMarginal() const; gtsam::GaussianBayesNet choose(const gtsam::DiscreteValues& assignment) const; + gtsam::DiscreteBayesNet discreteMarginal() const; + gtsam::DiscreteValues mpe() const; + gtsam::HybridValues optimize() const; gtsam::VectorValues optimize(const gtsam::DiscreteValues& assignment) const; gtsam::HybridValues sample(const gtsam::HybridValues& given, std::mt19937_64@ rng = nullptr) const; gtsam::HybridValues sample(std::mt19937_64@ rng = nullptr) const; + gtsam::HybridBayesNet prune(size_t maxNrLeaves) const; + gtsam::HybridBayesNet prune(size_t maxNrLeaves, double marginalThreshold) const; + // gtsam::HybridBayesNet prune(size_t maxNrLeaves, + // const std::optional &marginalThreshold = std::nullopt, + // gtsam::DiscreteValues *fixedValues = nullptr) const; + void print(string s = "HybridBayesNet\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -175,10 +252,12 @@ class HybridBayesNet { const gtsam::DotWriter& writer = gtsam::DotWriter()) const; }; -#include -class HybridGaussianFactorGraph { - HybridGaussianFactorGraph(); - HybridGaussianFactorGraph(const gtsam::HybridBayesNet& bayesNet); +#include +virtual class HybridFactorGraph { + HybridFactorGraph(); + gtsam::KeySet keys() const; + gtsam::KeySet discreteKeySet() const; + gtsam::KeySet continuousKeySet() const; // Building the graph void push_back(const gtsam::HybridFactor* factor); @@ -190,21 +269,60 @@ class HybridGaussianFactorGraph { void push_back(gtsam::DecisionTreeFactor* factor); void push_back(gtsam::TableFactor* factor); void push_back(gtsam::JacobianFactor* factor); - + void push_back(gtsam::NonlinearFactor* factor); + void push_back(gtsam::DiscreteFactor* factor); + void push_back(const gtsam::HybridNonlinearFactorGraph& graph); + + void add(const gtsam::HybridFactor* factor); + void add(const gtsam::HybridConditional* conditional); + void add(const gtsam::HybridGaussianFactorGraph& graph); + void add(const gtsam::HybridBayesNet& bayesNet); + void add(const gtsam::HybridBayesTree& bayesTree); + void add(const gtsam::HybridGaussianFactor* gmm); + void add(gtsam::DecisionTreeFactor* factor); + void add(gtsam::TableFactor* factor); + void add(gtsam::JacobianFactor* factor); + void add(gtsam::NonlinearFactor* factor); + void add(gtsam::DiscreteFactor* factor); + void add(const gtsam::HybridNonlinearFactorGraph& graph); + bool empty() const; void remove(size_t i); size_t size() const; - gtsam::KeySet keys() const; + void resize(size_t size); const gtsam::HybridFactor* at(size_t i) const; - void print(string s = "") const; + // evaluation + double error(const gtsam::HybridValues& values) const; + + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + string dot( + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, + const gtsam::DotWriter& writer = gtsam::DotWriter()) const; +}; + +#include +virtual class HybridGaussianFactorGraph : gtsam::HybridFactorGraph { + HybridGaussianFactorGraph(); + HybridGaussianFactorGraph(const gtsam::HybridBayesNet& bayesNet); + + // print added in base class bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const; - // evaluation - double error(const gtsam::HybridValues& values) const; + void printErrors(const gtsam::HybridValues& values, + string s = "HybridGaussianFactorGraph: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + + gtsam::AlgebraicDecisionTreeKey errorTree( + const gtsam::VectorValues& continuousValues) const; double probPrime(const gtsam::HybridValues& values) const; + gtsam::AlgebraicDecisionTreeKey discretePosterior( + const gtsam::VectorValues& continuousValues) const; + // Sequential Elimination gtsam::HybridBayesNet* eliminateSequential(); gtsam::HybridBayesNet* eliminateSequential( gtsam::Ordering::OrderingType type); @@ -212,6 +330,7 @@ class HybridGaussianFactorGraph { pair eliminatePartialSequential(const gtsam::Ordering& ordering); + // Multifrontal Elimination gtsam::HybridBayesTree* eliminateMultifrontal(); gtsam::HybridBayesTree* eliminateMultifrontal( gtsam::Ordering::OrderingType type); @@ -220,30 +339,31 @@ class HybridGaussianFactorGraph { pair eliminatePartialMultifrontal(const gtsam::Ordering& ordering); - string dot( - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const gtsam::DotWriter& writer = gtsam::DotWriter()) const; + gtsam::GaussianFactorGraph choose(const gtsam::DiscreteValues& assignment) const; + gtsam::GaussianFactorGraph operator()(const gtsam::DiscreteValues& assignment) const; + + gtsam::DiscreteFactorGraph discreteFactors() const; }; +const gtsam::Ordering HybridOrdering(const gtsam::HybridGaussianFactorGraph& graph); #include -class HybridNonlinearFactorGraph { +virtual class HybridNonlinearFactorGraph : gtsam::HybridFactorGraph { HybridNonlinearFactorGraph(); HybridNonlinearFactorGraph(const gtsam::HybridNonlinearFactorGraph& graph); - void push_back(gtsam::HybridFactor* factor); - void push_back(gtsam::NonlinearFactor* factor); - void push_back(gtsam::DiscreteFactor* factor); - void push_back(const gtsam::HybridNonlinearFactorGraph& graph); - // TODO(Varun) Wrap add() methods + + void printErrors(const gtsam::HybridValues& values, + string s = "HybridNonlinearFactorGraph: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + + gtsam::AlgebraicDecisionTreeKey errorTree(const gtsam::Values& continuousValues) const; gtsam::HybridGaussianFactorGraph linearize( const gtsam::Values& continuousValues) const; - bool empty() const; - void remove(size_t i); - size_t size() const; - void resize(size_t size); - gtsam::KeySet keys() const; - const gtsam::HybridFactor* at(size_t i) const; + gtsam::AlgebraicDecisionTreeKey discretePosterior( + const gtsam::Values& continuousValues) const; + gtsam::HybridNonlinearFactorGraph restrict( const gtsam::DiscreteValues& assignment) const; @@ -266,8 +386,10 @@ class HybridNonlinearFactor : gtsam::HybridFactor { const gtsam::DecisionTree< gtsam::Key, std::pair>& factors); + double error(const gtsam::HybridValues& hybridValues) const; double error(const gtsam::Values& continuousValues, - const gtsam::DiscreteValues& discreteValues) const; + const gtsam::DiscreteValues& assignment) const; + gtsam::AlgebraicDecisionTreeKey errorTree(const gtsam::Values &continuousValues); HybridGaussianFactor* linearize(const gtsam::Values& continuousValues) const; @@ -308,4 +430,57 @@ class HybridSmoother { gtsam::HybridValues optimize() const; }; +#include +virtual class HybridGaussianISAM : gtsam::HybridBayesTree { + HybridGaussianISAM(); + HybridGaussianISAM(const gtsam::HybridBayesTree& bayesTree); + void update(const gtsam::HybridGaussianFactorGraph& newFactors); + static gtsam::Ordering GetOrdering(gtsam::HybridGaussianFactorGraph& factors, + const gtsam::HybridGaussianFactorGraph& newFactors); +}; + +#include +class HybridNonlinearISAM { + HybridNonlinearISAM(int reorderInterval = 1); + + // Standard Interface + gtsam::Values estimate(); + const gtsam::HybridGaussianISAM& bayesTree() const; + void prune(const size_t maxNumberLeaves); + const gtsam::Values& getLinearizationPoint() const; + const gtsam::DiscreteValues& assignment() const; + int reorderInterval() const; + int reorderCounter() const; + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printStats() const; + void saveGraph(const std::string& s, + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void update(const gtsam::HybridNonlinearFactorGraph& newFactors, + const gtsam::Values& initialValues); + void reorderRelinearize(); +}; + +#include +class HybridEliminationTree { + HybridEliminationTree(const gtsam::HybridGaussianFactorGraph& factorGraph, + const gtsam::VariableIndex& structure, const gtsam::Ordering& order); + HybridEliminationTree(const gtsam::HybridGaussianFactorGraph& factorGraph, + const gtsam::Ordering& order); + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::HybridEliminationTree& other, double tol = 1e-9) const; +}; + +#include +class HybridJunctionTree { + HybridJunctionTree(const gtsam::HybridEliminationTree& eliminationTree); + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; +}; + } // namespace gtsam diff --git a/gtsam/hybrid/hybrid.md b/gtsam/hybrid/hybrid.md new file mode 100644 index 0000000000..8e576a646c --- /dev/null +++ b/gtsam/hybrid/hybrid.md @@ -0,0 +1,38 @@ +# Hybrid + +The `hybrid` module provides classes and algorithms designed for inference in probabilistic graphical models involving **both discrete and continuous variables**. It extends the core concepts from the `inference` module to handle these mixed variable types, enabling the modeling and solving of systems with discrete modes, decisions, or states alongside continuous states. + +## Core Hybrid Concepts + +- [HybridValues](doc/HybridValues.ipynb): Container holding assignments for discrete (`DiscreteValues`), continuous (`VectorValues`), and nonlinear (`Values`) variables simultaneously. +- [HybridFactor](doc/HybridFactor.ipynb): Abstract base class for factors involving discrete and/or continuous variables. +- [HybridConditional](doc/HybridConditional.ipynb): Type-erased wrapper for conditionals (Gaussian, Discrete, HybridGaussian) resulting from hybrid elimination. + +## Hybrid Factor Types + +- [HybridNonlinearFactor](doc/HybridNonlinearFactor.ipynb): Represents a factor where a discrete choice selects among different `NonlinearFactor` (NoiseModelFactor) components, potentially with associated scalar energies. +- [HybridGaussianFactor](doc/HybridGaussianFactor.ipynb): Represents a factor where a discrete choice selects among different Gaussian factor components, potentially with associated scalar energies. +- [HybridGaussianProductFactor](doc/HybridGaussianProductFactor.ipynb): Internal decision tree structure holding pairs of `GaussianFactorGraph` and `double` scalars, used during hybrid elimination. + +## Hybrid Factor Graphs + +- [HybridFactorGraph](doc/HybridFactorGraph.ipynb): Base class for factor graphs designed to hold hybrid factors. +- [HybridNonlinearFactorGraph](doc/HybridNonlinearFactorGraph.ipynb): A factor graph containing Nonlinear, Discrete, and/or `HybridNonlinearFactor` types, used to model nonlinear hybrid systems. +- [HybridGaussianFactorGraph](doc/HybridGaussianFactorGraph.ipynb): A factor graph containing Gaussian, Discrete, and/or `HybridGaussianFactor` types, typically resulting from linearization. Supports hybrid elimination. + +## Hybrid Bayes Nets and Bayes Trees + +- [HybridGaussianConditional](doc/HybridGaussianConditional.ipynb): Represents a conditional density $P(X | M, Z)$ where continuous variables $X$ depend on discrete parents $M$ and continuous parents $Z$, implemented as a decision tree of `GaussianConditional`s. +- [HybridBayesNet](doc/HybridBayesNet.ipynb): Represents the result of sequential variable elimination on a `HybridGaussianFactorGraph` as a DAG of `HybridConditional`s. +- [HybridBayesTree](doc/HybridBayesTree.ipynb): Represents the result of multifrontal variable elimination on a `HybridGaussianFactorGraph` as a tree of cliques, each containing a `HybridConditional`. + +## Incremental Hybrid Inference + +- [HybridGaussianISAM](doc/HybridGaussianISAM.ipynb): Incremental Smoothing and Mapping (ISAM) algorithm for `HybridGaussianFactorGraph`s, based on updating a `HybridBayesTree`. +- [HybridNonlinearISAM](doc/HybridNonlinearISAM.ipynb): Wrapper providing an ISAM interface for nonlinear hybrid problems, managing linearization and an underlying `HybridGaussianISAM`. +- [HybridSmoother](doc/HybridSmoother.ipynb): An incremental fixed-lag smoother interface for hybrid systems, managing updates to a `HybridBayesNet` posterior. + +## Hybrid Elimination Intermediates + +- [HybridEliminationTree](doc/HybridEliminationTree.ipynb): Tree structure representing the dependencies during sequential elimination of a `HybridGaussianFactorGraph`. +- [HybridJunctionTree](doc/HybridJunctionTree.ipynb): Intermediate cluster tree structure used in multifrontal elimination, holding original factors within cliques before elimination. diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 7381761148..ebf90c0b90 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include @@ -495,6 +496,50 @@ TEST(HybridBayesTree, Choose) { EXPECT(assert_equal(expected_gbt, gbt)); } +/* ************************************************************************* */ +TEST(HybridBayesTree, EliminateMultifrontal) { + // Create a HybridGaussianFactorGraph + HybridGaussianFactorGraph hgfg; + + // Gaussian Factor (prior on X0) + auto priorModel = noiseModel::Isotropic::Sigma(1, 1.0); + Matrix A = -Matrix::Identity(1, 1); + Vector b = Vector::Zero(1); + hgfg.emplace_shared(X(0), A, b, priorModel); + + // Discrete Factor (prior on D0) + DiscreteKey dk0(D(0), 2); + hgfg.emplace_shared(dk0, "0.7 0.3"); + + // Hybrid Gaussian Factor (measurement on X1 depends on D1) + DiscreteKey dk1(D(1), 2); + // Measurement model 0: N(X1; 0, 1) + auto meas0Model = noiseModel::Isotropic::Sigma(1, 1.0); + Matrix A1 = Matrix::Identity(1, 1); + Vector b1 = Vector::Zero(1); + std::vector components; + components.push_back(std::make_shared(X(1), A1, b1, meas0Model)); + // Measurement model 1: N(X1; 2, 0.5) + auto meas1Model = noiseModel::Isotropic::Sigma(1, 0.5); + Vector b2(1); b2 << 2.0; + components.push_back(std::make_shared(X(1), A1, b2, meas1Model)); + hgfg.emplace_shared(dk1, components); + + // Eliminate multifrontal and check number of cliques + auto hbt = hgfg.eliminateMultifrontal(); + EXPECT_LONGS_EQUAL(4, hbt->size()); + + std::string expected = + R"(digraph G{ +8646911284551352320[label="x0"]; +7205759403792793600[label="d0"]; +7205759403792793601[label="d1"]; +7205759403792793601->8646911284551352321 +8646911284551352321[label="x1 : d1"]; +})"; + EXPECT(assert_equal(expected, hbt->dot(DefaultKeyFormatter))); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 9642796ab1..981934244a 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -55,7 +56,7 @@ TEST(HybridGaussianFactor, Constructor) { } /* ************************************************************************* */ -namespace test_constructor { +namespace fixture { DiscreteKey m1(1, 2); auto A1 = Matrix::Zero(2, 1); @@ -64,12 +65,12 @@ auto b = Matrix::Zero(2, 1); auto f10 = std::make_shared(X(1), A1, X(2), A2, b); auto f11 = std::make_shared(X(1), A1, X(2), A2, b); -} // namespace test_constructor +} // namespace fixture /* ************************************************************************* */ // Test simple to complex constructors... TEST(HybridGaussianFactor, ConstructorVariants) { - using namespace test_constructor; + using namespace fixture; HybridGaussianFactor fromFactors(m1, {f10, f11}); std::vector pairs{{f10, 0.0}, {f11, 0.0}}; @@ -83,7 +84,7 @@ TEST(HybridGaussianFactor, ConstructorVariants) { /* ************************************************************************* */ TEST(HybridGaussianFactor, Keys) { - using namespace test_constructor; + using namespace fixture; HybridGaussianFactor hybridFactorA(m1, {f10, f11}); // Check the number of keys matches what we expect EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size()); @@ -105,7 +106,7 @@ TEST(HybridGaussianFactor, Keys) { /* ************************************************************************* */ TEST(HybridGaussianFactor, Printing) { - using namespace test_constructor; + using namespace fixture; HybridGaussianFactor hybridFactor(m1, {f10, f11}); std::string expected = @@ -191,9 +192,30 @@ TEST(HybridGaussianFactor, Error) { DiscreteValues discreteValues; discreteValues[m1.first] = 1; EXPECT_DOUBLES_EQUAL( - 4.0, hybridFactor.error({continuousValues, discreteValues}), 1e-9); + 4.0, hybridFactor.error({ continuousValues, discreteValues }), 1e-9); } +/* ************************************************************************* */ +// Test the restrict method of HybridGaussianFactor +TEST(HybridGaussianFactor, Restrict) { + using namespace fixture; + HybridGaussianFactor hfg(m1, {f10, f11}); + + // --- Test Case 1: Restrict by M1=0 --- + DiscreteValues assignment1; + assignment1[m1.first] = 0; + + auto f = hfg.restrict(assignment1); + auto restricted = std::dynamic_pointer_cast(f); + CHECK(restricted != nullptr); + + // Check discrete keys now empty + DiscreteKeys expected_dk1; + GTSAM_PRINT(restricted->discreteKeys()); + EXPECT(restricted->discreteKeys().empty()); +} + + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp index 9f8fb06382..6ef67dbe82 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp @@ -75,17 +75,17 @@ TEST(HybridGaussianFactor, ConstructorVariants) { // Test .print() output. TEST(HybridNonlinearFactor, Printing) { using namespace test_constructor; - HybridNonlinearFactor hybridFactor({m1}, {f0, f1}); + HybridNonlinearFactor hybridFactor({m1}, {{f0,22}, {f1,33}}); std::string expected = R"(Hybrid [x1 x2; 1] HybridNonlinearFactor Choice(1) - 0 Leaf BetweenFactor(x1,x2) + 0 Leaf (22) BetweenFactor(x1,x2) measured: 0 noise model: diagonal sigmas [1]; - 1 Leaf BetweenFactor(x1,x2) + 1 Leaf (33) BetweenFactor(x1,x2) measured: 1 noise model: diagonal sigmas [1]; diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index f97972e5ae..fc6a407b01 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -69,7 +69,10 @@ namespace gtsam { throw std::invalid_argument( "the root of Bayes tree has not been initialized!"); os << "digraph G{\n"; - for (const sharedClique& root : roots_) dot(os, root, keyFormatter); + for (const sharedClique& root : roots_) { + size_t key = root->conditional()->firstFrontalKey(); + dot(os, root, keyFormatter, key); + } os << "}"; std::flush(os); } @@ -95,8 +98,8 @@ namespace gtsam { template void BayesTree::dot(std::ostream& s, sharedClique clique, const KeyFormatter& keyFormatter, - int parentnum) const { - static int num = 0; + size_t parentnum) const { + size_t num = clique->conditional()->firstFrontalKey(); bool first = true; std::stringstream out; out << num; @@ -122,11 +125,9 @@ namespace gtsam { } parent += "\"];\n"; s << parent; - parentnum = num; for (sharedClique c : clique->children) { - num++; - dot(s, c, keyFormatter, parentnum); + dot(s, c, keyFormatter, num); } } diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 4a2ae7560f..0d2125e37f 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -249,7 +249,7 @@ namespace gtsam { /** private helper method for saving the Tree to a text file in GraphViz format */ void dot(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter, - int parentnum = 0) const; + size_t parentnum = 0) const; /** Gather data on a single clique */ void getCliqueData(sharedClique clique, BayesTreeCliqueData* stats) const; diff --git a/gtsam/inference/Factor.cpp b/gtsam/inference/Factor.cpp index 2590d7b59a..95d6522629 100644 --- a/gtsam/inference/Factor.cpp +++ b/gtsam/inference/Factor.cpp @@ -44,7 +44,7 @@ namespace gtsam { } /* ************************************************************************* */ - double Factor::error(const HybridValues& c) const { + double Factor::error(const HybridValues& hybridValues) const { throw std::runtime_error("Factor::error is not implemented"); } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 19741b22e4..378d8244d1 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -152,7 +152,7 @@ namespace gtsam { * All factor types need to implement an error function. * In factor graphs, this is the negative log-likelihood. */ - virtual double error(const HybridValues& c) const; + virtual double error(const HybridValues& hybridValues) const; /** * @return the number of variables involved in this factor diff --git a/myst.yml b/myst.yml index f0b62c88fb..7b36c0a359 100644 --- a/myst.yml +++ b/myst.yml @@ -16,6 +16,9 @@ project: - file: ./gtsam/inference/inference.md children: - pattern: ./gtsam/inference/doc/* + - file: ./gtsam/hybrid/hybrid.md + children: + - pattern: ./gtsam/hybrid/doc/* - file: ./gtsam/nonlinear/nonlinear.md children: - pattern: ./gtsam/nonlinear/doc/* diff --git a/python/gtsam/specializations/discrete.h b/python/gtsam/specializations/discrete.h index 458a2ea4c0..bc41756339 100644 --- a/python/gtsam/specializations/discrete.h +++ b/python/gtsam/specializations/discrete.h @@ -12,6 +12,21 @@ */ // Seems this is not a good idea with inherited stl -//py::bind_vector>(m_, "DiscreteKeys"); +// py::bind_vector>(m_, "DiscreteKeys"); py::bind_map(m_, "DiscreteValues"); + +#include +/// DiscreteValues print function for wrapper +m_.def( + "PrintDiscreteValues", + [](const gtsam::DiscreteValues& values, const std::string& s, + const gtsam::KeyFormatter& keyFormatter) { + py::print(s + ": "); + for (const auto& kv : values) { + py::print("(" + keyFormatter(kv.first) + ", " + + std::to_string(kv.second) + ")"); + } + }, + py::arg("values"), py::arg("s") = "", + py::arg("keyFormatter") = gtsam::DefaultKeyFormatter);