From bf885764c75d07000e797d74b0d8a2cf233eebea Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 10 Apr 2025 09:10:40 -0400 Subject: [PATCH 01/24] Initial Gemini 2.5 output --- gtsam/inference/doc/BayesNet.ipynb | 288 ++++++++++++++++++ gtsam/inference/doc/BayesTree.ipynb | 340 ++++++++++++++++++++++ gtsam/inference/doc/ClusterTree.ipynb | 168 +++++++++++ gtsam/inference/doc/Conditional.ipynb | 206 +++++++++++++ gtsam/inference/doc/DotWriter.ipynb | 289 ++++++++++++++++++ gtsam/inference/doc/EdgeKey.ipynb | 154 ++++++++++ gtsam/inference/doc/EliminationTree.ipynb | 198 +++++++++++++ gtsam/inference/doc/Factor.ipynb | 178 +++++++++++ gtsam/inference/doc/FactorGraph.ipynb | 298 +++++++++++++++++++ gtsam/inference/doc/ISAM.ipynb | 276 ++++++++++++++++++ gtsam/inference/doc/JunctionTree.ipynb | 160 ++++++++++ gtsam/inference/doc/Key.ipynb | 202 +++++++++++++ gtsam/inference/doc/LabeledSymbol.ipynb | 200 +++++++++++++ gtsam/inference/doc/Ordering.ipynb | 291 ++++++++++++++++++ gtsam/inference/doc/Symbol.ipynb | 199 +++++++++++++ gtsam/inference/doc/VariableIndex.ipynb | 233 +++++++++++++++ 16 files changed, 3680 insertions(+) create mode 100644 gtsam/inference/doc/BayesNet.ipynb create mode 100644 gtsam/inference/doc/BayesTree.ipynb create mode 100644 gtsam/inference/doc/ClusterTree.ipynb create mode 100644 gtsam/inference/doc/Conditional.ipynb create mode 100644 gtsam/inference/doc/DotWriter.ipynb create mode 100644 gtsam/inference/doc/EdgeKey.ipynb create mode 100644 gtsam/inference/doc/EliminationTree.ipynb create mode 100644 gtsam/inference/doc/Factor.ipynb create mode 100644 gtsam/inference/doc/FactorGraph.ipynb create mode 100644 gtsam/inference/doc/ISAM.ipynb create mode 100644 gtsam/inference/doc/JunctionTree.ipynb create mode 100644 gtsam/inference/doc/Key.ipynb create mode 100644 gtsam/inference/doc/LabeledSymbol.ipynb create mode 100644 gtsam/inference/doc/Ordering.ipynb create mode 100644 gtsam/inference/doc/Symbol.ipynb create mode 100644 gtsam/inference/doc/VariableIndex.ipynb diff --git a/gtsam/inference/doc/BayesNet.ipynb b/gtsam/inference/doc/BayesNet.ipynb new file mode 100644 index 000000000..e58cbb2a9 --- /dev/null +++ b/gtsam/inference/doc/BayesNet.ipynb @@ -0,0 +1,288 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_intro_md" + }, + "source": [ + "# BayesNet" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_desc_md" + }, + "source": [ + "A `BayesNet` in GTSAM represents a directed graphical model, specifically the result of running sequential variable elimination (like Cholesky or QR factorization) on a `FactorGraph`.\n", + "\n", + "It is essentially a collection of `Conditional` objects, ordered according to the elimination order. Each conditional represents $P(\text{variable} | \text{parents})$, where the parents are variables that appear later in the elimination ordering.\n", + "\n", + "Like `FactorGraph`, `BayesNet` is templated on the type of conditional it stores (e.g., `GaussianBayesNet`, `DiscreteBayesNet`)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayesnet_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayesnet_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# We need concrete graph types and elimination to get a BayesNet\n", + "from gtsam import GaussianFactorGraph, Ordering, GaussianBayesNet\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_create_md" + }, + "source": [ + "## Creating a BayesNet (via Elimination)\n", + "\n", + "BayesNets are typically obtained by eliminating a `FactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayesnet_eliminate_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "01234567-89ab-cdef-0123-456789abcdef" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original Factor Graph: size 3\n", + "Factor 0: JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "Factor 1: JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "Factor 2: JacobianFactor(keys = [8070450532247928833; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "\n", + "\n", + "Resulting BayesNet: size 3\n", + "Conditional 0: GaussianConditional( P(x0 | x1) = dx0 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.707107 ])\n", + "Conditional 1: GaussianConditional( P(x1 | x2) = dx1 - R*dx2 - d), R = [ 0.666667 ], d = [ 0 ], sigmas = [ 0.816497 ])\n", + "Conditional 2: GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.866025 ])\n", + "\n" + ] + } + ], + "source": [ + "# Create a simple Gaussian Factor Graph P(x0) P(x1|x0) P(x2|x1)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "print(\"Original Factor Graph:\")\n", + "graph.print()\n", + "\n", + "# Eliminate sequentially using a specific ordering\n", + "ordering = Ordering([X(0), X(1), X(2)])\n", + "bayes_net = graph.eliminateSequential(ordering)\n", + "\n", + "print(\"\\nResulting BayesNet:\")\n", + "bayes_net.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_props_md" + }, + "source": [ + "## Properties and Access\n", + "\n", + "A `BayesNet` provides access to its constituent conditionals and basic properties." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayesnet_access_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "12345678-9abc-def0-1234-56789abcdef0" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "BayesNet size: 3\n", + "Is BayesNet empty? False\n", + "Conditional at index 1: \n", + "GaussianConditional( P(x1 | x2) = dx1 - R*dx2 - d), R = [ 0.666667 ], d = [ 0 ], sigmas = [ 0.816497 ])\n", + "\n", + "Keys in BayesNet: {8070450532247928832, 8070450532247928833, 8070450532247928834}\n" + ] + } + ], + "source": [ + "print(f\"BayesNet size: {bayes_net.size()}\")\n", + "print(f\"Is BayesNet empty? {bayes_net.empty()}\")\n", + "\n", + "# Access conditional by index\n", + "conditional1 = bayes_net.at(1)\n", + "print(\"Conditional at index 1: \")\n", + "conditional1.print()\n", + "\n", + "# Get all keys involved\n", + "bn_keys = bayes_net.keys()\n", + "print(f\"Keys in BayesNet: {bn_keys}\")\n", + "\n", + "# Iterate through conditionals\n", + "# for i, conditional in enumerate(bayes_net):\n", + "# if conditional:\n", + "# print(f\"Conditional {i} Frontals: {conditional.frontals()}, Parents: {conditional.parents()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_eval_md" + }, + "source": [ + "## Evaluation and Solution\n", + "\n", + "The `logProbability(Values)` method computes the log probability of a variable assignment given the conditional distributions in the Bayes net. For Gaussian Bayes nets, the `optimize()` method can be used to find the maximum likelihood estimate (MLE) solution via back-substitution." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayesnet_eval_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "23456789-abcd-ef01-2345-6789abcdef01" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Log Probability at [0,0,0]: -2.756815765004782\n", + "Optimized Solution (MLE):\n", + "Values with 3 values:\n", + "Value x0: [0.]\n", + "Value x1: [0.]\n", + "Value x2: [0.]\n", + "\n" + ] + } + ], + "source": [ + "# For GaussianBayesNet, we use VectorValues\n", + "mle_solution = bayes_net.optimize()\n", + "\n", + "# Calculate log probability (requires providing values for all variables)\n", + "log_prob = bayes_net.logProbability(mle_solution)\n", + "print(f\"Log Probability at {mle_solution.at(X(0))[0]:.0f},{mle_solution.at(X(1))[0]:.0f},{mle_solution.at(X(2))[0]:.0f}]: {log_prob}\")\n", + "\n", + "print(\"Optimized Solution (MLE):\")\n", + "mle_solution.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_viz_md" + }, + "source": [ + "## Visualization\n", + "\n", + "Bayes nets can also be visualized using Graphviz." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayesnet_dot_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "3456789a-bcde-f012-3456-789abcdef012" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "digraph {\n", + " size=\"5,5\";\n", + "\n", + " var8070450532247928832[label=\"x0\"];\n", + " var8070450532247928833[label=\"x1\"];\n", + " var8070450532247928834[label=\"x2\"];\n", + "\n", + " var8070450532247928834->var8070450532247928833\n", + " var8070450532247928833->var8070450532247928832\n", + "}\n" + ] + } + ], + "source": [ + "dot_string = bayes_net.dot()\n", + "print(dot_string)\n", + "\n", + "# To render:\n", + "# dot -Tpng bayesnet.dot -o bayesnet.png\n", + "# import graphviz\n", + "# graphviz.Source(dot_string)" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb new file mode 100644 index 000000000..88a8cbd45 --- /dev/null +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -0,0 +1,340 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_intro_md" + }, + "source": [ + "# BayesTree" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_desc_md" + }, + "source": [ + "A `BayesTree` is a graphical model that represents the result of multifrontal variable elimination on a `FactorGraph`. It is a tree structure where each node is a 'clique' containing a set of conditional distributions $P(\text{Frontals} | \text{Separator})$.\n", + "\n", + "Key properties:\n", + "* **Cliques:** Each node (clique) groups variables that are eliminated together.\n", + "* **Frontal Variables:** Variables eliminated within a specific clique.\n", + "* **Separator Variables:** Variables shared between a clique and its parent in the tree. These variables were eliminated higher up in the tree.\n", + "* **Tree Structure:** Represents the dependencies introduced during elimination more compactly than a Bayes net, especially for sparse problems.\n", + "\n", + "Like `FactorGraph` and `BayesNet`, `BayesTree` is templated on the type of conditional/clique (e.g., `GaussianBayesTree`)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayestree_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayestree_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# We need concrete graph types and elimination to get a BayesTree\n", + "from gtsam import GaussianFactorGraph, Ordering, GaussianBayesTree\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_create_md" + }, + "source": [ + "## Creating a BayesTree (via Elimination)\n", + "\n", + "BayesTrees are typically obtained by performing multifrontal elimination on a `FactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayestree_eliminate_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "456789ab-cdef-0123-4567-89abcdef0123" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original Factor Graph: size 7\n", + "Factor 0: JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "Factor 1: JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "Factor 2: JacobianFactor(keys = [8070450532247928833; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "Factor 3: JacobianFactor(keys = [7783684379976990720; 8070450532247928832], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "Factor 4: JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "Factor 5: JacobianFactor(keys = [7783684379976990721; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "Factor 6: JacobianFactor(keys = [7783684379976990721; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "\n", + "\n", + "Resulting BayesTree: cliques: 3, variables: 5\n", + "Root(s):\n", + "Conditional density P(l1, x1 | l2, x2) = P(l1 | x1) P(x1 | l2, x2) \n", + " size: 2\n", + " Conditional P(l1 | x1): GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "\n", + " Conditional P(x1 | l2, x2): GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", + "\n", + "\n", + " Children:\n", + " Conditional density P(l2, x2 | x0) = P(l2 | x2) P(x2 | x0) \n", + " size: 2\n", + " Conditional P(l2 | x2): GaussianConditional( P(l2 | x2) = dl2 - R*dx2 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "\n", + " Conditional P(x2 | x0): GaussianConditional( P(x2 | x0) = dx2 - R*dx0 - d), R = [ 0.2 ], d = [ 0 ], sigmas = [ 0.774597 ])\n", + "\n", + "\n", + " Children:\n", + " Conditional density P(x0) = P(x0) \n", + " size: 1\n", + " Conditional P(x0): GaussianConditional( P(x0) = dx0 - d), d = [ 0 ], sigmas = [ 0.894427 ])\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# Create a simple Gaussian Factor Graph (more complex this time)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # x0 -> x1\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # x1 -> x2\n", + "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model) # l1 -> x0 (measurement)\n", + "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l1 -> x1 (measurement)\n", + "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l2 -> x1 (measurement)\n", + "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # l2 -> x2 (measurement)\n", + "\n", + "print(\"Original Factor Graph:\")\n", + "graph.print()\n", + "\n", + "# Eliminate multifrontally using COLAMD ordering\n", + "ordering = Ordering.Colamd(graph)\n", + "# Note: Multifrontal typically yields multiple roots if graph is disconnected\n", + "bayes_tree = graph.eliminateMultifrontal(ordering)\n", + "\n", + "print(\"\\nResulting BayesTree:\")\n", + "bayes_tree.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_props_md" + }, + "source": [ + "## Properties and Access\n", + "\n", + "A `BayesTree` allows access to its root cliques and provides a way to look up the clique containing a specific variable." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayestree_access_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "56789abc-def0-1234-5678-9abcdef01234" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "BayesTree number of cliques: 3\n", + "Number of roots: 1\n", + "Root clique 0 conditional frontals: [7783684379976990720, 8070450532247928833]\n", + "\n", + "Clique containing x1 (8070450532247928833):\n", + "Conditional density P(l1, x1 | l2, x2) = P(l1 | x1) P(x1 | l2, x2) \n", + " size: 2\n", + " Conditional P(l1 | x1): GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "\n", + " Conditional P(x1 | l2, x2): GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", + "\n", + "\n" + ] + } + ], + "source": [ + "print(f\"BayesTree number of cliques: {bayes_tree.size()}\")\n", + "\n", + "# Access roots\n", + "roots = bayes_tree.roots()\n", + "print(f\"Number of roots: {len(roots)}\")\n", + "if roots:\n", + " # Access the conditional associated with the first root clique\n", + " root_conditional = roots[0].conditional()\n", + " print(f\"Root clique 0 conditional frontals: {root_conditional.frontals()}\")\n", + "\n", + "# Find the clique containing a specific variable\n", + "clique_x1 = bayes_tree.clique(X(1))\n", + "# clique_x1 is a shared pointer to the clique (e.g., GaussianBayesTreeClique)\n", + "print(f\"\\nClique containing x1 ({X(1)}):\")\n", + "clique_x1.print() # Print the clique itself" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_solve_md" + }, + "source": [ + "## Solution and Marginals\n", + "\n", + "Similar to `BayesNet`, `BayesTree` (specifically derived types like `GaussianBayesTree`) provides an `optimize()` method for finding the MLE solution. It also allows for efficient computation of marginals on individual variables or joint marginals on pairs of variables using belief propagation or shortcut evaluation on the tree." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayestree_solve_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "6789abcd-ef01-2345-6789-abcdef012345" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Optimized Solution (MLE):\n", + "Values with 5 values:\n", + "Value l1: [0.]\n", + "Value l2: [0.]\n", + "Value x0: [0.]\n", + "Value x1: [0.]\n", + "Value x2: [0.]\n", + "\n", + "\n", + "Marginal Factor on x1:\n", + "Conditional density P(x1 | l2, x2) = P(x1 | l2, x2) \n", + " size: 1\n", + " Conditional P(x1 | l2, x2): GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", + "\n", + "\n", + "Joint Marginal Factor Graph on (x0, x2):\n", + "Factor Graph: size 2\n", + "Factor 0: GaussianConditional( P(x0 | x2) = dx0 - R*dx2 - d), R = [ 0.25 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "Factor 1: GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.774597 ])\n", + "\n" + ] + } + ], + "source": [ + "# Optimize to find the MLE solution (for GaussianBayesTree)\n", + "mle_solution = bayes_tree.optimize()\n", + "print(\"Optimized Solution (MLE):\")\n", + "mle_solution.print()\n", + "\n", + "# Compute marginal factor on a single variable (returns a Conditional)\n", + "marginal_x1 = bayes_tree.marginalFactor(X(1))\n", + "print(\"\\nMarginal Factor on x1:\")\n", + "marginal_x1.print()\n", + "\n", + "# Compute joint marginal factor graph on two variables\n", + "joint_x0_x2 = bayes_tree.joint(X(0), X(2))\n", + "print(\"\\nJoint Marginal Factor Graph on (x0, x2):\")\n", + "joint_x0_x2.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_viz_md" + }, + "source": [ + "## Visualization\n", + "\n", + "Bayes trees can be visualized using Graphviz." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayestree_dot_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "789abcde-f012-3456-789a-bcdef0123456" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "digraph G{\n", + "0[label=\"l1, x1 : l2, x2\"];\n", + "0->1\n", + "1[label=\"l2, x2 : x0\"];\n", + "1->2\n", + "2[label=\"x0 : \"];\n", + "}" + ] + } + ], + "source": [ + "dot_string = bayes_tree.dot()\n", + "print(dot_string)\n", + "\n", + "# To render:\n", + "# dot -Tpng bayestree.dot -o bayestree.png\n", + "# import graphviz\n", + "# graphviz.Source(dot_string)" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/ClusterTree.ipynb b/gtsam/inference/doc/ClusterTree.ipynb new file mode 100644 index 000000000..d637aad55 --- /dev/null +++ b/gtsam/inference/doc/ClusterTree.ipynb @@ -0,0 +1,168 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "clustertree_intro_md" + }, + "source": [ + "# ClusterTree" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "clustertree_desc_md" + }, + "source": [ + "A `ClusterTree` is a more general structure than an `EliminationTree` or `JunctionTree`. It represents a tree where each node (a 'Cluster') contains a subset of factors from an original factor graph. The key property is that the tree must be 'family preserving', meaning each original factor must belong entirely within a single cluster.\n", + "\n", + "`ClusterTree` itself is a base class. `EliminatableClusterTree` adds the ability to perform elimination, and `JunctionTree` is a specific type of `EliminatableClusterTree` derived from an `EliminationTree`.\n", + "\n", + "Direct use of `ClusterTree` in typical Python applications is less common than `JunctionTree` or `BayesTree`, as it's often an intermediate representation." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "clustertree_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "clustertree_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "clustertree_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "# Note: ClusterTree itself might not be directly exposed or used.\n", + "# We typically interact with JunctionTree or BayesTree which build upon it.\n", + "# We'll demonstrate concepts using JunctionTree which inherits Cluster features.\n", + "from gtsam import GaussianFactorGraph, Ordering, JunctionTree, GaussianBayesTree\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "clustertree_concept_md" + }, + "source": [ + "## Concept and Relation to JunctionTree\n", + "\n", + "A `JunctionTree` *is a* `ClusterTree` (specifically, an `EliminatableClusterTree`). It's constructed during multifrontal elimination. Each node in the `JunctionTree` is a `Cluster` containing factors that will be eliminated together to form a clique in the resulting `BayesTree`.\n", + "\n", + "We will create a `JunctionTree` and examine its properties, which include those inherited from `ClusterTree`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "clustertree_jt_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "ef012345-6789-abcd-ef01-23456789abcd" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Junction Tree (as ClusterTree): \n", + "Root(s):\n", + "Cluster (1) keys: l1 x1 \n", + " Factor 0: JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Cluster (1) keys: l2 x2 \n", + " Factor 0: JacobianFactor(keys = [7783684379976990721; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Cluster (1) keys: x0 \n", + " Factor 0: JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "\n", + "Accessing a root cluster (node):\n", + "Cluster (1) keys: l1 x1 \n", + " Factor 0: JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "\n", + "Number of roots: 1\n" + ] + } + ], + "source": [ + "# Create a graph (same as BayesTree example)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "\n", + "ordering = Ordering.Colamd(graph)\n", + "\n", + "# Create a Junction Tree (implicitly uses ClusterTree structure)\n", + "# Note: JunctionTree constructor might not be directly exposed.\n", + "# It's usually an intermediate in eliminateMultifrontal.\n", + "# We might need to construct it indirectly or focus on BayesTree access.\n", + "\n", + "# Let's get the BayesTree first, as JunctionTree creation is internal.\n", + "bayes_tree = graph.eliminateMultifrontal(ordering)\n", + "\n", + "# We can print the BayesTree, which shows the cluster structure\n", + "# (Cliques in BayesTree correspond to Clusters in JunctionTree)\n", + "print(\"Junction Tree (as ClusterTree): \")\n", + "bayes_tree.print() # Printing BayesTree shows clique structure\n", + "\n", + "# Access root cluster(s)\n", + "roots = bayes_tree.roots()\n", + "if roots:\n", + " print(\"\\nAccessing a root cluster (node):\")\n", + " root_clique = roots[0]\n", + " # In the JunctionTree, this node would contain the factors that *produced*\n", + " # the conditional in the BayesTree clique. We can see the involved keys.\n", + " root_clique.print(\"\", gtsam.DefaultKeyFormatter) # Print clique details\n", + "\n", + "print(f\"\\nNumber of roots: {len(roots)}\")\n", + "\n", + "# Direct instantiation or manipulation of Cluster/JunctionTree nodes\n", + "# is less common in Python than using the results (BayesTree)." + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/Conditional.ipynb b/gtsam/inference/doc/Conditional.ipynb new file mode 100644 index 000000000..022d1b7b0 --- /dev/null +++ b/gtsam/inference/doc/Conditional.ipynb @@ -0,0 +1,206 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_intro_md" + }, + "source": [ + "# Conditional" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_desc_md" + }, + "source": [ + "`gtsam.Conditional` is the base class for conditional probability distributions or densities that result from variable elimination. Conditionals are essentially specialized factors representing $P(\text{Frontals} | \text{Parents})$.\n", + "\n", + "Like `gtsam.Factor`, you typically don't instantiate `gtsam.Conditional` directly. Instead, you work with derived classes obtained from elimination, such as:\n", + "* `gtsam.GaussianConditional`\n", + "* `gtsam.DiscreteConditional`\n", + "* `gtsam.HybridGaussianConditional`\n", + "* `gtsam.SymbolicConditional`\n", + "\n", + "This notebook demonstrates the common interface provided by the base class." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "conditional_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "conditional_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# We need concrete graph types and elimination to get a Conditional\n", + "from gtsam import GaussianFactorGraph, Ordering\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_example_md" + }, + "source": [ + "## Example: Obtaining and Inspecting a Conditional\n", + "\n", + "We'll create a simple `GaussianFactorGraph` and eliminate one variable to get a `GaussianConditional`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "conditional_create_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "def01234-5678-9abc-def0-123456789abc" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Eliminating x0 from graph:\n", + "Factor Graph: size 2\n", + "Factor 0: JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "Factor 1: JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "\n", + "\n", + "Resulting BayesNet: size 1\n", + "Conditional 0: GaussianConditional( P(x0 | x1) = dx0 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.707107 ])\n", + "\n", + "\n", + "Conditional Keys (all): [8070450532247928832, 8070450532247928833]\n", + "Number of Frontals: 1\n", + "Frontal Keys: [8070450532247928832] (x0)\n", + "First Frontal Key: 8070450532247928832 (x0)\n", + "Number of Parents: 1\n", + "Parent Keys: [8070450532247928833] (x1)\n" + ] + } + ], + "source": [ + "# Create a simple Gaussian Factor Graph P(x0) P(x1|x0)\n", + "graph = GaussianFactorGraph()\n", + "model1 = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "model2 = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "\n", + "# Prior on x0\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model1)\n", + "# Factor between x0 and x1\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model2)\n", + "\n", + "print(\"Eliminating x0 from graph:\")\n", + "graph.print()\n", + "\n", + "# Eliminate x0\n", + "ordering = Ordering([X(0)])\n", + "bayes_net, remaining_graph = graph.eliminatePartialSequential(ordering)\n", + "\n", + "print(\"\\nResulting BayesNet:\")\n", + "bayes_net.print()\n", + "\n", + "# Get the resulting conditional P(x0 | x1)\n", + "# In this case, it's a GaussianConditional\n", + "conditional = bayes_net.at(0) # or bayes_net[0]\n", + "\n", + "# Access methods from the Conditional base class\n", + "print(f\"Conditional Keys (all): {conditional.keys()}\")\n", + "print(f\"Number of Frontals: {conditional.nrFrontals()}\")\n", + "print(f\"Frontal Keys: {conditional.frontals()} ({gtsam.DefaultKeyFormatter(list(conditional.frontals())[0])})\")\n", + "print(f\"First Frontal Key: {conditional.firstFrontalKey()} ({gtsam.DefaultKeyFormatter(conditional.firstFrontalKey())})\")\n", + "print(f\"Number of Parents: {conditional.nrParents()}\")\n", + "print(f\"Parent Keys: {conditional.parents()} ({gtsam.DefaultKeyFormatter(list(conditional.parents())[0])})\")\n", + "\n", + "# Conditional objects can also be printed\n", + "# conditional.print(\"P(x0 | x1): \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_eval_md" + }, + "source": [ + "## Evaluation (Derived Class Methods)\n", + "\n", + "Concrete conditional classes provide methods like `logProbability(values)` or `evaluate(values)` to compute the conditional probability (or density) given values for the parent variables. These methods are defined in the derived classes, not the `Conditional` base class itself." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "conditional_eval_code" + }, + "outputs": [], + "source": [ + "# Example for GaussianConditional (requires VectorValues)\n", + "vector_values = gtsam.VectorValues()\n", + "# vector_values.insert(X(0), np.array([0.0])) # Value for frontal variable\n", + "vector_values.insert(X(1), np.array([1.0])) # Value for parent variable\n", + "\n", + "# These methods are specific to GaussianConditional / other concrete types\n", + "try:\n", + " log_prob = conditional.logProbability(vector_values)\n", + " # print(f\"\\nLog Probability P(x0|x1=1.0): {log_prob}\")\n", + " prob = conditional.evaluate(vector_values)\n", + " # print(f\"Probability P(x0|x1=1.0): {prob}\")\n", + "except AttributeError:\n", + " print(\"\\nNote: logProbability/evaluate called on base Conditional pointer, needs derived type.\")\n", + " # In C++, you'd typically have a shared_ptr.\n", + " # In Python, if you know the type, you might access methods directly,\n", + " # but the base class wrapper doesn't expose derived methods.\n", + " pass\n", + "\n", + "# To properly evaluate, you often use the BayesNet/BayesTree directly\n", + "# bayes_net.logProbability(vector_values)" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/DotWriter.ipynb b/gtsam/inference/doc/DotWriter.ipynb new file mode 100644 index 000000000..ab6279755 --- /dev/null +++ b/gtsam/inference/doc/DotWriter.ipynb @@ -0,0 +1,289 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_intro_md" + }, + "source": [ + "# DotWriter" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_desc_md" + }, + "source": [ + "The `DotWriter` class is a helper utility in GTSAM used to customize the generation of Graphviz `.dot` file strings for visualizing factor graphs, Bayes nets, and Bayes trees.\n", + "\n", + "It allows you to control aspects like figure size, whether factors are plotted as points, how edges are drawn, and specify explicit positions for variables and factors." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "dotwriter_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam graphviz" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "dotwriter_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import DotWriter\n", + "from gtsam import SymbolicFactorGraph # Example graph type\n", + "from gtsam import symbol_shorthand\n", + "import graphviz # For rendering\n", + "import numpy as np\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_create_md" + }, + "source": [ + "## Creating and Configuring a DotWriter\n", + "\n", + "You create a `DotWriter` object and can then modify its public member variables to change the output format." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "dotwriter_config_code" + }, + "outputs": [], + "source": [ + "writer = DotWriter(\n", + " figureWidthInches = 8.0,\n", + " figureHeightInches = 5.0,\n", + " plotFactorPoints = True, # Draw black dots for factors\n", + " connectKeysToFactor = True, # Draw edges from variables to factor dots\n", + " binaryEdges = False # Don't simplify binary factors to single edges\n", + ")\n", + "\n", + "# --- Configuration Options ---\n", + "\n", + "# Specify explicit positions (used by neato -n)\n", + "writer.variablePositions = {\n", + " X(0): gtsam.Point2(0, 0),\n", + " X(1): gtsam.Point2(2, 0),\n", + " X(2): gtsam.Point2(4, 0),\n", + " L(1): gtsam.Point2(1, 2),\n", + " L(2): gtsam.Point2(3, 2)\n", + "}\n", + "\n", + "# Specify position hints (alternative, uses symbol char and index)\n", + "# writer.positionHints = {'x': 0.0, 'l': 2.0} # Puts 'x' vars at y=0, 'l' vars at y=2\n", + "\n", + "# Specify which variables should be boxes\n", + "writer.boxes = {L(1), L(2)}\n", + "\n", + "# Specify factor positions (less common)\n", + "# writer.factorPositions = {3: gtsam.Point2(0.5, 1.0)}" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_usage_md" + }, + "source": [ + "## Usage with Graph Objects\n", + "\n", + "The configured `DotWriter` object is passed as an argument to the `.dot()` method of `FactorGraph`, `BayesNet`, or `BayesTree`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "dotwriter_graph_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "bcdef012-3456-789a-bcde-f0123456789a" + }, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "%0\n", + "\n", + "\n", + "var8070450532247928832\n", + "\n", + "x0\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "var8070450532247928832--factor0\n", + "\n", + "\n", + "\n", + "var8070450532247928833\n", + "\n", + "x1\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "var8070450532247928832--factor1\n", + "\n", + "\n", + "\n", + "var8070450532247928833--factor1\n", + "\n", + "\n", + "\n", + "var8070450532247928834\n", + "\n", + "x2\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "var8070450532247928833--factor2\n", + "\n", + "\n", + "\n", + "var8070450532247928834--factor2\n", + "\n", + "\n", + "\n", + "var7783684379976990720\n", + "\n", + "l1\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "var7783684379976990720--factor3\n", + "\n", + "\n", + "\n", + "var8070450532247928832--factor3\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "var7783684379976990720--factor4\n", + "\n", + "\n", + "\n", + "var8070450532247928833--factor4\n", + "\n", + "\n", + "\n", + "var7783684379976990721\n", + "\n", + "l2\n", + "\n", + "\n", + "factor5\n", + "\n", + "\n", + "\n", + "var7783684379976990721--factor5\n", + "\n", + "\n", + "\n", + "var8070450532247928833--factor5\n", + "\n", + "\n", + "\n", + "factor6\n", + "\n", + "\n", + "\n", + "var7783684379976990721--factor6\n", + "\n", + "\n", + "\n", + "var8070450532247928834--factor6\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# Create the same graph as in VariableIndex example\n", + "graph = SymbolicFactorGraph()\n", + "graph.push_factor(X(0)) # Factor 0\n", + "graph.push_factor(X(0), X(1)) # Factor 1\n", + "graph.push_factor(X(1), X(2)) # Factor 2\n", + "graph.push_factor(X(0), L(1)) # Factor 3\n", + "graph.push_factor(X(1), L(1)) # Factor 4\n", + "graph.push_factor(X(1), L(2)) # Factor 5\n", + "graph.push_factor(X(2), L(2)) # Factor 6\n", + "\n", + "# Generate dot string using the configured writer\n", + "dot_string = graph.dot(writer=writer)\n", + "# print(dot_string)\n", + "\n", + "# Render the graph\n", + "graphviz.Source(dot_string)" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/EdgeKey.ipynb b/gtsam/inference/doc/EdgeKey.ipynb new file mode 100644 index 000000000..a76e31426 --- /dev/null +++ b/gtsam/inference/doc/EdgeKey.ipynb @@ -0,0 +1,154 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_intro_md" + }, + "source": [ + "# EdgeKey" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_desc_md" + }, + "source": [ + "An `EdgeKey` is a utility class in GTSAM used to encode a pair of 32-bit unsigned integers into a single 64-bit `gtsam.Key`. This can be useful for representing edges in a graph or other paired relationships where each element of the pair fits within 32 bits." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "edgekey_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "edgekey_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import EdgeKey" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_init_md" + }, + "source": [ + "## Initialization\n", + "\n", + "An `EdgeKey` can be created by providing two 32-bit unsigned integers (`i` and `j`). It can also be created by decoding an existing `gtsam.Key` (integer), assuming it was encoded using the `EdgeKey` format." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "edgekey_create_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "cdef1234-5678-90ab-cdef-1234567890ab" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "EdgeKey from (10, 20): {10, 20}\n", + "EdgeKey from key 42949672980: {10, 20}\n" + ] + } + ], + "source": [ + "# Create EdgeKey from integers i=10, j=20\n", + "ekey1 = EdgeKey(10, 20)\n", + "print(f\"EdgeKey from (10, 20): {ekey1}\") # Uses __str__ which calls operator std::string\n", + "\n", + "# Get the underlying integer key\n", + "key1 = ekey1.key()\n", + "\n", + "# Reconstruct EdgeKey from the key\n", + "ekey2 = EdgeKey(key1)\n", + "print(f\"EdgeKey from key {key1}: {ekey2}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_props_md" + }, + "source": [ + "## Properties and Usage\n", + "\n", + "You can access the original `i` and `j` values and the combined `Key`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "edgekey_access_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "def12345-6789-0abc-def1-234567890abc" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "EdgeKey: {123, 456}\n", + " i: 123\n", + " j: 456\n", + " Key: 528280977848\n" + ] + } + ], + "source": [ + "edge = EdgeKey(123, 456)\n", + "\n", + "print(f\"EdgeKey: {edge}\")\n", + "print(f\" i: {edge.i()}\")\n", + "print(f\" j: {edge.j()}\")\n", + "print(f\" Key: {edge.key()}\")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/EliminationTree.ipynb b/gtsam/inference/doc/EliminationTree.ipynb new file mode 100644 index 000000000..895a38729 --- /dev/null +++ b/gtsam/inference/doc/EliminationTree.ipynb @@ -0,0 +1,198 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "etree_intro_md" + }, + "source": [ + "# EliminationTree" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "etree_desc_md" + }, + "source": [ + "An `EliminationTree` represents the computational structure of sequential variable elimination (like Gaussian elimination) on a `FactorGraph` given a specific `Ordering`.\n", + "\n", + "Each node in the tree corresponds to a variable being eliminated. The children of a node represent variables that were eliminated earlier and produced factors involving the parent variable. The factors originally involving the variable at a node are stored at that node.\n", + "\n", + "Eliminating an `EliminationTree` yields a `BayesNet`.\n", + "\n", + "While fundamental to the theory, direct manipulation of `EliminationTree` objects in Python is less common than using the `eliminateSequential` method on a `FactorGraph`, which uses the `EliminationTree` internally." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "etree_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "etree_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "etree_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# EliminationTree is templated, need concrete types\n", + "from gtsam import GaussianFactorGraph, Ordering, GaussianEliminationTree, GaussianBayesNet\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "etree_create_md" + }, + "source": [ + "## Creating an EliminationTree\n", + "\n", + "An `EliminationTree` is constructed from a `FactorGraph` and an `Ordering`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "etree_create_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "f0123456-789a-bcde-f012-3456789abcde" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Elimination Tree: \n", + "Root(s):\n", + "Node (x2)\n", + "JacobianFactor(keys = [8070450532247928833; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "JacobianFactor(keys = [7783684379976990721; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Node (l2)\n", + " JacobianFactor(keys = [7783684379976990721; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Node (x1)\n", + " JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Node (l1)\n", + " JacobianFactor(keys = [7783684379976990720; 8070450532247928832], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Node (x0)\n", + " JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "\n" + ] + } + ], + "source": [ + "# Create a graph (same as BayesTree example)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "\n", + "# Define an ordering\n", + "ordering = Ordering([X(0), L(1), X(1), L(2), X(2)])\n", + "\n", + "# Construct the Elimination Tree\n", + "elimination_tree = GaussianEliminationTree(graph, ordering)\n", + "\n", + "elimination_tree.print(\"Elimination Tree: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "etree_eliminate_md" + }, + "source": [ + "## Elimination\n", + "\n", + "The primary use of an `EliminationTree` is to perform sequential elimination to produce a `BayesNet`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "etree_eliminate_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "01234567-89ab-cdef-0123-456789abcdef" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "BayesNet from EliminationTree: size 5\n", + "Conditional 0: GaussianConditional( P(x0 | l1) = dx0 - R*dl1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "Conditional 1: GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "Conditional 2: GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", + "Conditional 3: GaussianConditional( P(l2 | x2) = dl2 - R*dx2 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "Conditional 4: GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.774597 ])\n", + "\n" + ] + } + ], + "source": [ + "# The eliminate function needs to be specified (e.g., EliminateGaussian)\n", + "# In Python, this is usually handled internally by graph.eliminateSequential\n", + "# but the C++ EliminationTree has an eliminate method.\n", + "\n", + "# Let's call the graph's eliminateSequential which uses the tree internally\n", + "bayes_net, remaining_graph = graph.eliminatePartialSequential(ordering)\n", + "\n", + "print(\"BayesNet from EliminationTree:\")\n", + "bayes_net.print()" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/Factor.ipynb b/gtsam/inference/doc/Factor.ipynb new file mode 100644 index 000000000..5a0c322ba --- /dev/null +++ b/gtsam/inference/doc/Factor.ipynb @@ -0,0 +1,178 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "factor_intro_md" + }, + "source": [ + "# Factor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_desc_md" + }, + "source": [ + "`gtsam.Factor` is the abstract base class for all factors in GTSAM, including nonlinear factors, Gaussian factors, discrete factors, and conditionals. It defines the basic interface common to all factors, primarily centered around the set of variables (keys) the factor involves.\n", + "\n", + "You typically do not instantiate `gtsam.Factor` directly but rather work with its derived classes like `gtsam.NonlinearFactor`, `gtsam.JacobianFactor`, `gtsam.DiscreteFactor`, etc." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "factor_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "factor_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam.utils.test_case import GtsamTestCase\n", + "\n", + "# We need a concrete factor type for demonstration\n", + "from gtsam import PriorFactorPose2, BetweenFactorPose2, Pose2, Rot2, Point2\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_interface_md" + }, + "source": [ + "## Basic Interface\n", + "\n", + "All factors provide methods to access the keys of the variables they involve." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "factor_keys_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "01234567-89ab-cdef-0123-456789abcdef" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Prior factor keys: [8070450532247928832] (x0)\n", + "Prior factor size: 1\n", + "Between factor keys: [8070450532247928832, 8070450532247928833] (x0, x1)\n", + "Between factor size: 2\n", + "Is prior factor empty? False\n" + ] + } + ], + "source": [ + "noise_model = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n", + "\n", + "# Create some concrete factors\n", + "prior_factor = PriorFactorPose2(X(0), Pose2(0, 0, 0), noise_model)\n", + "between_factor = BetweenFactorPose2(X(0), X(1), Pose2(1, 0, 0), noise_model)\n", + "\n", + "# Access keys (methods inherited from gtsam.Factor)\n", + "prior_keys = prior_factor.keys()\n", + "print(f\"Prior factor keys: {prior_keys} ({gtsam.DefaultKeyFormatter(prior_keys[0])})\")\n", + "print(f\"Prior factor size: {prior_factor.size()}\")\n", + "\n", + "between_keys = between_factor.keys()\n", + "print(f\"Between factor keys: {between_keys} ({gtsam.DefaultKeyFormatter(between_keys[0])}, {gtsam.DefaultKeyFormatter(between_keys[1])})\")\n", + "print(f\"Between factor size: {between_factor.size()}\")\n", + "\n", + "print(f\"Is prior factor empty? {prior_factor.empty()}\")\n", + "\n", + "# Factors can be printed\n", + "# prior_factor.print(\"Prior Factor: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_error_md" + }, + "source": [ + "## Error Function\n", + "\n", + "A key method for many factor types (especially nonlinear and Gaussian) is `error(Values)`. This evaluates the negative log-likelihood of the factor given a specific assignment of variable values. For optimization, the goal is typically to find the `Values` that minimize the total error (sum of errors from all factors)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "factor_error_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "12345678-9abc-def0-1234-56789abcdef0" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error at ground truth: 0.0\n", + "Error with incorrect x1: 50.0\n" + ] + } + ], + "source": [ + "values = gtsam.Values()\n", + "values.insert(X(0), Pose2(0, 0, 0))\n", + "values.insert(X(1), Pose2(1, 0, 0))\n", + "\n", + "# Evaluate error (example with BetweenFactor)\n", + "error1 = between_factor.error(values)\n", + "print(f\"Error at ground truth: {error1}\")\n", + "\n", + "# Change a value and recalculate error\n", + "values.update(X(1), Pose2(0, 0, 0))\n", + "error2 = between_factor.error(values)\n", + "print(f\"Error with incorrect x1: {error2:.1f}\")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/FactorGraph.ipynb b/gtsam/inference/doc/FactorGraph.ipynb new file mode 100644 index 000000000..a58273310 --- /dev/null +++ b/gtsam/inference/doc/FactorGraph.ipynb @@ -0,0 +1,298 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "fg_intro_md" + }, + "source": [ + "# FactorGraph" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_desc_md" + }, + "source": [ + "A `FactorGraph` represents a factor graph, a bipartite graph connecting variables and factors. In GTSAM, the `FactorGraph` class (and its templated instantiations like `GaussianFactorGraph`, `NonlinearFactorGraph`, etc.) primarily stores a collection of factors.\n", + "\n", + "This class serves as the base for different types of factor graphs. You typically work with specific instantiations like `gtsam.GaussianFactorGraph` or `gtsam.NonlinearFactorGraph`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "fg_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "fg_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# Example uses NonlinearFactorGraph, but concepts apply to others\n", + "from gtsam import NonlinearFactorGraph, PriorFactorPose2, BetweenFactorPose2, Pose2, Point3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_init_md" + }, + "source": [ + "## Initialization and Adding Factors\n", + "\n", + "A `FactorGraph` is typically created empty and factors are added individually or from containers." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "fg_create_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "23456789-abcd-ef01-2345-6789abcdef01" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Graph size after adding factors: 3\n" + ] + } + ], + "source": [ + "graph = NonlinearFactorGraph()\n", + "\n", + "# Define noise models\n", + "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n", + "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.2, 0.2, 0.1))\n", + "\n", + "# Create factors\n", + "factor1 = PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise)\n", + "factor2 = BetweenFactorPose2(X(0), X(1), Pose2(1, 0, 0), odometry_noise)\n", + "factor3 = BetweenFactorPose2(X(1), X(2), Pose2(1, 0, 0), odometry_noise)\n", + "\n", + "# Add factors to the graph\n", + "graph.add(factor1) # add is synonym for push_back\n", + "graph.push_back(factor2)\n", + "\n", + "# Can also add using += or ,\n", + "graph += factor3\n", + "# graph += factor2, factor3 # Chaining also works\n", + "\n", + "print(f\"Graph size after adding factors: {graph.size()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_access_md" + }, + "source": [ + "## Accessing Factors and Properties" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "fg_access_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "3456789a-bcde-f012-3456-789abcdef012" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Is graph empty? False\n", + "Number of factors (size): 3\n", + "Number of non-null factors (nrFactors): 3\n", + "Factor at index 1: \n", + "BetweenFactor on 8070450532247928832 8070450532247928833\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1]\n", + "\n", + "Keys involved in the graph: {8070450532247928832, 8070450532247928833, 8070450532247928834}\n" + ] + } + ], + "source": [ + "print(f\"Is graph empty? {graph.empty()}\")\n", + "print(f\"Number of factors (size): {graph.size()}\")\n", + "print(f\"Number of non-null factors (nrFactors): {graph.nrFactors()}\") # Useful if factors were removed\n", + "\n", + "# Access factor by index\n", + "retrieved_factor = graph.at(1)\n", + "print(\"Factor at index 1: \")\n", + "retrieved_factor.print()\n", + "\n", + "# Get all unique keys involved in the graph\n", + "all_keys = graph.keys() # Returns a KeySet\n", + "print(f\"Keys involved in the graph: {all_keys}\")\n", + "\n", + "# Iterate through factors\n", + "# for i, factor in enumerate(graph):\n", + "# if factor:\n", + "# print(f\"Factor {i} keys: {factor.keys()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_error_md" + }, + "source": [ + "## Graph Error\n", + "\n", + "The `error(Values)` method calculates the total error of the graph for a given assignment of variable values. This is the sum of the errors from each individual factor." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "fg_error_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "456789ab-cdef-0123-4567-89abcdef0123" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Total graph error at ground truth: 0.0\n", + "Total graph error with incorrect x2: 50.0\n" + ] + } + ], + "source": [ + "values = gtsam.Values()\n", + "values.insert(X(0), Pose2(0, 0, 0))\n", + "values.insert(X(1), Pose2(1, 0, 0))\n", + "values.insert(X(2), Pose2(2, 0, 0))\n", + "\n", + "total_error1 = graph.error(values)\n", + "print(f\"Total graph error at ground truth: {total_error1}\")\n", + "\n", + "# Introduce an error\n", + "values.update(X(2), Pose2(1, 0, 0))\n", + "total_error2 = graph.error(values)\n", + "print(f\"Total graph error with incorrect x2: {total_error2:.1f}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_viz_md" + }, + "source": [ + "## Graph Visualization\n", + "\n", + "Factor graphs can be visualized using Graphviz via the `dot()` method." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "fg_dot_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "56789abc-def0-1234-5678-9abcdef01234" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "graph {\n", + " size=\"5,5\";\n", + "\n", + " var8070450532247928832[label=\"x0\"];\n", + " var8070450532247928833[label=\"x1\"];\n", + " var8070450532247928834[label=\"x2\"];\n", + "\n", + " factor0[label=\"\", shape=point];\n", + " var8070450532247928832--factor0;\n", + " factor1[label=\"\", shape=point];\n", + " var8070450532247928832--factor1;\n", + " var8070450532247928833--factor1;\n", + " factor2[label=\"\", shape=point];\n", + " var8070450532247928833--factor2;\n", + " var8070450532247928834--factor2;\n", + "}\n" + ] + } + ], + "source": [ + "dot_string = graph.dot()\n", + "print(dot_string)\n", + "\n", + "# To render, save dot_string to a file (e.g., graph.dot) and run:\n", + "# dot -Tpng graph.dot -o graph.png\n", + "# Or use a Python library like graphviz\n", + "# import graphviz\n", + "# graphviz.Source(dot_string)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_elim_md" + }, + "source": [ + "## Elimination\n", + "\n", + "A key purpose of factor graphs is inference via variable elimination. `FactorGraph` itself doesn't perform elimination, but its derived classes (like `GaussianFactorGraph`, `SymbolicFactorGraph`) inherit `eliminateSequential` and `eliminateMultifrontal` methods from `EliminateableFactorGraph`." + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/ISAM.ipynb b/gtsam/inference/doc/ISAM.ipynb new file mode 100644 index 000000000..82d841fad --- /dev/null +++ b/gtsam/inference/doc/ISAM.ipynb @@ -0,0 +1,276 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "isam_intro_md" + }, + "source": [ + "# ISAM" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "isam_desc_md" + }, + "source": [ + "`gtsam.ISAM` (Incremental Smoothing and Mapping) is a class that inherits from `BayesTree` and adds an `update` method. This method allows for efficient incremental updates to the solution when new factors (e.g., new measurements) are added to the problem.\n", + "\n", + "Instead of re-eliminating the entire factor graph from scratch, iSAM identifies the part of the Bayes tree affected by the new factors, removes that part, and re-eliminates only the necessary variables, merging the results back into the existing tree.\n", + "\n", + "Like `BayesTree`, it's templated (e.g., `GaussianISAM` which inherits from `GaussianBayesTree`). For practical applications requiring incremental updates, `ISAM2` is often preferred due to further optimizations like fluid relinearization and support for variable removal, but `ISAM` demonstrates the core incremental update concept based on the Bayes tree." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "isam_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "isam_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "isam_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# Use Gaussian variants for demonstration\n", + "from gtsam import GaussianFactorGraph, Ordering, GaussianISAM, GaussianBayesTree\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "isam_create_md" + }, + "source": [ + "## Initialization\n", + "\n", + "An `ISAM` object can be created empty or initialized from an existing `BayesTree`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "isam_init_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "89abcdef-0123-4567-89ab-cdef01234567" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initial ISAM (empty):\n", + ": cliques: 0, variables: 0\n", + "\n", + "Initial BayesTree:\n", + ": cliques: 1, variables: 1\n", + "Root(s):\n", + "Conditional density P(x0) = P(x0) \n", + " size: 1\n", + " Conditional P(x0): GaussianConditional( P(x0) = dx0 - d), d = [ 0 ], sigmas = [ 1 ])\n", + "\n", + "\n", + "ISAM from BayesTree:\n", + ": cliques: 1, variables: 1\n", + "Root(s):\n", + "Conditional density P(x0) = P(x0) \n", + " size: 1\n", + " Conditional P(x0): GaussianConditional( P(x0) = dx0 - d), d = [ 0 ], sigmas = [ 1 ])\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# Create an empty ISAM object\n", + "isam1 = GaussianISAM()\n", + "print(\"Initial ISAM (empty):\")\n", + "isam1.print()\n", + "\n", + "# Create from an existing Bayes Tree (e.g., from an initial batch solve)\n", + "initial_graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "initial_graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n", + "\n", + "initial_bayes_tree = initial_graph.eliminateMultifrontal(Ordering([X(0)]))\n", + "print(\"Initial BayesTree:\")\n", + "initial_bayes_tree.print()\n", + "\n", + "isam2 = GaussianISAM(initial_bayes_tree)\n", + "print(\"ISAM from BayesTree:\")\n", + "isam2.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "isam_update_md" + }, + "source": [ + "## Incremental Update\n", + "\n", + "The core functionality is the `update(newFactors)` method." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "isam_update_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "9abcdef0-1234-5678-9abc-def012345678" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "ISAM after first update (x0, x1):\n", + ": cliques: 2, variables: 2\n", + "Root(s):\n", + "Conditional density P(x0 | x1) = P(x0 | x1) \n", + " size: 1\n", + " Conditional P(x0 | x1): GaussianConditional( P(x0 | x1) = dx0 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.707107 ])\n", + "\n", + "\n", + " Children:\n", + " Conditional density P(x1) = P(x1) \n", + " size: 1\n", + " Conditional P(x1): GaussianConditional( P(x1) = dx1 - d), d = [ 0 ], sigmas = [ 0.707107 ])\n", + "\n", + "\n", + "\n", + "ISAM after second update (x0, x1, x2):\n", + ": cliques: 3, variables: 3\n", + "Root(s):\n", + "Conditional density P(x0 | x1) = P(x0 | x1) \n", + " size: 1\n", + " Conditional P(x0 | x1): GaussianConditional( P(x0 | x1) = dx0 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.707107 ])\n", + "\n", + "\n", + " Children:\n", + " Conditional density P(x1 | x2) = P(x1 | x2) \n", + " size: 1\n", + " Conditional P(x1 | x2): GaussianConditional( P(x1 | x2) = dx1 - R*dx2 - d), R = [ 0.666667 ], d = [ 0 ], sigmas = [ 0.816497 ])\n", + "\n", + "\n", + " Children:\n", + " Conditional density P(x2) = P(x2) \n", + " size: 1\n", + " Conditional P(x2): GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.866025 ])\n", + "\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# Start with the ISAM object containing the prior on x0\n", + "isam = GaussianISAM(initial_bayes_tree)\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "\n", + "# --- First Update ---\n", + "new_factors1 = GaussianFactorGraph()\n", + "new_factors1.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # x0 -> x1\n", + "isam.update(new_factors1)\n", + "\n", + "print(\"ISAM after first update (x0, x1):\")\n", + "isam.print()\n", + "\n", + "# --- Second Update ---\n", + "new_factors2 = GaussianFactorGraph()\n", + "new_factors2.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # x1 -> x2\n", + "isam.update(new_factors2)\n", + "\n", + "print(\"\\nISAM after second update (x0, x1, x2):\")\n", + "isam.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "isam_solve_md" + }, + "source": [ + "## Solution and Marginals\n", + "\n", + "Since `ISAM` inherits from `BayesTree`, you can use the same methods like `optimize()` and `marginalFactor()` after performing updates." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "isam_solve_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "abcdef01-2345-6789-abcd-ef0123456789" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Optimized Solution after updates:\n", + "Values with 3 values:\n", + "Value x0: [0.]\n", + "Value x1: [0.]\n", + "Value x2: [0.]\n", + "\n" + ] + } + ], + "source": [ + "# Get the solution from the final ISAM state\n", + "solution = isam.optimize()\n", + "print(\"Optimized Solution after updates:\")\n", + "solution.print()" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/JunctionTree.ipynb b/gtsam/inference/doc/JunctionTree.ipynb new file mode 100644 index 000000000..26388a8bd --- /dev/null +++ b/gtsam/inference/doc/JunctionTree.ipynb @@ -0,0 +1,160 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "jtree_intro_md" + }, + "source": [ + "# JunctionTree" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "jtree_desc_md" + }, + "source": [ + "A `JunctionTree` is an intermediate data structure used in GTSAM's multifrontal variable elimination. It is a `ClusterTree` where each node (cluster) corresponds to a clique in the chordal graph formed during elimination.\n", + "\n", + "Key differences from related structures:\n", + "* **vs. EliminationTree:** Junction tree nodes can represent the elimination of multiple variables simultaneously (a 'frontal' set), whereas elimination tree nodes typically represent single variable eliminations.\n", + "* **vs. BayesTree:** A JunctionTree node contains the original factors associated with the variables being eliminated in that clique. A BayesTree node contains the *result* of eliminating those factors (i.e., a conditional density $P(\text{Frontals} | \text{Separator})$).\n", + "\n", + "Like `EliminationTree`, direct manipulation of `JunctionTree` objects in Python is uncommon. It's primarily an internal structure used by `eliminateMultifrontal` when producing a `BayesTree`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "jtree_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "jtree_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "jtree_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# JunctionTree is templated, need concrete types\n", + "from gtsam import GaussianFactorGraph, Ordering, GaussianJunctionTree, GaussianBayesTree\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "jtree_create_md" + }, + "source": [ + "## Creating a JunctionTree\n", + "\n", + "A `JunctionTree` is typically constructed from an `EliminationTree` as part of the multifrontal elimination process. The direct constructor might not be exposed in Python, as it's usually created internally." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "jtree_create_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "12345678-9abc-def0-1234-56789abcdef0" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Resulting BayesTree (structure mirrors JunctionTree):\n", + ": cliques: 3, variables: 5\n", + "Root(s):\n", + "Conditional density P(l1, x1 | l2, x2) = P(l1 | x1) P(x1 | l2, x2) \n", + " size: 2\n", + " Conditional P(l1 | x1): GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "\n", + " Conditional P(x1 | l2, x2): GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", + "\n", + "\n", + " Children:\n", + " Conditional density P(l2, x2 | x0) = P(l2 | x2) P(x2 | x0) \n", + " size: 2\n", + " Conditional P(l2 | x2): GaussianConditional( P(l2 | x2) = dl2 - R*dx2 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "\n", + " Conditional P(x2 | x0): GaussianConditional( P(x2 | x0) = dx2 - R*dx0 - d), R = [ 0.2 ], d = [ 0 ], sigmas = [ 0.774597 ])\n", + "\n", + "\n", + " Children:\n", + " Conditional density P(x0) = P(x0) \n", + " size: 1\n", + " Conditional P(x0): GaussianConditional( P(x0) = dx0 - d), d = [ 0 ], sigmas = [ 0.894427 ])\n", + "\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# Create a graph (same as BayesTree example)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "\n", + "ordering = Ordering.Colamd(graph)\n", + "\n", + "# Perform multifrontal elimination, which uses a JunctionTree internally\n", + "bayes_tree, remaining_graph = graph.eliminatePartialMultifrontal(ordering)\n", + "\n", + "# The resulting BayesTree reflects the structure of the intermediate JunctionTree\n", + "print(\"Resulting BayesTree (structure mirrors JunctionTree):\")\n", + "bayes_tree.print()\n", + "\n", + "# Accessing the JunctionTree directly isn't typical in Python workflows.\n", + "# Its structure is implicitly captured by the BayesTree cliques." + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/Key.ipynb b/gtsam/inference/doc/Key.ipynb new file mode 100644 index 000000000..d5a6991b4 --- /dev/null +++ b/gtsam/inference/doc/Key.ipynb @@ -0,0 +1,202 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "key_intro_md" + }, + "source": [ + "# Key" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "key_desc_md" + }, + "source": [ + "A `Key` in GTSAM is simply a `typedef` for `std::uint64_t`. It serves as a unique identifier for variables within a factor graph or values within a `Values` container. While you can use raw integer keys, GTSAM provides helper classes like `Symbol` and `LabeledSymbol` to create semantically meaningful keys that encode type and index information within the 64-bit integer." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "key_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "key_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "key_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import Symbol, LabeledSymbol\n", + "import numpy as np" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "key_usage_md" + }, + "source": [ + "## Basic Usage\n", + "\n", + "Keys are typically created using `Symbol` or `LabeledSymbol` and then implicitly or explicitly cast to the `Key` type (integer)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "key_create_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "a1b2c3d4-e5f6-7890-abcd-ef1234567890" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbol Key (x0): 8070450532247928832\n", + "Type: \n", + "LabeledSymbol Key (aB1): 6988956933745737729\n", + "Type: \n", + "Plain Integer Key: 12345\n", + "Type: \n" + ] + } + ], + "source": [ + "sym = Symbol('x', 0)\n", + "key_from_symbol = sym.key() # Or just 'sym' where a Key is expected\n", + "print(f\"Symbol Key (x0): {key_from_symbol}\")\n", + "print(f\"Type: {type(key_from_symbol)}\")\n", + "\n", + "lsym = LabeledSymbol('a', 'B', 1)\n", + "key_from_labeled_symbol = lsym.key()\n", + "print(f\"LabeledSymbol Key (aB1): {key_from_labeled_symbol}\")\n", + "print(f\"Type: {type(key_from_labeled_symbol)}\")\n", + "\n", + "# You can also use plain integers, but it's less descriptive\n", + "plain_key = 12345\n", + "print(f\"Plain Integer Key: {plain_key}\")\n", + "print(f\"Type: {type(plain_key)}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "key_formatter_md" + }, + "source": [ + "## Key Formatting\n", + "\n", + "When printing GTSAM objects that contain keys (like Factor Graphs or Values), you can specify a `KeyFormatter` to control how keys are displayed. The default formatter tries to interpret keys as `Symbol`s. `MultiRobotKeyFormatter` also checks for `LabeledSymbol`s." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "key_format_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "b2c3d4e5-f6a7-8901-bcde-f12345678901" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Default Formatter:\n", + " Symbol Key: x0\n", + " LabeledSymbol Key: 6988956933745737729 (Default doesn't know LabeledSymbol)\n", + " Plain Key: 12345\n", + "\n", + "MultiRobot Formatter:\n", + " Symbol Key: x0\n", + " LabeledSymbol Key: aB1\n", + " Plain Key: 12345\n", + "\n", + "Custom Formatter:\n", + " Symbol Key: KEY[x0]\n", + " LabeledSymbol Key: KEY[aB1]\n", + " Plain Key: KEY[12345]\n" + ] + } + ], + "source": [ + "print(\"Default Formatter:\")\n", + "print(f\" Symbol Key: {gtsam.DefaultKeyFormatter(key_from_symbol)}\")\n", + "print(f\" LabeledSymbol Key: {gtsam.DefaultKeyFormatter(key_from_labeled_symbol)}\")\n", + "print(f\" Plain Key: {gtsam.DefaultKeyFormatter(plain_key)}\")\n", + "\n", + "print(\"MultiRobot Formatter:\")\n", + "print(f\" Symbol Key: {gtsam.MultiRobotKeyFormatter(key_from_symbol)}\")\n", + "print(f\" LabeledSymbol Key: {gtsam.MultiRobotKeyFormatter(key_from_labeled_symbol)}\")\n", + "print(f\" Plain Key: {gtsam.MultiRobotKeyFormatter(plain_key)}\")\n", + "\n", + "# Example of a custom formatter\n", + "def my_formatter(key):\n", + " # Try interpreting as LabeledSymbol, then Symbol, then default\n", + " try:\n", + " lsym = gtsam.LabeledSymbol(key)\n", + " if lsym.label() != 0: # Check if it's likely a valid LabeledSymbol\n", + " return f\"KEY[{lsym.string()}]\"\n", + " except:\n", + " pass\n", + " try:\n", + " sym = gtsam.Symbol(key)\n", + " if sym.chr() != 0: # Check if it's likely a valid Symbol\n", + " return f\"KEY[{sym.string()}]\"\n", + " except:\n", + " pass\n", + " return f\"KEY[{key}]\"\n", + "\n", + "print(\"Custom Formatter:\")\n", + "print(f\" Symbol Key: {my_formatter(key_from_symbol)}\")\n", + "print(f\" LabeledSymbol Key: {my_formatter(key_from_labeled_symbol)}\")\n", + "print(f\" Plain Key: {my_formatter(plain_key)}\")\n", + "\n", + "# KeyVectors, KeyLists, KeySets can also be printed using formatters\n", + "key_vector = gtsam.KeyVector([key_from_symbol, key_from_labeled_symbol, plain_key])\n", + "# key_vector.print(\"My Vector: \", my_formatter) # .print() method uses formatter directly" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/LabeledSymbol.ipynb b/gtsam/inference/doc/LabeledSymbol.ipynb new file mode 100644 index 000000000..9c2e50469 --- /dev/null +++ b/gtsam/inference/doc/LabeledSymbol.ipynb @@ -0,0 +1,200 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_intro_md" + }, + "source": [ + "# LabeledSymbol" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_desc_md" + }, + "source": [ + "A `LabeledSymbol` is a specialized version of `gtsam.Symbol` designed primarily for multi-robot applications or scenarios where an additional label is needed besides the type character and index. It encodes a type character (`unsigned char`), a label character (`unsigned char`), and an index (`uint64_t`) into a single 64-bit `gtsam.Key`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "lsymbol_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "lsymbol_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import LabeledSymbol" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_init_md" + }, + "source": [ + "## Initialization\n", + "\n", + "A `LabeledSymbol` can be created by providing a type character, a label character, and an index. It can also be created by decoding an existing `gtsam.Key` (integer)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "lsymbol_create_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "f1a2b3c4-d5e6-7890-f1a2-bcdef1234567" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "LabeledSymbol from char/label/index: xA7\n", + "LabeledSymbol from key 8107010787457335296: xA0\n" + ] + } + ], + "source": [ + "# Create LabeledSymbol 'x' from robot 'A' with index 7\n", + "lsym1 = LabeledSymbol('x', 'A', 7)\n", + "print(f\"LabeledSymbol from char/label/index: {lsym1.string()}\")\n", + "\n", + "# Get the underlying integer key\n", + "key1 = lsym1.key()\n", + "\n", + "# Reconstruct LabeledSymbol from the key\n", + "# Note: Decoding a key assumes it was encoded as a LabeledSymbol.\n", + "# If you decode a standard Symbol key, the label might be garbage.\n", + "x0_key = gtsam.Symbol('x', 0).key()\n", + "lsym2 = LabeledSymbol(x0_key)\n", + "print(f\"LabeledSymbol from key {x0_key}: {lsym2.string()}\") # Label might be non-printable or 0" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_props_md" + }, + "source": [ + "## Properties and Usage\n", + "\n", + "You can access the type character, label character, index, and underlying integer key." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "lsymbol_access_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "a2b3c4d5-e6f7-8901-a2b3-cdef12345678" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "LabeledSymbol: lB3\n", + " Char (Type): l\n", + " Label (Robot): B\n", + " Index: 3\n", + " Key: 7783740146724503555\n" + ] + } + ], + "source": [ + "robotB_landmark = LabeledSymbol('l', 'B', 3)\n", + "\n", + "print(f\"LabeledSymbol: {robotB_landmark.string()}\")\n", + "print(f\" Char (Type): {robotB_landmark.chr()}\")\n", + "print(f\" Label (Robot): {robotB_landmark.label()}\")\n", + "print(f\" Index: {robotB_landmark.index()}\")\n", + "print(f\" Key: {robotB_landmark.key()}\")\n", + "\n", + "# LabeledSymbols are often used directly where Keys are expected.\n", + "# Use the MultiRobotKeyFormatter for printing these keys meaningfully.\n", + "# e.g., graph.print(\"Graph: \\n\", gtsam.MultiRobotKeyFormatter)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_shorthand_md" + }, + "source": [ + "## Shorthand Function\n", + "\n", + "GTSAM provides a convenient shorthand function `gtsam.mrsymbol(c, label, j)`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "lsymbol_shorthand_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "b3c4d5e6-f7a8-9012-b3c4-def123456789" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "LabeledSymbol('p', 'C', 2).key() == gtsam.mrsymbol(ord('p'), ord('C'), 2): True\n" + ] + } + ], + "source": [ + "# Note: mrsymbol expects integer representations of chars (use ord())\n", + "pc2_key = gtsam.mrsymbol(ord('p'), ord('C'), 2)\n", + "\n", + "print(f\"LabeledSymbol('p', 'C', 2).key() == gtsam.mrsymbol(ord('p'), ord('C'), 2): {LabeledSymbol('p', 'C', 2).key() == pc2_key}\")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/Ordering.ipynb b/gtsam/inference/doc/Ordering.ipynb new file mode 100644 index 000000000..512cacdc8 --- /dev/null +++ b/gtsam/inference/doc/Ordering.ipynb @@ -0,0 +1,291 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_intro_md" + }, + "source": [ + "# Ordering" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_desc_md" + }, + "source": [ + "An `Ordering` specifies the order in which variables are eliminated during inference (e.g., Gaussian elimination, multifrontal QR). The choice of ordering significantly impacts the computational cost and fill-in (sparsity) of the resulting Bayes net or Bayes tree.\n", + "\n", + "GTSAM provides several algorithms to compute good orderings automatically (like COLAMD, METIS) or allows you to specify a custom ordering." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "ordering_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "ordering_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import Ordering\n", + "# Need a graph type for automatic ordering\n", + "from gtsam import SymbolicFactorGraph\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_create_md" + }, + "source": [ + "## Creating an Ordering\n", + "\n", + "Orderings can be created manually or computed automatically from a factor graph." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "ordering_manual_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "6789abcd-ef01-2345-6789-abcdef012345" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Manual Ordering: \n", + "Position 0: x1\n", + "Position 1: l1\n", + "Position 2: x2\n", + "Position 3: l2\n", + "Position 4: x0\n" + ] + } + ], + "source": [ + "# Manual creation (list of keys)\n", + "manual_ordering = Ordering([X(1), L(1), X(2), L(2), X(0)])\n", + "manual_ordering.print(\"Manual Ordering: \")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "ordering_auto_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "789abcde-f012-3456-789a-bcdef0123456" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "COLAMD Ordering: \n", + "Position 0: x0\n", + "Position 1: l1\n", + "Position 2: x1\n", + "Position 3: l2\n", + "Position 4: x2\n", + "\n", + "Constrained COLAMD (x0, x2 last): \n", + "Position 0: l1\n", + "Position 1: l2\n", + "Position 2: x1\n", + "Position 3: x0\n", + "Position 4: x2\n" + ] + } + ], + "source": [ + "# Automatic creation requires a factor graph\n", + "# Let's use a simple SymbolicFactorGraph for structure\n", + "graph = SymbolicFactorGraph()\n", + "graph.push_factor(X(0))\n", + "graph.push_factor(X(0), X(1))\n", + "graph.push_factor(X(1), X(2))\n", + "graph.push_factor(X(0), L(1))\n", + "graph.push_factor(X(1), L(1))\n", + "graph.push_factor(X(1), L(2))\n", + "graph.push_factor(X(2), L(2))\n", + "\n", + "# COLAMD (Column Approximate Minimum Degree) ordering\n", + "colamd_ordering = Ordering.Colamd(graph)\n", + "colamd_ordering.print(\"COLAMD Ordering: \")\n", + "\n", + "# Constrained COLAMD (force x0 and x2 to be eliminated last)\n", + "constrained_ordering = Ordering.ColamdConstrainedLast(graph, gtsam.KeyVector([X(0), X(2)]))\n", + "constrained_ordering.print(\"Constrained COLAMD (x0, x2 last): \")\n", + "\n", + "# METIS ordering (if GTSAM was built with METIS support)\n", + "try:\n", + " metis_ordering = Ordering.Metis(graph)\n", + " # metis_ordering.print(\"METIS Ordering: \")\n", + "except RuntimeError as e:\n", + " print(f\"\\nSkipping METIS: {e}\")\n", + "\n", + "# Natural ordering (based on key magnitude)\n", + "natural_ordering = Ordering.Natural(graph)\n", + "# natural_ordering.print(\"Natural Ordering: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_access_md" + }, + "source": [ + "## Accessing Elements\n", + "\n", + "An `Ordering` behaves like a vector of keys." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "ordering_access_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "89abcdef-0123-4567-89ab-cdef01234567" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Ordering size: 5\n", + "Key at position 0: x0\n", + "Key at position 2: x1\n", + "Iterating through ordering:\n", + " x0\n", + " l1\n", + " x1\n", + " l2\n", + " x2\n", + "Inverted map (Key -> Position):\n", + " x0 -> 0\n", + " l1 -> 1\n", + " x1 -> 2\n", + " l2 -> 3\n", + " x2 -> 4\n" + ] + } + ], + "source": [ + "print(f\"Ordering size: {colamd_ordering.size()}\")\n", + "\n", + "# Access by index\n", + "key_at_0 = colamd_ordering.at(0)\n", + "key_at_2 = colamd_ordering[2] # Also supports [] operator\n", + "print(f\"Key at position 0: {gtsam.DefaultKeyFormatter(key_at_0)}\")\n", + "print(f\"Key at position 2: {gtsam.DefaultKeyFormatter(key_at_2)}\")\n", + "\n", + "# Iterate through the ordering\n", + "print(\"Iterating through ordering:\")\n", + "for key in colamd_ordering:\n", + " print(f\" {gtsam.DefaultKeyFormatter(key)}\")\n", + "\n", + "# Invert the ordering (map from Key to its position)\n", + "inverted = colamd_ordering.invert()\n", + "print(\"Inverted map (Key -> Position):\")\n", + "for key, pos in inverted.items():\n", + " print(f\" {gtsam.DefaultKeyFormatter(key)} -> {pos}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_append_md" + }, + "source": [ + "## Appending Keys\n", + "\n", + "You can append keys using `push_back`, `+=`, or `,`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "ordering_append_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "9abcdef0-1234-5678-9abc-def012345678" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Appended Ordering: \n", + "Position 0: x0\n", + "Position 1: l1\n", + "Position 2: x1\n", + "Position 3: l2\n", + "Position 4: x2\n", + "Position 5: l0\n", + "Position 6: x3\n" + ] + } + ], + "source": [ + "appended_ordering = Ordering(colamd_ordering)\n", + "appended_ordering.push_back(L(0))\n", + "appended_ordering += X(3)\n", + "# appended_ordering += L(0), X(3) # Can also chain\n", + "\n", + "appended_ordering.print(\"Appended Ordering: \")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/Symbol.ipynb b/gtsam/inference/doc/Symbol.ipynb new file mode 100644 index 000000000..077db72b0 --- /dev/null +++ b/gtsam/inference/doc/Symbol.ipynb @@ -0,0 +1,199 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_intro_md" + }, + "source": [ + "# Symbol" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_desc_md" + }, + "source": [ + "A `Symbol` is a GTSAM class used to create semantically meaningful keys (`gtsam.Key`) for variables. It combines a character (`unsigned char`) and an index (`uint64_t`) into a single 64-bit integer key. This allows for easy identification of variable types and their indices, e.g., 'x' for poses and 'l' for landmarks, like `x0`, `x1`, `l0`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "symbol_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "symbol_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import Symbol" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_init_md" + }, + "source": [ + "## Initialization\n", + "\n", + "A `Symbol` can be created by providing a character and an index. It can also be created by decoding an existing `gtsam.Key` (integer)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "symbol_create_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "c1d2e3f4-a5b6-7890-bcde-f12345678901" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbol from char/index: x5\n", + "Symbol from key 8070450532247928837: x5\n" + ] + } + ], + "source": [ + "# Create Symbol 'x' with index 5\n", + "sym1 = Symbol('x', 5)\n", + "print(f\"Symbol from char/index: {sym1.string()}\")\n", + "\n", + "# Get the underlying integer key\n", + "key1 = sym1.key()\n", + "\n", + "# Reconstruct Symbol from the key\n", + "sym2 = Symbol(key1)\n", + "print(f\"Symbol from key {key1}: {sym2.string()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_props_md" + }, + "source": [ + "## Properties and Usage\n", + "\n", + "You can access the character, index, and underlying integer key." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "symbol_access_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "d2e3f4a5-b6c7-8901-cdef-123456789012" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbol: l10\n", + " Char: l\n", + " Index: 10\n", + " Key: 7783684379976990730\n" + ] + } + ], + "source": [ + "landmark_sym = Symbol('l', 10)\n", + "\n", + "print(f\"Symbol: {landmark_sym.string()}\")\n", + "print(f\" Char: {landmark_sym.chr()}\")\n", + "print(f\" Index: {landmark_sym.index()}\")\n", + "print(f\" Key: {landmark_sym.key()}\")\n", + "\n", + "# Symbols are often used directly where Keys are expected in GTSAM functions,\n", + "# as they implicitly convert.\n", + "# e.g., values.insert(landmark_sym, gtsam.Point3(1,2,3))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_shorthand_md" + }, + "source": [ + "## Shorthand Functions\n", + "\n", + "GTSAM provides convenient shorthand functions `gtsam.symbol_shorthand.X(j)`, `gtsam.symbol_shorthand.L(j)`, etc., which are equivalent to `gtsam.Symbol('x', j)`, `gtsam.Symbol('l', j)`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "symbol_shorthand_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "e3f4a5b6-c7d8-9012-def0-234567890123" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbol('x', 0).key() == gtsam.symbol_shorthand.X(0): True\n", + "Symbol('l', 1).key() == gtsam.symbol_shorthand.L(1): True\n" + ] + } + ], + "source": [ + "from gtsam import symbol_shorthand\n", + "\n", + "x0_key = symbol_shorthand.X(0)\n", + "l1_key = symbol_shorthand.L(1)\n", + "\n", + "print(f\"Symbol('x', 0).key() == gtsam.symbol_shorthand.X(0): {Symbol('x', 0).key() == x0_key}\")\n", + "print(f\"Symbol('l', 1).key() == gtsam.symbol_shorthand.L(1): {Symbol('l', 1).key() == l1_key}\")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file diff --git a/gtsam/inference/doc/VariableIndex.ipynb b/gtsam/inference/doc/VariableIndex.ipynb new file mode 100644 index 000000000..6367c749d --- /dev/null +++ b/gtsam/inference/doc/VariableIndex.ipynb @@ -0,0 +1,233 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_intro_md" + }, + "source": [ + "# VariableIndex" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_desc_md" + }, + "source": [ + "A `VariableIndex` provides an efficient way to look up which factors in a `FactorGraph` involve a particular variable (Key). It stores, for each variable, a list of the indices of the factors that include that variable.\n", + "\n", + "This structure is often computed internally by GTSAM algorithms (like ordering methods or elimination) but can also be created explicitly if needed, for example, to improve performance when multiple operations need this information." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "vindex_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "vindex_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import VariableIndex\n", + "# Need a graph type for creation\n", + "from gtsam import SymbolicFactorGraph\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_create_md" + }, + "source": [ + "## Creating a VariableIndex\n", + "\n", + "A `VariableIndex` is typically created from an existing `FactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "vindex_create_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "abcdef01-2345-6789-abcd-ef0123456789" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "VariableIndex: \n", + "nEntries = 13, nFactors = 7\n", + "var l1: 3 4 \n", + "var l2: 5 6 \n", + "var x0: 0 1 3 \n", + "var x1: 1 2 4 5 \n", + "var x2: 2 6 \n", + "\n" + ] + } + ], + "source": [ + "# Create a simple SymbolicFactorGraph\n", + "graph = SymbolicFactorGraph()\n", + "graph.push_factor(X(0)) # Factor 0\n", + "graph.push_factor(X(0), X(1)) # Factor 1\n", + "graph.push_factor(X(1), X(2)) # Factor 2\n", + "graph.push_factor(X(0), L(1)) # Factor 3\n", + "graph.push_factor(X(1), L(1)) # Factor 4\n", + "graph.push_factor(X(1), L(2)) # Factor 5\n", + "graph.push_factor(X(2), L(2)) # Factor 6\n", + "\n", + "# Create VariableIndex from the graph\n", + "variable_index = VariableIndex(graph)\n", + "\n", + "# Print the index\n", + "variable_index.print(\"VariableIndex: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_access_md" + }, + "source": [ + "## Accessing Information\n", + "\n", + "You can query the number of variables, factors, and entries, and look up the factors associated with a specific variable." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "vindex_access_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "bcdef012-3456-789a-bcde-f0123456789a" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Number of variables (size): 5\n", + "Number of factors (nFactors): 7\n", + "Number of variable-factor entries (nEntries): 13\n", + "\n", + "Factors involving x1: [1, 2, 4, 5]\n", + "Factors involving l1: [3, 4]\n", + "\n", + "Iterating through VariableIndex:\n", + " Variable l1 involves factors: [3, 4]\n", + " Variable l2 involves factors: [5, 6]\n", + " Variable x0 involves factors: [0, 1, 3]\n", + " Variable x1 involves factors: [1, 2, 4, 5]\n", + " Variable x2 involves factors: [2, 6]\n" + ] + } + ], + "source": [ + "print(f\"Number of variables (size): {variable_index.size()}\")\n", + "print(f\"Number of factors (nFactors): {variable_index.nFactors()}\")\n", + "print(f\"Number of variable-factor entries (nEntries): {variable_index.nEntries()}\")\n", + "\n", + "# Get factors involving a specific variable\n", + "factors_x1 = variable_index[X(1)] # Returns a FactorIndices (FastVector)\n", + "print(f\"Factors involving x1: {factors_x1}\")\n", + "\n", + "# Use key directly\n", + "factors_l1 = variable_index[L(1)]\n", + "print(f\"Factors involving l1: {factors_l1}\")\n", + "\n", + "# Iterate through the index (items are pairs of Key, FactorIndices)\n", + "print(\"Iterating through VariableIndex:\")\n", + "for key, factor_indices in variable_index.items(): # Use .items() like a dict\n", + " print(f\" Variable {gtsam.DefaultKeyFormatter(key)} involves factors: {factor_indices}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_use_md" + }, + "source": [ + "## Usage in Algorithms\n", + "\n", + "`VariableIndex` is primarily used as input to other algorithms, particularly ordering methods like `Ordering.Colamd`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "vindex_use_code", + "colab": { + "base_uri": "https://localhost:8080/" + }, + "outputId": "cdef0123-4567-89ab-cdef-0123456789ab" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "COLAMD Ordering from VariableIndex: \n", + "Position 0: x0\n", + "Position 1: l1\n", + "Position 2: x1\n", + "Position 3: l2\n", + "Position 4: x2\n" + ] + } + ], + "source": [ + "# Compute COLAMD ordering directly from the VariableIndex\n", + "colamd_ordering = Ordering.Colamd(variable_index)\n", + "colamd_ordering.print(\"COLAMD Ordering from VariableIndex: \")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 + } \ No newline at end of file From aae8b5d1408b47766d3e955a4962d578b9262890 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 10 Apr 2025 10:38:46 -0400 Subject: [PATCH 02/24] Fixing code & adding formulae --- gtsam/inference/doc/BayesNet.ipynb | 679 ++++++++++-------- gtsam/inference/doc/BayesTree.ipynb | 811 +++++++++++++--------- gtsam/inference/doc/ClusterTree.ipynb | 368 +++++----- gtsam/inference/doc/Conditional.ipynb | 473 +++++++------ gtsam/inference/doc/DotWriter.ipynb | 614 ++++++++-------- gtsam/inference/doc/EdgeKey.ipynb | 315 +++++---- gtsam/inference/doc/EliminationTree.ipynb | 412 +++++------ gtsam/inference/doc/Factor.ipynb | 371 +++++----- gtsam/inference/doc/FactorGraph.ipynb | 666 ++++++++++-------- gtsam/inference/doc/ISAM.ipynb | 523 +++++++------- gtsam/inference/doc/JunctionTree.ipynb | 320 ++++----- gtsam/inference/doc/Key.ipynb | 414 +++++------ gtsam/inference/doc/LabeledSymbol.ipynb | 411 +++++------ gtsam/inference/doc/Ordering.ipynb | 563 ++++++++------- gtsam/inference/doc/Symbol.ipynb | 402 +++++------ gtsam/inference/doc/VariableIndex.ipynb | 464 +++++++------ 16 files changed, 4174 insertions(+), 3632 deletions(-) diff --git a/gtsam/inference/doc/BayesNet.ipynb b/gtsam/inference/doc/BayesNet.ipynb index e58cbb2a9..786fdd9c9 100644 --- a/gtsam/inference/doc/BayesNet.ipynb +++ b/gtsam/inference/doc/BayesNet.ipynb @@ -1,288 +1,397 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "bayesnet_intro_md" - }, - "source": [ - "# BayesNet" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayesnet_desc_md" - }, - "source": [ - "A `BayesNet` in GTSAM represents a directed graphical model, specifically the result of running sequential variable elimination (like Cholesky or QR factorization) on a `FactorGraph`.\n", - "\n", - "It is essentially a collection of `Conditional` objects, ordered according to the elimination order. Each conditional represents $P(\text{variable} | \text{parents})$, where the parents are variables that appear later in the elimination ordering.\n", - "\n", - "Like `FactorGraph`, `BayesNet` is templated on the type of conditional it stores (e.g., `GaussianBayesNet`, `DiscreteBayesNet`)." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayesnet_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayesnet_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayesnet_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "import numpy as np\n", - "\n", - "# We need concrete graph types and elimination to get a BayesNet\n", - "from gtsam import GaussianFactorGraph, Ordering, GaussianBayesNet\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayesnet_create_md" - }, - "source": [ - "## Creating a BayesNet (via Elimination)\n", - "\n", - "BayesNets are typically obtained by eliminating a `FactorGraph`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayesnet_eliminate_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "01234567-89ab-cdef-0123-456789abcdef" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Original Factor Graph: size 3\n", - "Factor 0: JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "Factor 1: JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "Factor 2: JacobianFactor(keys = [8070450532247928833; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "\n", - "\n", - "Resulting BayesNet: size 3\n", - "Conditional 0: GaussianConditional( P(x0 | x1) = dx0 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.707107 ])\n", - "Conditional 1: GaussianConditional( P(x1 | x2) = dx1 - R*dx2 - d), R = [ 0.666667 ], d = [ 0 ], sigmas = [ 0.816497 ])\n", - "Conditional 2: GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.866025 ])\n", - "\n" - ] - } - ], - "source": [ - "# Create a simple Gaussian Factor Graph P(x0) P(x1|x0) P(x2|x1)\n", - "graph = GaussianFactorGraph()\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", - "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "print(\"Original Factor Graph:\")\n", - "graph.print()\n", - "\n", - "# Eliminate sequentially using a specific ordering\n", - "ordering = Ordering([X(0), X(1), X(2)])\n", - "bayes_net = graph.eliminateSequential(ordering)\n", - "\n", - "print(\"\\nResulting BayesNet:\")\n", - "bayes_net.print()" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayesnet_props_md" - }, - "source": [ - "## Properties and Access\n", - "\n", - "A `BayesNet` provides access to its constituent conditionals and basic properties." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayesnet_access_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "12345678-9abc-def0-1234-56789abcdef0" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "BayesNet size: 3\n", - "Is BayesNet empty? False\n", - "Conditional at index 1: \n", - "GaussianConditional( P(x1 | x2) = dx1 - R*dx2 - d), R = [ 0.666667 ], d = [ 0 ], sigmas = [ 0.816497 ])\n", - "\n", - "Keys in BayesNet: {8070450532247928832, 8070450532247928833, 8070450532247928834}\n" - ] - } - ], - "source": [ - "print(f\"BayesNet size: {bayes_net.size()}\")\n", - "print(f\"Is BayesNet empty? {bayes_net.empty()}\")\n", - "\n", - "# Access conditional by index\n", - "conditional1 = bayes_net.at(1)\n", - "print(\"Conditional at index 1: \")\n", - "conditional1.print()\n", - "\n", - "# Get all keys involved\n", - "bn_keys = bayes_net.keys()\n", - "print(f\"Keys in BayesNet: {bn_keys}\")\n", - "\n", - "# Iterate through conditionals\n", - "# for i, conditional in enumerate(bayes_net):\n", - "# if conditional:\n", - "# print(f\"Conditional {i} Frontals: {conditional.frontals()}, Parents: {conditional.parents()}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayesnet_eval_md" - }, - "source": [ - "## Evaluation and Solution\n", - "\n", - "The `logProbability(Values)` method computes the log probability of a variable assignment given the conditional distributions in the Bayes net. For Gaussian Bayes nets, the `optimize()` method can be used to find the maximum likelihood estimate (MLE) solution via back-substitution." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayesnet_eval_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "23456789-abcd-ef01-2345-6789abcdef01" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Log Probability at [0,0,0]: -2.756815765004782\n", - "Optimized Solution (MLE):\n", - "Values with 3 values:\n", - "Value x0: [0.]\n", - "Value x1: [0.]\n", - "Value x2: [0.]\n", - "\n" - ] - } - ], - "source": [ - "# For GaussianBayesNet, we use VectorValues\n", - "mle_solution = bayes_net.optimize()\n", - "\n", - "# Calculate log probability (requires providing values for all variables)\n", - "log_prob = bayes_net.logProbability(mle_solution)\n", - "print(f\"Log Probability at {mle_solution.at(X(0))[0]:.0f},{mle_solution.at(X(1))[0]:.0f},{mle_solution.at(X(2))[0]:.0f}]: {log_prob}\")\n", - "\n", - "print(\"Optimized Solution (MLE):\")\n", - "mle_solution.print()" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayesnet_viz_md" - }, - "source": [ - "## Visualization\n", - "\n", - "Bayes nets can also be visualized using Graphviz." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayesnet_dot_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "3456789a-bcde-f012-3456-789abcdef012" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "digraph {\n", - " size=\"5,5\";\n", - "\n", - " var8070450532247928832[label=\"x0\"];\n", - " var8070450532247928833[label=\"x1\"];\n", - " var8070450532247928834[label=\"x2\"];\n", - "\n", - " var8070450532247928834->var8070450532247928833\n", - " var8070450532247928833->var8070450532247928832\n", - "}\n" - ] - } - ], - "source": [ - "dot_string = bayes_net.dot()\n", - "print(dot_string)\n", - "\n", - "# To render:\n", - "# dot -Tpng bayesnet.dot -o bayesnet.png\n", - "# import graphviz\n", - "# graphviz.Source(dot_string)" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# BayesNet" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_desc_md" + }, + "source": [ + "A `BayesNet` in GTSAM represents a directed graphical model, specifically the result of running sequential variable elimination (like Cholesky or QR factorization) on a `FactorGraph`.\n", + "\n", + "It is essentially a collection of `Conditional` objects, ordered according to the elimination order. Each conditional represents $P(\text{variable} | \text{parents})$, where the parents are variables that appear later in the elimination ordering.\n", + "\n", + "A Bayes net represents the joint probability distribution as a product of conditional probabilities stored in the net:\n", + "\n", + "$$\n", + "P(X_1, X_2, \\dots, X_N) = \\prod_{i=1}^N P(X_i | \\text{Parents}(X_i))\n", + "$$\n", + "The total log-probability of an assignment is the sum of the log-probabilities of its conditionals:\n", + "$$\n", + "\\log P(X_1, \\dots, X_N) = \\sum_{i=1}^N \\log P(X_i | \\text{Parents}(X_i))\n", + "$$\n", + "\n", + "Like `FactorGraph`, `BayesNet` is templated on the type of conditional it stores (e.g., `GaussianBayesNet`, `DiscreteBayesNet`)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "bayesnet_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "bayesnet_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# We need concrete graph types and elimination to get a BayesNet\n", + "from gtsam import GaussianFactorGraph, Ordering, GaussianBayesNet\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_create_md" + }, + "source": [ + "## Creating a BayesNet (via Elimination)\n", + "\n", + "BayesNets are typically obtained by eliminating a `FactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayesnet_eliminate_code", + "outputId": "01234567-89ab-cdef-0123-456789abcdef" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original Factor Graph:\n", + "\n", + "size: 3\n", + "factor 0: \n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "factor 1: \n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "factor 2: \n", + " A[x1] = [\n", + "\t-1\n", + "]\n", + " A[x2] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "\n", + "Resulting BayesNet:\n", + "\n", + "size: 3\n", + "conditional 0: p(x0 | x1)\n", + " R = [ 1.41421 ]\n", + " S[x1] = [ -0.707107 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.572365\n", + " No noise model\n", + "conditional 1: p(x1 | x2)\n", + " R = [ 1.22474 ]\n", + " S[x2] = [ -0.816497 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.716206\n", + " No noise model\n", + "conditional 2: p(x2)\n", + " R = [ 0.57735 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x2: 0\n", + " logNormalizationConstant: -1.46824\n", + " No noise model\n" + ] + } + ], + "source": [ + "# Create a simple Gaussian Factor Graph P(x0) P(x1|x0) P(x2|x1)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "print(\"Original Factor Graph:\")\n", + "graph.print()\n", + "\n", + "# Eliminate sequentially using a specific ordering\n", + "ordering = Ordering([X(0), X(1), X(2)])\n", + "bayes_net = graph.eliminateSequential(ordering)\n", + "\n", + "print(\"\\nResulting BayesNet:\")\n", + "bayes_net.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_props_md" + }, + "source": [ + "## Properties and Access\n", + "\n", + "A `BayesNet` provides access to its constituent conditionals and basic properties." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayesnet_access_code", + "outputId": "12345678-9abc-def0-1234-56789abcdef0" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "BayesNet size: 3\n", + "Conditional at index 1: \n", + "GaussianConditional p(x1 | x2)\n", + " R = [ 1.22474 ]\n", + " S[x2] = [ -0.816497 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.716206\n", + " No noise model\n", + "Keys in BayesNet: x0x1x2\n" + ] + } + ], + "source": [ + "print(f\"BayesNet size: {bayes_net.size()}\")\n", + "\n", + "# Access conditional by index\n", + "conditional1 = bayes_net.at(1)\n", + "print(\"Conditional at index 1: \")\n", + "conditional1.print()\n", + "\n", + "# Get all keys involved\n", + "bn_keys = bayes_net.keys()\n", + "print(f\"Keys in BayesNet: {bn_keys}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_eval_md" + }, + "source": [ + "## Evaluation and Solution\n", + "\n", + "The `logProbability(Values)` method computes the log probability of a variable assignment given the conditional distributions in the Bayes net. For Gaussian Bayes nets, the `optimize()` method can be used to find the maximum likelihood estimate (MLE) solution via back-substitution." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayesnet_eval_code", + "outputId": "23456789-abcd-ef01-2345-6789abcdef01" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Log Probability at 0,0,0]: -2.7568155996140185\n", + "Optimized Solution (MLE):\n", + "VectorValues: 3 elements\n", + " x0: 0\n", + " x1: 0\n", + " x2: 0\n" + ] + } + ], + "source": [ + "# For GaussianBayesNet, we use VectorValues\n", + "mle_solution = bayes_net.optimize()\n", + "\n", + "# Calculate log probability (requires providing values for all variables)\n", + "log_prob = bayes_net.logProbability(mle_solution)\n", + "print(f\"Log Probability at {mle_solution.at(X(0))[0]:.0f},{mle_solution.at(X(1))[0]:.0f},{mle_solution.at(X(2))[0]:.0f}]: {log_prob}\")\n", + "\n", + "print(\"Optimized Solution (MLE):\")\n", + "mle_solution.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_viz_md" + }, + "source": [ + "## Visualization\n", + "\n", + "Bayes nets can also be visualized using Graphviz." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayesnet_dot_code", + "outputId": "3456789a-bcde-f012-3456-789abcdef012" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "digraph {\n", + " size=\"5,5\";\n", + "\n", + " var8646911284551352320[label=\"x0\"];\n", + " var8646911284551352321[label=\"x1\"];\n", + " var8646911284551352322[label=\"x2\"];\n", + "\n", + " var8646911284551352322->var8646911284551352321\n", + " var8646911284551352321->var8646911284551352320\n", + "}\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", + "var8646911284551352321->var8646911284551352320\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "dot_string = bayes_net.dot()\n", + "print(dot_string)\n", + "\n", + "# To render:\n", + "# dot -Tpng bayesnet.dot -o bayesnet.png\n", + "import graphviz\n", + "graphviz.Source(dot_string)" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index 88a8cbd45..9af8838e6 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -1,340 +1,477 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "bayestree_intro_md" - }, - "source": [ - "# BayesTree" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayestree_desc_md" - }, - "source": [ - "A `BayesTree` is a graphical model that represents the result of multifrontal variable elimination on a `FactorGraph`. It is a tree structure where each node is a 'clique' containing a set of conditional distributions $P(\text{Frontals} | \text{Separator})$.\n", - "\n", - "Key properties:\n", - "* **Cliques:** Each node (clique) groups variables that are eliminated together.\n", - "* **Frontal Variables:** Variables eliminated within a specific clique.\n", - "* **Separator Variables:** Variables shared between a clique and its parent in the tree. These variables were eliminated higher up in the tree.\n", - "* **Tree Structure:** Represents the dependencies introduced during elimination more compactly than a Bayes net, especially for sparse problems.\n", - "\n", - "Like `FactorGraph` and `BayesNet`, `BayesTree` is templated on the type of conditional/clique (e.g., `GaussianBayesTree`)." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayestree_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayestree_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayestree_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "import numpy as np\n", - "\n", - "# We need concrete graph types and elimination to get a BayesTree\n", - "from gtsam import GaussianFactorGraph, Ordering, GaussianBayesTree\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayestree_create_md" - }, - "source": [ - "## Creating a BayesTree (via Elimination)\n", - "\n", - "BayesTrees are typically obtained by performing multifrontal elimination on a `FactorGraph`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayestree_eliminate_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "456789ab-cdef-0123-4567-89abcdef0123" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Original Factor Graph: size 7\n", - "Factor 0: JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "Factor 1: JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "Factor 2: JacobianFactor(keys = [8070450532247928833; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "Factor 3: JacobianFactor(keys = [7783684379976990720; 8070450532247928832], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "Factor 4: JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "Factor 5: JacobianFactor(keys = [7783684379976990721; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "Factor 6: JacobianFactor(keys = [7783684379976990721; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "\n", - "\n", - "Resulting BayesTree: cliques: 3, variables: 5\n", - "Root(s):\n", - "Conditional density P(l1, x1 | l2, x2) = P(l1 | x1) P(x1 | l2, x2) \n", - " size: 2\n", - " Conditional P(l1 | x1): GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "\n", - " Conditional P(x1 | l2, x2): GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", - "\n", - "\n", - " Children:\n", - " Conditional density P(l2, x2 | x0) = P(l2 | x2) P(x2 | x0) \n", - " size: 2\n", - " Conditional P(l2 | x2): GaussianConditional( P(l2 | x2) = dl2 - R*dx2 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "\n", - " Conditional P(x2 | x0): GaussianConditional( P(x2 | x0) = dx2 - R*dx0 - d), R = [ 0.2 ], d = [ 0 ], sigmas = [ 0.774597 ])\n", - "\n", - "\n", - " Children:\n", - " Conditional density P(x0) = P(x0) \n", - " size: 1\n", - " Conditional P(x0): GaussianConditional( P(x0) = dx0 - d), d = [ 0 ], sigmas = [ 0.894427 ])\n", - "\n", - "\n" - ] - } - ], - "source": [ - "# Create a simple Gaussian Factor Graph (more complex this time)\n", - "graph = GaussianFactorGraph()\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n", - "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # x0 -> x1\n", - "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # x1 -> x2\n", - "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model) # l1 -> x0 (measurement)\n", - "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l1 -> x1 (measurement)\n", - "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l2 -> x1 (measurement)\n", - "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # l2 -> x2 (measurement)\n", - "\n", - "print(\"Original Factor Graph:\")\n", - "graph.print()\n", - "\n", - "# Eliminate multifrontally using COLAMD ordering\n", - "ordering = Ordering.Colamd(graph)\n", - "# Note: Multifrontal typically yields multiple roots if graph is disconnected\n", - "bayes_tree = graph.eliminateMultifrontal(ordering)\n", - "\n", - "print(\"\\nResulting BayesTree:\")\n", - "bayes_tree.print()" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayestree_props_md" - }, - "source": [ - "## Properties and Access\n", - "\n", - "A `BayesTree` allows access to its root cliques and provides a way to look up the clique containing a specific variable." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayestree_access_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "56789abc-def0-1234-5678-9abcdef01234" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "BayesTree number of cliques: 3\n", - "Number of roots: 1\n", - "Root clique 0 conditional frontals: [7783684379976990720, 8070450532247928833]\n", - "\n", - "Clique containing x1 (8070450532247928833):\n", - "Conditional density P(l1, x1 | l2, x2) = P(l1 | x1) P(x1 | l2, x2) \n", - " size: 2\n", - " Conditional P(l1 | x1): GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "\n", - " Conditional P(x1 | l2, x2): GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", - "\n", - "\n" - ] - } - ], - "source": [ - "print(f\"BayesTree number of cliques: {bayes_tree.size()}\")\n", - "\n", - "# Access roots\n", - "roots = bayes_tree.roots()\n", - "print(f\"Number of roots: {len(roots)}\")\n", - "if roots:\n", - " # Access the conditional associated with the first root clique\n", - " root_conditional = roots[0].conditional()\n", - " print(f\"Root clique 0 conditional frontals: {root_conditional.frontals()}\")\n", - "\n", - "# Find the clique containing a specific variable\n", - "clique_x1 = bayes_tree.clique(X(1))\n", - "# clique_x1 is a shared pointer to the clique (e.g., GaussianBayesTreeClique)\n", - "print(f\"\\nClique containing x1 ({X(1)}):\")\n", - "clique_x1.print() # Print the clique itself" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayestree_solve_md" - }, - "source": [ - "## Solution and Marginals\n", - "\n", - "Similar to `BayesNet`, `BayesTree` (specifically derived types like `GaussianBayesTree`) provides an `optimize()` method for finding the MLE solution. It also allows for efficient computation of marginals on individual variables or joint marginals on pairs of variables using belief propagation or shortcut evaluation on the tree." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayestree_solve_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "6789abcd-ef01-2345-6789-abcdef012345" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Optimized Solution (MLE):\n", - "Values with 5 values:\n", - "Value l1: [0.]\n", - "Value l2: [0.]\n", - "Value x0: [0.]\n", - "Value x1: [0.]\n", - "Value x2: [0.]\n", - "\n", - "\n", - "Marginal Factor on x1:\n", - "Conditional density P(x1 | l2, x2) = P(x1 | l2, x2) \n", - " size: 1\n", - " Conditional P(x1 | l2, x2): GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", - "\n", - "\n", - "Joint Marginal Factor Graph on (x0, x2):\n", - "Factor Graph: size 2\n", - "Factor 0: GaussianConditional( P(x0 | x2) = dx0 - R*dx2 - d), R = [ 0.25 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "Factor 1: GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.774597 ])\n", - "\n" - ] - } - ], - "source": [ - "# Optimize to find the MLE solution (for GaussianBayesTree)\n", - "mle_solution = bayes_tree.optimize()\n", - "print(\"Optimized Solution (MLE):\")\n", - "mle_solution.print()\n", - "\n", - "# Compute marginal factor on a single variable (returns a Conditional)\n", - "marginal_x1 = bayes_tree.marginalFactor(X(1))\n", - "print(\"\\nMarginal Factor on x1:\")\n", - "marginal_x1.print()\n", - "\n", - "# Compute joint marginal factor graph on two variables\n", - "joint_x0_x2 = bayes_tree.joint(X(0), X(2))\n", - "print(\"\\nJoint Marginal Factor Graph on (x0, x2):\")\n", - "joint_x0_x2.print()" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayestree_viz_md" - }, - "source": [ - "## Visualization\n", - "\n", - "Bayes trees can be visualized using Graphviz." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "bayestree_dot_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "789abcde-f012-3456-789a-bcdef0123456" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "digraph G{\n", - "0[label=\"l1, x1 : l2, x2\"];\n", - "0->1\n", - "1[label=\"l2, x2 : x0\"];\n", - "1->2\n", - "2[label=\"x0 : \"];\n", - "}" - ] - } - ], - "source": [ - "dot_string = bayes_tree.dot()\n", - "print(dot_string)\n", - "\n", - "# To render:\n", - "# dot -Tpng bayestree.dot -o bayestree.png\n", - "# import graphviz\n", - "# graphviz.Source(dot_string)" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# BayesTree" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_desc_md" + }, + "source": [ + "A `BayesTree` is a graphical model that represents the result of multifrontal variable elimination on a `FactorGraph`. It is a tree structure where each node is a 'clique' containing a set of conditional distributions $P(\text{Frontals} | \text{Separator})$.\n", + "\n", + "Each clique k contains a conditional $P(F_k∣S_k)$, where $F_k$ are frontal variables and $S_k$ are separator variables. The joint probability distribution encoded by the Bayes tree is given by the product of all clique conditionals:\n", + "\n", + "$$\n", + "P(X) = \\prod_k P(F_k | S_k)\n", + "$$\n", + "Alternatively, it can be expressed using clique $P(C_k) = P(F_k, S_k)$ and separator $P(S_k)$ marginals (though GTSAM stores conditionals):\n", + "$$\n", + "P(X) = \\frac{\\prod_{\\text{cliques } k} P(C_k)}{\\prod_{\\text{separators } S} P(S)^{\\nu(S)-1}}\n", + "$$\n", + "where $\\nu(S)$ is the number of cliques containing the separator $S$. The first formula $P(X) = \\prod_k P(F_k | S_k)$ is more directly related to the GTSAM `BayesTree` structure.\n", + "\n", + "Key properties:\n", + "* **Cliques:** Each node (clique) groups variables that are eliminated together.\n", + "* **Frontal Variables:** Variables eliminated within a specific clique.\n", + "* **Separator Variables:** Variables shared between a clique and its parent in the tree. These variables were eliminated higher up in the tree.\n", + "* **Tree Structure:** Represents the dependencies introduced during elimination more compactly than a Bayes net, especially for sparse problems.\n", + "\n", + "Like `FactorGraph` and `BayesNet`, `BayesTree` is templated on the type of conditional/clique (e.g., `GaussianBayesTree`)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "bayestree_pip_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Requirement already satisfied: gtsam in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (4.3a0)Note: you may need to restart the kernel to use updated packages.\n", + "\n", + "Requirement already satisfied: numpy>=1.11.0 in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (from gtsam) (2.2.1)\n" + ] + } + ], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "bayestree_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# We need concrete graph types and elimination to get a BayesTree\n", + "from gtsam import GaussianFactorGraph, Ordering, GaussianBayesTree, VariableIndex\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_create_md" + }, + "source": [ + "## Creating a BayesTree (via Elimination)\n", + "\n", + "BayesTrees are typically obtained by performing multifrontal elimination on a `FactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayestree_eliminate_code", + "outputId": "456789ab-cdef-0123-4567-89abcdef0123" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original Factor Graph:\n", + "\n", + "size: 7\n", + "factor 0: \n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "factor 1: \n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "factor 2: \n", + " A[x1] = [\n", + "\t-1\n", + "]\n", + " A[x2] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "factor 3: \n", + " A[l1] = [\n", + "\t-1\n", + "]\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "factor 4: \n", + " A[l1] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "factor 5: \n", + " A[l2] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "factor 6: \n", + " A[l2] = [\n", + "\t-1\n", + "]\n", + " A[x2] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "\n", + "Resulting BayesTree:\n", + ": cliques: 2, variables: 5\n", + "- p(x1 l2 x2 )\n", + " R = [ 1.61245 -0.620174 -0.620174 ]\n", + " [ 0 1.27098 -1.08941 ]\n", + " [ 0 0 0.654654 ]\n", + " d = [ 0 0 0 ]\n", + " mean: 3 elements\n", + " l2: 0\n", + " x1: 0\n", + " x2: 0\n", + " logNormalizationConstant: -2.46292\n", + " No noise model\n", + "| - p(l1 x0 | x1)\n", + " R = [ 1.41421 -0.707107 ]\n", + " [ 0 1.58114 ]\n", + " S[x1] = [ -0.707107 ]\n", + " [ -0.948683 ]\n", + " d = [ 0 0 ]\n", + " logNormalizationConstant: -1.03316\n", + " No noise model\n" + ] + } + ], + "source": [ + "# Create a simple Gaussian Factor Graph (more complex this time)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # x0 -> x1\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # x1 -> x2\n", + "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model) # l1 -> x0 (measurement)\n", + "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l1 -> x1 (measurement)\n", + "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l2 -> x1 (measurement)\n", + "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # l2 -> x2 (measurement)\n", + "\n", + "print(\"Original Factor Graph:\")\n", + "graph.print()\n", + "\n", + "# Eliminate multifrontally using COLAMD ordering\n", + "ordering = Ordering.Colamd(VariableIndex(graph))\n", + "# Note: Multifrontal typically yields multiple roots if graph is disconnected\n", + "bayes_tree = graph.eliminateMultifrontal(ordering)\n", + "\n", + "print(\"\\nResulting BayesTree:\")\n", + "bayes_tree.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_props_md" + }, + "source": [ + "## Properties and Access\n", + "\n", + "A `BayesTree` allows access to its root cliques and provides a way to look up the clique containing a specific variable." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayestree_access_code", + "outputId": "56789abc-def0-1234-5678-9abcdef01234" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "BayesTree number of cliques: 2\n" + ] + }, + { + "ename": "AttributeError", + "evalue": "'gtsam.gtsam.GaussianBayesTree' object has no attribute 'roots'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[4], line 4\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mBayesTree number of cliques: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mbayes_tree\u001b[38;5;241m.\u001b[39msize()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 3\u001b[0m \u001b[38;5;66;03m# Access roots\u001b[39;00m\n\u001b[1;32m----> 4\u001b[0m roots \u001b[38;5;241m=\u001b[39m \u001b[43mbayes_tree\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mroots\u001b[49m()\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mNumber of roots: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[38;5;28mlen\u001b[39m(roots)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m roots:\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Access the conditional associated with the first root clique\u001b[39;00m\n", + "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.GaussianBayesTree' object has no attribute 'roots'" + ] + } + ], + "source": [ + "print(f\"BayesTree number of cliques: {bayes_tree.size()}\")\n", + "\n", + "# Access roots\n", + "roots = bayes_tree.roots()\n", + "print(f\"Number of roots: {len(roots)}\")\n", + "if roots:\n", + " # Access the conditional associated with the first root clique\n", + " root_conditional = roots[0].conditional()\n", + " print(f\"Root clique 0 conditional frontals: {root_conditional.frontals()}\")\n", + "\n", + "# Find the clique containing a specific variable\n", + "clique_x1 = bayes_tree.clique(X(1))\n", + "# clique_x1 is a shared pointer to the clique (e.g., GaussianBayesTreeClique)\n", + "print(f\"\\nClique containing x1 ({X(1)}):\")\n", + "clique_x1.print() # Print the clique itself" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_solve_md" + }, + "source": [ + "## Solution and Marginals\n", + "\n", + "Similar to `BayesNet`, `BayesTree` (specifically derived types like `GaussianBayesTree`) provides an `optimize()` method for finding the MLE solution. It also allows for efficient computation of marginals on individual variables or joint marginals on pairs of variables using belief propagation or shortcut evaluation on the tree." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayestree_solve_code", + "outputId": "6789abcd-ef01-2345-6789-abcdef012345" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Optimized Solution (MLE):\n", + "VectorValues: 5 elements\n", + " l1: 0\n", + " l2: 0\n", + " x0: 0\n", + " x1: 0\n", + " x2: 0\n", + "\n", + "Marginal Factor on x1:\n", + "GaussianConditional p(x1)\n", + " R = [ 0.774597 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x1: 0\n", + " logNormalizationConstant: -1.17435\n", + " No noise model\n", + "\n", + "Joint Marginal Factor Graph on (x0, x2):\n", + "\n", + "size: 2\n", + "factor 0: p(x0 | x2)\n", + " R = [ 1.32288 ]\n", + " S[x2] = [ -0.566947 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.639131\n", + " No noise model\n", + "factor 1: p(x2)\n", + " R = [ 0.654654 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x2: 0\n", + " logNormalizationConstant: -1.34259\n", + " No noise model\n" + ] + } + ], + "source": [ + "# Optimize to find the MLE solution (for GaussianBayesTree)\n", + "mle_solution = bayes_tree.optimize()\n", + "print(\"Optimized Solution (MLE):\")\n", + "mle_solution.print()\n", + "\n", + "# Compute marginal factor on a single variable (returns a Conditional)\n", + "marginal_x1 = bayes_tree.marginalFactor(X(1))\n", + "print(\"\\nMarginal Factor on x1:\")\n", + "marginal_x1.print()\n", + "\n", + "# Compute joint marginal factor graph on two variables\n", + "joint_x0_x2 = bayes_tree.joint(X(0), X(2))\n", + "print(\"\\nJoint Marginal Factor Graph on (x0, x2):\")\n", + "joint_x0_x2.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayestree_viz_md" + }, + "source": [ + "## Visualization\n", + "\n", + "Bayes trees can be visualized using Graphviz." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayestree_dot_code", + "outputId": "789abcde-f012-3456-789a-bcdef0123456" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "digraph G{\n", + "0[label=\"x1, l2, x2\"];\n", + "0->1\n", + "1[label=\"l1, x0 : x1\"];\n", + "}\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "0\n", + "\n", + "x1, l2, x2\n", + "\n", + "\n", + "\n", + "1\n", + "\n", + "l1, x0 : x1\n", + "\n", + "\n", + "\n", + "0->1\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "dot_string = bayes_tree.dot()\n", + "print(dot_string)\n", + "\n", + "# To render:\n", + "# dot -Tpng bayestree.dot -o bayestree.png\n", + "import graphviz\n", + "graphviz.Source(dot_string)" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/ClusterTree.ipynb b/gtsam/inference/doc/ClusterTree.ipynb index d637aad55..015ddf25f 100644 --- a/gtsam/inference/doc/ClusterTree.ipynb +++ b/gtsam/inference/doc/ClusterTree.ipynb @@ -1,168 +1,206 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "clustertree_intro_md" - }, - "source": [ - "# ClusterTree" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "clustertree_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "clustertree_desc_md" - }, - "source": [ - "A `ClusterTree` is a more general structure than an `EliminationTree` or `JunctionTree`. It represents a tree where each node (a 'Cluster') contains a subset of factors from an original factor graph. The key property is that the tree must be 'family preserving', meaning each original factor must belong entirely within a single cluster.\n", - "\n", - "`ClusterTree` itself is a base class. `EliminatableClusterTree` adds the ability to perform elimination, and `JunctionTree` is a specific type of `EliminatableClusterTree` derived from an `EliminationTree`.\n", - "\n", - "Direct use of `ClusterTree` in typical Python applications is less common than `JunctionTree` or `BayesTree`, as it's often an intermediate representation." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "clustertree_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "clustertree_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "clustertree_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "# Note: ClusterTree itself might not be directly exposed or used.\n", - "# We typically interact with JunctionTree or BayesTree which build upon it.\n", - "# We'll demonstrate concepts using JunctionTree which inherits Cluster features.\n", - "from gtsam import GaussianFactorGraph, Ordering, JunctionTree, GaussianBayesTree\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "clustertree_concept_md" - }, - "source": [ - "## Concept and Relation to JunctionTree\n", - "\n", - "A `JunctionTree` *is a* `ClusterTree` (specifically, an `EliminatableClusterTree`). It's constructed during multifrontal elimination. Each node in the `JunctionTree` is a `Cluster` containing factors that will be eliminated together to form a clique in the resulting `BayesTree`.\n", - "\n", - "We will create a `JunctionTree` and examine its properties, which include those inherited from `ClusterTree`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "clustertree_jt_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "ef012345-6789-abcd-ef01-23456789abcd" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Junction Tree (as ClusterTree): \n", - "Root(s):\n", - "Cluster (1) keys: l1 x1 \n", - " Factor 0: JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Cluster (1) keys: l2 x2 \n", - " Factor 0: JacobianFactor(keys = [7783684379976990721; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Cluster (1) keys: x0 \n", - " Factor 0: JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "\n", - "Accessing a root cluster (node):\n", - "Cluster (1) keys: l1 x1 \n", - " Factor 0: JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "\n", - "Number of roots: 1\n" - ] - } - ], - "source": [ - "# Create a graph (same as BayesTree example)\n", - "graph = GaussianFactorGraph()\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", - "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "\n", - "ordering = Ordering.Colamd(graph)\n", - "\n", - "# Create a Junction Tree (implicitly uses ClusterTree structure)\n", - "# Note: JunctionTree constructor might not be directly exposed.\n", - "# It's usually an intermediate in eliminateMultifrontal.\n", - "# We might need to construct it indirectly or focus on BayesTree access.\n", - "\n", - "# Let's get the BayesTree first, as JunctionTree creation is internal.\n", - "bayes_tree = graph.eliminateMultifrontal(ordering)\n", - "\n", - "# We can print the BayesTree, which shows the cluster structure\n", - "# (Cliques in BayesTree correspond to Clusters in JunctionTree)\n", - "print(\"Junction Tree (as ClusterTree): \")\n", - "bayes_tree.print() # Printing BayesTree shows clique structure\n", - "\n", - "# Access root cluster(s)\n", - "roots = bayes_tree.roots()\n", - "if roots:\n", - " print(\"\\nAccessing a root cluster (node):\")\n", - " root_clique = roots[0]\n", - " # In the JunctionTree, this node would contain the factors that *produced*\n", - " # the conditional in the BayesTree clique. We can see the involved keys.\n", - " root_clique.print(\"\", gtsam.DefaultKeyFormatter) # Print clique details\n", - "\n", - "print(f\"\\nNumber of roots: {len(roots)}\")\n", - "\n", - "# Direct instantiation or manipulation of Cluster/JunctionTree nodes\n", - "# is less common in Python than using the results (BayesTree)." - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# ClusterTree" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "clustertree_desc_md" + }, + "source": [ + "A `ClusterTree` is a more general structure than an `EliminationTree` or `JunctionTree`. It represents a tree where each node (a 'Cluster') contains a subset of factors from an original factor graph. The key property is that the tree must be 'family preserving', meaning each original factor must belong entirely within a single cluster.\n", + "\n", + "`ClusterTree` itself is a base class. `EliminatableClusterTree` adds the ability to perform elimination, and `JunctionTree` is a specific type of `EliminatableClusterTree` derived from an `EliminationTree`.\n", + "\n", + "Direct use of `ClusterTree` in typical Python applications is less common than `JunctionTree` or `BayesTree`, as it's often an intermediate representation." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "clustertree_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "clustertree_pip_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Requirement already satisfied: gtsam in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (4.3a0)Note: you may need to restart the kernel to use updated packages.\n", + "\n", + "Requirement already satisfied: numpy>=1.11.0 in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (from gtsam) (2.2.1)\n" + ] + } + ], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "id": "clustertree_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# Note: ClusterTree itself might not be directly exposed or used.\n", + "# We typically interact with JunctionTree or BayesTree which build upon it.\n", + "# We'll demonstrate concepts using JunctionTree which inherits Cluster features.\n", + "from gtsam import GaussianFactorGraph, Ordering, VariableIndex, GaussianBayesTree\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "clustertree_concept_md" + }, + "source": [ + "## Concept and Relation to JunctionTree\n", + "\n", + "A `JunctionTree` *is a* `ClusterTree` (specifically, an `EliminatableClusterTree`). It's constructed during multifrontal elimination. Each node in the `JunctionTree` is a `Cluster` containing factors that will be eliminated together to form a clique in the resulting `BayesTree`.\n", + "\n", + "We will create a `JunctionTree` and examine its properties, which include those inherited from `ClusterTree`." + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "clustertree_jt_code", + "outputId": "ef012345-6789-abcd-ef01-23456789abcd" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Junction Tree (as ClusterTree): \n", + ": cliques: 2, variables: 5\n", + "- p(x1 l2 x2 )\n", + " R = [ 1.61245 -0.620174 -0.620174 ]\n", + " [ 0 1.27098 -1.08941 ]\n", + " [ 0 0 0.654654 ]\n", + " d = [ 0 0 0 ]\n", + " mean: 3 elements\n", + " l2: 0\n", + " x1: 0\n", + " x2: 0\n", + " logNormalizationConstant: -2.46292\n", + " No noise model\n", + "| - p(l1 x0 | x1)\n", + " R = [ 1.41421 -0.707107 ]\n", + " [ 0 1.58114 ]\n", + " S[x1] = [ -0.707107 ]\n", + " [ -0.948683 ]\n", + " d = [ 0 0 ]\n", + " logNormalizationConstant: -1.03316\n", + " No noise model\n" + ] + }, + { + "ename": "AttributeError", + "evalue": "'gtsam.gtsam.GaussianBayesTree' object has no attribute 'roots'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[8], line 28\u001b[0m\n\u001b[0;32m 25\u001b[0m bayes_tree\u001b[38;5;241m.\u001b[39mprint() \u001b[38;5;66;03m# Printing BayesTree shows clique structure\u001b[39;00m\n\u001b[0;32m 27\u001b[0m \u001b[38;5;66;03m# Access root cluster(s)\u001b[39;00m\n\u001b[1;32m---> 28\u001b[0m roots \u001b[38;5;241m=\u001b[39m \u001b[43mbayes_tree\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mroots\u001b[49m()\n\u001b[0;32m 29\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m roots:\n\u001b[0;32m 30\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mAccessing a root cluster (node):\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n", + "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.GaussianBayesTree' object has no attribute 'roots'" + ] + } + ], + "source": [ + "# Create a graph (same as BayesTree example)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "\n", + "ordering = Ordering.Colamd(VariableIndex(graph))\n", + "\n", + "# Create a Junction Tree (implicitly uses ClusterTree structure)\n", + "# Note: JunctionTree constructor might not be directly exposed.\n", + "# It's usually an intermediate in eliminateMultifrontal.\n", + "# We might need to construct it indirectly or focus on BayesTree access.\n", + "\n", + "# Let's get the BayesTree first, as JunctionTree creation is internal.\n", + "bayes_tree = graph.eliminateMultifrontal(ordering)\n", + "\n", + "# We can print the BayesTree, which shows the cluster structure\n", + "# (Cliques in BayesTree correspond to Clusters in JunctionTree)\n", + "print(\"Junction Tree (as ClusterTree): \")\n", + "bayes_tree.print() # Printing BayesTree shows clique structure\n", + "\n", + "# Access root cluster(s)\n", + "roots = bayes_tree.roots()\n", + "if roots:\n", + " print(\"\\nAccessing a root cluster (node):\")\n", + " root_clique = roots[0]\n", + " # In the JunctionTree, this node would contain the factors that *produced*\n", + " # the conditional in the BayesTree clique. We can see the involved keys.\n", + " root_clique.print(\"\", gtsam.DefaultKeyFormatter) # Print clique details\n", + "\n", + "print(f\"\\nNumber of roots: {len(roots)}\")\n", + "\n", + "# Direct instantiation or manipulation of Cluster/JunctionTree nodes\n", + "# is less common in Python than using the results (BayesTree)." + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/Conditional.ipynb b/gtsam/inference/doc/Conditional.ipynb index 022d1b7b0..3c8c664ba 100644 --- a/gtsam/inference/doc/Conditional.ipynb +++ b/gtsam/inference/doc/Conditional.ipynb @@ -1,206 +1,273 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "conditional_intro_md" - }, - "source": [ - "# Conditional" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "conditional_desc_md" - }, - "source": [ - "`gtsam.Conditional` is the base class for conditional probability distributions or densities that result from variable elimination. Conditionals are essentially specialized factors representing $P(\text{Frontals} | \text{Parents})$.\n", - "\n", - "Like `gtsam.Factor`, you typically don't instantiate `gtsam.Conditional` directly. Instead, you work with derived classes obtained from elimination, such as:\n", - "* `gtsam.GaussianConditional`\n", - "* `gtsam.DiscreteConditional`\n", - "* `gtsam.HybridGaussianConditional`\n", - "* `gtsam.SymbolicConditional`\n", - "\n", - "This notebook demonstrates the common interface provided by the base class." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "conditional_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "conditional_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "conditional_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "import numpy as np\n", - "\n", - "# We need concrete graph types and elimination to get a Conditional\n", - "from gtsam import GaussianFactorGraph, Ordering\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "conditional_example_md" - }, - "source": [ - "## Example: Obtaining and Inspecting a Conditional\n", - "\n", - "We'll create a simple `GaussianFactorGraph` and eliminate one variable to get a `GaussianConditional`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "conditional_create_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "def01234-5678-9abc-def0-123456789abc" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Eliminating x0 from graph:\n", - "Factor Graph: size 2\n", - "Factor 0: JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "Factor 1: JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "\n", - "\n", - "Resulting BayesNet: size 1\n", - "Conditional 0: GaussianConditional( P(x0 | x1) = dx0 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.707107 ])\n", - "\n", - "\n", - "Conditional Keys (all): [8070450532247928832, 8070450532247928833]\n", - "Number of Frontals: 1\n", - "Frontal Keys: [8070450532247928832] (x0)\n", - "First Frontal Key: 8070450532247928832 (x0)\n", - "Number of Parents: 1\n", - "Parent Keys: [8070450532247928833] (x1)\n" - ] - } - ], - "source": [ - "# Create a simple Gaussian Factor Graph P(x0) P(x1|x0)\n", - "graph = GaussianFactorGraph()\n", - "model1 = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "model2 = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "\n", - "# Prior on x0\n", - "graph.add(X(0), -np.eye(1), np.zeros(1), model1)\n", - "# Factor between x0 and x1\n", - "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model2)\n", - "\n", - "print(\"Eliminating x0 from graph:\")\n", - "graph.print()\n", - "\n", - "# Eliminate x0\n", - "ordering = Ordering([X(0)])\n", - "bayes_net, remaining_graph = graph.eliminatePartialSequential(ordering)\n", - "\n", - "print(\"\\nResulting BayesNet:\")\n", - "bayes_net.print()\n", - "\n", - "# Get the resulting conditional P(x0 | x1)\n", - "# In this case, it's a GaussianConditional\n", - "conditional = bayes_net.at(0) # or bayes_net[0]\n", - "\n", - "# Access methods from the Conditional base class\n", - "print(f\"Conditional Keys (all): {conditional.keys()}\")\n", - "print(f\"Number of Frontals: {conditional.nrFrontals()}\")\n", - "print(f\"Frontal Keys: {conditional.frontals()} ({gtsam.DefaultKeyFormatter(list(conditional.frontals())[0])})\")\n", - "print(f\"First Frontal Key: {conditional.firstFrontalKey()} ({gtsam.DefaultKeyFormatter(conditional.firstFrontalKey())})\")\n", - "print(f\"Number of Parents: {conditional.nrParents()}\")\n", - "print(f\"Parent Keys: {conditional.parents()} ({gtsam.DefaultKeyFormatter(list(conditional.parents())[0])})\")\n", - "\n", - "# Conditional objects can also be printed\n", - "# conditional.print(\"P(x0 | x1): \")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "conditional_eval_md" - }, - "source": [ - "## Evaluation (Derived Class Methods)\n", - "\n", - "Concrete conditional classes provide methods like `logProbability(values)` or `evaluate(values)` to compute the conditional probability (or density) given values for the parent variables. These methods are defined in the derived classes, not the `Conditional` base class itself." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "conditional_eval_code" - }, - "outputs": [], - "source": [ - "# Example for GaussianConditional (requires VectorValues)\n", - "vector_values = gtsam.VectorValues()\n", - "# vector_values.insert(X(0), np.array([0.0])) # Value for frontal variable\n", - "vector_values.insert(X(1), np.array([1.0])) # Value for parent variable\n", - "\n", - "# These methods are specific to GaussianConditional / other concrete types\n", - "try:\n", - " log_prob = conditional.logProbability(vector_values)\n", - " # print(f\"\\nLog Probability P(x0|x1=1.0): {log_prob}\")\n", - " prob = conditional.evaluate(vector_values)\n", - " # print(f\"Probability P(x0|x1=1.0): {prob}\")\n", - "except AttributeError:\n", - " print(\"\\nNote: logProbability/evaluate called on base Conditional pointer, needs derived type.\")\n", - " # In C++, you'd typically have a shared_ptr.\n", - " # In Python, if you know the type, you might access methods directly,\n", - " # but the base class wrapper doesn't expose derived methods.\n", - " pass\n", - "\n", - "# To properly evaluate, you often use the BayesNet/BayesTree directly\n", - "# bayes_net.logProbability(vector_values)" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# Conditional" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_desc_md" + }, + "source": [ + "`gtsam.Conditional` is the base class for conditional probability distributions or densities that result from variable elimination.\n", + "\n", + "Let $F$ be the set of frontal variables and $S$ be the set of parent (separator) variables. A conditional represents:\n", + "\n", + "$$\n", + "P(F | S)\n", + "$$\n", + "The methods `evaluate`, `logProbability`, and `error` are related:\n", + "$$\n", + "\\text{evaluate}(F, S) = P(F | S)\n", + "$$\n", + "$$\n", + "\\text{logProbability}(F, S) = \\log P(F | S)\n", + "$$\n", + "$$\n", + "\\text{logProbability}(F, S) = -(\\text{negLogConstant} + \\text{error}(F, S))\n", + "$$\n", + "where `negLogConstant` is $-\\log k$ for the normalization constant $k$ ensuring $\\int P(F|S) dF = 1$.\n", + "\n", + "Like `gtsam.Factor`, you typically don't instantiate `gtsam.Conditional` directly. Instead, you work with derived classes obtained from elimination, such as:\n", + "* `gtsam.GaussianConditional`\n", + "* `gtsam.DiscreteConditional`\n", + "* `gtsam.HybridGaussianConditional`\n", + "* `gtsam.SymbolicConditional`\n", + "\n", + "This notebook demonstrates the common interface provided by the base class." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "conditional_pip_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Requirement already satisfied: gtsam in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (4.3a0)\n", + "Requirement already satisfied: numpy>=1.11.0 in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (from gtsam) (2.2.1)\n", + "Note: you may need to restart the kernel to use updated packages.\n" + ] + } + ], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "conditional_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# We need concrete graph types and elimination to get a Conditional\n", + "from gtsam import GaussianFactorGraph, Ordering\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_example_md" + }, + "source": [ + "## Example: Obtaining and Inspecting a Conditional\n", + "\n", + "We'll create a simple `GaussianFactorGraph` and eliminate one variable to get a `GaussianConditional`." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "conditional_create_code", + "outputId": "def01234-5678-9abc-def0-123456789abc" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Eliminating x0 from graph:\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[x0] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "\n", + "Resulting BayesNet:\n", + "\n", + "size: 1\n", + "conditional 0: p(x0 | x1)\n", + " R = [ 1.41421 ]\n", + " S[x1] = [ -0.707107 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.572365\n", + " No noise model\n", + "Conditional Keys (all): [8646911284551352320, 8646911284551352321]\n", + "First Frontal Key: 8646911284551352320 (x0)\n" + ] + } + ], + "source": [ + "# Create a simple Gaussian Factor Graph P(x0) P(x1|x0)\n", + "graph = GaussianFactorGraph()\n", + "model1 = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "model2 = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "\n", + "# Prior on x0\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model1)\n", + "# Factor between x0 and x1\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model2)\n", + "\n", + "print(\"Eliminating x0 from graph:\")\n", + "graph.print()\n", + "\n", + "# Eliminate x0\n", + "ordering = Ordering([X(0)])\n", + "bayes_net, remaining_graph = graph.eliminatePartialSequential(ordering)\n", + "\n", + "print(\"\\nResulting BayesNet:\")\n", + "bayes_net.print()\n", + "\n", + "# Get the resulting conditional P(x0 | x1)\n", + "# In this case, it's a GaussianConditional\n", + "conditional = bayes_net.at(0) # or bayes_net[0]\n", + "\n", + "# Access methods from the Conditional base class\n", + "print(f\"Conditional Keys (all): {conditional.keys()}\")\n", + "print(f\"First Frontal Key: {conditional.firstFrontalKey()} ({gtsam.DefaultKeyFormatter(conditional.firstFrontalKey())})\")\n", + "\n", + "# Conditional objects can also be printed\n", + "# conditional.print(\"P(x0 | x1): \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "conditional_eval_md" + }, + "source": [ + "## Evaluation (Derived Class Methods)\n", + "\n", + "Concrete conditional classes provide methods like `logProbability(values)` or `evaluate(values)` to compute the conditional probability (or density) given values for the parent variables. These methods are defined in the derived classes, not the `Conditional` base class itself." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "id": "conditional_eval_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Log Probability P(x0|x1=1.0): -0.8223649429247\n", + "Probability P(x0|x1=1.0): 0.43939128946772243\n" + ] + }, + { + "data": { + "text/plain": [ + "-0.8223649429247" + ] + }, + "execution_count": 9, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Example for GaussianConditional (requires VectorValues)\n", + "vector_values = gtsam.VectorValues()\n", + "vector_values.insert(X(0), np.array([0.0])) # Value for frontal variable\n", + "vector_values.insert(X(1), np.array([1.0])) # Value for parent variable\n", + "\n", + "# These methods are specific to GaussianConditional / other concrete types\n", + "try:\n", + " log_prob = conditional.logProbability(vector_values)\n", + " print(f\"\\nLog Probability P(x0|x1=1.0): {log_prob}\")\n", + " prob = conditional.evaluate(vector_values)\n", + " print(f\"Probability P(x0|x1=1.0): {prob}\")\n", + "except AttributeError:\n", + " print(\"\\nNote: logProbability/evaluate called on base Conditional pointer, needs derived type.\")\n", + " # In C++, you'd typically have a shared_ptr.\n", + " # In Python, if you know the type, you might access methods directly,\n", + " # but the base class wrapper doesn't expose derived methods.\n", + " pass\n", + "\n", + "# To properly evaluate, you often use the BayesNet/BayesTree directly\n", + "bayes_net.logProbability(vector_values)" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/DotWriter.ipynb b/gtsam/inference/doc/DotWriter.ipynb index ab6279755..c048bf353 100644 --- a/gtsam/inference/doc/DotWriter.ipynb +++ b/gtsam/inference/doc/DotWriter.ipynb @@ -1,289 +1,331 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "dotwriter_intro_md" - }, - "source": [ - "# DotWriter" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "dotwriter_desc_md" - }, - "source": [ - "The `DotWriter` class is a helper utility in GTSAM used to customize the generation of Graphviz `.dot` file strings for visualizing factor graphs, Bayes nets, and Bayes trees.\n", - "\n", - "It allows you to control aspects like figure size, whether factors are plotted as points, how edges are drawn, and specify explicit positions for variables and factors." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "dotwriter_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "dotwriter_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam graphviz" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "dotwriter_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "from gtsam import DotWriter\n", - "from gtsam import SymbolicFactorGraph # Example graph type\n", - "from gtsam import symbol_shorthand\n", - "import graphviz # For rendering\n", - "import numpy as np\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "dotwriter_create_md" - }, - "source": [ - "## Creating and Configuring a DotWriter\n", - "\n", - "You create a `DotWriter` object and can then modify its public member variables to change the output format." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "dotwriter_config_code" - }, - "outputs": [], - "source": [ - "writer = DotWriter(\n", - " figureWidthInches = 8.0,\n", - " figureHeightInches = 5.0,\n", - " plotFactorPoints = True, # Draw black dots for factors\n", - " connectKeysToFactor = True, # Draw edges from variables to factor dots\n", - " binaryEdges = False # Don't simplify binary factors to single edges\n", - ")\n", - "\n", - "# --- Configuration Options ---\n", - "\n", - "# Specify explicit positions (used by neato -n)\n", - "writer.variablePositions = {\n", - " X(0): gtsam.Point2(0, 0),\n", - " X(1): gtsam.Point2(2, 0),\n", - " X(2): gtsam.Point2(4, 0),\n", - " L(1): gtsam.Point2(1, 2),\n", - " L(2): gtsam.Point2(3, 2)\n", - "}\n", - "\n", - "# Specify position hints (alternative, uses symbol char and index)\n", - "# writer.positionHints = {'x': 0.0, 'l': 2.0} # Puts 'x' vars at y=0, 'l' vars at y=2\n", - "\n", - "# Specify which variables should be boxes\n", - "writer.boxes = {L(1), L(2)}\n", - "\n", - "# Specify factor positions (less common)\n", - "# writer.factorPositions = {3: gtsam.Point2(0.5, 1.0)}" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "dotwriter_usage_md" - }, - "source": [ - "## Usage with Graph Objects\n", - "\n", - "The configured `DotWriter` object is passed as an argument to the `.dot()` method of `FactorGraph`, `BayesNet`, or `BayesTree`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "dotwriter_graph_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "bcdef012-3456-789a-bcde-f0123456789a" - }, - "outputs": [ - { - "data": { - "image/svg+xml": [ - "\n", - "\n", - "%0\n", - "\n", - "\n", - "var8070450532247928832\n", - "\n", - "x0\n", - "\n", - "\n", - "factor0\n", - "\n", - "\n", - "\n", - "var8070450532247928832--factor0\n", - "\n", - "\n", - "\n", - "var8070450532247928833\n", - "\n", - "x1\n", - "\n", - "\n", - "factor1\n", - "\n", - "\n", - "\n", - "var8070450532247928832--factor1\n", - "\n", - "\n", - "\n", - "var8070450532247928833--factor1\n", - "\n", - "\n", - "\n", - "var8070450532247928834\n", - "\n", - "x2\n", - "\n", - "\n", - "factor2\n", - "\n", - "\n", - "\n", - "var8070450532247928833--factor2\n", - "\n", - "\n", - "\n", - "var8070450532247928834--factor2\n", - "\n", - "\n", - "\n", - "var7783684379976990720\n", - "\n", - "l1\n", - "\n", - "\n", - "factor3\n", - "\n", - "\n", - "\n", - "var7783684379976990720--factor3\n", - "\n", - "\n", - "\n", - "var8070450532247928832--factor3\n", - "\n", - "\n", - "\n", - "factor4\n", - "\n", - "\n", - "\n", - "var7783684379976990720--factor4\n", - "\n", - "\n", - "\n", - "var8070450532247928833--factor4\n", - "\n", - "\n", - "\n", - "var7783684379976990721\n", - "\n", - "l2\n", - "\n", - "\n", - "factor5\n", - "\n", - "\n", - "\n", - "var7783684379976990721--factor5\n", - "\n", - "\n", - "\n", - "var8070450532247928833--factor5\n", - "\n", - "\n", - "\n", - "factor6\n", - "\n", - "\n", - "\n", - "var7783684379976990721--factor6\n", - "\n", - "\n", - "\n", - "var8070450532247928834--factor6\n", - "\n", - "\n", - "\n", - "\n" - ], - "text/plain": [ - "" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], - "source": [ - "# Create the same graph as in VariableIndex example\n", - "graph = SymbolicFactorGraph()\n", - "graph.push_factor(X(0)) # Factor 0\n", - "graph.push_factor(X(0), X(1)) # Factor 1\n", - "graph.push_factor(X(1), X(2)) # Factor 2\n", - "graph.push_factor(X(0), L(1)) # Factor 3\n", - "graph.push_factor(X(1), L(1)) # Factor 4\n", - "graph.push_factor(X(1), L(2)) # Factor 5\n", - "graph.push_factor(X(2), L(2)) # Factor 6\n", - "\n", - "# Generate dot string using the configured writer\n", - "dot_string = graph.dot(writer=writer)\n", - "# print(dot_string)\n", - "\n", - "# Render the graph\n", - "graphviz.Source(dot_string)" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# DotWriter" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_desc_md" + }, + "source": [ + "The `DotWriter` class is a helper utility in GTSAM used to customize the generation of Graphviz `.dot` file strings for visualizing factor graphs, Bayes nets, and Bayes trees.\n", + "\n", + "It allows you to control aspects like figure size, whether factors are plotted as points, how edges are drawn, and specify explicit positions for variables and factors." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "dotwriter_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam graphviz" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "dotwriter_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import DotWriter\n", + "from gtsam import SymbolicFactorGraph # Example graph type\n", + "from gtsam import symbol_shorthand\n", + "import graphviz # For rendering\n", + "import numpy as np\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_create_md" + }, + "source": [ + "## Creating and Configuring a DotWriter\n", + "\n", + "You create a `DotWriter` object and can then modify its public member variables to change the output format." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "dotwriter_config_code" + }, + "outputs": [], + "source": [ + "writer = DotWriter(\n", + " figureWidthInches = 8.0,\n", + " figureHeightInches = 5.0,\n", + " plotFactorPoints = True, # Draw black dots for factors\n", + " connectKeysToFactor = True, # Draw edges from variables to factor dots\n", + " binaryEdges = False # Don't simplify binary factors to single edges\n", + ")\n", + "\n", + "# --- Configuration Options ---\n", + "\n", + "# Specify explicit positions (used by neato -n)\n", + "writer.variablePositions = {\n", + " X(0): gtsam.Point2(0, 0),\n", + " X(1): gtsam.Point2(2, 0),\n", + " X(2): gtsam.Point2(4, 0),\n", + " L(1): gtsam.Point2(1, 2),\n", + " L(2): gtsam.Point2(3, 2)\n", + "}\n", + "\n", + "# Specify position hints (alternative, uses symbol char and index)\n", + "# writer.positionHints = {'x': 0.0, 'l': 2.0} # Puts 'x' vars at y=0, 'l' vars at y=2\n", + "\n", + "# Specify which variables should be boxes\n", + "writer.boxes = {L(1), L(2)}\n", + "\n", + "# Specify factor positions (less common)\n", + "# writer.factorPositions = {3: gtsam.Point2(0.5, 1.0)}" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "dotwriter_usage_md" + }, + "source": [ + "## Usage with Graph Objects\n", + "\n", + "The configured `DotWriter` object is passed as an argument to the `.dot()` method of `FactorGraph`, `BayesNet`, or `BayesTree`." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "dotwriter_graph_code", + "outputId": "bcdef012-3456-789a-bcde-f0123456789a" + }, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089\n", + "\n", + "l1\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor3\n", + "\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090\n", + "\n", + "l2\n", + "\n", + "\n", + "\n", + "factor5\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090--factor5\n", + "\n", + "\n", + "\n", + "\n", + "factor6\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090--factor6\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", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor5\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor6\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Create the same graph as in VariableIndex example\n", + "graph = SymbolicFactorGraph()\n", + "graph.push_factor(X(0)) # Factor 0\n", + "graph.push_factor(X(0), X(1)) # Factor 1\n", + "graph.push_factor(X(1), X(2)) # Factor 2\n", + "graph.push_factor(X(0), L(1)) # Factor 3\n", + "graph.push_factor(X(1), L(1)) # Factor 4\n", + "graph.push_factor(X(1), L(2)) # Factor 5\n", + "graph.push_factor(X(2), L(2)) # Factor 6\n", + "\n", + "# Generate dot string using the configured writer\n", + "dot_string = graph.dot(writer=writer)\n", + "# print(dot_string)\n", + "\n", + "# Render the graph\n", + "graphviz.Source(dot_string)" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/EdgeKey.ipynb b/gtsam/inference/doc/EdgeKey.ipynb index a76e31426..d80e7cfa6 100644 --- a/gtsam/inference/doc/EdgeKey.ipynb +++ b/gtsam/inference/doc/EdgeKey.ipynb @@ -1,154 +1,167 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "edgekey_intro_md" - }, - "source": [ - "# EdgeKey" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "edgekey_desc_md" - }, - "source": [ - "An `EdgeKey` is a utility class in GTSAM used to encode a pair of 32-bit unsigned integers into a single 64-bit `gtsam.Key`. This can be useful for representing edges in a graph or other paired relationships where each element of the pair fits within 32 bits." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "edgekey_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "edgekey_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "edgekey_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "from gtsam import EdgeKey" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "edgekey_init_md" - }, - "source": [ - "## Initialization\n", - "\n", - "An `EdgeKey` can be created by providing two 32-bit unsigned integers (`i` and `j`). It can also be created by decoding an existing `gtsam.Key` (integer), assuming it was encoded using the `EdgeKey` format." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "edgekey_create_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "cdef1234-5678-90ab-cdef-1234567890ab" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "EdgeKey from (10, 20): {10, 20}\n", - "EdgeKey from key 42949672980: {10, 20}\n" - ] - } - ], - "source": [ - "# Create EdgeKey from integers i=10, j=20\n", - "ekey1 = EdgeKey(10, 20)\n", - "print(f\"EdgeKey from (10, 20): {ekey1}\") # Uses __str__ which calls operator std::string\n", - "\n", - "# Get the underlying integer key\n", - "key1 = ekey1.key()\n", - "\n", - "# Reconstruct EdgeKey from the key\n", - "ekey2 = EdgeKey(key1)\n", - "print(f\"EdgeKey from key {key1}: {ekey2}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "edgekey_props_md" - }, - "source": [ - "## Properties and Usage\n", - "\n", - "You can access the original `i` and `j` values and the combined `Key`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "edgekey_access_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "def12345-6789-0abc-def1-234567890abc" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "EdgeKey: {123, 456}\n", - " i: 123\n", - " j: 456\n", - " Key: 528280977848\n" - ] - } - ], - "source": [ - "edge = EdgeKey(123, 456)\n", - "\n", - "print(f\"EdgeKey: {edge}\")\n", - "print(f\" i: {edge.i()}\")\n", - "print(f\" j: {edge.j()}\")\n", - "print(f\" Key: {edge.key()}\")" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# EdgeKey" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_desc_md" + }, + "source": [ + "An `EdgeKey` is a utility class in GTSAM used to encode a pair of 32-bit unsigned integers into a single 64-bit `gtsam.Key`. This can be useful for representing edges in a graph or other paired relationships where each element of the pair fits within 32 bits." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "edgekey_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "edgekey_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import EdgeKey" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_init_md" + }, + "source": [ + "## Initialization\n", + "\n", + "An `EdgeKey` can be created by providing two 32-bit unsigned integers (`i` and `j`). It can also be created by decoding an existing `gtsam.Key` (integer), assuming it was encoded using the `EdgeKey` format." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "edgekey_create_code", + "outputId": "cdef1234-5678-90ab-cdef-1234567890ab" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "EdgeKey from (10, 20): {10, 20}\n", + "\n", + "EdgeKey from key 42949672980: {10, 20}\n", + "\n" + ] + } + ], + "source": [ + "# Create EdgeKey from integers i=10, j=20\n", + "ekey1 = EdgeKey(10, 20)\n", + "print(f\"EdgeKey from (10, 20): {ekey1}\") # Uses __str__ which calls operator std::string\n", + "\n", + "# Get the underlying integer key\n", + "key1 = ekey1.key()\n", + "\n", + "# Reconstruct EdgeKey from the key\n", + "ekey2 = EdgeKey(key1)\n", + "print(f\"EdgeKey from key {key1}: {ekey2}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "edgekey_props_md" + }, + "source": [ + "## Properties and Usage\n", + "\n", + "You can access the original `i` and `j` values and the combined `Key`." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "edgekey_access_code", + "outputId": "def12345-6789-0abc-def1-234567890abc" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "EdgeKey: {123, 456}\n", + "\n", + " i: 123\n", + " j: 456\n", + " Key: 528280977864\n" + ] + } + ], + "source": [ + "edge = EdgeKey(123, 456)\n", + "\n", + "print(f\"EdgeKey: {edge}\")\n", + "print(f\" i: {edge.i()}\")\n", + "print(f\" j: {edge.j()}\")\n", + "print(f\" Key: {edge.key()}\")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/EliminationTree.ipynb b/gtsam/inference/doc/EliminationTree.ipynb index 895a38729..703415455 100644 --- a/gtsam/inference/doc/EliminationTree.ipynb +++ b/gtsam/inference/doc/EliminationTree.ipynb @@ -1,198 +1,220 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "etree_intro_md" - }, - "source": [ - "# EliminationTree" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "etree_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "etree_desc_md" - }, - "source": [ - "An `EliminationTree` represents the computational structure of sequential variable elimination (like Gaussian elimination) on a `FactorGraph` given a specific `Ordering`.\n", - "\n", - "Each node in the tree corresponds to a variable being eliminated. The children of a node represent variables that were eliminated earlier and produced factors involving the parent variable. The factors originally involving the variable at a node are stored at that node.\n", - "\n", - "Eliminating an `EliminationTree` yields a `BayesNet`.\n", - "\n", - "While fundamental to the theory, direct manipulation of `EliminationTree` objects in Python is less common than using the `eliminateSequential` method on a `FactorGraph`, which uses the `EliminationTree` internally." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "etree_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "etree_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "etree_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "import numpy as np\n", - "\n", - "# EliminationTree is templated, need concrete types\n", - "from gtsam import GaussianFactorGraph, Ordering, GaussianEliminationTree, GaussianBayesNet\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "etree_create_md" - }, - "source": [ - "## Creating an EliminationTree\n", - "\n", - "An `EliminationTree` is constructed from a `FactorGraph` and an `Ordering`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "etree_create_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "f0123456-789a-bcde-f012-3456789abcde" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Elimination Tree: \n", - "Root(s):\n", - "Node (x2)\n", - "JacobianFactor(keys = [8070450532247928833; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "JacobianFactor(keys = [7783684379976990721; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Node (l2)\n", - " JacobianFactor(keys = [7783684379976990721; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Node (x1)\n", - " JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Node (l1)\n", - " JacobianFactor(keys = [7783684379976990720; 8070450532247928832], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Node (x0)\n", - " JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "\n" - ] - } - ], - "source": [ - "# Create a graph (same as BayesTree example)\n", - "graph = GaussianFactorGraph()\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", - "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "\n", - "# Define an ordering\n", - "ordering = Ordering([X(0), L(1), X(1), L(2), X(2)])\n", - "\n", - "# Construct the Elimination Tree\n", - "elimination_tree = GaussianEliminationTree(graph, ordering)\n", - "\n", - "elimination_tree.print(\"Elimination Tree: \")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "etree_eliminate_md" - }, - "source": [ - "## Elimination\n", - "\n", - "The primary use of an `EliminationTree` is to perform sequential elimination to produce a `BayesNet`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "etree_eliminate_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "01234567-89ab-cdef-0123-456789abcdef" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "BayesNet from EliminationTree: size 5\n", - "Conditional 0: GaussianConditional( P(x0 | l1) = dx0 - R*dl1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "Conditional 1: GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "Conditional 2: GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", - "Conditional 3: GaussianConditional( P(l2 | x2) = dl2 - R*dx2 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "Conditional 4: GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.774597 ])\n", - "\n" - ] - } - ], - "source": [ - "# The eliminate function needs to be specified (e.g., EliminateGaussian)\n", - "# In Python, this is usually handled internally by graph.eliminateSequential\n", - "# but the C++ EliminationTree has an eliminate method.\n", - "\n", - "# Let's call the graph's eliminateSequential which uses the tree internally\n", - "bayes_net, remaining_graph = graph.eliminatePartialSequential(ordering)\n", - "\n", - "print(\"BayesNet from EliminationTree:\")\n", - "bayes_net.print()" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# EliminationTree" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "etree_desc_md" + }, + "source": [ + "An `EliminationTree` represents the computational structure of sequential variable elimination (like Gaussian elimination) on a `FactorGraph` given a specific `Ordering`.\n", + "\n", + "Each node in the tree corresponds to a variable being eliminated. The children of a node represent variables that were eliminated earlier and produced factors involving the parent variable. The factors originally involving the variable at a node are stored at that node.\n", + "\n", + "Eliminating an `EliminationTree` yields a `BayesNet`.\n", + "\n", + "While fundamental to the theory, direct manipulation of `EliminationTree` objects in Python is less common than using the `eliminateSequential` method on a `FactorGraph`, which uses the `EliminationTree` internally." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "etree_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "etree_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "etree_import_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'GaussianEliminationTree' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[2], line 5\u001b[0m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# EliminationTree is templated, need concrete types\u001b[39;00m\n\u001b[1;32m----> 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m GaussianFactorGraph, Ordering, GaussianEliminationTree, GaussianBayesNet\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 8\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'GaussianEliminationTree' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# EliminationTree is templated, need concrete types\n", + "from gtsam import GaussianFactorGraph, Ordering, GaussianEliminationTree, GaussianBayesNet\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "etree_create_md" + }, + "source": [ + "## Creating an EliminationTree\n", + "\n", + "An `EliminationTree` is constructed from a `FactorGraph` and an `Ordering`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "etree_create_code", + "outputId": "f0123456-789a-bcde-f012-3456789abcde" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Elimination Tree: \n", + "Root(s):\n", + "Node (x2)\n", + "JacobianFactor(keys = [8070450532247928833; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "JacobianFactor(keys = [7783684379976990721; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Node (l2)\n", + " JacobianFactor(keys = [7783684379976990721; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Node (x1)\n", + " JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Node (l1)\n", + " JacobianFactor(keys = [7783684379976990720; 8070450532247928832], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + " Children:\n", + " Node (x0)\n", + " JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", + "\n" + ] + } + ], + "source": [ + "# Create a graph (same as BayesTree example)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "\n", + "# Define an ordering\n", + "ordering = Ordering([X(0), L(1), X(1), L(2), X(2)])\n", + "\n", + "# Construct the Elimination Tree\n", + "elimination_tree = GaussianEliminationTree(graph, ordering)\n", + "\n", + "elimination_tree.print(\"Elimination Tree: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "etree_eliminate_md" + }, + "source": [ + "## Elimination\n", + "\n", + "The primary use of an `EliminationTree` is to perform sequential elimination to produce a `BayesNet`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "etree_eliminate_code", + "outputId": "01234567-89ab-cdef-0123-456789abcdef" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "BayesNet from EliminationTree: size 5\n", + "Conditional 0: GaussianConditional( P(x0 | l1) = dx0 - R*dl1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "Conditional 1: GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "Conditional 2: GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", + "Conditional 3: GaussianConditional( P(l2 | x2) = dl2 - R*dx2 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", + "Conditional 4: GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.774597 ])\n", + "\n" + ] + } + ], + "source": [ + "# The eliminate function needs to be specified (e.g., EliminateGaussian)\n", + "# In Python, this is usually handled internally by graph.eliminateSequential\n", + "# but the C++ EliminationTree has an eliminate method.\n", + "\n", + "# Let's call the graph's eliminateSequential which uses the tree internally\n", + "bayes_net, remaining_graph = graph.eliminatePartialSequential(ordering)\n", + "\n", + "print(\"BayesNet from EliminationTree:\")\n", + "bayes_net.print()" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/Factor.ipynb b/gtsam/inference/doc/Factor.ipynb index 5a0c322ba..81812bcc2 100644 --- a/gtsam/inference/doc/Factor.ipynb +++ b/gtsam/inference/doc/Factor.ipynb @@ -1,178 +1,199 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "factor_intro_md" - }, - "source": [ - "# Factor" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "factor_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "factor_desc_md" - }, - "source": [ - "`gtsam.Factor` is the abstract base class for all factors in GTSAM, including nonlinear factors, Gaussian factors, discrete factors, and conditionals. It defines the basic interface common to all factors, primarily centered around the set of variables (keys) the factor involves.\n", - "\n", - "You typically do not instantiate `gtsam.Factor` directly but rather work with its derived classes like `gtsam.NonlinearFactor`, `gtsam.JacobianFactor`, `gtsam.DiscreteFactor`, etc." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "factor_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "factor_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "factor_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "from gtsam.utils.test_case import GtsamTestCase\n", - "\n", - "# We need a concrete factor type for demonstration\n", - "from gtsam import PriorFactorPose2, BetweenFactorPose2, Pose2, Rot2, Point2\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "factor_interface_md" - }, - "source": [ - "## Basic Interface\n", - "\n", - "All factors provide methods to access the keys of the variables they involve." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "factor_keys_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "01234567-89ab-cdef-0123-456789abcdef" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Prior factor keys: [8070450532247928832] (x0)\n", - "Prior factor size: 1\n", - "Between factor keys: [8070450532247928832, 8070450532247928833] (x0, x1)\n", - "Between factor size: 2\n", - "Is prior factor empty? False\n" - ] - } - ], - "source": [ - "noise_model = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n", - "\n", - "# Create some concrete factors\n", - "prior_factor = PriorFactorPose2(X(0), Pose2(0, 0, 0), noise_model)\n", - "between_factor = BetweenFactorPose2(X(0), X(1), Pose2(1, 0, 0), noise_model)\n", - "\n", - "# Access keys (methods inherited from gtsam.Factor)\n", - "prior_keys = prior_factor.keys()\n", - "print(f\"Prior factor keys: {prior_keys} ({gtsam.DefaultKeyFormatter(prior_keys[0])})\")\n", - "print(f\"Prior factor size: {prior_factor.size()}\")\n", - "\n", - "between_keys = between_factor.keys()\n", - "print(f\"Between factor keys: {between_keys} ({gtsam.DefaultKeyFormatter(between_keys[0])}, {gtsam.DefaultKeyFormatter(between_keys[1])})\")\n", - "print(f\"Between factor size: {between_factor.size()}\")\n", - "\n", - "print(f\"Is prior factor empty? {prior_factor.empty()}\")\n", - "\n", - "# Factors can be printed\n", - "# prior_factor.print(\"Prior Factor: \")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "factor_error_md" - }, - "source": [ - "## Error Function\n", - "\n", - "A key method for many factor types (especially nonlinear and Gaussian) is `error(Values)`. This evaluates the negative log-likelihood of the factor given a specific assignment of variable values. For optimization, the goal is typically to find the `Values` that minimize the total error (sum of errors from all factors)." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "factor_error_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "12345678-9abc-def0-1234-56789abcdef0" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Error at ground truth: 0.0\n", - "Error with incorrect x1: 50.0\n" - ] - } - ], - "source": [ - "values = gtsam.Values()\n", - "values.insert(X(0), Pose2(0, 0, 0))\n", - "values.insert(X(1), Pose2(1, 0, 0))\n", - "\n", - "# Evaluate error (example with BetweenFactor)\n", - "error1 = between_factor.error(values)\n", - "print(f\"Error at ground truth: {error1}\")\n", - "\n", - "# Change a value and recalculate error\n", - "values.update(X(1), Pose2(0, 0, 0))\n", - "error2 = between_factor.error(values)\n", - "print(f\"Error with incorrect x1: {error2:.1f}\")" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# Factor" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "factor_desc_md" + }, + "source": [ + "`gtsam.Factor` is the abstract base class for all factors in GTSAM, including nonlinear factors, Gaussian factors, discrete factors, and conditionals. It defines the basic interface common to all factors, primarily centered around the set of variables (keys) the factor involves.\n", + "\n", + "You typically do not instantiate `gtsam.Factor` directly but rather work with its derived classes like `gtsam.NonlinearFactor`, `gtsam.JacobianFactor`, `gtsam.DiscreteFactor`, etc.\n", + "\n", + "The `error` function of a factor is typically related to a probability or likelihood $P(X)$ or $\\phi(X)$ via the negative log-likelihood:\n", + "\n", + "$$\n", + "\\text{error}(X) = - \\log \\phi(X) + K\n", + "$$\n", + "or equivalently:\n", + "$$\n", + "\\phi(X) \\propto \\exp(-\\text{error}(X))\n", + "$$\n", + "where $X$ are the variables involved in the factor, $\\phi(X)$ is the potential function (proportional to probability or likelihood), and $K$ is some constant. Minimizing the error corresponds to maximizing the probability/likelihood represented by the factor." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "factor_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "id": "factor_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam.utils.test_case import GtsamTestCase\n", + "\n", + "# We need a concrete factor type for demonstration\n", + "from gtsam import PriorFactorPose2, BetweenFactorPose2, Pose2, Point3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_interface_md" + }, + "source": [ + "## Basic Interface\n", + "\n", + "All factors provide methods to access the keys of the variables they involve." + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "factor_keys_code", + "outputId": "01234567-89ab-cdef-0123-456789abcdef" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Prior factor keys: [8646911284551352320] (x0)\n", + "Prior factor size: 1\n", + "Between factor keys: [8646911284551352320, 8646911284551352321] (x0, x1)\n", + "Between factor size: 2\n", + "Is prior factor empty? False\n" + ] + } + ], + "source": [ + "noise_model = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n", + "\n", + "# Create some concrete factors\n", + "prior_factor = PriorFactorPose2(X(0), Pose2(0, 0, 0), noise_model)\n", + "between_factor = BetweenFactorPose2(X(0), X(1), Pose2(1, 0, 0), noise_model)\n", + "\n", + "# Access keys (methods inherited from gtsam.Factor)\n", + "prior_keys = prior_factor.keys()\n", + "print(f\"Prior factor keys: {prior_keys} ({gtsam.DefaultKeyFormatter(prior_keys[0])})\")\n", + "print(f\"Prior factor size: {prior_factor.size()}\")\n", + "\n", + "between_keys = between_factor.keys()\n", + "print(f\"Between factor keys: {between_keys} ({gtsam.DefaultKeyFormatter(between_keys[0])}, {gtsam.DefaultKeyFormatter(between_keys[1])})\")\n", + "print(f\"Between factor size: {between_factor.size()}\")\n", + "\n", + "print(f\"Is prior factor empty? {prior_factor.empty()}\")\n", + "\n", + "# Factors can be printed\n", + "# prior_factor.print(\"Prior Factor: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_error_md" + }, + "source": [ + "## Error Function\n", + "\n", + "A key method for many factor types (especially nonlinear and Gaussian) is `error(Values)`. This evaluates the negative log-likelihood of the factor given a specific assignment of variable values. For optimization, the goal is typically to find the `Values` that minimize the total error (sum of errors from all factors)." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "factor_error_code", + "outputId": "12345678-9abc-def0-1234-56789abcdef0" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error at ground truth: 0.0\n", + "Error with incorrect x1: 50.0\n" + ] + } + ], + "source": [ + "values = gtsam.Values()\n", + "values.insert(X(0), Pose2(0, 0, 0))\n", + "values.insert(X(1), Pose2(1, 0, 0))\n", + "\n", + "# Evaluate error (example with BetweenFactor)\n", + "error1 = between_factor.error(values)\n", + "print(f\"Error at ground truth: {error1}\")\n", + "\n", + "# Change a value and recalculate error\n", + "values.update(X(1), Pose2(0, 0, 0))\n", + "error2 = between_factor.error(values)\n", + "print(f\"Error with incorrect x1: {error2:.1f}\")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/FactorGraph.ipynb b/gtsam/inference/doc/FactorGraph.ipynb index a58273310..01e111e1e 100644 --- a/gtsam/inference/doc/FactorGraph.ipynb +++ b/gtsam/inference/doc/FactorGraph.ipynb @@ -1,298 +1,374 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "fg_intro_md" - }, - "source": [ - "# FactorGraph" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "fg_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "fg_desc_md" - }, - "source": [ - "A `FactorGraph` represents a factor graph, a bipartite graph connecting variables and factors. In GTSAM, the `FactorGraph` class (and its templated instantiations like `GaussianFactorGraph`, `NonlinearFactorGraph`, etc.) primarily stores a collection of factors.\n", - "\n", - "This class serves as the base for different types of factor graphs. You typically work with specific instantiations like `gtsam.GaussianFactorGraph` or `gtsam.NonlinearFactorGraph`." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "fg_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "fg_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "fg_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "import numpy as np\n", - "\n", - "# Example uses NonlinearFactorGraph, but concepts apply to others\n", - "from gtsam import NonlinearFactorGraph, PriorFactorPose2, BetweenFactorPose2, Pose2, Point3\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "fg_init_md" - }, - "source": [ - "## Initialization and Adding Factors\n", - "\n", - "A `FactorGraph` is typically created empty and factors are added individually or from containers." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "fg_create_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "23456789-abcd-ef01-2345-6789abcdef01" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Graph size after adding factors: 3\n" - ] - } - ], - "source": [ - "graph = NonlinearFactorGraph()\n", - "\n", - "# Define noise models\n", - "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n", - "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.2, 0.2, 0.1))\n", - "\n", - "# Create factors\n", - "factor1 = PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise)\n", - "factor2 = BetweenFactorPose2(X(0), X(1), Pose2(1, 0, 0), odometry_noise)\n", - "factor3 = BetweenFactorPose2(X(1), X(2), Pose2(1, 0, 0), odometry_noise)\n", - "\n", - "# Add factors to the graph\n", - "graph.add(factor1) # add is synonym for push_back\n", - "graph.push_back(factor2)\n", - "\n", - "# Can also add using += or ,\n", - "graph += factor3\n", - "# graph += factor2, factor3 # Chaining also works\n", - "\n", - "print(f\"Graph size after adding factors: {graph.size()}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "fg_access_md" - }, - "source": [ - "## Accessing Factors and Properties" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "fg_access_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "3456789a-bcde-f012-3456-789abcdef012" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Is graph empty? False\n", - "Number of factors (size): 3\n", - "Number of non-null factors (nrFactors): 3\n", - "Factor at index 1: \n", - "BetweenFactor on 8070450532247928832 8070450532247928833\n", - " measured: (1, 0, 0)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1]\n", - "\n", - "Keys involved in the graph: {8070450532247928832, 8070450532247928833, 8070450532247928834}\n" - ] - } - ], - "source": [ - "print(f\"Is graph empty? {graph.empty()}\")\n", - "print(f\"Number of factors (size): {graph.size()}\")\n", - "print(f\"Number of non-null factors (nrFactors): {graph.nrFactors()}\") # Useful if factors were removed\n", - "\n", - "# Access factor by index\n", - "retrieved_factor = graph.at(1)\n", - "print(\"Factor at index 1: \")\n", - "retrieved_factor.print()\n", - "\n", - "# Get all unique keys involved in the graph\n", - "all_keys = graph.keys() # Returns a KeySet\n", - "print(f\"Keys involved in the graph: {all_keys}\")\n", - "\n", - "# Iterate through factors\n", - "# for i, factor in enumerate(graph):\n", - "# if factor:\n", - "# print(f\"Factor {i} keys: {factor.keys()}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "fg_error_md" - }, - "source": [ - "## Graph Error\n", - "\n", - "The `error(Values)` method calculates the total error of the graph for a given assignment of variable values. This is the sum of the errors from each individual factor." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "fg_error_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "456789ab-cdef-0123-4567-89abcdef0123" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Total graph error at ground truth: 0.0\n", - "Total graph error with incorrect x2: 50.0\n" - ] - } - ], - "source": [ - "values = gtsam.Values()\n", - "values.insert(X(0), Pose2(0, 0, 0))\n", - "values.insert(X(1), Pose2(1, 0, 0))\n", - "values.insert(X(2), Pose2(2, 0, 0))\n", - "\n", - "total_error1 = graph.error(values)\n", - "print(f\"Total graph error at ground truth: {total_error1}\")\n", - "\n", - "# Introduce an error\n", - "values.update(X(2), Pose2(1, 0, 0))\n", - "total_error2 = graph.error(values)\n", - "print(f\"Total graph error with incorrect x2: {total_error2:.1f}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "fg_viz_md" - }, - "source": [ - "## Graph Visualization\n", - "\n", - "Factor graphs can be visualized using Graphviz via the `dot()` method." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "fg_dot_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "56789abc-def0-1234-5678-9abcdef01234" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "graph {\n", - " size=\"5,5\";\n", - "\n", - " var8070450532247928832[label=\"x0\"];\n", - " var8070450532247928833[label=\"x1\"];\n", - " var8070450532247928834[label=\"x2\"];\n", - "\n", - " factor0[label=\"\", shape=point];\n", - " var8070450532247928832--factor0;\n", - " factor1[label=\"\", shape=point];\n", - " var8070450532247928832--factor1;\n", - " var8070450532247928833--factor1;\n", - " factor2[label=\"\", shape=point];\n", - " var8070450532247928833--factor2;\n", - " var8070450532247928834--factor2;\n", - "}\n" - ] - } - ], - "source": [ - "dot_string = graph.dot()\n", - "print(dot_string)\n", - "\n", - "# To render, save dot_string to a file (e.g., graph.dot) and run:\n", - "# dot -Tpng graph.dot -o graph.png\n", - "# Or use a Python library like graphviz\n", - "# import graphviz\n", - "# graphviz.Source(dot_string)" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "fg_elim_md" - }, - "source": [ - "## Elimination\n", - "\n", - "A key purpose of factor graphs is inference via variable elimination. `FactorGraph` itself doesn't perform elimination, but its derived classes (like `GaussianFactorGraph`, `SymbolicFactorGraph`) inherit `eliminateSequential` and `eliminateMultifrontal` methods from `EliminateableFactorGraph`." - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# FactorGraph" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "fg_desc_md" + }, + "source": [ + "A `FactorGraph` represents a factor graph, a bipartite graph connecting variables and factors. In GTSAM, the `FactorGraph` class (and its templated instantiations like `GaussianFactorGraph`, `NonlinearFactorGraph`, etc.) primarily stores a collection of factors.\n", + "\n", + "This class serves as the base for different types of factor graphs. You typically work with specific instantiations like `gtsam.GaussianFactorGraph` or `gtsam.NonlinearFactorGraph`.\n", + "\n", + "The total probability $P(X)$ represented by a factor graph is proportional to the product of its individual factor potentials $\\phi_i$:\n", + "$$\n", + "P(X) \\propto \\prod_i \\phi_i(X_i)\n", + "$$\n", + "where $X_i$ are the variables involved in factor $i$. In terms of error (negative log-likelihood):\n", + "$$\n", + "P(X) \\propto \\exp\\left(-\\sum_i \\text{error}_i(X_i)\\right)\n", + "$$\n", + "The total error for the graph given an assignment $X$ is the sum of the errors of the individual factors:\n", + "$$\n", + "\\text{error}(X) = \\sum_i \\text{error}_i(X_i)\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "fg_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "fg_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# Example uses NonlinearFactorGraph, but concepts apply to others\n", + "from gtsam import NonlinearFactorGraph, PriorFactorPose2, BetweenFactorPose2, Pose2, Point3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_init_md" + }, + "source": [ + "## Initialization and Adding Factors\n", + "\n", + "A `FactorGraph` is typically created empty and factors are added individually or from containers." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "fg_create_code", + "outputId": "23456789-abcd-ef01-2345-6789abcdef01" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Graph size after adding factors: 2\n" + ] + } + ], + "source": [ + "graph = NonlinearFactorGraph()\n", + "\n", + "# Define noise models\n", + "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n", + "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.2, 0.2, 0.1))\n", + "\n", + "# Create factors\n", + "factor1 = PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise)\n", + "factor2 = BetweenFactorPose2(X(0), X(1), Pose2(1, 0, 0), odometry_noise)\n", + "factor3 = BetweenFactorPose2(X(1), X(2), Pose2(1, 0, 0), odometry_noise)\n", + "\n", + "# Add factors to the graph\n", + "graph.add(factor1) # add is synonym for push_back\n", + "graph.push_back(factor2)\n", + "\n", + "print(f\"Graph size after adding factors: {graph.size()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_access_md" + }, + "source": [ + "## Accessing Factors and Properties" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "fg_access_code", + "outputId": "3456789a-bcde-f012-3456-789abcdef012" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Is graph empty? False\n", + "Number of factors (size): 2\n", + "Number of non-null factors (nrFactors): 2\n", + "Factor at index 1: \n", + "BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Keys involved in the graph: x0x1\n" + ] + } + ], + "source": [ + "print(f\"Is graph empty? {graph.empty()}\")\n", + "print(f\"Number of factors (size): {graph.size()}\")\n", + "print(f\"Number of non-null factors (nrFactors): {graph.nrFactors()}\") # Useful if factors were removed\n", + "\n", + "# Access factor by index\n", + "retrieved_factor = graph.at(1)\n", + "print(\"Factor at index 1: \")\n", + "retrieved_factor.print()\n", + "\n", + "# Get all unique keys involved in the graph\n", + "all_keys = graph.keys() # Returns a KeySet\n", + "print(f\"Keys involved in the graph: {all_keys}\")\n", + "\n", + "# Iterate through factors\n", + "# for i, factor in enumerate(graph):\n", + "# if factor:\n", + "# print(f\"Factor {i} keys: {factor.keys()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_error_md" + }, + "source": [ + "## Graph Error\n", + "\n", + "The `error(Values)` method calculates the total error of the graph for a given assignment of variable values. This is the sum of the errors from each individual factor." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "fg_error_code", + "outputId": "456789ab-cdef-0123-4567-89abcdef0123" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Total graph error at ground truth: 0.0\n", + "Total graph error with incorrect x2: 0.0\n" + ] + } + ], + "source": [ + "values = gtsam.Values()\n", + "values.insert(X(0), Pose2(0, 0, 0))\n", + "values.insert(X(1), Pose2(1, 0, 0))\n", + "values.insert(X(2), Pose2(2, 0, 0))\n", + "\n", + "total_error1 = graph.error(values)\n", + "print(f\"Total graph error at ground truth: {total_error1}\")\n", + "\n", + "# Introduce an error\n", + "values.update(X(2), Pose2(1, 0, 0))\n", + "total_error2 = graph.error(values)\n", + "print(f\"Total graph error with incorrect x2: {total_error2:.1f}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_viz_md" + }, + "source": [ + "## Graph Visualization\n", + "\n", + "Factor graphs can be visualized using Graphviz via the `dot()` method." + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "fg_dot_code", + "outputId": "56789abc-def0-1234-5678-9abcdef01234" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "graph {\n", + " size=\"5,5\";\n", + "\n", + " var8646911284551352320[label=\"x0\", pos=\"0,0!\"];\n", + " var8646911284551352321[label=\"x1\", pos=\"0,1!\"];\n", + "\n", + " factor0[label=\"\", shape=point];\n", + " var8646911284551352320--factor0;\n", + " factor1[label=\"\", shape=point];\n", + " var8646911284551352320--factor1;\n", + " var8646911284551352321--factor1;\n", + "}\n", + "\n" + ] + }, + { + "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", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor1\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "dot_string = graph.dot(values)\n", + "print(dot_string)\n", + "\n", + "# To render, save dot_string to a file (e.g., graph.dot) and run:\n", + "# dot -Tpng graph.dot -o graph.png\n", + "# Or use a Python library like graphviz\n", + "import graphviz\n", + "graphviz.Source(dot_string)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fg_elim_md" + }, + "source": [ + "## Elimination\n", + "\n", + "A key purpose of factor graphs is inference via variable elimination. `FactorGraph` itself doesn't perform elimination, but its derived classes (like `GaussianFactorGraph`, `SymbolicFactorGraph`) inherit `eliminateSequential` and `eliminateMultifrontal` methods from `EliminateableFactorGraph`." + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/ISAM.ipynb b/gtsam/inference/doc/ISAM.ipynb index 82d841fad..3d1e4cddf 100644 --- a/gtsam/inference/doc/ISAM.ipynb +++ b/gtsam/inference/doc/ISAM.ipynb @@ -1,276 +1,253 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "isam_intro_md" - }, - "source": [ - "# ISAM" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "isam_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "isam_desc_md" - }, - "source": [ - "`gtsam.ISAM` (Incremental Smoothing and Mapping) is a class that inherits from `BayesTree` and adds an `update` method. This method allows for efficient incremental updates to the solution when new factors (e.g., new measurements) are added to the problem.\n", - "\n", - "Instead of re-eliminating the entire factor graph from scratch, iSAM identifies the part of the Bayes tree affected by the new factors, removes that part, and re-eliminates only the necessary variables, merging the results back into the existing tree.\n", - "\n", - "Like `BayesTree`, it's templated (e.g., `GaussianISAM` which inherits from `GaussianBayesTree`). For practical applications requiring incremental updates, `ISAM2` is often preferred due to further optimizations like fluid relinearization and support for variable removal, but `ISAM` demonstrates the core incremental update concept based on the Bayes tree." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "isam_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "isam_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "isam_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "import numpy as np\n", - "\n", - "# Use Gaussian variants for demonstration\n", - "from gtsam import GaussianFactorGraph, Ordering, GaussianISAM, GaussianBayesTree\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "isam_create_md" - }, - "source": [ - "## Initialization\n", - "\n", - "An `ISAM` object can be created empty or initialized from an existing `BayesTree`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "isam_init_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "89abcdef-0123-4567-89ab-cdef01234567" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Initial ISAM (empty):\n", - ": cliques: 0, variables: 0\n", - "\n", - "Initial BayesTree:\n", - ": cliques: 1, variables: 1\n", - "Root(s):\n", - "Conditional density P(x0) = P(x0) \n", - " size: 1\n", - " Conditional P(x0): GaussianConditional( P(x0) = dx0 - d), d = [ 0 ], sigmas = [ 1 ])\n", - "\n", - "\n", - "ISAM from BayesTree:\n", - ": cliques: 1, variables: 1\n", - "Root(s):\n", - "Conditional density P(x0) = P(x0) \n", - " size: 1\n", - " Conditional P(x0): GaussianConditional( P(x0) = dx0 - d), d = [ 0 ], sigmas = [ 1 ])\n", - "\n", - "\n" - ] - } - ], - "source": [ - "# Create an empty ISAM object\n", - "isam1 = GaussianISAM()\n", - "print(\"Initial ISAM (empty):\")\n", - "isam1.print()\n", - "\n", - "# Create from an existing Bayes Tree (e.g., from an initial batch solve)\n", - "initial_graph = GaussianFactorGraph()\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "initial_graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n", - "\n", - "initial_bayes_tree = initial_graph.eliminateMultifrontal(Ordering([X(0)]))\n", - "print(\"Initial BayesTree:\")\n", - "initial_bayes_tree.print()\n", - "\n", - "isam2 = GaussianISAM(initial_bayes_tree)\n", - "print(\"ISAM from BayesTree:\")\n", - "isam2.print()" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "isam_update_md" - }, - "source": [ - "## Incremental Update\n", - "\n", - "The core functionality is the `update(newFactors)` method." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "isam_update_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "9abcdef0-1234-5678-9abc-def012345678" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "ISAM after first update (x0, x1):\n", - ": cliques: 2, variables: 2\n", - "Root(s):\n", - "Conditional density P(x0 | x1) = P(x0 | x1) \n", - " size: 1\n", - " Conditional P(x0 | x1): GaussianConditional( P(x0 | x1) = dx0 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.707107 ])\n", - "\n", - "\n", - " Children:\n", - " Conditional density P(x1) = P(x1) \n", - " size: 1\n", - " Conditional P(x1): GaussianConditional( P(x1) = dx1 - d), d = [ 0 ], sigmas = [ 0.707107 ])\n", - "\n", - "\n", - "\n", - "ISAM after second update (x0, x1, x2):\n", - ": cliques: 3, variables: 3\n", - "Root(s):\n", - "Conditional density P(x0 | x1) = P(x0 | x1) \n", - " size: 1\n", - " Conditional P(x0 | x1): GaussianConditional( P(x0 | x1) = dx0 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.707107 ])\n", - "\n", - "\n", - " Children:\n", - " Conditional density P(x1 | x2) = P(x1 | x2) \n", - " size: 1\n", - " Conditional P(x1 | x2): GaussianConditional( P(x1 | x2) = dx1 - R*dx2 - d), R = [ 0.666667 ], d = [ 0 ], sigmas = [ 0.816497 ])\n", - "\n", - "\n", - " Children:\n", - " Conditional density P(x2) = P(x2) \n", - " size: 1\n", - " Conditional P(x2): GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.866025 ])\n", - "\n", - "\n", - "\n" - ] - } - ], - "source": [ - "# Start with the ISAM object containing the prior on x0\n", - "isam = GaussianISAM(initial_bayes_tree)\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "\n", - "# --- First Update ---\n", - "new_factors1 = GaussianFactorGraph()\n", - "new_factors1.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # x0 -> x1\n", - "isam.update(new_factors1)\n", - "\n", - "print(\"ISAM after first update (x0, x1):\")\n", - "isam.print()\n", - "\n", - "# --- Second Update ---\n", - "new_factors2 = GaussianFactorGraph()\n", - "new_factors2.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # x1 -> x2\n", - "isam.update(new_factors2)\n", - "\n", - "print(\"\\nISAM after second update (x0, x1, x2):\")\n", - "isam.print()" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "isam_solve_md" - }, - "source": [ - "## Solution and Marginals\n", - "\n", - "Since `ISAM` inherits from `BayesTree`, you can use the same methods like `optimize()` and `marginalFactor()` after performing updates." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "isam_solve_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "abcdef01-2345-6789-abcd-ef0123456789" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Optimized Solution after updates:\n", - "Values with 3 values:\n", - "Value x0: [0.]\n", - "Value x1: [0.]\n", - "Value x2: [0.]\n", - "\n" - ] - } - ], - "source": [ - "# Get the solution from the final ISAM state\n", - "solution = isam.optimize()\n", - "print(\"Optimized Solution after updates:\")\n", - "solution.print()" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# ISAM" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "isam_desc_md" + }, + "source": [ + "`gtsam.ISAM` (Incremental Smoothing and Mapping) is a class that inherits from `BayesTree` and adds an `update` method. This method allows for efficient incremental updates to the solution when new factors (e.g., new measurements) are added to the problem.\n", + "\n", + "Instead of re-eliminating the entire factor graph from scratch, iSAM identifies the part of the Bayes tree affected by the new factors, removes that part, and re-eliminates only the necessary variables, merging the results back into the existing tree.\n", + "\n", + "Like `BayesTree`, it's templated (e.g., `GaussianISAM` which inherits from `GaussianBayesTree`). For practical applications requiring incremental updates, `ISAM2` is often preferred due to further optimizations like fluid relinearization and support for variable removal, but `ISAM` demonstrates the core incremental update concept based on the Bayes tree." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "isam_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "isam_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "isam_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# Use Gaussian variants for demonstration\n", + "from gtsam import GaussianFactorGraph, Ordering, GaussianISAM, GaussianBayesTree\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "isam_create_md" + }, + "source": [ + "## Initialization\n", + "\n", + "An `ISAM` object can be created empty or initialized from an existing `BayesTree`." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "isam_init_code", + "outputId": "89abcdef-0123-4567-89ab-cdef01234567" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initial BayesTree:\n", + ": cliques: 1, variables: 1\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" + ] + }, + { + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.GaussianISAM()\n\nInvoked with: : cliques: 1, variables: 1\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", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[5], line 13\u001b[0m\n\u001b[0;32m 10\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mInitial BayesTree:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 11\u001b[0m initial_bayes_tree\u001b[38;5;241m.\u001b[39mprint()\n\u001b[1;32m---> 13\u001b[0m isam2 \u001b[38;5;241m=\u001b[39m \u001b[43mGaussianISAM\u001b[49m\u001b[43m(\u001b[49m\u001b[43minitial_bayes_tree\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mISAM from BayesTree:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 15\u001b[0m isam2\u001b[38;5;241m.\u001b[39mprint()\n", + "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.GaussianISAM()\n\nInvoked with: : cliques: 1, variables: 1\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" + ] + } + ], + "source": [ + "# Create an empty ISAM object\n", + "isam1 = GaussianISAM()\n", + "\n", + "# Create from an existing Bayes Tree (e.g., from an initial batch solve)\n", + "initial_graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "initial_graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n", + "\n", + "initial_bayes_tree = initial_graph.eliminateMultifrontal(Ordering([X(0)]))\n", + "print(\"Initial BayesTree:\")\n", + "initial_bayes_tree.print()\n", + "\n", + "isam2 = GaussianISAM(initial_bayes_tree)\n", + "print(\"ISAM from BayesTree:\")\n", + "isam2.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "isam_update_md" + }, + "source": [ + "## Incremental Update\n", + "\n", + "The core functionality is the `update(newFactors)` method." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "isam_update_code", + "outputId": "9abcdef0-1234-5678-9abc-def012345678" + }, + "outputs": [ + { + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.GaussianISAM()\n\nInvoked with: : cliques: 1, variables: 1\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", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[6], line 2\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;66;03m# Start with the ISAM object containing the prior on x0\u001b[39;00m\n\u001b[1;32m----> 2\u001b[0m isam \u001b[38;5;241m=\u001b[39m \u001b[43mGaussianISAM\u001b[49m\u001b[43m(\u001b[49m\u001b[43minitial_bayes_tree\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 3\u001b[0m model \u001b[38;5;241m=\u001b[39m gtsam\u001b[38;5;241m.\u001b[39mnoiseModel\u001b[38;5;241m.\u001b[39mIsotropic\u001b[38;5;241m.\u001b[39mSigma(\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m1.0\u001b[39m)\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# --- First Update ---\u001b[39;00m\n", + "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.GaussianISAM()\n\nInvoked with: : cliques: 1, variables: 1\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" + ] + } + ], + "source": [ + "# Start with the ISAM object containing the prior on x0\n", + "isam = GaussianISAM(initial_bayes_tree)\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "\n", + "# --- First Update ---\n", + "new_factors1 = GaussianFactorGraph()\n", + "new_factors1.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # x0 -> x1\n", + "isam.update(new_factors1)\n", + "\n", + "print(\"ISAM after first update (x0, x1):\")\n", + "isam.print()\n", + "\n", + "# --- Second Update ---\n", + "new_factors2 = GaussianFactorGraph()\n", + "new_factors2.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # x1 -> x2\n", + "isam.update(new_factors2)\n", + "\n", + "print(\"\\nISAM after second update (x0, x1, x2):\")\n", + "isam.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "isam_solve_md" + }, + "source": [ + "## Solution and Marginals\n", + "\n", + "Since `ISAM` inherits from `BayesTree`, you can use the same methods like `optimize()` and `marginalFactor()` after performing updates." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "isam_solve_code", + "outputId": "abcdef01-2345-6789-abcd-ef0123456789" + }, + "outputs": [ + { + "ename": "NameError", + "evalue": "name 'isam' is not defined", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mNameError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[7], line 2\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;66;03m# Get the solution from the final ISAM state\u001b[39;00m\n\u001b[1;32m----> 2\u001b[0m solution \u001b[38;5;241m=\u001b[39m \u001b[43misam\u001b[49m\u001b[38;5;241m.\u001b[39moptimize()\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOptimized Solution after updates:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m solution\u001b[38;5;241m.\u001b[39mprint()\n", + "\u001b[1;31mNameError\u001b[0m: name 'isam' is not defined" + ] + } + ], + "source": [ + "# Get the solution from the final ISAM state\n", + "solution = isam.optimize()\n", + "print(\"Optimized Solution after updates:\")\n", + "solution.print()" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/JunctionTree.ipynb b/gtsam/inference/doc/JunctionTree.ipynb index 26388a8bd..9229a9004 100644 --- a/gtsam/inference/doc/JunctionTree.ipynb +++ b/gtsam/inference/doc/JunctionTree.ipynb @@ -1,160 +1,166 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "jtree_intro_md" - }, - "source": [ - "# JunctionTree" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "jtree_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "jtree_desc_md" - }, - "source": [ - "A `JunctionTree` is an intermediate data structure used in GTSAM's multifrontal variable elimination. It is a `ClusterTree` where each node (cluster) corresponds to a clique in the chordal graph formed during elimination.\n", - "\n", - "Key differences from related structures:\n", - "* **vs. EliminationTree:** Junction tree nodes can represent the elimination of multiple variables simultaneously (a 'frontal' set), whereas elimination tree nodes typically represent single variable eliminations.\n", - "* **vs. BayesTree:** A JunctionTree node contains the original factors associated with the variables being eliminated in that clique. A BayesTree node contains the *result* of eliminating those factors (i.e., a conditional density $P(\text{Frontals} | \text{Separator})$).\n", - "\n", - "Like `EliminationTree`, direct manipulation of `JunctionTree` objects in Python is uncommon. It's primarily an internal structure used by `eliminateMultifrontal` when producing a `BayesTree`." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "jtree_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "jtree_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "jtree_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "import numpy as np\n", - "\n", - "# JunctionTree is templated, need concrete types\n", - "from gtsam import GaussianFactorGraph, Ordering, GaussianJunctionTree, GaussianBayesTree\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "jtree_create_md" - }, - "source": [ - "## Creating a JunctionTree\n", - "\n", - "A `JunctionTree` is typically constructed from an `EliminationTree` as part of the multifrontal elimination process. The direct constructor might not be exposed in Python, as it's usually created internally." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "jtree_create_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "12345678-9abc-def0-1234-56789abcdef0" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Resulting BayesTree (structure mirrors JunctionTree):\n", - ": cliques: 3, variables: 5\n", - "Root(s):\n", - "Conditional density P(l1, x1 | l2, x2) = P(l1 | x1) P(x1 | l2, x2) \n", - " size: 2\n", - " Conditional P(l1 | x1): GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "\n", - " Conditional P(x1 | l2, x2): GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", - "\n", - "\n", - " Children:\n", - " Conditional density P(l2, x2 | x0) = P(l2 | x2) P(x2 | x0) \n", - " size: 2\n", - " Conditional P(l2 | x2): GaussianConditional( P(l2 | x2) = dl2 - R*dx2 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "\n", - " Conditional P(x2 | x0): GaussianConditional( P(x2 | x0) = dx2 - R*dx0 - d), R = [ 0.2 ], d = [ 0 ], sigmas = [ 0.774597 ])\n", - "\n", - "\n", - " Children:\n", - " Conditional density P(x0) = P(x0) \n", - " size: 1\n", - " Conditional P(x0): GaussianConditional( P(x0) = dx0 - d), d = [ 0 ], sigmas = [ 0.894427 ])\n", - "\n", - "\n", - "\n" - ] - } - ], - "source": [ - "# Create a graph (same as BayesTree example)\n", - "graph = GaussianFactorGraph()\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", - "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "\n", - "ordering = Ordering.Colamd(graph)\n", - "\n", - "# Perform multifrontal elimination, which uses a JunctionTree internally\n", - "bayes_tree, remaining_graph = graph.eliminatePartialMultifrontal(ordering)\n", - "\n", - "# The resulting BayesTree reflects the structure of the intermediate JunctionTree\n", - "print(\"Resulting BayesTree (structure mirrors JunctionTree):\")\n", - "bayes_tree.print()\n", - "\n", - "# Accessing the JunctionTree directly isn't typical in Python workflows.\n", - "# Its structure is implicitly captured by the BayesTree cliques." - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# JunctionTree" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "jtree_desc_md" + }, + "source": [ + "A `JunctionTree` is an intermediate data structure used in GTSAM's multifrontal variable elimination. It is a `ClusterTree` where each node (cluster) corresponds to a clique in the chordal graph formed during elimination.\n", + "\n", + "Key differences from related structures:\n", + "* **vs. EliminationTree:** Junction tree nodes can represent the elimination of multiple variables simultaneously (a 'frontal' set), whereas elimination tree nodes typically represent single variable eliminations.\n", + "* **vs. BayesTree:** A JunctionTree node contains the original factors associated with the variables being eliminated in that clique. A BayesTree node contains the *result* of eliminating those factors (i.e., a conditional density $P(\text{Frontals} | \text{Separator})$).\n", + "\n", + "Like `EliminationTree`, direct manipulation of `JunctionTree` objects in Python is uncommon. It's primarily an internal structure used by `eliminateMultifrontal` when producing a `BayesTree`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "jtree_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "jtree_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "id": "jtree_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "\n", + "# JunctionTree is templated, need concrete types\n", + "from gtsam import GaussianFactorGraph, Ordering, VariableIndex\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "jtree_create_md" + }, + "source": [ + "## Creating a JunctionTree\n", + "\n", + "A `JunctionTree` is typically constructed from an `EliminationTree` as part of the multifrontal elimination process. The direct constructor might not be exposed in Python, as it's usually created internally." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "jtree_create_code", + "outputId": "12345678-9abc-def0-1234-56789abcdef0" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Resulting BayesTree (structure mirrors JunctionTree):\n", + ": cliques: 2, variables: 5\n", + "- p(x1 l2 x2 )\n", + " R = [ 1.61245 -0.620174 -0.620174 ]\n", + " [ 0 1.27098 -1.08941 ]\n", + " [ 0 0 0.654654 ]\n", + " d = [ 0 0 0 ]\n", + " mean: 3 elements\n", + " l2: 0\n", + " x1: 0\n", + " x2: 0\n", + " logNormalizationConstant: -2.46292\n", + " No noise model\n", + "| - p(l1 x0 | x1)\n", + " R = [ 1.41421 -0.707107 ]\n", + " [ 0 1.58114 ]\n", + " S[x1] = [ -0.707107 ]\n", + " [ -0.948683 ]\n", + " d = [ 0 0 ]\n", + " logNormalizationConstant: -1.03316\n", + " No noise model\n" + ] + } + ], + "source": [ + "# Create a graph (same as BayesTree example)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "\n", + "ordering = Ordering.Colamd(VariableIndex(graph))\n", + "\n", + "# Perform multifrontal elimination, which uses a JunctionTree internally\n", + "bayes_tree, remaining_graph = graph.eliminatePartialMultifrontal(ordering)\n", + "\n", + "# The resulting BayesTree reflects the structure of the intermediate JunctionTree\n", + "print(\"Resulting BayesTree (structure mirrors JunctionTree):\")\n", + "bayes_tree.print()\n", + "\n", + "# Accessing the JunctionTree directly isn't typical in Python workflows.\n", + "# Its structure is implicitly captured by the BayesTree cliques." + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/Key.ipynb b/gtsam/inference/doc/Key.ipynb index d5a6991b4..2acbd8c63 100644 --- a/gtsam/inference/doc/Key.ipynb +++ b/gtsam/inference/doc/Key.ipynb @@ -1,202 +1,218 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "key_intro_md" - }, - "source": [ - "# Key" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "key_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "key_desc_md" - }, - "source": [ - "A `Key` in GTSAM is simply a `typedef` for `std::uint64_t`. It serves as a unique identifier for variables within a factor graph or values within a `Values` container. While you can use raw integer keys, GTSAM provides helper classes like `Symbol` and `LabeledSymbol` to create semantically meaningful keys that encode type and index information within the 64-bit integer." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "key_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "key_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "key_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "from gtsam import Symbol, LabeledSymbol\n", - "import numpy as np" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "key_usage_md" - }, - "source": [ - "## Basic Usage\n", - "\n", - "Keys are typically created using `Symbol` or `LabeledSymbol` and then implicitly or explicitly cast to the `Key` type (integer)." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "key_create_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "a1b2c3d4-e5f6-7890-abcd-ef1234567890" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Symbol Key (x0): 8070450532247928832\n", - "Type: \n", - "LabeledSymbol Key (aB1): 6988956933745737729\n", - "Type: \n", - "Plain Integer Key: 12345\n", - "Type: \n" - ] - } - ], - "source": [ - "sym = Symbol('x', 0)\n", - "key_from_symbol = sym.key() # Or just 'sym' where a Key is expected\n", - "print(f\"Symbol Key (x0): {key_from_symbol}\")\n", - "print(f\"Type: {type(key_from_symbol)}\")\n", - "\n", - "lsym = LabeledSymbol('a', 'B', 1)\n", - "key_from_labeled_symbol = lsym.key()\n", - "print(f\"LabeledSymbol Key (aB1): {key_from_labeled_symbol}\")\n", - "print(f\"Type: {type(key_from_labeled_symbol)}\")\n", - "\n", - "# You can also use plain integers, but it's less descriptive\n", - "plain_key = 12345\n", - "print(f\"Plain Integer Key: {plain_key}\")\n", - "print(f\"Type: {type(plain_key)}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "key_formatter_md" - }, - "source": [ - "## Key Formatting\n", - "\n", - "When printing GTSAM objects that contain keys (like Factor Graphs or Values), you can specify a `KeyFormatter` to control how keys are displayed. The default formatter tries to interpret keys as `Symbol`s. `MultiRobotKeyFormatter` also checks for `LabeledSymbol`s." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "key_format_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "b2c3d4e5-f6a7-8901-bcde-f12345678901" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Default Formatter:\n", - " Symbol Key: x0\n", - " LabeledSymbol Key: 6988956933745737729 (Default doesn't know LabeledSymbol)\n", - " Plain Key: 12345\n", - "\n", - "MultiRobot Formatter:\n", - " Symbol Key: x0\n", - " LabeledSymbol Key: aB1\n", - " Plain Key: 12345\n", - "\n", - "Custom Formatter:\n", - " Symbol Key: KEY[x0]\n", - " LabeledSymbol Key: KEY[aB1]\n", - " Plain Key: KEY[12345]\n" - ] - } - ], - "source": [ - "print(\"Default Formatter:\")\n", - "print(f\" Symbol Key: {gtsam.DefaultKeyFormatter(key_from_symbol)}\")\n", - "print(f\" LabeledSymbol Key: {gtsam.DefaultKeyFormatter(key_from_labeled_symbol)}\")\n", - "print(f\" Plain Key: {gtsam.DefaultKeyFormatter(plain_key)}\")\n", - "\n", - "print(\"MultiRobot Formatter:\")\n", - "print(f\" Symbol Key: {gtsam.MultiRobotKeyFormatter(key_from_symbol)}\")\n", - "print(f\" LabeledSymbol Key: {gtsam.MultiRobotKeyFormatter(key_from_labeled_symbol)}\")\n", - "print(f\" Plain Key: {gtsam.MultiRobotKeyFormatter(plain_key)}\")\n", - "\n", - "# Example of a custom formatter\n", - "def my_formatter(key):\n", - " # Try interpreting as LabeledSymbol, then Symbol, then default\n", - " try:\n", - " lsym = gtsam.LabeledSymbol(key)\n", - " if lsym.label() != 0: # Check if it's likely a valid LabeledSymbol\n", - " return f\"KEY[{lsym.string()}]\"\n", - " except:\n", - " pass\n", - " try:\n", - " sym = gtsam.Symbol(key)\n", - " if sym.chr() != 0: # Check if it's likely a valid Symbol\n", - " return f\"KEY[{sym.string()}]\"\n", - " except:\n", - " pass\n", - " return f\"KEY[{key}]\"\n", - "\n", - "print(\"Custom Formatter:\")\n", - "print(f\" Symbol Key: {my_formatter(key_from_symbol)}\")\n", - "print(f\" LabeledSymbol Key: {my_formatter(key_from_labeled_symbol)}\")\n", - "print(f\" Plain Key: {my_formatter(plain_key)}\")\n", - "\n", - "# KeyVectors, KeyLists, KeySets can also be printed using formatters\n", - "key_vector = gtsam.KeyVector([key_from_symbol, key_from_labeled_symbol, plain_key])\n", - "# key_vector.print(\"My Vector: \", my_formatter) # .print() method uses formatter directly" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# Key" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "key_desc_md" + }, + "source": [ + "A `Key` in GTSAM is simply a `typedef` for `std::uint64_t`. It serves as a unique identifier for variables within a factor graph or values within a `Values` container. While you can use raw integer keys, GTSAM provides helper classes like `Symbol` and `LabeledSymbol` to create semantically meaningful keys that encode type and index information within the 64-bit integer." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "key_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "key_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "key_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import Symbol, LabeledSymbol\n", + "import numpy as np" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "key_usage_md" + }, + "source": [ + "## Basic Usage\n", + "\n", + "Keys are typically created using `Symbol` or `LabeledSymbol` and then implicitly or explicitly cast to the `Key` type (integer)." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "key_create_code", + "outputId": "a1b2c3d4-e5f6-7890-abcd-ef1234567890" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbol Key (x0): 8646911284551352320\n", + "Type: \n" + ] + }, + { + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'a', 'B', 1", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[3], line 6\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mSymbol Key (x0): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mkey_from_symbol\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mType: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[38;5;28mtype\u001b[39m(key_from_symbol)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m----> 6\u001b[0m lsym \u001b[38;5;241m=\u001b[39m \u001b[43mLabeledSymbol\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43ma\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mB\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m1\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[0;32m 7\u001b[0m key_from_labeled_symbol \u001b[38;5;241m=\u001b[39m lsym\u001b[38;5;241m.\u001b[39mkey()\n\u001b[0;32m 8\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mLabeledSymbol Key (aB1): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mkey_from_labeled_symbol\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", + "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'a', 'B', 1" + ] + } + ], + "source": [ + "sym = Symbol('x', 0)\n", + "key_from_symbol = sym.key() # Or just 'sym' where a Key is expected\n", + "print(f\"Symbol Key (x0): {key_from_symbol}\")\n", + "print(f\"Type: {type(key_from_symbol)}\")\n", + "\n", + "lsym = LabeledSymbol('a', 'B', 1)\n", + "key_from_labeled_symbol = lsym.key()\n", + "print(f\"LabeledSymbol Key (aB1): {key_from_labeled_symbol}\")\n", + "print(f\"Type: {type(key_from_labeled_symbol)}\")\n", + "\n", + "# You can also use plain integers, but it's less descriptive\n", + "plain_key = 12345\n", + "print(f\"Plain Integer Key: {plain_key}\")\n", + "print(f\"Type: {type(plain_key)}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "key_formatter_md" + }, + "source": [ + "## Key Formatting\n", + "\n", + "When printing GTSAM objects that contain keys (like Factor Graphs or Values), you can specify a `KeyFormatter` to control how keys are displayed. The default formatter tries to interpret keys as `Symbol`s. `MultiRobotKeyFormatter` also checks for `LabeledSymbol`s." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "key_format_code", + "outputId": "b2c3d4e5-f6a7-8901-bcde-f12345678901" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Default Formatter:\n", + " Symbol Key: x0\n" + ] + }, + { + "ename": "NameError", + "evalue": "name 'key_from_labeled_symbol' is not defined", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mNameError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[4], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mDefault Formatter:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m Symbol Key: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mgtsam\u001b[38;5;241m.\u001b[39mDefaultKeyFormatter(key_from_symbol)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m LabeledSymbol Key: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mgtsam\u001b[38;5;241m.\u001b[39mDefaultKeyFormatter(\u001b[43mkey_from_labeled_symbol\u001b[49m)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m Plain Key: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mgtsam\u001b[38;5;241m.\u001b[39mDefaultKeyFormatter(plain_key)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mMultiRobot Formatter:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n", + "\u001b[1;31mNameError\u001b[0m: name 'key_from_labeled_symbol' is not defined" + ] + } + ], + "source": [ + "print(\"Default Formatter:\")\n", + "print(f\" Symbol Key: {gtsam.DefaultKeyFormatter(key_from_symbol)}\")\n", + "print(f\" LabeledSymbol Key: {gtsam.DefaultKeyFormatter(key_from_labeled_symbol)}\")\n", + "print(f\" Plain Key: {gtsam.DefaultKeyFormatter(plain_key)}\")\n", + "\n", + "print(\"MultiRobot Formatter:\")\n", + "print(f\" Symbol Key: {gtsam.MultiRobotKeyFormatter(key_from_symbol)}\")\n", + "print(f\" LabeledSymbol Key: {gtsam.MultiRobotKeyFormatter(key_from_labeled_symbol)}\")\n", + "print(f\" Plain Key: {gtsam.MultiRobotKeyFormatter(plain_key)}\")\n", + "\n", + "# Example of a custom formatter\n", + "def my_formatter(key):\n", + " # Try interpreting as LabeledSymbol, then Symbol, then default\n", + " try:\n", + " lsym = gtsam.LabeledSymbol(key)\n", + " if lsym.label() != 0: # Check if it's likely a valid LabeledSymbol\n", + " return f\"KEY[{lsym.string()}]\"\n", + " except:\n", + " pass\n", + " try:\n", + " sym = gtsam.Symbol(key)\n", + " if sym.chr() != 0: # Check if it's likely a valid Symbol\n", + " return f\"KEY[{sym.string()}]\"\n", + " except:\n", + " pass\n", + " return f\"KEY[{key}]\"\n", + "\n", + "print(\"Custom Formatter:\")\n", + "print(f\" Symbol Key: {my_formatter(key_from_symbol)}\")\n", + "print(f\" LabeledSymbol Key: {my_formatter(key_from_labeled_symbol)}\")\n", + "print(f\" Plain Key: {my_formatter(plain_key)}\")\n", + "\n", + "# KeyVectors, KeyLists, KeySets can also be printed using formatters\n", + "key_vector = gtsam.KeyVector([key_from_symbol, key_from_labeled_symbol, plain_key])\n", + "# key_vector.print(\"My Vector: \", my_formatter) # .print() method uses formatter directly" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/LabeledSymbol.ipynb b/gtsam/inference/doc/LabeledSymbol.ipynb index 9c2e50469..1c49fdff9 100644 --- a/gtsam/inference/doc/LabeledSymbol.ipynb +++ b/gtsam/inference/doc/LabeledSymbol.ipynb @@ -1,200 +1,217 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "lsymbol_intro_md" - }, - "source": [ - "# LabeledSymbol" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "lsymbol_desc_md" - }, - "source": [ - "A `LabeledSymbol` is a specialized version of `gtsam.Symbol` designed primarily for multi-robot applications or scenarios where an additional label is needed besides the type character and index. It encodes a type character (`unsigned char`), a label character (`unsigned char`), and an index (`uint64_t`) into a single 64-bit `gtsam.Key`." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "lsymbol_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "lsymbol_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "lsymbol_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "from gtsam import LabeledSymbol" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "lsymbol_init_md" - }, - "source": [ - "## Initialization\n", - "\n", - "A `LabeledSymbol` can be created by providing a type character, a label character, and an index. It can also be created by decoding an existing `gtsam.Key` (integer)." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "lsymbol_create_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "f1a2b3c4-d5e6-7890-f1a2-bcdef1234567" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "LabeledSymbol from char/label/index: xA7\n", - "LabeledSymbol from key 8107010787457335296: xA0\n" - ] - } - ], - "source": [ - "# Create LabeledSymbol 'x' from robot 'A' with index 7\n", - "lsym1 = LabeledSymbol('x', 'A', 7)\n", - "print(f\"LabeledSymbol from char/label/index: {lsym1.string()}\")\n", - "\n", - "# Get the underlying integer key\n", - "key1 = lsym1.key()\n", - "\n", - "# Reconstruct LabeledSymbol from the key\n", - "# Note: Decoding a key assumes it was encoded as a LabeledSymbol.\n", - "# If you decode a standard Symbol key, the label might be garbage.\n", - "x0_key = gtsam.Symbol('x', 0).key()\n", - "lsym2 = LabeledSymbol(x0_key)\n", - "print(f\"LabeledSymbol from key {x0_key}: {lsym2.string()}\") # Label might be non-printable or 0" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "lsymbol_props_md" - }, - "source": [ - "## Properties and Usage\n", - "\n", - "You can access the type character, label character, index, and underlying integer key." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "lsymbol_access_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "a2b3c4d5-e6f7-8901-a2b3-cdef12345678" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "LabeledSymbol: lB3\n", - " Char (Type): l\n", - " Label (Robot): B\n", - " Index: 3\n", - " Key: 7783740146724503555\n" - ] - } - ], - "source": [ - "robotB_landmark = LabeledSymbol('l', 'B', 3)\n", - "\n", - "print(f\"LabeledSymbol: {robotB_landmark.string()}\")\n", - "print(f\" Char (Type): {robotB_landmark.chr()}\")\n", - "print(f\" Label (Robot): {robotB_landmark.label()}\")\n", - "print(f\" Index: {robotB_landmark.index()}\")\n", - "print(f\" Key: {robotB_landmark.key()}\")\n", - "\n", - "# LabeledSymbols are often used directly where Keys are expected.\n", - "# Use the MultiRobotKeyFormatter for printing these keys meaningfully.\n", - "# e.g., graph.print(\"Graph: \\n\", gtsam.MultiRobotKeyFormatter)" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "lsymbol_shorthand_md" - }, - "source": [ - "## Shorthand Function\n", - "\n", - "GTSAM provides a convenient shorthand function `gtsam.mrsymbol(c, label, j)`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "lsymbol_shorthand_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "b3c4d5e6-f7a8-9012-b3c4-def123456789" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "LabeledSymbol('p', 'C', 2).key() == gtsam.mrsymbol(ord('p'), ord('C'), 2): True\n" - ] - } - ], - "source": [ - "# Note: mrsymbol expects integer representations of chars (use ord())\n", - "pc2_key = gtsam.mrsymbol(ord('p'), ord('C'), 2)\n", - "\n", - "print(f\"LabeledSymbol('p', 'C', 2).key() == gtsam.mrsymbol(ord('p'), ord('C'), 2): {LabeledSymbol('p', 'C', 2).key() == pc2_key}\")" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# LabeledSymbol" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_desc_md" + }, + "source": [ + "A `LabeledSymbol` is a specialized version of `gtsam.Symbol` designed primarily for multi-robot applications or scenarios where an additional label is needed besides the type character and index. It encodes a type character (`unsigned char`), a label character (`unsigned char`), and an index (`uint64_t`) into a single 64-bit `gtsam.Key`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "lsymbol_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "lsymbol_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import LabeledSymbol" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_init_md" + }, + "source": [ + "## Initialization\n", + "\n", + "A `LabeledSymbol` can be created by providing a type character, a label character, and an index. It can also be created by decoding an existing `gtsam.Key` (integer)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "lsymbol_create_code", + "outputId": "f1a2b3c4-d5e6-7890-f1a2-bcdef1234567" + }, + "outputs": [ + { + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'x', 'A', 7", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[2], line 2\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;66;03m# Create LabeledSymbol 'x' from robot 'A' with index 7\u001b[39;00m\n\u001b[1;32m----> 2\u001b[0m lsym1 \u001b[38;5;241m=\u001b[39m \u001b[43mLabeledSymbol\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mx\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mA\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m7\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mLabeledSymbol from char/label/index: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mlsym1\u001b[38;5;241m.\u001b[39mstring()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# Get the underlying integer key\u001b[39;00m\n", + "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'x', 'A', 7" + ] + } + ], + "source": [ + "# Create LabeledSymbol 'x' from robot 'A' with index 7\n", + "lsym1 = LabeledSymbol('x', 'A', 7)\n", + "print(f\"LabeledSymbol from char/label/index: {lsym1.string()}\")\n", + "\n", + "# Get the underlying integer key\n", + "key1 = lsym1.key()\n", + "\n", + "# Reconstruct LabeledSymbol from the key\n", + "# Note: Decoding a key assumes it was encoded as a LabeledSymbol.\n", + "# If you decode a standard Symbol key, the label might be garbage.\n", + "x0_key = gtsam.Symbol('x', 0).key()\n", + "lsym2 = LabeledSymbol(x0_key)\n", + "print(f\"LabeledSymbol from key {x0_key}: {lsym2.string()}\") # Label might be non-printable or 0" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_props_md" + }, + "source": [ + "## Properties and Usage\n", + "\n", + "You can access the type character, label character, index, and underlying integer key." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "lsymbol_access_code", + "outputId": "a2b3c4d5-e6f7-8901-a2b3-cdef12345678" + }, + "outputs": [ + { + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'l', 'B', 3", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[3], line 1\u001b[0m\n\u001b[1;32m----> 1\u001b[0m robotB_landmark \u001b[38;5;241m=\u001b[39m \u001b[43mLabeledSymbol\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43ml\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mB\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m3\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mLabeledSymbol: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mrobotB_landmark\u001b[38;5;241m.\u001b[39mstring()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m Char (Type): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mrobotB_landmark\u001b[38;5;241m.\u001b[39mchr()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", + "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'l', 'B', 3" + ] + } + ], + "source": [ + "robotB_landmark = LabeledSymbol('l', 'B', 3)\n", + "\n", + "print(f\"LabeledSymbol: {robotB_landmark.string()}\")\n", + "print(f\" Char (Type): {robotB_landmark.chr()}\")\n", + "print(f\" Label (Robot): {robotB_landmark.label()}\")\n", + "print(f\" Index: {robotB_landmark.index()}\")\n", + "print(f\" Key: {robotB_landmark.key()}\")\n", + "\n", + "# LabeledSymbols are often used directly where Keys are expected.\n", + "# Use the MultiRobotKeyFormatter for printing these keys meaningfully.\n", + "# e.g., graph.print(\"Graph: \\n\", gtsam.MultiRobotKeyFormatter)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "lsymbol_shorthand_md" + }, + "source": [ + "## Shorthand Function\n", + "\n", + "GTSAM provides a convenient shorthand function `gtsam.mrsymbol(c, label, j)`." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "lsymbol_shorthand_code", + "outputId": "b3c4d5e6-f7a8-9012-b3c4-def123456789" + }, + "outputs": [ + { + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'p', 'C', 2", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[4], line 4\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;66;03m# Note: mrsymbol expects integer representations of chars (use ord())\u001b[39;00m\n\u001b[0;32m 2\u001b[0m pc2_key \u001b[38;5;241m=\u001b[39m gtsam\u001b[38;5;241m.\u001b[39mmrsymbol(\u001b[38;5;28mord\u001b[39m(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mp\u001b[39m\u001b[38;5;124m'\u001b[39m), \u001b[38;5;28mord\u001b[39m(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mC\u001b[39m\u001b[38;5;124m'\u001b[39m), \u001b[38;5;241m2\u001b[39m)\n\u001b[1;32m----> 4\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mLabeledSymbol(\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mp\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m, \u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mC\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m, 2).key() == gtsam.mrsymbol(ord(\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mp\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m), ord(\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mC\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m), 2): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[43mLabeledSymbol\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mp\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[38;5;250;43m \u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mC\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[38;5;250;43m \u001b[39;49m\u001b[38;5;241;43m2\u001b[39;49m\u001b[43m)\u001b[49m\u001b[38;5;241m.\u001b[39mkey()\u001b[38;5;250m \u001b[39m\u001b[38;5;241m==\u001b[39m\u001b[38;5;250m \u001b[39mpc2_key\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", + "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'p', 'C', 2" + ] + } + ], + "source": [ + "# Note: mrsymbol expects integer representations of chars (use ord())\n", + "pc2_key = gtsam.mrsymbol(ord('p'), ord('C'), 2)\n", + "\n", + "print(f\"LabeledSymbol('p', 'C', 2).key() == gtsam.mrsymbol(ord('p'), ord('C'), 2): {LabeledSymbol('p', 'C', 2).key() == pc2_key}\")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/Ordering.ipynb b/gtsam/inference/doc/Ordering.ipynb index 512cacdc8..d8099ac7a 100644 --- a/gtsam/inference/doc/Ordering.ipynb +++ b/gtsam/inference/doc/Ordering.ipynb @@ -1,291 +1,278 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "ordering_intro_md" - }, - "source": [ - "# Ordering" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "ordering_desc_md" - }, - "source": [ - "An `Ordering` specifies the order in which variables are eliminated during inference (e.g., Gaussian elimination, multifrontal QR). The choice of ordering significantly impacts the computational cost and fill-in (sparsity) of the resulting Bayes net or Bayes tree.\n", - "\n", - "GTSAM provides several algorithms to compute good orderings automatically (like COLAMD, METIS) or allows you to specify a custom ordering." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "ordering_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "ordering_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "ordering_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "from gtsam import Ordering\n", - "# Need a graph type for automatic ordering\n", - "from gtsam import SymbolicFactorGraph\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "ordering_create_md" - }, - "source": [ - "## Creating an Ordering\n", - "\n", - "Orderings can be created manually or computed automatically from a factor graph." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "ordering_manual_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "6789abcd-ef01-2345-6789-abcdef012345" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Manual Ordering: \n", - "Position 0: x1\n", - "Position 1: l1\n", - "Position 2: x2\n", - "Position 3: l2\n", - "Position 4: x0\n" - ] - } - ], - "source": [ - "# Manual creation (list of keys)\n", - "manual_ordering = Ordering([X(1), L(1), X(2), L(2), X(0)])\n", - "manual_ordering.print(\"Manual Ordering: \")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "ordering_auto_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "789abcde-f012-3456-789a-bcdef0123456" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "COLAMD Ordering: \n", - "Position 0: x0\n", - "Position 1: l1\n", - "Position 2: x1\n", - "Position 3: l2\n", - "Position 4: x2\n", - "\n", - "Constrained COLAMD (x0, x2 last): \n", - "Position 0: l1\n", - "Position 1: l2\n", - "Position 2: x1\n", - "Position 3: x0\n", - "Position 4: x2\n" - ] - } - ], - "source": [ - "# Automatic creation requires a factor graph\n", - "# Let's use a simple SymbolicFactorGraph for structure\n", - "graph = SymbolicFactorGraph()\n", - "graph.push_factor(X(0))\n", - "graph.push_factor(X(0), X(1))\n", - "graph.push_factor(X(1), X(2))\n", - "graph.push_factor(X(0), L(1))\n", - "graph.push_factor(X(1), L(1))\n", - "graph.push_factor(X(1), L(2))\n", - "graph.push_factor(X(2), L(2))\n", - "\n", - "# COLAMD (Column Approximate Minimum Degree) ordering\n", - "colamd_ordering = Ordering.Colamd(graph)\n", - "colamd_ordering.print(\"COLAMD Ordering: \")\n", - "\n", - "# Constrained COLAMD (force x0 and x2 to be eliminated last)\n", - "constrained_ordering = Ordering.ColamdConstrainedLast(graph, gtsam.KeyVector([X(0), X(2)]))\n", - "constrained_ordering.print(\"Constrained COLAMD (x0, x2 last): \")\n", - "\n", - "# METIS ordering (if GTSAM was built with METIS support)\n", - "try:\n", - " metis_ordering = Ordering.Metis(graph)\n", - " # metis_ordering.print(\"METIS Ordering: \")\n", - "except RuntimeError as e:\n", - " print(f\"\\nSkipping METIS: {e}\")\n", - "\n", - "# Natural ordering (based on key magnitude)\n", - "natural_ordering = Ordering.Natural(graph)\n", - "# natural_ordering.print(\"Natural Ordering: \")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "ordering_access_md" - }, - "source": [ - "## Accessing Elements\n", - "\n", - "An `Ordering` behaves like a vector of keys." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "ordering_access_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "89abcdef-0123-4567-89ab-cdef01234567" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Ordering size: 5\n", - "Key at position 0: x0\n", - "Key at position 2: x1\n", - "Iterating through ordering:\n", - " x0\n", - " l1\n", - " x1\n", - " l2\n", - " x2\n", - "Inverted map (Key -> Position):\n", - " x0 -> 0\n", - " l1 -> 1\n", - " x1 -> 2\n", - " l2 -> 3\n", - " x2 -> 4\n" - ] - } - ], - "source": [ - "print(f\"Ordering size: {colamd_ordering.size()}\")\n", - "\n", - "# Access by index\n", - "key_at_0 = colamd_ordering.at(0)\n", - "key_at_2 = colamd_ordering[2] # Also supports [] operator\n", - "print(f\"Key at position 0: {gtsam.DefaultKeyFormatter(key_at_0)}\")\n", - "print(f\"Key at position 2: {gtsam.DefaultKeyFormatter(key_at_2)}\")\n", - "\n", - "# Iterate through the ordering\n", - "print(\"Iterating through ordering:\")\n", - "for key in colamd_ordering:\n", - " print(f\" {gtsam.DefaultKeyFormatter(key)}\")\n", - "\n", - "# Invert the ordering (map from Key to its position)\n", - "inverted = colamd_ordering.invert()\n", - "print(\"Inverted map (Key -> Position):\")\n", - "for key, pos in inverted.items():\n", - " print(f\" {gtsam.DefaultKeyFormatter(key)} -> {pos}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "ordering_append_md" - }, - "source": [ - "## Appending Keys\n", - "\n", - "You can append keys using `push_back`, `+=`, or `,`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "ordering_append_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "9abcdef0-1234-5678-9abc-def012345678" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Appended Ordering: \n", - "Position 0: x0\n", - "Position 1: l1\n", - "Position 2: x1\n", - "Position 3: l2\n", - "Position 4: x2\n", - "Position 5: l0\n", - "Position 6: x3\n" - ] - } - ], - "source": [ - "appended_ordering = Ordering(colamd_ordering)\n", - "appended_ordering.push_back(L(0))\n", - "appended_ordering += X(3)\n", - "# appended_ordering += L(0), X(3) # Can also chain\n", - "\n", - "appended_ordering.print(\"Appended Ordering: \")" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# Ordering" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_desc_md" + }, + "source": [ + "An `Ordering` specifies the order in which variables are eliminated during inference (e.g., Gaussian elimination, multifrontal QR). The choice of ordering significantly impacts the computational cost and fill-in (sparsity) of the resulting Bayes net or Bayes tree.\n", + "\n", + "GTSAM provides several algorithms to compute good orderings automatically (like COLAMD, METIS) or allows you to specify a custom ordering." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "ordering_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "ordering_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import Ordering, VariableIndex\n", + "# Need a graph type for automatic ordering\n", + "from gtsam import SymbolicFactorGraph\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_create_md" + }, + "source": [ + "## Creating an Ordering\n", + "\n", + "Orderings can be created manually or computed automatically from a factor graph." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "ordering_manual_code", + "outputId": "6789abcd-ef01-2345-6789-abcdef012345" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Manual Ordering: Position 0: x1, l1, x2, l2, x0\n" + ] + } + ], + "source": [ + "# Manual creation (list of keys)\n", + "manual_ordering = Ordering([X(1), L(1), X(2), L(2), X(0)])\n", + "manual_ordering.print(\"Manual Ordering: \")" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "ordering_auto_code", + "outputId": "789abcde-f012-3456-789a-bcdef0123456" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "COLAMD Ordering: Position 0: l1, x0, x1, l2, x2\n" + ] + }, + { + "ename": "AttributeError", + "evalue": "type object 'gtsam.gtsam.Ordering' has no attribute 'ColamdConstrainedLast'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[7], line 17\u001b[0m\n\u001b[0;32m 14\u001b[0m colamd_ordering\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mCOLAMD Ordering: \u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 16\u001b[0m \u001b[38;5;66;03m# Constrained COLAMD (force x0 and x2 to be eliminated last)\u001b[39;00m\n\u001b[1;32m---> 17\u001b[0m constrained_ordering \u001b[38;5;241m=\u001b[39m \u001b[43mOrdering\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mColamdConstrainedLast\u001b[49m(graph, gtsam\u001b[38;5;241m.\u001b[39mKeyVector([X(\u001b[38;5;241m0\u001b[39m), X(\u001b[38;5;241m2\u001b[39m)]))\n\u001b[0;32m 18\u001b[0m constrained_ordering\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mConstrained COLAMD (x0, x2 last): \u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 20\u001b[0m \u001b[38;5;66;03m# METIS ordering (if GTSAM was built with METIS support)\u001b[39;00m\n", + "\u001b[1;31mAttributeError\u001b[0m: type object 'gtsam.gtsam.Ordering' has no attribute 'ColamdConstrainedLast'" + ] + } + ], + "source": [ + "# Automatic creation requires a factor graph\n", + "# Let's use a simple SymbolicFactorGraph for structure\n", + "graph = SymbolicFactorGraph()\n", + "graph.push_factor(X(0))\n", + "graph.push_factor(X(0), X(1))\n", + "graph.push_factor(X(1), X(2))\n", + "graph.push_factor(X(0), L(1))\n", + "graph.push_factor(X(1), L(1))\n", + "graph.push_factor(X(1), L(2))\n", + "graph.push_factor(X(2), L(2))\n", + "\n", + "# COLAMD (Column Approximate Minimum Degree) ordering\n", + "colamd_ordering = Ordering.Colamd(VariableIndex(graph))\n", + "colamd_ordering.print(\"COLAMD Ordering: \")\n", + "\n", + "# Constrained COLAMD (force x0 and x2 to be eliminated last)\n", + "constrained_ordering = Ordering.ColamdConstrainedLast(graph, gtsam.KeyVector([X(0), X(2)]))\n", + "constrained_ordering.print(\"Constrained COLAMD (x0, x2 last): \")\n", + "\n", + "# METIS ordering (if GTSAM was built with METIS support)\n", + "try:\n", + " metis_ordering = Ordering.Metis(graph)\n", + " # metis_ordering.print(\"METIS Ordering: \")\n", + "except RuntimeError as e:\n", + " print(f\"\\nSkipping METIS: {e}\")\n", + "\n", + "# Natural ordering (based on key magnitude)\n", + "natural_ordering = Ordering.Natural(graph)\n", + "# natural_ordering.print(\"Natural Ordering: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_access_md" + }, + "source": [ + "## Accessing Elements\n", + "\n", + "An `Ordering` behaves like a vector of keys." + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "ordering_access_code", + "outputId": "89abcdef-0123-4567-89ab-cdef01234567" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Ordering size: 5\n", + "Key at position 0: l1\n" + ] + }, + { + "ename": "AttributeError", + "evalue": "'gtsam.gtsam.Ordering' object has no attribute 'invert'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[11], line 8\u001b[0m\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mKey at position 0: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mgtsam\u001b[38;5;241m.\u001b[39mDefaultKeyFormatter(key_at_0)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Invert the ordering (map from Key to its position)\u001b[39;00m\n\u001b[1;32m----> 8\u001b[0m inverted \u001b[38;5;241m=\u001b[39m \u001b[43mcolamd_ordering\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minvert\u001b[49m()\n\u001b[0;32m 9\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mInverted map (Key -> Position):\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 10\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m key, pos \u001b[38;5;129;01min\u001b[39;00m inverted\u001b[38;5;241m.\u001b[39mitems():\n", + "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.Ordering' object has no attribute 'invert'" + ] + } + ], + "source": [ + "print(f\"Ordering size: {colamd_ordering.size()}\")\n", + "\n", + "# Access by index\n", + "key_at_0 = colamd_ordering.at(0)\n", + "print(f\"Key at position 0: {gtsam.DefaultKeyFormatter(key_at_0)}\")\n", + "\n", + "# Invert the ordering (map from Key to its position)\n", + "inverted = colamd_ordering.invert()\n", + "print(\"Inverted map (Key -> Position):\")\n", + "for key, pos in inverted.items():\n", + " print(f\" {gtsam.DefaultKeyFormatter(key)} -> {pos}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_append_md" + }, + "source": [ + "## Appending Keys\n", + "\n", + "You can append keys using `push_back`, `+=`, or `,`." + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "ordering_append_code", + "outputId": "9abcdef0-1234-5678-9abc-def012345678" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Appended Ordering: Position 0: l1, x0, x1, l2, x2, l0, x3\n" + ] + } + ], + "source": [ + "appended_ordering = Ordering(colamd_ordering)\n", + "appended_ordering.push_back(L(0))\n", + "appended_ordering.push_back(X(3))\n", + "\n", + "appended_ordering.print(\"Appended Ordering: \")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/Symbol.ipynb b/gtsam/inference/doc/Symbol.ipynb index 077db72b0..6f5430a64 100644 --- a/gtsam/inference/doc/Symbol.ipynb +++ b/gtsam/inference/doc/Symbol.ipynb @@ -1,199 +1,209 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "symbol_intro_md" - }, - "source": [ - "# Symbol" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "symbol_desc_md" - }, - "source": [ - "A `Symbol` is a GTSAM class used to create semantically meaningful keys (`gtsam.Key`) for variables. It combines a character (`unsigned char`) and an index (`uint64_t`) into a single 64-bit integer key. This allows for easy identification of variable types and their indices, e.g., 'x' for poses and 'l' for landmarks, like `x0`, `x1`, `l0`." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "symbol_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "symbol_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "symbol_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "from gtsam import Symbol" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "symbol_init_md" - }, - "source": [ - "## Initialization\n", - "\n", - "A `Symbol` can be created by providing a character and an index. It can also be created by decoding an existing `gtsam.Key` (integer)." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "symbol_create_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "c1d2e3f4-a5b6-7890-bcde-f12345678901" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Symbol from char/index: x5\n", - "Symbol from key 8070450532247928837: x5\n" - ] - } - ], - "source": [ - "# Create Symbol 'x' with index 5\n", - "sym1 = Symbol('x', 5)\n", - "print(f\"Symbol from char/index: {sym1.string()}\")\n", - "\n", - "# Get the underlying integer key\n", - "key1 = sym1.key()\n", - "\n", - "# Reconstruct Symbol from the key\n", - "sym2 = Symbol(key1)\n", - "print(f\"Symbol from key {key1}: {sym2.string()}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "symbol_props_md" - }, - "source": [ - "## Properties and Usage\n", - "\n", - "You can access the character, index, and underlying integer key." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "symbol_access_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "d2e3f4a5-b6c7-8901-cdef-123456789012" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Symbol: l10\n", - " Char: l\n", - " Index: 10\n", - " Key: 7783684379976990730\n" - ] - } - ], - "source": [ - "landmark_sym = Symbol('l', 10)\n", - "\n", - "print(f\"Symbol: {landmark_sym.string()}\")\n", - "print(f\" Char: {landmark_sym.chr()}\")\n", - "print(f\" Index: {landmark_sym.index()}\")\n", - "print(f\" Key: {landmark_sym.key()}\")\n", - "\n", - "# Symbols are often used directly where Keys are expected in GTSAM functions,\n", - "# as they implicitly convert.\n", - "# e.g., values.insert(landmark_sym, gtsam.Point3(1,2,3))" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "symbol_shorthand_md" - }, - "source": [ - "## Shorthand Functions\n", - "\n", - "GTSAM provides convenient shorthand functions `gtsam.symbol_shorthand.X(j)`, `gtsam.symbol_shorthand.L(j)`, etc., which are equivalent to `gtsam.Symbol('x', j)`, `gtsam.Symbol('l', j)`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "symbol_shorthand_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "e3f4a5b6-c7d8-9012-def0-234567890123" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Symbol('x', 0).key() == gtsam.symbol_shorthand.X(0): True\n", - "Symbol('l', 1).key() == gtsam.symbol_shorthand.L(1): True\n" - ] - } - ], - "source": [ - "from gtsam import symbol_shorthand\n", - "\n", - "x0_key = symbol_shorthand.X(0)\n", - "l1_key = symbol_shorthand.L(1)\n", - "\n", - "print(f\"Symbol('x', 0).key() == gtsam.symbol_shorthand.X(0): {Symbol('x', 0).key() == x0_key}\")\n", - "print(f\"Symbol('l', 1).key() == gtsam.symbol_shorthand.L(1): {Symbol('l', 1).key() == l1_key}\")" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# Symbol" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_desc_md" + }, + "source": [ + "A `Symbol` is a GTSAM class used to create semantically meaningful keys (`gtsam.Key`) for variables. It combines a character (`unsigned char`) and an index (`uint64_t`) into a single 64-bit integer key. This allows for easy identification of variable types and their indices, e.g., 'x' for poses and 'l' for landmarks, like `x0`, `x1`, `l0`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "symbol_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "symbol_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import Symbol" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_init_md" + }, + "source": [ + "## Initialization\n", + "\n", + "A `Symbol` can be created by providing a character and an index. It can also be created by decoding an existing `gtsam.Key` (integer)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "symbol_create_code", + "outputId": "c1d2e3f4-a5b6-7890-bcde-f12345678901" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbol from char/index: x5\n", + "Symbol from key 8646911284551352325: x5\n" + ] + } + ], + "source": [ + "# Create Symbol 'x' with index 5\n", + "sym1 = Symbol('x', 5)\n", + "print(f\"Symbol from char/index: {sym1.string()}\")\n", + "\n", + "# Get the underlying integer key\n", + "key1 = sym1.key()\n", + "\n", + "# Reconstruct Symbol from the key\n", + "sym2 = Symbol(key1)\n", + "print(f\"Symbol from key {key1}: {sym2.string()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_props_md" + }, + "source": [ + "## Properties and Usage\n", + "\n", + "You can access the character, index, and underlying integer key." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "symbol_access_code", + "outputId": "d2e3f4a5-b6c7-8901-cdef-123456789012" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbol: l10\n", + " Char: 108\n", + " Index: 10\n", + " Key: 7782220156096217098\n" + ] + } + ], + "source": [ + "landmark_sym = Symbol('l', 10)\n", + "\n", + "print(f\"Symbol: {landmark_sym.string()}\")\n", + "print(f\" Char: {landmark_sym.chr()}\")\n", + "print(f\" Index: {landmark_sym.index()}\")\n", + "print(f\" Key: {landmark_sym.key()}\")\n", + "\n", + "# Symbols are often used directly where Keys are expected in GTSAM functions,\n", + "# as they implicitly convert.\n", + "# e.g., values.insert(landmark_sym, gtsam.Point3(1,2,3))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "symbol_shorthand_md" + }, + "source": [ + "## Shorthand Functions\n", + "\n", + "GTSAM provides convenient shorthand functions `gtsam.symbol_shorthand.X(j)`, `gtsam.symbol_shorthand.L(j)`, etc., which are equivalent to `gtsam.Symbol('x', j)`, `gtsam.Symbol('l', j)`." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "symbol_shorthand_code", + "outputId": "e3f4a5b6-c7d8-9012-def0-234567890123" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbol('x', 0).key() == gtsam.symbol_shorthand.X(0): True\n", + "Symbol('l', 1).key() == gtsam.symbol_shorthand.L(1): True\n" + ] + } + ], + "source": [ + "from gtsam import symbol_shorthand\n", + "\n", + "x0_key = symbol_shorthand.X(0)\n", + "l1_key = symbol_shorthand.L(1)\n", + "\n", + "print(f\"Symbol('x', 0).key() == gtsam.symbol_shorthand.X(0): {Symbol('x', 0).key() == x0_key}\")\n", + "print(f\"Symbol('l', 1).key() == gtsam.symbol_shorthand.L(1): {Symbol('l', 1).key() == l1_key}\")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} diff --git a/gtsam/inference/doc/VariableIndex.ipynb b/gtsam/inference/doc/VariableIndex.ipynb index 6367c749d..86ce4ab75 100644 --- a/gtsam/inference/doc/VariableIndex.ipynb +++ b/gtsam/inference/doc/VariableIndex.ipynb @@ -1,233 +1,237 @@ { - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "id": "vindex_intro_md" - }, - "source": [ - "# VariableIndex" - ] + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_intro_md" }, - { - "cell_type": "markdown", - "metadata": { - "id": "vindex_desc_md" - }, - "source": [ - "A `VariableIndex` provides an efficient way to look up which factors in a `FactorGraph` involve a particular variable (Key). It stores, for each variable, a list of the indices of the factors that include that variable.\n", - "\n", - "This structure is often computed internally by GTSAM algorithms (like ordering methods or elimination) but can also be created explicitly if needed, for example, to improve performance when multiple operations need this information." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "vindex_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "vindex_pip_code" - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "vindex_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "from gtsam import VariableIndex\n", - "# Need a graph type for creation\n", - "from gtsam import SymbolicFactorGraph\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "vindex_create_md" - }, - "source": [ - "## Creating a VariableIndex\n", - "\n", - "A `VariableIndex` is typically created from an existing `FactorGraph`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "vindex_create_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "abcdef01-2345-6789-abcd-ef0123456789" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "VariableIndex: \n", - "nEntries = 13, nFactors = 7\n", - "var l1: 3 4 \n", - "var l2: 5 6 \n", - "var x0: 0 1 3 \n", - "var x1: 1 2 4 5 \n", - "var x2: 2 6 \n", - "\n" - ] - } - ], - "source": [ - "# Create a simple SymbolicFactorGraph\n", - "graph = SymbolicFactorGraph()\n", - "graph.push_factor(X(0)) # Factor 0\n", - "graph.push_factor(X(0), X(1)) # Factor 1\n", - "graph.push_factor(X(1), X(2)) # Factor 2\n", - "graph.push_factor(X(0), L(1)) # Factor 3\n", - "graph.push_factor(X(1), L(1)) # Factor 4\n", - "graph.push_factor(X(1), L(2)) # Factor 5\n", - "graph.push_factor(X(2), L(2)) # Factor 6\n", - "\n", - "# Create VariableIndex from the graph\n", - "variable_index = VariableIndex(graph)\n", - "\n", - "# Print the index\n", - "variable_index.print(\"VariableIndex: \")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "vindex_access_md" - }, - "source": [ - "## Accessing Information\n", - "\n", - "You can query the number of variables, factors, and entries, and look up the factors associated with a specific variable." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "vindex_access_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "bcdef012-3456-789a-bcde-f0123456789a" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Number of variables (size): 5\n", - "Number of factors (nFactors): 7\n", - "Number of variable-factor entries (nEntries): 13\n", - "\n", - "Factors involving x1: [1, 2, 4, 5]\n", - "Factors involving l1: [3, 4]\n", - "\n", - "Iterating through VariableIndex:\n", - " Variable l1 involves factors: [3, 4]\n", - " Variable l2 involves factors: [5, 6]\n", - " Variable x0 involves factors: [0, 1, 3]\n", - " Variable x1 involves factors: [1, 2, 4, 5]\n", - " Variable x2 involves factors: [2, 6]\n" - ] - } - ], - "source": [ - "print(f\"Number of variables (size): {variable_index.size()}\")\n", - "print(f\"Number of factors (nFactors): {variable_index.nFactors()}\")\n", - "print(f\"Number of variable-factor entries (nEntries): {variable_index.nEntries()}\")\n", - "\n", - "# Get factors involving a specific variable\n", - "factors_x1 = variable_index[X(1)] # Returns a FactorIndices (FastVector)\n", - "print(f\"Factors involving x1: {factors_x1}\")\n", - "\n", - "# Use key directly\n", - "factors_l1 = variable_index[L(1)]\n", - "print(f\"Factors involving l1: {factors_l1}\")\n", - "\n", - "# Iterate through the index (items are pairs of Key, FactorIndices)\n", - "print(\"Iterating through VariableIndex:\")\n", - "for key, factor_indices in variable_index.items(): # Use .items() like a dict\n", - " print(f\" Variable {gtsam.DefaultKeyFormatter(key)} involves factors: {factor_indices}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "vindex_use_md" - }, - "source": [ - "## Usage in Algorithms\n", - "\n", - "`VariableIndex` is primarily used as input to other algorithms, particularly ordering methods like `Ordering.Colamd`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "vindex_use_code", - "colab": { - "base_uri": "https://localhost:8080/" - }, - "outputId": "cdef0123-4567-89ab-cdef-0123456789ab" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "COLAMD Ordering from VariableIndex: \n", - "Position 0: x0\n", - "Position 1: l1\n", - "Position 2: x1\n", - "Position 3: l2\n", - "Position 4: x2\n" - ] - } - ], - "source": [ - "# Compute COLAMD ordering directly from the VariableIndex\n", - "colamd_ordering = Ordering.Colamd(variable_index)\n", - "colamd_ordering.print(\"COLAMD Ordering from VariableIndex: \")" - ] - } - ], - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "display_name": "Python 3", - "name": "python3" - }, - "language_info": { - "name": "python" - } + "source": [ + "# VariableIndex" + ] }, - "nbformat": 4, - "nbformat_minor": 0 - } \ No newline at end of file + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_desc_md" + }, + "source": [ + "A `VariableIndex` provides an efficient way to look up which factors in a `FactorGraph` involve a particular variable (Key). It stores, for each variable, a list of the indices of the factors that include that variable.\n", + "\n", + "This structure is often computed internally by GTSAM algorithms (like ordering methods or elimination) but can also be created explicitly if needed, for example, to improve performance when multiple operations need this information." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_colab_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "vindex_pip_code" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "id": "vindex_import_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import VariableIndex, Ordering\n", + "# Need a graph type for creation\n", + "from gtsam import SymbolicFactorGraph\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_create_md" + }, + "source": [ + "## Creating a VariableIndex\n", + "\n", + "A `VariableIndex` is typically created from an existing `FactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "vindex_create_code", + "outputId": "abcdef01-2345-6789-abcd-ef0123456789" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "VariableIndex: nEntries = 13, nFactors = 7\n", + "var l1: 3 4\n", + "var l2: 5 6\n", + "var x0: 0 1 3\n", + "var x1: 1 2 4 5\n", + "var x2: 2 6\n" + ] + } + ], + "source": [ + "# Create a simple SymbolicFactorGraph\n", + "graph = SymbolicFactorGraph()\n", + "graph.push_factor(X(0)) # Factor 0\n", + "graph.push_factor(X(0), X(1)) # Factor 1\n", + "graph.push_factor(X(1), X(2)) # Factor 2\n", + "graph.push_factor(X(0), L(1)) # Factor 3\n", + "graph.push_factor(X(1), L(1)) # Factor 4\n", + "graph.push_factor(X(1), L(2)) # Factor 5\n", + "graph.push_factor(X(2), L(2)) # Factor 6\n", + "\n", + "# Create VariableIndex from the graph\n", + "variable_index = VariableIndex(graph)\n", + "\n", + "# Print the index\n", + "variable_index.print(\"VariableIndex: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_access_md" + }, + "source": [ + "## Accessing Information\n", + "\n", + "You can query the number of variables, factors, and entries, and look up the factors associated with a specific variable." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "vindex_access_code", + "outputId": "bcdef012-3456-789a-bcde-f0123456789a" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Number of variables (size): 5\n", + "Number of factors (nFactors): 7\n", + "Number of variable-factor entries (nEntries): 13\n" + ] + }, + { + "ename": "TypeError", + "evalue": "'gtsam.gtsam.VariableIndex' object is not subscriptable", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[3], line 6\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mNumber of variable-factor entries (nEntries): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mvariable_index\u001b[38;5;241m.\u001b[39mnEntries()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# Get factors involving a specific variable\u001b[39;00m\n\u001b[1;32m----> 6\u001b[0m factors_x1 \u001b[38;5;241m=\u001b[39m \u001b[43mvariable_index\u001b[49m\u001b[43m[\u001b[49m\u001b[43mX\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m1\u001b[39;49m\u001b[43m)\u001b[49m\u001b[43m]\u001b[49m \u001b[38;5;66;03m# Returns a FactorIndices (FastVector)\u001b[39;00m\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mFactors involving x1: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mfactors_x1\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 9\u001b[0m \u001b[38;5;66;03m# Use key directly\u001b[39;00m\n", + "\u001b[1;31mTypeError\u001b[0m: 'gtsam.gtsam.VariableIndex' object is not subscriptable" + ] + } + ], + "source": [ + "print(f\"Number of variables (size): {variable_index.size()}\")\n", + "print(f\"Number of factors (nFactors): {variable_index.nFactors()}\")\n", + "print(f\"Number of variable-factor entries (nEntries): {variable_index.nEntries()}\")\n", + "\n", + "# Get factors involving a specific variable\n", + "factors_x1 = variable_index[X(1)] # Returns a FactorIndices (FastVector)\n", + "print(f\"Factors involving x1: {factors_x1}\")\n", + "\n", + "# Use key directly\n", + "factors_l1 = variable_index[L(1)]\n", + "print(f\"Factors involving l1: {factors_l1}\")\n", + "\n", + "# Iterate through the index (items are pairs of Key, FactorIndices)\n", + "print(\"Iterating through VariableIndex:\")\n", + "for key, factor_indices in variable_index.items(): # Use .items() like a dict\n", + " print(f\" Variable {gtsam.DefaultKeyFormatter(key)} involves factors: {factor_indices}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vindex_use_md" + }, + "source": [ + "## Usage in Algorithms\n", + "\n", + "`VariableIndex` is primarily used as input to other algorithms, particularly ordering methods like `Ordering.Colamd`." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "vindex_use_code", + "outputId": "cdef0123-4567-89ab-cdef-0123456789ab" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "COLAMD Ordering from VariableIndex: Position 0: l1, x0, x1, l2, x2\n" + ] + } + ], + "source": [ + "# Compute COLAMD ordering directly from the VariableIndex\n", + "colamd_ordering = Ordering.Colamd(variable_index)\n", + "colamd_ordering.print(\"COLAMD Ordering from VariableIndex: \")" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "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": 0 +} From 7074ca2005d1ad59b83cc1a5c14e4c9ffca31040 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 10 Apr 2025 10:43:24 -0400 Subject: [PATCH 03/24] Summarize, add to myst --- gtsam/inference/inference.md | 36 ++++++++++++++++++++++++++++++++++++ myst.yml | 3 +++ 2 files changed, 39 insertions(+) create mode 100644 gtsam/inference/inference.md diff --git a/gtsam/inference/inference.md b/gtsam/inference/inference.md new file mode 100644 index 000000000..157b403c3 --- /dev/null +++ b/gtsam/inference/inference.md @@ -0,0 +1,36 @@ +# Inference + +The `inference` module provides the foundational classes and algorithms for probabilistic graphical models in GTSAM, focusing on variable elimination and the resulting structures like Bayes Nets and Bayes Trees. + +## Core Concepts + +- [Key](doc/Key.ipynb): Base type (`uint64_t`) for uniquely identifying variables. +- [Symbol](doc/Symbol.ipynb): A Key type encoding a character and an index (e.g., `x0`). +- [LabeledSymbol](doc/LabeledSymbol.ipynb): A `Symbol` variant with an additional label character, useful for multi-robot scenarios (e.g., `xA0`). +- [EdgeKey](doc/EdgeKey.ipynb): A Key type encoding a pair of 32-bit integers. +- [Factor](doc/Factor.ipynb): Abstract base class for all factors (relationships between variables). +- [FactorGraph](doc/FactorGraph.ipynb): Base class representing a collection of factors. +- [Conditional](doc/Conditional.ipynb): Abstract base class for conditional distributions/densities resulting from elimination ( $P(\text{Frontals} | \text{Parents})$ ). + +## Elimination Algorithms & Control + +- [Ordering](doc/Ordering.ipynb): Specifies the order in which variables are eliminated, crucial for efficiency. +- [VariableIndex](doc/VariableIndex.ipynb): Maps variables to the factors they appear in, used for efficient elimination ordering and construction. +- [EliminateableFactorGraph](https://github.com/borglab/gtsam/blob/develop/gtsam/inference/EliminateableFactorGraph.h): A mixin class providing `eliminateSequential` and `eliminateMultifrontal` methods to concrete factor graph types (like `GaussianFactorGraph`, `SymbolicFactorGraph`). + +## Elimination Results & Structures + +- [BayesNet](doc/BayesNet.ipynb): Represents the result of sequential variable elimination as a directed acyclic graph (DAG) of conditionals. +- [EliminationTree](doc/EliminationTree.ipynb): Tree structure representing the dependencies and computations during sequential elimination. +- [ClusterTree](doc/ClusterTree.ipynb): Base class for tree structures where nodes are clusters of factors (e.g., JunctionTree). +- [JunctionTree](doc/JunctionTree.ipynb): A cluster tree representing the cliques formed during multifrontal elimination, holding the factors before they are eliminated into conditionals. +- [BayesTreeCliqueBase](https://github.com/borglab/gtsam/blob/develop/gtsam/inference/BayesTreeCliqueBase.h): Abstract base class for the nodes (cliques) within a BayesTree. +- [BayesTree](doc/BayesTree.ipynb): Represents the result of multifrontal variable elimination as a tree of cliques, where each clique contains the conditional $P(\text{Frontals} | \text{Separator})$. + +## Incremental Inference + +- [ISAM](doc/ISAM.ipynb): Incremental Smoothing and Mapping algorithm based on updating a BayesTree (original version, often superseded by ISAM2 in `nonlinear`). + +## Visualization + +- [DotWriter](doc/DotWriter.ipynb): Helper class to customize the generation of Graphviz `.dot` files for visualizing graphs and trees. \ No newline at end of file diff --git a/myst.yml b/myst.yml index de5278d85..5db7be6ff 100644 --- a/myst.yml +++ b/myst.yml @@ -13,6 +13,9 @@ project: - file: ./gtsam/geometry/geometry.md children: - pattern: ./gtsam/geometry/doc/* + - file: ./gtsam/inference/inference.md + children: + - pattern: ./gtsam/inference/doc/* - file: ./gtsam/nonlinear/nonlinear.md children: - pattern: ./gtsam/nonlinear/doc/* From 04bba9156c091e01c0aaea887813bdd168cedb4b Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 10 Apr 2025 11:20:05 -0400 Subject: [PATCH 04/24] Remove install runs --- gtsam/inference/doc/BayesTree.ipynb | 14 ++------------ gtsam/inference/doc/ClusterTree.ipynb | 14 ++------------ gtsam/inference/doc/Conditional.ipynb | 14 ++------------ 3 files changed, 6 insertions(+), 36 deletions(-) diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index 9af8838e6..3a3d2049e 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -48,21 +48,11 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": { "id": "bayestree_pip_code" }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Requirement already satisfied: gtsam in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (4.3a0)Note: you may need to restart the kernel to use updated packages.\n", - "\n", - "Requirement already satisfied: numpy>=1.11.0 in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (from gtsam) (2.2.1)\n" - ] - } - ], + "outputs": [], "source": [ "%pip install gtsam" ] diff --git a/gtsam/inference/doc/ClusterTree.ipynb b/gtsam/inference/doc/ClusterTree.ipynb index 015ddf25f..b29a78970 100644 --- a/gtsam/inference/doc/ClusterTree.ipynb +++ b/gtsam/inference/doc/ClusterTree.ipynb @@ -33,21 +33,11 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": { "id": "clustertree_pip_code" }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Requirement already satisfied: gtsam in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (4.3a0)Note: you may need to restart the kernel to use updated packages.\n", - "\n", - "Requirement already satisfied: numpy>=1.11.0 in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (from gtsam) (2.2.1)\n" - ] - } - ], + "outputs": [], "source": [ "%pip install gtsam" ] diff --git a/gtsam/inference/doc/Conditional.ipynb b/gtsam/inference/doc/Conditional.ipynb index 3c8c664ba..2bfd0f10c 100644 --- a/gtsam/inference/doc/Conditional.ipynb +++ b/gtsam/inference/doc/Conditional.ipynb @@ -54,21 +54,11 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": { "id": "conditional_pip_code" }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Requirement already satisfied: gtsam in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (4.3a0)\n", - "Requirement already satisfied: numpy>=1.11.0 in c:\\users\\porte\\miniconda3\\envs\\gtsam\\lib\\site-packages (from gtsam) (2.2.1)\n", - "Note: you may need to restart the kernel to use updated packages.\n" - ] - } - ], + "outputs": [], "source": [ "%pip install gtsam" ] From db8ead2c9af51efa2c94ff3d08b46cea82542a61 Mon Sep 17 00:00:00 2001 From: p-zach Date: Mon, 14 Apr 2025 18:55:36 -0400 Subject: [PATCH 05/24] Add remove-cell tags --- gtsam/inference/doc/BayesNet.ipynb | 5 +++- gtsam/inference/doc/BayesTree.ipynb | 5 +++- gtsam/inference/doc/ClusterTree.ipynb | 5 +++- gtsam/inference/doc/Conditional.ipynb | 5 +++- gtsam/inference/doc/DotWriter.ipynb | 7 ++++-- gtsam/inference/doc/EdgeKey.ipynb | 5 +++- gtsam/inference/doc/EliminationTree.ipynb | 5 +++- gtsam/inference/doc/Factor.ipynb | 5 +++- gtsam/inference/doc/FactorGraph.ipynb | 5 +++- gtsam/inference/doc/ISAM.ipynb | 5 +++- gtsam/inference/doc/JunctionTree.ipynb | 5 +++- gtsam/inference/doc/Key.ipynb | 5 +++- gtsam/inference/doc/LabeledSymbol.ipynb | 5 +++- gtsam/inference/doc/Ordering.ipynb | 28 ++++++++++------------- gtsam/inference/doc/Symbol.ipynb | 5 +++- gtsam/inference/doc/VariableIndex.ipynb | 5 +++- 16 files changed, 73 insertions(+), 32 deletions(-) diff --git a/gtsam/inference/doc/BayesNet.ipynb b/gtsam/inference/doc/BayesNet.ipynb index 786fdd9c9..82f545363 100644 --- a/gtsam/inference/doc/BayesNet.ipynb +++ b/gtsam/inference/doc/BayesNet.ipynb @@ -45,7 +45,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "bayesnet_pip_code" + "id": "bayesnet_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index 3a3d2049e..a5f6479e4 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -50,7 +50,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "bayestree_pip_code" + "id": "bayestree_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/ClusterTree.ipynb b/gtsam/inference/doc/ClusterTree.ipynb index b29a78970..402680078 100644 --- a/gtsam/inference/doc/ClusterTree.ipynb +++ b/gtsam/inference/doc/ClusterTree.ipynb @@ -35,7 +35,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "clustertree_pip_code" + "id": "clustertree_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/Conditional.ipynb b/gtsam/inference/doc/Conditional.ipynb index 2bfd0f10c..9f6a53665 100644 --- a/gtsam/inference/doc/Conditional.ipynb +++ b/gtsam/inference/doc/Conditional.ipynb @@ -56,7 +56,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "conditional_pip_code" + "id": "conditional_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/DotWriter.ipynb b/gtsam/inference/doc/DotWriter.ipynb index c048bf353..81a72316d 100644 --- a/gtsam/inference/doc/DotWriter.ipynb +++ b/gtsam/inference/doc/DotWriter.ipynb @@ -33,11 +33,14 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "dotwriter_pip_code" + "id": "dotwriter_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ - "%pip install gtsam graphviz" + "%pip install gtsam" ] }, { diff --git a/gtsam/inference/doc/EdgeKey.ipynb b/gtsam/inference/doc/EdgeKey.ipynb index d80e7cfa6..ca55d6619 100644 --- a/gtsam/inference/doc/EdgeKey.ipynb +++ b/gtsam/inference/doc/EdgeKey.ipynb @@ -31,7 +31,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "edgekey_pip_code" + "id": "edgekey_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/EliminationTree.ipynb b/gtsam/inference/doc/EliminationTree.ipynb index 703415455..11b3d2ec9 100644 --- a/gtsam/inference/doc/EliminationTree.ipynb +++ b/gtsam/inference/doc/EliminationTree.ipynb @@ -37,7 +37,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "etree_pip_code" + "id": "etree_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/Factor.ipynb b/gtsam/inference/doc/Factor.ipynb index 81812bcc2..40a53193d 100644 --- a/gtsam/inference/doc/Factor.ipynb +++ b/gtsam/inference/doc/Factor.ipynb @@ -44,7 +44,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "factor_pip_code" + "id": "factor_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/FactorGraph.ipynb b/gtsam/inference/doc/FactorGraph.ipynb index 01e111e1e..07633ae9c 100644 --- a/gtsam/inference/doc/FactorGraph.ipynb +++ b/gtsam/inference/doc/FactorGraph.ipynb @@ -46,7 +46,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "fg_pip_code" + "id": "fg_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/ISAM.ipynb b/gtsam/inference/doc/ISAM.ipynb index 3d1e4cddf..7e6a43269 100644 --- a/gtsam/inference/doc/ISAM.ipynb +++ b/gtsam/inference/doc/ISAM.ipynb @@ -35,7 +35,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "isam_pip_code" + "id": "isam_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/JunctionTree.ipynb b/gtsam/inference/doc/JunctionTree.ipynb index 9229a9004..7ebafb831 100644 --- a/gtsam/inference/doc/JunctionTree.ipynb +++ b/gtsam/inference/doc/JunctionTree.ipynb @@ -37,7 +37,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "jtree_pip_code" + "id": "jtree_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/Key.ipynb b/gtsam/inference/doc/Key.ipynb index 2acbd8c63..d643869da 100644 --- a/gtsam/inference/doc/Key.ipynb +++ b/gtsam/inference/doc/Key.ipynb @@ -31,7 +31,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "key_pip_code" + "id": "key_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/LabeledSymbol.ipynb b/gtsam/inference/doc/LabeledSymbol.ipynb index 1c49fdff9..2e8295086 100644 --- a/gtsam/inference/doc/LabeledSymbol.ipynb +++ b/gtsam/inference/doc/LabeledSymbol.ipynb @@ -31,7 +31,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "lsymbol_pip_code" + "id": "lsymbol_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/Ordering.ipynb b/gtsam/inference/doc/Ordering.ipynb index d8099ac7a..9dfcd0b4a 100644 --- a/gtsam/inference/doc/Ordering.ipynb +++ b/gtsam/inference/doc/Ordering.ipynb @@ -33,7 +33,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "ordering_pip_code" + "id": "ordering_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ @@ -42,7 +45,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 1, "metadata": { "id": "ordering_import_code" }, @@ -96,7 +99,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -106,21 +109,14 @@ }, "outputs": [ { - "name": "stdout", - "output_type": "stream", - "text": [ - "COLAMD Ordering: Position 0: l1, x0, x1, l2, x2\n" - ] - }, - { - "ename": "AttributeError", - "evalue": "type object 'gtsam.gtsam.Ordering' has no attribute 'ColamdConstrainedLast'", + "ename": "TypeError", + "evalue": "Colamd(): incompatible function arguments. The following argument types are supported:\n 1. (variableIndex: gtsam::VariableIndex) -> gtsam.gtsam.Ordering\n\nInvoked with: kwargs: graph=SymbolicFactorGraph \nsize: 7\nfactor 0: x0\nfactor 1: x0 x1\nfactor 2: x1 x2\nfactor 3: x0 l1\nfactor 4: x1 l1\nfactor 5: x1 l2\nfactor 6: x2 l2\n", "output_type": "error", "traceback": [ "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[7], line 17\u001b[0m\n\u001b[0;32m 14\u001b[0m colamd_ordering\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mCOLAMD Ordering: \u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 16\u001b[0m \u001b[38;5;66;03m# Constrained COLAMD (force x0 and x2 to be eliminated last)\u001b[39;00m\n\u001b[1;32m---> 17\u001b[0m constrained_ordering \u001b[38;5;241m=\u001b[39m \u001b[43mOrdering\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mColamdConstrainedLast\u001b[49m(graph, gtsam\u001b[38;5;241m.\u001b[39mKeyVector([X(\u001b[38;5;241m0\u001b[39m), X(\u001b[38;5;241m2\u001b[39m)]))\n\u001b[0;32m 18\u001b[0m constrained_ordering\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mConstrained COLAMD (x0, x2 last): \u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 20\u001b[0m \u001b[38;5;66;03m# METIS ordering (if GTSAM was built with METIS support)\u001b[39;00m\n", - "\u001b[1;31mAttributeError\u001b[0m: type object 'gtsam.gtsam.Ordering' has no attribute 'ColamdConstrainedLast'" + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[3], line 13\u001b[0m\n\u001b[0;32m 10\u001b[0m graph\u001b[38;5;241m.\u001b[39mpush_factor(X(\u001b[38;5;241m2\u001b[39m), L(\u001b[38;5;241m2\u001b[39m))\n\u001b[0;32m 12\u001b[0m \u001b[38;5;66;03m# COLAMD (Column Approximate Minimum Degree) ordering\u001b[39;00m\n\u001b[1;32m---> 13\u001b[0m colamd_ordering \u001b[38;5;241m=\u001b[39m \u001b[43mOrdering\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mColamd\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mgraph\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m colamd_ordering\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mCOLAMD Ordering: \u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 16\u001b[0m \u001b[38;5;66;03m# Constrained COLAMD (force x0 and x2 to be eliminated last)\u001b[39;00m\n", + "\u001b[1;31mTypeError\u001b[0m: Colamd(): incompatible function arguments. The following argument types are supported:\n 1. (variableIndex: gtsam::VariableIndex) -> gtsam.gtsam.Ordering\n\nInvoked with: kwargs: graph=SymbolicFactorGraph \nsize: 7\nfactor 0: x0\nfactor 1: x0 x1\nfactor 2: x1 x2\nfactor 3: x0 l1\nfactor 4: x1 l1\nfactor 5: x1 l2\nfactor 6: x2 l2\n" ] } ], @@ -137,7 +133,7 @@ "graph.push_factor(X(2), L(2))\n", "\n", "# COLAMD (Column Approximate Minimum Degree) ordering\n", - "colamd_ordering = Ordering.Colamd(VariableIndex(graph))\n", + "colamd_ordering = Ordering.Colamd(graph)\n", "colamd_ordering.print(\"COLAMD Ordering: \")\n", "\n", "# Constrained COLAMD (force x0 and x2 to be eliminated last)\n", diff --git a/gtsam/inference/doc/Symbol.ipynb b/gtsam/inference/doc/Symbol.ipynb index 6f5430a64..20d9fd7e2 100644 --- a/gtsam/inference/doc/Symbol.ipynb +++ b/gtsam/inference/doc/Symbol.ipynb @@ -31,7 +31,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "symbol_pip_code" + "id": "symbol_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ diff --git a/gtsam/inference/doc/VariableIndex.ipynb b/gtsam/inference/doc/VariableIndex.ipynb index 86ce4ab75..0a3af8c3f 100644 --- a/gtsam/inference/doc/VariableIndex.ipynb +++ b/gtsam/inference/doc/VariableIndex.ipynb @@ -33,7 +33,10 @@ "cell_type": "code", "execution_count": null, "metadata": { - "id": "vindex_pip_code" + "id": "vindex_pip_code", + "tags": [ + "remove-cell" + ] }, "outputs": [], "source": [ From 06a13bed70721bc1ad0412fa2064f0231857b7f2 Mon Sep 17 00:00:00 2001 From: p-zach Date: Mon, 14 Apr 2025 18:59:40 -0400 Subject: [PATCH 06/24] ClusterTree --- gtsam/inference/doc/ClusterTree.ipynb | 151 +------------------------- 1 file changed, 1 insertion(+), 150 deletions(-) diff --git a/gtsam/inference/doc/ClusterTree.ipynb b/gtsam/inference/doc/ClusterTree.ipynb index 402680078..2610edba7 100644 --- a/gtsam/inference/doc/ClusterTree.ipynb +++ b/gtsam/inference/doc/ClusterTree.ipynb @@ -19,156 +19,7 @@ "\n", "`ClusterTree` itself is a base class. `EliminatableClusterTree` adds the ability to perform elimination, and `JunctionTree` is a specific type of `EliminatableClusterTree` derived from an `EliminationTree`.\n", "\n", - "Direct use of `ClusterTree` in typical Python applications is less common than `JunctionTree` or `BayesTree`, as it's often an intermediate representation." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "clustertree_colab_md" - }, - "source": [ - "\"Open" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "clustertree_pip_code", - "tags": [ - "remove-cell" - ] - }, - "outputs": [], - "source": [ - "%pip install gtsam" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": { - "id": "clustertree_import_code" - }, - "outputs": [], - "source": [ - "import gtsam\n", - "import numpy as np\n", - "\n", - "# Note: ClusterTree itself might not be directly exposed or used.\n", - "# We typically interact with JunctionTree or BayesTree which build upon it.\n", - "# We'll demonstrate concepts using JunctionTree which inherits Cluster features.\n", - "from gtsam import GaussianFactorGraph, Ordering, VariableIndex, GaussianBayesTree\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "clustertree_concept_md" - }, - "source": [ - "## Concept and Relation to JunctionTree\n", - "\n", - "A `JunctionTree` *is a* `ClusterTree` (specifically, an `EliminatableClusterTree`). It's constructed during multifrontal elimination. Each node in the `JunctionTree` is a `Cluster` containing factors that will be eliminated together to form a clique in the resulting `BayesTree`.\n", - "\n", - "We will create a `JunctionTree` and examine its properties, which include those inherited from `ClusterTree`." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "clustertree_jt_code", - "outputId": "ef012345-6789-abcd-ef01-23456789abcd" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Junction Tree (as ClusterTree): \n", - ": cliques: 2, variables: 5\n", - "- p(x1 l2 x2 )\n", - " R = [ 1.61245 -0.620174 -0.620174 ]\n", - " [ 0 1.27098 -1.08941 ]\n", - " [ 0 0 0.654654 ]\n", - " d = [ 0 0 0 ]\n", - " mean: 3 elements\n", - " l2: 0\n", - " x1: 0\n", - " x2: 0\n", - " logNormalizationConstant: -2.46292\n", - " No noise model\n", - "| - p(l1 x0 | x1)\n", - " R = [ 1.41421 -0.707107 ]\n", - " [ 0 1.58114 ]\n", - " S[x1] = [ -0.707107 ]\n", - " [ -0.948683 ]\n", - " d = [ 0 0 ]\n", - " logNormalizationConstant: -1.03316\n", - " No noise model\n" - ] - }, - { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.GaussianBayesTree' object has no attribute 'roots'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[8], line 28\u001b[0m\n\u001b[0;32m 25\u001b[0m bayes_tree\u001b[38;5;241m.\u001b[39mprint() \u001b[38;5;66;03m# Printing BayesTree shows clique structure\u001b[39;00m\n\u001b[0;32m 27\u001b[0m \u001b[38;5;66;03m# Access root cluster(s)\u001b[39;00m\n\u001b[1;32m---> 28\u001b[0m roots \u001b[38;5;241m=\u001b[39m \u001b[43mbayes_tree\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mroots\u001b[49m()\n\u001b[0;32m 29\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m roots:\n\u001b[0;32m 30\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mAccessing a root cluster (node):\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.GaussianBayesTree' object has no attribute 'roots'" - ] - } - ], - "source": [ - "# Create a graph (same as BayesTree example)\n", - "graph = GaussianFactorGraph()\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", - "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "\n", - "ordering = Ordering.Colamd(VariableIndex(graph))\n", - "\n", - "# Create a Junction Tree (implicitly uses ClusterTree structure)\n", - "# Note: JunctionTree constructor might not be directly exposed.\n", - "# It's usually an intermediate in eliminateMultifrontal.\n", - "# We might need to construct it indirectly or focus on BayesTree access.\n", - "\n", - "# Let's get the BayesTree first, as JunctionTree creation is internal.\n", - "bayes_tree = graph.eliminateMultifrontal(ordering)\n", - "\n", - "# We can print the BayesTree, which shows the cluster structure\n", - "# (Cliques in BayesTree correspond to Clusters in JunctionTree)\n", - "print(\"Junction Tree (as ClusterTree): \")\n", - "bayes_tree.print() # Printing BayesTree shows clique structure\n", - "\n", - "# Access root cluster(s)\n", - "roots = bayes_tree.roots()\n", - "if roots:\n", - " print(\"\\nAccessing a root cluster (node):\")\n", - " root_clique = roots[0]\n", - " # In the JunctionTree, this node would contain the factors that *produced*\n", - " # the conditional in the BayesTree clique. We can see the involved keys.\n", - " root_clique.print(\"\", gtsam.DefaultKeyFormatter) # Print clique details\n", - "\n", - "print(f\"\\nNumber of roots: {len(roots)}\")\n", - "\n", - "# Direct instantiation or manipulation of Cluster/JunctionTree nodes\n", - "# is less common in Python than using the results (BayesTree)." + "Direct use of `ClusterTree` in Python applications is uncommon; see the subclasses." ] } ], From 241d6dc07a7cdd6e42c7f9cee17a10985e1172a5 Mon Sep 17 00:00:00 2001 From: p-zach Date: Mon, 14 Apr 2025 19:02:52 -0400 Subject: [PATCH 07/24] Tab copy errors --- gtsam/inference/doc/BayesNet.ipynb | 2 +- gtsam/inference/doc/BayesTree.ipynb | 2 +- gtsam/inference/doc/JunctionTree.ipynb | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/doc/BayesNet.ipynb b/gtsam/inference/doc/BayesNet.ipynb index 82f545363..c2b8ff67f 100644 --- a/gtsam/inference/doc/BayesNet.ipynb +++ b/gtsam/inference/doc/BayesNet.ipynb @@ -17,7 +17,7 @@ "source": [ "A `BayesNet` in GTSAM represents a directed graphical model, specifically the result of running sequential variable elimination (like Cholesky or QR factorization) on a `FactorGraph`.\n", "\n", - "It is essentially a collection of `Conditional` objects, ordered according to the elimination order. Each conditional represents $P(\text{variable} | \text{parents})$, where the parents are variables that appear later in the elimination ordering.\n", + "It is essentially a collection of `Conditional` objects, ordered according to the elimination order. Each conditional represents $P(\\text{variable} | \\text{parents})$, where the parents are variables that appear later in the elimination ordering.\n", "\n", "A Bayes net represents the joint probability distribution as a product of conditional probabilities stored in the net:\n", "\n", diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index a5f6479e4..50863543f 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -15,7 +15,7 @@ "id": "bayestree_desc_md" }, "source": [ - "A `BayesTree` is a graphical model that represents the result of multifrontal variable elimination on a `FactorGraph`. It is a tree structure where each node is a 'clique' containing a set of conditional distributions $P(\text{Frontals} | \text{Separator})$.\n", + "A `BayesTree` is a graphical model that represents the result of multifrontal variable elimination on a `FactorGraph`. It is a tree structure where each node is a 'clique' containing a set of conditional distributions $P(\\text{Frontals} | \\text{Separator})$.\n", "\n", "Each clique k contains a conditional $P(F_k∣S_k)$, where $F_k$ are frontal variables and $S_k$ are separator variables. The joint probability distribution encoded by the Bayes tree is given by the product of all clique conditionals:\n", "\n", diff --git a/gtsam/inference/doc/JunctionTree.ipynb b/gtsam/inference/doc/JunctionTree.ipynb index 7ebafb831..35b326dda 100644 --- a/gtsam/inference/doc/JunctionTree.ipynb +++ b/gtsam/inference/doc/JunctionTree.ipynb @@ -19,7 +19,7 @@ "\n", "Key differences from related structures:\n", "* **vs. EliminationTree:** Junction tree nodes can represent the elimination of multiple variables simultaneously (a 'frontal' set), whereas elimination tree nodes typically represent single variable eliminations.\n", - "* **vs. BayesTree:** A JunctionTree node contains the original factors associated with the variables being eliminated in that clique. A BayesTree node contains the *result* of eliminating those factors (i.e., a conditional density $P(\text{Frontals} | \text{Separator})$).\n", + "* **vs. BayesTree:** A JunctionTree node contains the original factors associated with the variables being eliminated in that clique. A BayesTree node contains the *result* of eliminating those factors (i.e., a conditional density $P(\\text{Frontals} | \\text{Separator})$).\n", "\n", "Like `EliminationTree`, direct manipulation of `JunctionTree` objects in Python is uncommon. It's primarily an internal structure used by `eliminateMultifrontal` when producing a `BayesTree`." ] From b54ad4e3f01c69ce305dcde023c7da73d842d107 Mon Sep 17 00:00:00 2001 From: p-zach Date: Mon, 14 Apr 2025 19:07:42 -0400 Subject: [PATCH 08/24] BayesTree cleanup --- gtsam/inference/doc/BayesTree.ipynb | 49 +++++++++++++++++------------ 1 file changed, 29 insertions(+), 20 deletions(-) diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index 50863543f..14100971d 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -22,11 +22,6 @@ "$$\n", "P(X) = \\prod_k P(F_k | S_k)\n", "$$\n", - "Alternatively, it can be expressed using clique $P(C_k) = P(F_k, S_k)$ and separator $P(S_k)$ marginals (though GTSAM stores conditionals):\n", - "$$\n", - "P(X) = \\frac{\\prod_{\\text{cliques } k} P(C_k)}{\\prod_{\\text{separators } S} P(S)^{\\nu(S)-1}}\n", - "$$\n", - "where $\\nu(S)$ is the number of cliques containing the separator $S$. The first formula $P(X) = \\prod_k P(F_k | S_k)$ is more directly related to the GTSAM `BayesTree` structure.\n", "\n", "Key properties:\n", "* **Cliques:** Each node (clique) groups variables that are eliminated together.\n", @@ -167,7 +162,35 @@ "\t1\n", "]\n", " b = [ 0 ]\n", - " Noise model: unit (1) \n", + " Noise model: unit (1) \n" + ] + } + ], + "source": [ + "# Create a simple Gaussian Factor Graph (more complex this time)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # x0 -> x1\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # x1 -> x2\n", + "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model) # l1 -> x0 (measurement)\n", + "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l1 -> x1 (measurement)\n", + "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l2 -> x1 (measurement)\n", + "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # l2 -> x2 (measurement)\n", + "\n", + "print(\"Original Factor Graph:\")\n", + "graph.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ "\n", "Resulting BayesTree:\n", ": cliques: 2, variables: 5\n", @@ -194,20 +217,6 @@ } ], "source": [ - "# Create a simple Gaussian Factor Graph (more complex this time)\n", - "graph = GaussianFactorGraph()\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n", - "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # x0 -> x1\n", - "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # x1 -> x2\n", - "graph.add(L(1), -np.eye(1), X(0), np.eye(1), np.zeros(1), model) # l1 -> x0 (measurement)\n", - "graph.add(L(1), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l1 -> x1 (measurement)\n", - "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # l2 -> x1 (measurement)\n", - "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # l2 -> x2 (measurement)\n", - "\n", - "print(\"Original Factor Graph:\")\n", - "graph.print()\n", - "\n", "# Eliminate multifrontally using COLAMD ordering\n", "ordering = Ordering.Colamd(VariableIndex(graph))\n", "# Note: Multifrontal typically yields multiple roots if graph is disconnected\n", From f48e94fa25682350768e255d77b0e1f64ab4eda5 Mon Sep 17 00:00:00 2001 From: p-zach Date: Mon, 14 Apr 2025 19:14:52 -0400 Subject: [PATCH 09/24] Graphviz/dot cleanup --- gtsam/inference/doc/BayesNet.ipynb | 39 ++++++------------------ gtsam/inference/doc/BayesTree.ipynb | 34 ++++++--------------- gtsam/inference/doc/DotWriter.ipynb | 3 +- gtsam/inference/doc/FactorGraph.ipynb | 43 ++++++--------------------- 4 files changed, 28 insertions(+), 91 deletions(-) diff --git a/gtsam/inference/doc/BayesNet.ipynb b/gtsam/inference/doc/BayesNet.ipynb index c2b8ff67f..7a0964f73 100644 --- a/gtsam/inference/doc/BayesNet.ipynb +++ b/gtsam/inference/doc/BayesNet.ipynb @@ -57,7 +57,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 8, "metadata": { "id": "bayesnet_import_code" }, @@ -65,6 +65,7 @@ "source": [ "import gtsam\n", "import numpy as np\n", + "import graphviz\n", "\n", "# We need concrete graph types and elimination to get a BayesNet\n", "from gtsam import GaussianFactorGraph, Ordering, GaussianBayesNet\n", @@ -87,7 +88,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 9, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -184,7 +185,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 10, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -235,7 +236,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 11, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -282,7 +283,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 12, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -291,22 +292,6 @@ "outputId": "3456789a-bcde-f012-3456-789abcdef012" }, "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "digraph {\n", - " size=\"5,5\";\n", - "\n", - " var8646911284551352320[label=\"x0\"];\n", - " var8646911284551352321[label=\"x1\"];\n", - " var8646911284551352322[label=\"x2\"];\n", - "\n", - " var8646911284551352322->var8646911284551352321\n", - " var8646911284551352321->var8646911284551352320\n", - "}\n" - ] - }, { "data": { "image/svg+xml": [ @@ -354,22 +339,16 @@ "\n" ], "text/plain": [ - "" + "" ] }, - "execution_count": 5, + "execution_count": 12, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "dot_string = bayes_net.dot()\n", - "print(dot_string)\n", - "\n", - "# To render:\n", - "# dot -Tpng bayesnet.dot -o bayesnet.png\n", - "import graphviz\n", - "graphviz.Source(dot_string)" + "graphviz.Source(bayes_net.dot())" ] } ], diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index 14100971d..fcefe9821 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -57,7 +57,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 8, "metadata": { "id": "bayestree_import_code" }, @@ -65,6 +65,7 @@ "source": [ "import gtsam\n", "import numpy as np\n", + "import graphviz\n", "\n", "# We need concrete graph types and elimination to get a BayesTree\n", "from gtsam import GaussianFactorGraph, Ordering, GaussianBayesTree, VariableIndex\n", @@ -239,7 +240,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 5, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -262,7 +263,7 @@ "traceback": [ "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[4], line 4\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mBayesTree number of cliques: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mbayes_tree\u001b[38;5;241m.\u001b[39msize()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 3\u001b[0m \u001b[38;5;66;03m# Access roots\u001b[39;00m\n\u001b[1;32m----> 4\u001b[0m roots \u001b[38;5;241m=\u001b[39m \u001b[43mbayes_tree\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mroots\u001b[49m()\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mNumber of roots: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[38;5;28mlen\u001b[39m(roots)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m roots:\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Access the conditional associated with the first root clique\u001b[39;00m\n", + "Cell \u001b[1;32mIn[5], line 4\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mBayesTree number of cliques: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mbayes_tree\u001b[38;5;241m.\u001b[39msize()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 3\u001b[0m \u001b[38;5;66;03m# Access roots\u001b[39;00m\n\u001b[1;32m----> 4\u001b[0m roots \u001b[38;5;241m=\u001b[39m \u001b[43mbayes_tree\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mroots\u001b[49m()\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mNumber of roots: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[38;5;28mlen\u001b[39m(roots)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m roots:\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Access the conditional associated with the first root clique\u001b[39;00m\n", "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.GaussianBayesTree' object has no attribute 'roots'" ] } @@ -298,7 +299,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 6, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -377,7 +378,7 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 9, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -386,17 +387,6 @@ "outputId": "789abcde-f012-3456-789a-bcdef0123456" }, "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "digraph G{\n", - "0[label=\"x1, l2, x2\"];\n", - "0->1\n", - "1[label=\"l1, x0 : x1\"];\n", - "}\n" - ] - }, { "data": { "image/svg+xml": [ @@ -433,22 +423,16 @@ "\n" ], "text/plain": [ - "" + "" ] }, - "execution_count": 6, + "execution_count": 9, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "dot_string = bayes_tree.dot()\n", - "print(dot_string)\n", - "\n", - "# To render:\n", - "# dot -Tpng bayestree.dot -o bayestree.png\n", - "import graphviz\n", - "graphviz.Source(dot_string)" + "graphviz.Source(bayes_tree.dot())" ] } ], diff --git a/gtsam/inference/doc/DotWriter.ipynb b/gtsam/inference/doc/DotWriter.ipynb index 81a72316d..6ad795add 100644 --- a/gtsam/inference/doc/DotWriter.ipynb +++ b/gtsam/inference/doc/DotWriter.ipynb @@ -123,7 +123,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -300,7 +300,6 @@ "\n", "# Generate dot string using the configured writer\n", "dot_string = graph.dot(writer=writer)\n", - "# print(dot_string)\n", "\n", "# Render the graph\n", "graphviz.Source(dot_string)" diff --git a/gtsam/inference/doc/FactorGraph.ipynb b/gtsam/inference/doc/FactorGraph.ipynb index 07633ae9c..a48d56ef7 100644 --- a/gtsam/inference/doc/FactorGraph.ipynb +++ b/gtsam/inference/doc/FactorGraph.ipynb @@ -58,7 +58,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 1, "metadata": { "id": "fg_import_code" }, @@ -66,6 +66,7 @@ "source": [ "import gtsam\n", "import numpy as np\n", + "import graphviz\n", "\n", "# Example uses NonlinearFactorGraph, but concepts apply to others\n", "from gtsam import NonlinearFactorGraph, PriorFactorPose2, BetweenFactorPose2, Pose2, Point3\n", @@ -87,7 +88,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 2, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -134,7 +135,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 3, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -191,7 +192,7 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 4, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -237,7 +238,7 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 5, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -246,25 +247,6 @@ "outputId": "56789abc-def0-1234-5678-9abcdef01234" }, "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "graph {\n", - " size=\"5,5\";\n", - "\n", - " var8646911284551352320[label=\"x0\", pos=\"0,0!\"];\n", - " var8646911284551352321[label=\"x1\", pos=\"0,1!\"];\n", - "\n", - " factor0[label=\"\", shape=point];\n", - " var8646911284551352320--factor0;\n", - " factor1[label=\"\", shape=point];\n", - " var8646911284551352320--factor1;\n", - " var8646911284551352321--factor1;\n", - "}\n", - "\n" - ] - }, { "data": { "image/svg+xml": [ @@ -319,23 +301,16 @@ "\n" ], "text/plain": [ - "" + "" ] }, - "execution_count": 8, + "execution_count": 5, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "dot_string = graph.dot(values)\n", - "print(dot_string)\n", - "\n", - "# To render, save dot_string to a file (e.g., graph.dot) and run:\n", - "# dot -Tpng graph.dot -o graph.png\n", - "# Or use a Python library like graphviz\n", - "import graphviz\n", - "graphviz.Source(dot_string)" + "graphviz.Source(graph.dot(values))" ] }, { From 91924d92db470a57515a9538ac92017cd90c5a80 Mon Sep 17 00:00:00 2001 From: p-zach Date: Mon, 14 Apr 2025 19:18:00 -0400 Subject: [PATCH 10/24] Remove .roots() --- gtsam/inference/doc/BayesTree.ipynb | 40 ++--------------------------- 1 file changed, 2 insertions(+), 38 deletions(-) diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index fcefe9821..e45b1f8fa 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -227,20 +227,9 @@ "bayes_tree.print()" ] }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayestree_props_md" - }, - "source": [ - "## Properties and Access\n", - "\n", - "A `BayesTree` allows access to its root cliques and provides a way to look up the clique containing a specific variable." - ] - }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 11, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -255,35 +244,10 @@ "text": [ "BayesTree number of cliques: 2\n" ] - }, - { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.GaussianBayesTree' object has no attribute 'roots'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[5], line 4\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mBayesTree number of cliques: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mbayes_tree\u001b[38;5;241m.\u001b[39msize()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 3\u001b[0m \u001b[38;5;66;03m# Access roots\u001b[39;00m\n\u001b[1;32m----> 4\u001b[0m roots \u001b[38;5;241m=\u001b[39m \u001b[43mbayes_tree\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mroots\u001b[49m()\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mNumber of roots: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[38;5;28mlen\u001b[39m(roots)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m roots:\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Access the conditional associated with the first root clique\u001b[39;00m\n", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.GaussianBayesTree' object has no attribute 'roots'" - ] } ], "source": [ - "print(f\"BayesTree number of cliques: {bayes_tree.size()}\")\n", - "\n", - "# Access roots\n", - "roots = bayes_tree.roots()\n", - "print(f\"Number of roots: {len(roots)}\")\n", - "if roots:\n", - " # Access the conditional associated with the first root clique\n", - " root_conditional = roots[0].conditional()\n", - " print(f\"Root clique 0 conditional frontals: {root_conditional.frontals()}\")\n", - "\n", - "# Find the clique containing a specific variable\n", - "clique_x1 = bayes_tree.clique(X(1))\n", - "# clique_x1 is a shared pointer to the clique (e.g., GaussianBayesTreeClique)\n", - "print(f\"\\nClique containing x1 ({X(1)}):\")\n", - "clique_x1.print() # Print the clique itself" + "print(f\"BayesTree number of cliques: {bayes_tree.size()}\")" ] }, { From d1e8fd3863cfea2f7a5d120fdba55849d7b99928 Mon Sep 17 00:00:00 2001 From: p-zach Date: Mon, 14 Apr 2025 20:01:02 -0400 Subject: [PATCH 11/24] Ordering fixes --- gtsam/inference/doc/Ordering.ipynb | 59 +++++++----------------------- 1 file changed, 14 insertions(+), 45 deletions(-) diff --git a/gtsam/inference/doc/Ordering.ipynb b/gtsam/inference/doc/Ordering.ipynb index 9dfcd0b4a..7e3bab97e 100644 --- a/gtsam/inference/doc/Ordering.ipynb +++ b/gtsam/inference/doc/Ordering.ipynb @@ -45,7 +45,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 6, "metadata": { "id": "ordering_import_code" }, @@ -74,7 +74,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 7, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -99,7 +99,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 12, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -109,14 +109,11 @@ }, "outputs": [ { - "ename": "TypeError", - "evalue": "Colamd(): incompatible function arguments. The following argument types are supported:\n 1. (variableIndex: gtsam::VariableIndex) -> gtsam.gtsam.Ordering\n\nInvoked with: kwargs: graph=SymbolicFactorGraph \nsize: 7\nfactor 0: x0\nfactor 1: x0 x1\nfactor 2: x1 x2\nfactor 3: x0 l1\nfactor 4: x1 l1\nfactor 5: x1 l2\nfactor 6: x2 l2\n", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[3], line 13\u001b[0m\n\u001b[0;32m 10\u001b[0m graph\u001b[38;5;241m.\u001b[39mpush_factor(X(\u001b[38;5;241m2\u001b[39m), L(\u001b[38;5;241m2\u001b[39m))\n\u001b[0;32m 12\u001b[0m \u001b[38;5;66;03m# COLAMD (Column Approximate Minimum Degree) ordering\u001b[39;00m\n\u001b[1;32m---> 13\u001b[0m colamd_ordering \u001b[38;5;241m=\u001b[39m \u001b[43mOrdering\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mColamd\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mgraph\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m colamd_ordering\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mCOLAMD Ordering: \u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 16\u001b[0m \u001b[38;5;66;03m# Constrained COLAMD (force x0 and x2 to be eliminated last)\u001b[39;00m\n", - "\u001b[1;31mTypeError\u001b[0m: Colamd(): incompatible function arguments. The following argument types are supported:\n 1. (variableIndex: gtsam::VariableIndex) -> gtsam.gtsam.Ordering\n\nInvoked with: kwargs: graph=SymbolicFactorGraph \nsize: 7\nfactor 0: x0\nfactor 1: x0 x1\nfactor 2: x1 x2\nfactor 3: x0 l1\nfactor 4: x1 l1\nfactor 5: x1 l2\nfactor 6: x2 l2\n" + "name": "stdout", + "output_type": "stream", + "text": [ + "COLAMD Ordering: Position 0: l1, x0, x1, l2, x2\n", + "Constrained COLAMD (x0, x2 last): Position 0: l2, l1, x1, x2, x0\n" ] } ], @@ -133,23 +130,12 @@ "graph.push_factor(X(2), L(2))\n", "\n", "# COLAMD (Column Approximate Minimum Degree) ordering\n", - "colamd_ordering = Ordering.Colamd(graph)\n", + "colamd_ordering = Ordering.Colamd(VariableIndex(graph))\n", "colamd_ordering.print(\"COLAMD Ordering: \")\n", "\n", "# Constrained COLAMD (force x0 and x2 to be eliminated last)\n", - "constrained_ordering = Ordering.ColamdConstrainedLast(graph, gtsam.KeyVector([X(0), X(2)]))\n", - "constrained_ordering.print(\"Constrained COLAMD (x0, x2 last): \")\n", - "\n", - "# METIS ordering (if GTSAM was built with METIS support)\n", - "try:\n", - " metis_ordering = Ordering.Metis(graph)\n", - " # metis_ordering.print(\"METIS Ordering: \")\n", - "except RuntimeError as e:\n", - " print(f\"\\nSkipping METIS: {e}\")\n", - "\n", - "# Natural ordering (based on key magnitude)\n", - "natural_ordering = Ordering.Natural(graph)\n", - "# natural_ordering.print(\"Natural Ordering: \")" + "constrained_ordering = Ordering.ColamdConstrainedLastSymbolicFactorGraph(graph, gtsam.KeyVector([X(0), X(2)]))\n", + "constrained_ordering.print(\"Constrained COLAMD (x0, x2 last): \")" ] }, { @@ -165,7 +151,7 @@ }, { "cell_type": "code", - "execution_count": 11, + "execution_count": 14, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -181,17 +167,6 @@ "Ordering size: 5\n", "Key at position 0: l1\n" ] - }, - { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.Ordering' object has no attribute 'invert'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[11], line 8\u001b[0m\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mKey at position 0: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mgtsam\u001b[38;5;241m.\u001b[39mDefaultKeyFormatter(key_at_0)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Invert the ordering (map from Key to its position)\u001b[39;00m\n\u001b[1;32m----> 8\u001b[0m inverted \u001b[38;5;241m=\u001b[39m \u001b[43mcolamd_ordering\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minvert\u001b[49m()\n\u001b[0;32m 9\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mInverted map (Key -> Position):\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 10\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m key, pos \u001b[38;5;129;01min\u001b[39;00m inverted\u001b[38;5;241m.\u001b[39mitems():\n", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.Ordering' object has no attribute 'invert'" - ] } ], "source": [ @@ -199,13 +174,7 @@ "\n", "# Access by index\n", "key_at_0 = colamd_ordering.at(0)\n", - "print(f\"Key at position 0: {gtsam.DefaultKeyFormatter(key_at_0)}\")\n", - "\n", - "# Invert the ordering (map from Key to its position)\n", - "inverted = colamd_ordering.invert()\n", - "print(\"Inverted map (Key -> Position):\")\n", - "for key, pos in inverted.items():\n", - " print(f\" {gtsam.DefaultKeyFormatter(key)} -> {pos}\")" + "print(f\"Key at position 0: {gtsam.DefaultKeyFormatter(key_at_0)}\")" ] }, { @@ -216,7 +185,7 @@ "source": [ "## Appending Keys\n", "\n", - "You can append keys using `push_back`, `+=`, or `,`." + "You can append keys using `push_back`." ] }, { From 369acec51163dd9272ce5068e12e2410052c4181 Mon Sep 17 00:00:00 2001 From: p-zach Date: Mon, 14 Apr 2025 20:03:32 -0400 Subject: [PATCH 12/24] Fix DBT --- python/gtsam/examples/DiscreteBayesTree.ipynb | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/python/gtsam/examples/DiscreteBayesTree.ipynb b/python/gtsam/examples/DiscreteBayesTree.ipynb index f980a22c0..90fbac747 100644 --- a/python/gtsam/examples/DiscreteBayesTree.ipynb +++ b/python/gtsam/examples/DiscreteBayesTree.ipynb @@ -43,8 +43,7 @@ }, "outputs": [], "source": [ - "# This needs gtbook:\n", - "% pip install --quiet gtbook" + "%pip install --quiet gtsam-develop" ] }, { From 45dec16225b842113bc686b000d8beb2fef82e35 Mon Sep 17 00:00:00 2001 From: p-zach Date: Tue, 15 Apr 2025 14:00:04 -0400 Subject: [PATCH 13/24] Fix Python char nonexistence with ord() --- gtsam/inference/doc/Key.ipynb | 47 ++++++---------- gtsam/inference/doc/LabeledSymbol.ipynb | 72 +++++++++++-------------- 2 files changed, 47 insertions(+), 72 deletions(-) diff --git a/gtsam/inference/doc/Key.ipynb b/gtsam/inference/doc/Key.ipynb index d643869da..c8ad5c8c3 100644 --- a/gtsam/inference/doc/Key.ipynb +++ b/gtsam/inference/doc/Key.ipynb @@ -43,7 +43,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 1, "metadata": { "id": "key_import_code" }, @@ -67,7 +67,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 2, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -81,19 +81,12 @@ "output_type": "stream", "text": [ "Symbol Key (x0): 8646911284551352320\n", + "Type: \n", + "LabeledSymbol Key (aB1): 7008163970141913089\n", + "Type: \n", + "Plain Integer Key: 12345\n", "Type: \n" ] - }, - { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'a', 'B', 1", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[3], line 6\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mSymbol Key (x0): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mkey_from_symbol\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mType: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[38;5;28mtype\u001b[39m(key_from_symbol)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m----> 6\u001b[0m lsym \u001b[38;5;241m=\u001b[39m \u001b[43mLabeledSymbol\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43ma\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mB\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m1\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[0;32m 7\u001b[0m key_from_labeled_symbol \u001b[38;5;241m=\u001b[39m lsym\u001b[38;5;241m.\u001b[39mkey()\n\u001b[0;32m 8\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mLabeledSymbol Key (aB1): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mkey_from_labeled_symbol\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", - "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'a', 'B', 1" - ] } ], "source": [ @@ -102,7 +95,7 @@ "print(f\"Symbol Key (x0): {key_from_symbol}\")\n", "print(f\"Type: {type(key_from_symbol)}\")\n", "\n", - "lsym = LabeledSymbol('a', 'B', 1)\n", + "lsym = LabeledSymbol(ord('a'), ord('B'), 1)\n", "key_from_labeled_symbol = lsym.key()\n", "print(f\"LabeledSymbol Key (aB1): {key_from_labeled_symbol}\")\n", "print(f\"Type: {type(key_from_labeled_symbol)}\")\n", @@ -121,7 +114,7 @@ "source": [ "## Key Formatting\n", "\n", - "When printing GTSAM objects that contain keys (like Factor Graphs or Values), you can specify a `KeyFormatter` to control how keys are displayed. The default formatter tries to interpret keys as `Symbol`s. `MultiRobotKeyFormatter` also checks for `LabeledSymbol`s." + "When printing GTSAM objects that contain keys (like Factor Graphs or Values), you can specify a `KeyFormatter` to control how keys are displayed. The default formatter tries to interpret keys as `Symbol`s." ] }, { @@ -140,18 +133,13 @@ "output_type": "stream", "text": [ "Default Formatter:\n", - " Symbol Key: x0\n" - ] - }, - { - "ename": "NameError", - "evalue": "name 'key_from_labeled_symbol' is not defined", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mNameError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[4], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mDefault Formatter:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m Symbol Key: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mgtsam\u001b[38;5;241m.\u001b[39mDefaultKeyFormatter(key_from_symbol)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m LabeledSymbol Key: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mgtsam\u001b[38;5;241m.\u001b[39mDefaultKeyFormatter(\u001b[43mkey_from_labeled_symbol\u001b[49m)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m Plain Key: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mgtsam\u001b[38;5;241m.\u001b[39mDefaultKeyFormatter(plain_key)\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mMultiRobot Formatter:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n", - "\u001b[1;31mNameError\u001b[0m: name 'key_from_labeled_symbol' is not defined" + " Symbol Key: x0\n", + " LabeledSymbol Key: a18577348\n", + " Plain Key: 12345\n", + "Custom Formatter:\n", + " Symbol Key: KEY[x0]\n", + " LabeledSymbol Key: KEY[a18577348]\n", + " Plain Key: KEY[12345]\n" ] } ], @@ -161,11 +149,6 @@ "print(f\" LabeledSymbol Key: {gtsam.DefaultKeyFormatter(key_from_labeled_symbol)}\")\n", "print(f\" Plain Key: {gtsam.DefaultKeyFormatter(plain_key)}\")\n", "\n", - "print(\"MultiRobot Formatter:\")\n", - "print(f\" Symbol Key: {gtsam.MultiRobotKeyFormatter(key_from_symbol)}\")\n", - "print(f\" LabeledSymbol Key: {gtsam.MultiRobotKeyFormatter(key_from_labeled_symbol)}\")\n", - "print(f\" Plain Key: {gtsam.MultiRobotKeyFormatter(plain_key)}\")\n", - "\n", "# Example of a custom formatter\n", "def my_formatter(key):\n", " # Try interpreting as LabeledSymbol, then Symbol, then default\n", diff --git a/gtsam/inference/doc/LabeledSymbol.ipynb b/gtsam/inference/doc/LabeledSymbol.ipynb index 2e8295086..c383cbbaa 100644 --- a/gtsam/inference/doc/LabeledSymbol.ipynb +++ b/gtsam/inference/doc/LabeledSymbol.ipynb @@ -43,7 +43,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "lsymbol_import_code" }, @@ -66,7 +66,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -76,21 +76,21 @@ }, "outputs": [ { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'x', 'A', 7", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[2], line 2\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;66;03m# Create LabeledSymbol 'x' from robot 'A' with index 7\u001b[39;00m\n\u001b[1;32m----> 2\u001b[0m lsym1 \u001b[38;5;241m=\u001b[39m \u001b[43mLabeledSymbol\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mx\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mA\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m7\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mLabeledSymbol from char/label/index: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mlsym1\u001b[38;5;241m.\u001b[39mstring()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# Get the underlying integer key\u001b[39;00m\n", - "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'x', 'A', 7" + "name": "stdout", + "output_type": "stream", + "text": [ + "LabeledSymbol from char/label/index: : xA7\n", + "\n", + "LabeledSymbol from key 8646911284551352320: : x\n", + "\n" ] } ], "source": [ "# Create LabeledSymbol 'x' from robot 'A' with index 7\n", - "lsym1 = LabeledSymbol('x', 'A', 7)\n", - "print(f\"LabeledSymbol from char/label/index: {lsym1.string()}\")\n", + "# The underlying C++ expects chars, so we have to convert single-character strings with ord()\n", + "lsym1 = LabeledSymbol(ord('x'), ord('A'), 7)\n", + "print(f\"LabeledSymbol from char/label/index: {lsym1}\")\n", "\n", "# Get the underlying integer key\n", "key1 = lsym1.key()\n", @@ -100,7 +100,7 @@ "# If you decode a standard Symbol key, the label might be garbage.\n", "x0_key = gtsam.Symbol('x', 0).key()\n", "lsym2 = LabeledSymbol(x0_key)\n", - "print(f\"LabeledSymbol from key {x0_key}: {lsym2.string()}\") # Label might be non-printable or 0" + "print(f\"LabeledSymbol from key {x0_key}: {lsym2}\")" ] }, { @@ -116,7 +116,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 8, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -126,29 +126,26 @@ }, "outputs": [ { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'l', 'B', 3", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[3], line 1\u001b[0m\n\u001b[1;32m----> 1\u001b[0m robotB_landmark \u001b[38;5;241m=\u001b[39m \u001b[43mLabeledSymbol\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43ml\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mB\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m3\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mLabeledSymbol: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mrobotB_landmark\u001b[38;5;241m.\u001b[39mstring()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m Char (Type): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mrobotB_landmark\u001b[38;5;241m.\u001b[39mchr()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", - "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'l', 'B', 3" + "name": "stdout", + "output_type": "stream", + "text": [ + "LabeledSymbol: : lB3\n", + "\n", + " Char (Type): 108\n", + " Label (Robot): 66\n", + " Index: 3\n", + " Key: 7800797504559120387\n" ] } ], "source": [ - "robotB_landmark = LabeledSymbol('l', 'B', 3)\n", + "robotB_landmark = LabeledSymbol(ord('l'), ord('B'), 3)\n", "\n", - "print(f\"LabeledSymbol: {robotB_landmark.string()}\")\n", + "print(f\"LabeledSymbol: {robotB_landmark}\")\n", "print(f\" Char (Type): {robotB_landmark.chr()}\")\n", "print(f\" Label (Robot): {robotB_landmark.label()}\")\n", "print(f\" Index: {robotB_landmark.index()}\")\n", - "print(f\" Key: {robotB_landmark.key()}\")\n", - "\n", - "# LabeledSymbols are often used directly where Keys are expected.\n", - "# Use the MultiRobotKeyFormatter for printing these keys meaningfully.\n", - "# e.g., graph.print(\"Graph: \\n\", gtsam.MultiRobotKeyFormatter)" + "print(f\" Key: {robotB_landmark.key()}\")" ] }, { @@ -164,7 +161,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -174,22 +171,17 @@ }, "outputs": [ { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'p', 'C', 2", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[4], line 4\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;66;03m# Note: mrsymbol expects integer representations of chars (use ord())\u001b[39;00m\n\u001b[0;32m 2\u001b[0m pc2_key \u001b[38;5;241m=\u001b[39m gtsam\u001b[38;5;241m.\u001b[39mmrsymbol(\u001b[38;5;28mord\u001b[39m(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mp\u001b[39m\u001b[38;5;124m'\u001b[39m), \u001b[38;5;28mord\u001b[39m(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mC\u001b[39m\u001b[38;5;124m'\u001b[39m), \u001b[38;5;241m2\u001b[39m)\n\u001b[1;32m----> 4\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mLabeledSymbol(\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mp\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m, \u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mC\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m, 2).key() == gtsam.mrsymbol(ord(\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mp\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m), ord(\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mC\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m), 2): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[43mLabeledSymbol\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mp\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[38;5;250;43m \u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mC\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m,\u001b[49m\u001b[38;5;250;43m \u001b[39;49m\u001b[38;5;241;43m2\u001b[39;49m\u001b[43m)\u001b[49m\u001b[38;5;241m.\u001b[39mkey()\u001b[38;5;250m \u001b[39m\u001b[38;5;241m==\u001b[39m\u001b[38;5;250m \u001b[39mpc2_key\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", - "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.LabeledSymbol(full_key: int)\n 2. gtsam.gtsam.LabeledSymbol(key: gtsam.gtsam.LabeledSymbol)\n 3. gtsam.gtsam.LabeledSymbol(valType: int, label: int, j: int)\n\nInvoked with: 'p', 'C', 2" + "name": "stdout", + "output_type": "stream", + "text": [ + "LabeledSymbol(ord('p'), ord('C'), 2).key() == gtsam.mrsymbol(ord('p'), ord('C'), 2): True\n" ] } ], "source": [ - "# Note: mrsymbol expects integer representations of chars (use ord())\n", "pc2_key = gtsam.mrsymbol(ord('p'), ord('C'), 2)\n", "\n", - "print(f\"LabeledSymbol('p', 'C', 2).key() == gtsam.mrsymbol(ord('p'), ord('C'), 2): {LabeledSymbol('p', 'C', 2).key() == pc2_key}\")" + "print(f\"LabeledSymbol(ord('p'), ord('C'), 2).key() == gtsam.mrsymbol(ord('p'), ord('C'), 2): {LabeledSymbol(ord('p'), ord('C'), 2).key() == pc2_key}\")" ] } ], From 7e912c5cdd371ebdb153ac591760138ed4051626 Mon Sep 17 00:00:00 2001 From: p-zach Date: Tue, 15 Apr 2025 14:01:36 -0400 Subject: [PATCH 14/24] Clean up BN, Factor --- gtsam/inference/doc/BayesNet.ipynb | 38 ++++++++++++++++++++---------- gtsam/inference/doc/Factor.ipynb | 13 ++++++---- 2 files changed, 34 insertions(+), 17 deletions(-) diff --git a/gtsam/inference/doc/BayesNet.ipynb b/gtsam/inference/doc/BayesNet.ipynb index 7a0964f73..db64eda3d 100644 --- a/gtsam/inference/doc/BayesNet.ipynb +++ b/gtsam/inference/doc/BayesNet.ipynb @@ -57,7 +57,7 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 13, "metadata": { "id": "bayesnet_import_code" }, @@ -88,7 +88,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 14, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -127,7 +127,30 @@ "\t1\n", "]\n", " b = [ 0 ]\n", - " Noise model: unit (1) \n", + " Noise model: unit (1) \n" + ] + } + ], + "source": [ + "# Create a simple Gaussian Factor Graph P(x0) P(x1|x0) P(x2|x1)\n", + "graph = GaussianFactorGraph()\n", + "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", + "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", + "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", + "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", + "print(\"Original Factor Graph:\")\n", + "graph.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ "\n", "Resulting BayesNet:\n", "\n", @@ -155,15 +178,6 @@ } ], "source": [ - "# Create a simple Gaussian Factor Graph P(x0) P(x1|x0) P(x2|x1)\n", - "graph = GaussianFactorGraph()\n", - "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", - "graph.add(X(0), -np.eye(1), np.zeros(1), model)\n", - "graph.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", - "graph.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", - "print(\"Original Factor Graph:\")\n", - "graph.print()\n", - "\n", "# Eliminate sequentially using a specific ordering\n", "ordering = Ordering([X(0), X(1), X(2)])\n", "bayes_net = graph.eliminateSequential(ordering)\n", diff --git a/gtsam/inference/doc/Factor.ipynb b/gtsam/inference/doc/Factor.ipynb index 40a53193d..6e6ce1515 100644 --- a/gtsam/inference/doc/Factor.ipynb +++ b/gtsam/inference/doc/Factor.ipynb @@ -56,7 +56,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 1, "metadata": { "id": "factor_import_code" }, @@ -85,7 +85,7 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 2, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -102,7 +102,10 @@ "Prior factor size: 1\n", "Between factor keys: [8646911284551352320, 8646911284551352321] (x0, x1)\n", "Between factor size: 2\n", - "Is prior factor empty? False\n" + "Is prior factor empty? False\n", + "Prior Factor: PriorFactor on x0\n", + " prior mean: (0, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.05];\n" ] } ], @@ -125,7 +128,7 @@ "print(f\"Is prior factor empty? {prior_factor.empty()}\")\n", "\n", "# Factors can be printed\n", - "# prior_factor.print(\"Prior Factor: \")" + "prior_factor.print(\"Prior Factor: \")" ] }, { @@ -141,7 +144,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 3, "metadata": { "colab": { "base_uri": "https://localhost:8080/" From 198efe23c7cc588acc52536a2268ec7877230296 Mon Sep 17 00:00:00 2001 From: p-zach Date: Tue, 15 Apr 2025 18:16:51 -0400 Subject: [PATCH 15/24] Remove COLAMD varindex overload --- gtsam/inference/inference.i | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index 5084355a8..13c4c9853 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -122,8 +122,11 @@ class Ordering { template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> + + // Can't have both of these Colamd implementations; the second overrides the first. + // Python wrapper is for ease of use, so choose the one that is more commonly used. static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); - static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex); + // static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, From 098704ac22fc571f239667e5504d33640b3a7772 Mon Sep 17 00:00:00 2001 From: p-zach Date: Tue, 15 Apr 2025 18:19:25 -0400 Subject: [PATCH 16/24] Wrap VariableIndex.at(), .empty() --- gtsam/inference/VariableIndex.cpp | 18 ++++++++++++++++++ gtsam/inference/VariableIndex.h | 16 ++++------------ gtsam/inference/inference.i | 3 +++ 3 files changed, 25 insertions(+), 12 deletions(-) diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 53d400223..3d22501b3 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -23,6 +23,24 @@ namespace gtsam { using namespace std; +const FactorIndices& operator[](Key variable) const { + KeyMap::const_iterator item = index_.find(variable); +} + +const FactorIndices& at(Key variable) const { + KeyMap::const_iterator item = (*this)[variable]; + if(item == index_.end()) + throw std::invalid_argument("Requested non-existent variable '" + + DefaultKeyFormatter(variable) + + "' from VariableIndex"); + else + return item->second; +} + +bool empty(Key variable) const { + return (*this)[variable].empty(); +} + /* ************************************************************************* */ bool VariableIndex::equals(const VariableIndex& other, double tol) const { return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_ diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 2f872b25d..9885cb1cb 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -84,20 +84,12 @@ class GTSAM_EXPORT VariableIndex { size_t nEntries() const { return nEntries_; } /// Access a list of factors by variable - const FactorIndices& operator[](Key variable) const { - KeyMap::const_iterator item = index_.find(variable); - if(item == index_.end()) - throw std::invalid_argument("Requested non-existent variable '" + - DefaultKeyFormatter(variable) + - "' from VariableIndex"); - else - return item->second; - } + const FactorIndices& operator[](Key variable) const; + + const FactorIndices& at(Key variable) const; /// Return true if no factors associated with a variable - bool empty(Key variable) const { - return (*this)[variable].empty(); - } + bool empty(Key variable) const; /// @} /// @name Testable diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index 13c4c9853..084d97a01 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -199,6 +199,9 @@ class VariableIndex { VariableIndex(const T& factorGraph); VariableIndex(const gtsam::VariableIndex& other); + const FactorIndices& at(Key variable) const; + bool empty(Key variable) const; + // Testable bool equals(const gtsam::VariableIndex& other, double tol) const; void print(string s = "VariableIndex: ", From 97b6e879eacc4f2a4e72a6f9e247fd6037fb6453 Mon Sep 17 00:00:00 2001 From: p-zach Date: Tue, 15 Apr 2025 18:19:54 -0400 Subject: [PATCH 17/24] Wrap GaussianEliminationTree --- gtsam/linear/linear.i | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 90ee493f8..39947125a 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -687,6 +687,19 @@ virtual class GaussianBayesTree { gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; }; +#include +virtual class GaussianEliminationTree { + GaussianEliminationTree(const GaussianFactorGraph& factorGraph, + const VariableIndex& structure, const Ordering& order); + GaussianEliminationTree(const GaussianFactorGraph& factorGraph, + const Ordering& order); + + bool GaussianEliminationTree::equals(const This& other, double tol) const; + + void print(const std::string& name = "GaussianEliminationTree: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const; +} + #include class GaussianISAM { //Constructor From 3db7920f6559035b9c04cd8ef0aa998f4c6aa1e1 Mon Sep 17 00:00:00 2001 From: p-zach Date: Tue, 15 Apr 2025 23:53:43 -0400 Subject: [PATCH 18/24] Fix syntax errors --- gtsam/inference/VariableIndex.cpp | 10 +++++----- gtsam/inference/inference.i | 4 ++-- gtsam/linear/linear.i | 16 ++++++++-------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 3d22501b3..ba291c1f2 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -23,12 +23,12 @@ namespace gtsam { using namespace std; -const FactorIndices& operator[](Key variable) const { - KeyMap::const_iterator item = index_.find(variable); +const FactorIndices& VariableIndex::operator[](Key variable) const { + return index_.find(variable)->second; } -const FactorIndices& at(Key variable) const { - KeyMap::const_iterator item = (*this)[variable]; +const FactorIndices& VariableIndex::at(Key variable) const { + KeyMap::const_iterator item = index_.find(variable); if(item == index_.end()) throw std::invalid_argument("Requested non-existent variable '" + DefaultKeyFormatter(variable) + @@ -37,7 +37,7 @@ const FactorIndices& at(Key variable) const { return item->second; } -bool empty(Key variable) const { +bool VariableIndex::empty(Key variable) const { return (*this)[variable].empty(); } diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index 084d97a01..2b05adf89 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -199,8 +199,8 @@ class VariableIndex { VariableIndex(const T& factorGraph); VariableIndex(const gtsam::VariableIndex& other); - const FactorIndices& at(Key variable) const; - bool empty(Key variable) const; + gtsam::FactorIndices& at(gtsam::Key variable) const; + bool empty(gtsam::Key variable) const; // Testable bool equals(const gtsam::VariableIndex& other, double tol) const; diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 39947125a..3ad6be6c4 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -689,16 +689,16 @@ virtual class GaussianBayesTree { #include virtual class GaussianEliminationTree { - GaussianEliminationTree(const GaussianFactorGraph& factorGraph, - const VariableIndex& structure, const Ordering& order); - GaussianEliminationTree(const GaussianFactorGraph& factorGraph, - const Ordering& order); + GaussianEliminationTree(const gtsam::GaussianFactorGraph& factorGraph, + const gtsam::VariableIndex& structure, const gtsam::Ordering& order); + GaussianEliminationTree(const gtsam::GaussianFactorGraph& factorGraph, + const gtsam::Ordering& order); - bool GaussianEliminationTree::equals(const This& other, double tol) const; + bool equals(const This& other, double tol) const; - void print(const std::string& name = "GaussianEliminationTree: ", - const KeyFormatter& formatter = DefaultKeyFormatter) const; -} + void print(const string name = "GaussianEliminationTree: ", + const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const; +}; #include class GaussianISAM { From 4c4bdd4ba7630c964619bfabede55b25d89307cc Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 00:02:07 -0400 Subject: [PATCH 19/24] Actually the Colamd wrap is fine --- gtsam/inference/inference.i | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index 2b05adf89..0d7bd784e 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -122,11 +122,9 @@ class Ordering { template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> - - // Can't have both of these Colamd implementations; the second overrides the first. - // Python wrapper is for ease of use, so choose the one that is more commonly used. + static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); - // static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex); + static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, From 29f62f1d03b719f9c5a6a1b5efae98fc3e710778 Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 00:40:21 -0400 Subject: [PATCH 20/24] GaussianISAM interface --- gtsam/linear/linear.i | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 3ad6be6c4..4b6498424 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -704,11 +704,20 @@ virtual class GaussianEliminationTree { class GaussianISAM { //Constructor GaussianISAM(); + GaussianISAM(const gtsam::GaussianBayesTree& bayesTree); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + + gtsam::GaussianConditional* marginalFactor(size_t key) const; //Standard Interface void update(const gtsam::GaussianFactorGraph& newFactors); void saveGraph(string s) const; void clear(); + + void print(const string name = "GaussianISAM: ", + const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const; }; #include From 0cbd9cd4de53075819f56eaa77612b7ff6f1b61f Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 00:41:29 -0400 Subject: [PATCH 21/24] Notebook errors fixed after wrapper updates --- gtsam/inference/doc/BayesTree.ipynb | 18 +-- gtsam/inference/doc/EliminationTree.ipynb | 147 +++++++++++++++------- gtsam/inference/doc/ISAM.ipynb | 84 ++++++++----- gtsam/inference/doc/JunctionTree.ipynb | 6 +- gtsam/inference/doc/Ordering.ipynb | 12 +- gtsam/inference/doc/VariableIndex.ipynb | 32 ++--- 6 files changed, 185 insertions(+), 114 deletions(-) diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index e45b1f8fa..5364adc0a 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -57,7 +57,7 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 1, "metadata": { "id": "bayestree_import_code" }, @@ -88,7 +88,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 2, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -185,7 +185,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 5, "metadata": {}, "outputs": [ { @@ -219,7 +219,7 @@ ], "source": [ "# Eliminate multifrontally using COLAMD ordering\n", - "ordering = Ordering.Colamd(VariableIndex(graph))\n", + "ordering = Ordering.ColamdGaussianFactorGraph(graph)\n", "# Note: Multifrontal typically yields multiple roots if graph is disconnected\n", "bayes_tree = graph.eliminateMultifrontal(ordering)\n", "\n", @@ -229,7 +229,7 @@ }, { "cell_type": "code", - "execution_count": 11, + "execution_count": 6, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -263,7 +263,7 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 7, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -342,7 +342,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 8, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -387,10 +387,10 @@ "\n" ], "text/plain": [ - "" + "" ] }, - "execution_count": 9, + "execution_count": 8, "metadata": {}, "output_type": "execute_result" } diff --git a/gtsam/inference/doc/EliminationTree.ipynb b/gtsam/inference/doc/EliminationTree.ipynb index 11b3d2ec9..75378e7d7 100644 --- a/gtsam/inference/doc/EliminationTree.ipynb +++ b/gtsam/inference/doc/EliminationTree.ipynb @@ -49,23 +49,11 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 1, "metadata": { "id": "etree_import_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'GaussianEliminationTree' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[2], line 5\u001b[0m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# EliminationTree is templated, need concrete types\u001b[39;00m\n\u001b[1;32m----> 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m GaussianFactorGraph, Ordering, GaussianEliminationTree, GaussianBayesNet\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 8\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'GaussianEliminationTree' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", @@ -91,7 +79,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -104,25 +92,71 @@ "name": "stdout", "output_type": "stream", "text": [ - "Elimination Tree: \n", - "Root(s):\n", - "Node (x2)\n", - "JacobianFactor(keys = [8070450532247928833; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "JacobianFactor(keys = [7783684379976990721; 8070450532247928834], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Node (l2)\n", - " JacobianFactor(keys = [7783684379976990721; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Node (x1)\n", - " JacobianFactor(keys = [8070450532247928832; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " JacobianFactor(keys = [7783684379976990720; 8070450532247928833], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Node (l1)\n", - " JacobianFactor(keys = [7783684379976990720; 8070450532247928832], A[0] = [ -1 1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - " Children:\n", - " Node (x0)\n", - " JacobianFactor(keys = [8070450532247928832], Z = [ -1 ], b = [ 0 ], model = diagonal sigmas [1])\n", - "\n" + "Elimination Tree: -(x2)\n", + "Elimination Tree: | -(l2)\n", + "Elimination Tree: | -\n", + " A[l2] = [\n", + "\t-1\n", + "]\n", + " A[x2] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "Elimination Tree: | | -(x1)\n", + "Elimination Tree: | | -\n", + " A[x1] = [\n", + "\t-1\n", + "]\n", + " A[x2] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "Elimination Tree: | | -\n", + " A[l2] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "Elimination Tree: | | | -(l1)\n", + "Elimination Tree: | | | -\n", + " A[l1] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "Elimination Tree: | | | | -(x0)\n", + "Elimination Tree: | | | | -\n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "Elimination Tree: | | | | -\n", + " A[x0] = [\n", + "\t-1\n", + "]\n", + " A[x1] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n", + "Elimination Tree: | | | | -\n", + " A[l1] = [\n", + "\t-1\n", + "]\n", + " A[x0] = [\n", + "\t1\n", + "]\n", + " b = [ 0 ]\n", + " Noise model: unit (1) \n" ] } ], @@ -160,7 +194,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 3, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -173,13 +207,42 @@ "name": "stdout", "output_type": "stream", "text": [ - "BayesNet from EliminationTree: size 5\n", - "Conditional 0: GaussianConditional( P(x0 | l1) = dx0 - R*dl1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "Conditional 1: GaussianConditional( P(l1 | x1) = dl1 - R*dx1 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "Conditional 2: GaussianConditional( P(x1 | l2, x2) = dx1 - R1*dl2 - R2*dx2 - d), R1 = [ 0.333333 ], R2 = [ 0.333333 ], d = [ 0 ], sigmas = [ 0.745356 ])\n", - "Conditional 3: GaussianConditional( P(l2 | x2) = dl2 - R*dx2 - d), R = [ 0.5 ], d = [ 0 ], sigmas = [ 0.866025 ])\n", - "Conditional 4: GaussianConditional( P(x2) = dx2 - d), d = [ 0 ], sigmas = [ 0.774597 ])\n", - "\n" + "BayesNet from EliminationTree:\n", + "\n", + "size: 5\n", + "conditional 0: p(x0 | l1 x1)\n", + " R = [ 1.73205 ]\n", + " S[l1] = [ -0.57735 ]\n", + " S[x1] = [ -0.57735 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.369632\n", + " No noise model\n", + "conditional 1: p(l1 | x1)\n", + " R = [ 1.29099 ]\n", + " S[x1] = [ -1.0328 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.663526\n", + " No noise model\n", + "conditional 2: p(x1 | l2 x2)\n", + " R = [ 1.61245 ]\n", + " S[l2] = [ -0.620174 ]\n", + " S[x2] = [ -0.620174 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.441183\n", + " No noise model\n", + "conditional 3: p(l2 | x2)\n", + " R = [ 1.27098 ]\n", + " S[x2] = [ -1.08941 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.679152\n", + " No noise model\n", + "conditional 4: p(x2)\n", + " R = [ 0.654654 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x2: 0\n", + " logNormalizationConstant: -1.34259\n", + " No noise model\n" ] } ], diff --git a/gtsam/inference/doc/ISAM.ipynb b/gtsam/inference/doc/ISAM.ipynb index 7e6a43269..f4daa82a8 100644 --- a/gtsam/inference/doc/ISAM.ipynb +++ b/gtsam/inference/doc/ISAM.ipynb @@ -47,7 +47,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 1, "metadata": { "id": "isam_import_code" }, @@ -77,7 +77,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 2, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -98,19 +98,17 @@ " mean: 1 elements\n", " x0: 0\n", " logNormalizationConstant: -0.918939\n", + " No noise model\n", + "ISAM from BayesTree:\n", + "GaussianISAM: : cliques: 1, variables: 1\n", + "GaussianISAM: - p(x0)\n", + " R = [ 1 ]\n", + " d = [ 0 ]\n", + " mean: 1 elements\n", + " x0: 0\n", + " logNormalizationConstant: -0.918939\n", " No noise model\n" ] - }, - { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.GaussianISAM()\n\nInvoked with: : cliques: 1, variables: 1\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", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[5], line 13\u001b[0m\n\u001b[0;32m 10\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mInitial BayesTree:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 11\u001b[0m initial_bayes_tree\u001b[38;5;241m.\u001b[39mprint()\n\u001b[1;32m---> 13\u001b[0m isam2 \u001b[38;5;241m=\u001b[39m \u001b[43mGaussianISAM\u001b[49m\u001b[43m(\u001b[49m\u001b[43minitial_bayes_tree\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mISAM from BayesTree:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 15\u001b[0m isam2\u001b[38;5;241m.\u001b[39mprint()\n", - "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.GaussianISAM()\n\nInvoked with: : cliques: 1, variables: 1\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" - ] } ], "source": [ @@ -144,7 +142,7 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 4, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -154,14 +152,38 @@ }, "outputs": [ { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.GaussianISAM()\n\nInvoked with: : cliques: 1, variables: 1\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", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[6], line 2\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;66;03m# Start with the ISAM object containing the prior on x0\u001b[39;00m\n\u001b[1;32m----> 2\u001b[0m isam \u001b[38;5;241m=\u001b[39m \u001b[43mGaussianISAM\u001b[49m\u001b[43m(\u001b[49m\u001b[43minitial_bayes_tree\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 3\u001b[0m model \u001b[38;5;241m=\u001b[39m gtsam\u001b[38;5;241m.\u001b[39mnoiseModel\u001b[38;5;241m.\u001b[39mIsotropic\u001b[38;5;241m.\u001b[39mSigma(\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m1.0\u001b[39m)\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# --- First Update ---\u001b[39;00m\n", - "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.GaussianISAM()\n\nInvoked with: : cliques: 1, variables: 1\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" + "name": "stdout", + "output_type": "stream", + "text": [ + "ISAM after first update (x0, x1):\n", + "GaussianISAM: : cliques: 1, variables: 2\n", + "GaussianISAM: - p(x1 x0 )\n", + " R = [ 1 -1 ]\n", + " [ 0 1 ]\n", + " d = [ 0 0 ]\n", + " mean: 2 elements\n", + " x0: 0\n", + " x1: 0\n", + " logNormalizationConstant: -1.83788\n", + " No noise model\n", + "\n", + "ISAM after second update (x0, x1, x2):\n", + "GaussianISAM: : cliques: 2, variables: 3\n", + "GaussianISAM: - p(x0 x1 )\n", + " R = [ 1.41421 -0.707107 ]\n", + " [ 0 0.707107 ]\n", + " d = [ 0 0 ]\n", + " mean: 2 elements\n", + " x0: 0\n", + " x1: 0\n", + " logNormalizationConstant: -1.83788\n", + " No noise model\n", + "GaussianISAM: | - p(x2 | x1)\n", + " R = [ 1 ]\n", + " S[x1] = [ -1 ]\n", + " d = [ 0 ]\n", + " logNormalizationConstant: -0.918939\n", + " No noise model\n" ] } ], @@ -200,7 +222,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 5, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -210,14 +232,14 @@ }, "outputs": [ { - "ename": "NameError", - "evalue": "name 'isam' is not defined", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mNameError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[7], line 2\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;66;03m# Get the solution from the final ISAM state\u001b[39;00m\n\u001b[1;32m----> 2\u001b[0m solution \u001b[38;5;241m=\u001b[39m \u001b[43misam\u001b[49m\u001b[38;5;241m.\u001b[39moptimize()\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOptimized Solution after updates:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m solution\u001b[38;5;241m.\u001b[39mprint()\n", - "\u001b[1;31mNameError\u001b[0m: name 'isam' is not defined" + "name": "stdout", + "output_type": "stream", + "text": [ + "Optimized Solution after updates:\n", + "VectorValues: 3 elements\n", + " x0: 0\n", + " x1: 0\n", + " x2: 0\n" ] } ], diff --git a/gtsam/inference/doc/JunctionTree.ipynb b/gtsam/inference/doc/JunctionTree.ipynb index 35b326dda..b5835f9df 100644 --- a/gtsam/inference/doc/JunctionTree.ipynb +++ b/gtsam/inference/doc/JunctionTree.ipynb @@ -49,7 +49,7 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 1, "metadata": { "id": "jtree_import_code" }, @@ -79,7 +79,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 2, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -128,7 +128,7 @@ "graph.add(L(2), -np.eye(1), X(1), np.eye(1), np.zeros(1), model)\n", "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model)\n", "\n", - "ordering = Ordering.Colamd(VariableIndex(graph))\n", + "ordering = Ordering.ColamdGaussianFactorGraph(graph)\n", "\n", "# Perform multifrontal elimination, which uses a JunctionTree internally\n", "bayes_tree, remaining_graph = graph.eliminatePartialMultifrontal(ordering)\n", diff --git a/gtsam/inference/doc/Ordering.ipynb b/gtsam/inference/doc/Ordering.ipynb index 7e3bab97e..80dbc58f9 100644 --- a/gtsam/inference/doc/Ordering.ipynb +++ b/gtsam/inference/doc/Ordering.ipynb @@ -45,7 +45,7 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 15, "metadata": { "id": "ordering_import_code" }, @@ -74,7 +74,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 16, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -99,7 +99,7 @@ }, { "cell_type": "code", - "execution_count": 12, + "execution_count": 18, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -130,7 +130,7 @@ "graph.push_factor(X(2), L(2))\n", "\n", "# COLAMD (Column Approximate Minimum Degree) ordering\n", - "colamd_ordering = Ordering.Colamd(VariableIndex(graph))\n", + "colamd_ordering = Ordering.ColamdSymbolicFactorGraph(graph)\n", "colamd_ordering.print(\"COLAMD Ordering: \")\n", "\n", "# Constrained COLAMD (force x0 and x2 to be eliminated last)\n", @@ -151,7 +151,7 @@ }, { "cell_type": "code", - "execution_count": 14, + "execution_count": 19, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -190,7 +190,7 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 20, "metadata": { "colab": { "base_uri": "https://localhost:8080/" diff --git a/gtsam/inference/doc/VariableIndex.ipynb b/gtsam/inference/doc/VariableIndex.ipynb index 0a3af8c3f..080e03fa1 100644 --- a/gtsam/inference/doc/VariableIndex.ipynb +++ b/gtsam/inference/doc/VariableIndex.ipynb @@ -40,12 +40,12 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install gtsam-develop" ] }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 1, "metadata": { "id": "vindex_import_code" }, @@ -127,7 +127,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 5, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -142,18 +142,9 @@ "text": [ "Number of variables (size): 5\n", "Number of factors (nFactors): 7\n", - "Number of variable-factor entries (nEntries): 13\n" - ] - }, - { - "ename": "TypeError", - "evalue": "'gtsam.gtsam.VariableIndex' object is not subscriptable", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[3], line 6\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mNumber of variable-factor entries (nEntries): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mvariable_index\u001b[38;5;241m.\u001b[39mnEntries()\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# Get factors involving a specific variable\u001b[39;00m\n\u001b[1;32m----> 6\u001b[0m factors_x1 \u001b[38;5;241m=\u001b[39m \u001b[43mvariable_index\u001b[49m\u001b[43m[\u001b[49m\u001b[43mX\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m1\u001b[39;49m\u001b[43m)\u001b[49m\u001b[43m]\u001b[49m \u001b[38;5;66;03m# Returns a FactorIndices (FastVector)\u001b[39;00m\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mFactors involving x1: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mfactors_x1\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 9\u001b[0m \u001b[38;5;66;03m# Use key directly\u001b[39;00m\n", - "\u001b[1;31mTypeError\u001b[0m: 'gtsam.gtsam.VariableIndex' object is not subscriptable" + "Number of variable-factor entries (nEntries): 13\n", + "Factors involving x1: [1, 2, 4, 5]\n", + "Factors involving l1: [3, 4]\n" ] } ], @@ -163,17 +154,12 @@ "print(f\"Number of variable-factor entries (nEntries): {variable_index.nEntries()}\")\n", "\n", "# Get factors involving a specific variable\n", - "factors_x1 = variable_index[X(1)] # Returns a FactorIndices (FastVector)\n", + "factors_x1 = variable_index.at(X(1)) # Returns a FactorIndices (FastVector)\n", "print(f\"Factors involving x1: {factors_x1}\")\n", "\n", "# Use key directly\n", - "factors_l1 = variable_index[L(1)]\n", - "print(f\"Factors involving l1: {factors_l1}\")\n", - "\n", - "# Iterate through the index (items are pairs of Key, FactorIndices)\n", - "print(\"Iterating through VariableIndex:\")\n", - "for key, factor_indices in variable_index.items(): # Use .items() like a dict\n", - " print(f\" Variable {gtsam.DefaultKeyFormatter(key)} involves factors: {factor_indices}\")" + "factors_l1 = variable_index.at(L(1))\n", + "print(f\"Factors involving l1: {factors_l1}\")" ] }, { From bcfd0ec3a9e2c7b8d7322942e82fb55caf1f069d Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 16:28:17 -0400 Subject: [PATCH 22/24] Documentation --- gtsam/inference/VariableIndex.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 9885cb1cb..ccc5a11c1 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -83,9 +83,9 @@ class GTSAM_EXPORT VariableIndex { /// The number of nonzero blocks, i.e. the number of variable-factor entries size_t nEntries() const { return nEntries_; } - /// Access a list of factors by variable + /// Access a list of factors by variable without checking for existence const FactorIndices& operator[](Key variable) const; - + /// Access a list of factors by variable const FactorIndices& at(Key variable) const; /// Return true if no factors associated with a variable From 60d00621ff08fb6961c9abba58d2633256c4da59 Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 16:28:58 -0400 Subject: [PATCH 23/24] Better visualizations --- gtsam/inference/doc/BayesTree.ipynb | 350 +++++++++++++++++----------- 1 file changed, 208 insertions(+), 142 deletions(-) diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index 5364adc0a..a9452f4d4 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -57,7 +57,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 9, "metadata": { "id": "bayestree_import_code" }, @@ -88,7 +88,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 10, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -101,74 +101,165 @@ "name": "stdout", "output_type": "stream", "text": [ - "Original Factor Graph:\n", - "\n", - "size: 7\n", - "factor 0: \n", - " A[x0] = [\n", - "\t-1\n", - "]\n", - " b = [ 0 ]\n", - " Noise model: unit (1) \n", - "factor 1: \n", - " A[x0] = [\n", - "\t-1\n", - "]\n", - " A[x1] = [\n", - "\t1\n", - "]\n", - " b = [ 0 ]\n", - " Noise model: unit (1) \n", - "factor 2: \n", - " A[x1] = [\n", - "\t-1\n", - "]\n", - " A[x2] = [\n", - "\t1\n", - "]\n", - " b = [ 0 ]\n", - " Noise model: unit (1) \n", - "factor 3: \n", - " A[l1] = [\n", - "\t-1\n", - "]\n", - " A[x0] = [\n", - "\t1\n", - "]\n", - " b = [ 0 ]\n", - " Noise model: unit (1) \n", - "factor 4: \n", - " A[l1] = [\n", - "\t-1\n", - "]\n", - " A[x1] = [\n", - "\t1\n", - "]\n", - " b = [ 0 ]\n", - " Noise model: unit (1) \n", - "factor 5: \n", - " A[l2] = [\n", - "\t-1\n", - "]\n", - " A[x1] = [\n", - "\t1\n", - "]\n", - " b = [ 0 ]\n", - " Noise model: unit (1) \n", - "factor 6: \n", - " A[l2] = [\n", - "\t-1\n", - "]\n", - " A[x2] = [\n", - "\t1\n", - "]\n", - " b = [ 0 ]\n", - " Noise model: unit (1) \n" + "Original Factor Graph:\n" ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089\n", + "\n", + "l1\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor3\n", + "\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090\n", + "\n", + "l2\n", + "\n", + "\n", + "\n", + "factor5\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090--factor5\n", + "\n", + "\n", + "\n", + "\n", + "factor6\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090--factor6\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", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor5\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor6\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" } ], "source": [ - "# Create a simple Gaussian Factor Graph (more complex this time)\n", + "# Create a simple Gaussian Factor Graph\n", "graph = GaussianFactorGraph()\n", "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n", "graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n", @@ -180,19 +271,18 @@ "graph.add(L(2), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # l2 -> x2 (measurement)\n", "\n", "print(\"Original Factor Graph:\")\n", - "graph.print()" + "display(graphviz.Source(graph.dot()))" ] }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 12, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "\n", "Resulting BayesTree:\n", ": cliques: 2, variables: 5\n", "- p(x1 l2 x2 )\n", @@ -213,8 +303,52 @@ " [ -0.948683 ]\n", " d = [ 0 0 ]\n", " logNormalizationConstant: -1.03316\n", - " No noise model\n" + " No noise model\n", + "\n", + "Visualization:\n" ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "2\n", + "\n", + "x1, l2, x2\n", + "\n", + "\n", + "\n", + "3\n", + "\n", + "l1, x0 : x1\n", + "\n", + "\n", + "\n", + "2->3\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" } ], "source": [ @@ -223,8 +357,10 @@ "# Note: Multifrontal typically yields multiple roots if graph is disconnected\n", "bayes_tree = graph.eliminateMultifrontal(ordering)\n", "\n", - "print(\"\\nResulting BayesTree:\")\n", - "bayes_tree.print()" + "print(\"Resulting BayesTree:\")\n", + "bayes_tree.print()\n", + "print(\"\\nVisualization:\")\n", + "display(graphviz.Source(bayes_tree.dot()))" ] }, { @@ -328,76 +464,6 @@ "print(\"\\nJoint Marginal Factor Graph on (x0, x2):\")\n", "joint_x0_x2.print()" ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "bayestree_viz_md" - }, - "source": [ - "## Visualization\n", - "\n", - "Bayes trees can be visualized using Graphviz." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "bayestree_dot_code", - "outputId": "789abcde-f012-3456-789a-bcdef0123456" - }, - "outputs": [ - { - "data": { - "image/svg+xml": [ - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "G\n", - "\n", - "\n", - "\n", - "0\n", - "\n", - "x1, l2, x2\n", - "\n", - "\n", - "\n", - "1\n", - "\n", - "l1, x0 : x1\n", - "\n", - "\n", - "\n", - "0->1\n", - "\n", - "\n", - "\n", - "\n", - "\n" - ], - "text/plain": [ - "" - ] - }, - "execution_count": 8, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "graphviz.Source(bayes_tree.dot())" - ] } ], "metadata": { From 6ea62e1f043215b9d8fa2d415406ec2b1e8fbf42 Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 16:29:11 -0400 Subject: [PATCH 24/24] Asia example --- gtsam/inference/doc/BayesNet.ipynb | 378 ++++++++++++++++++++++++++++- 1 file changed, 369 insertions(+), 9 deletions(-) diff --git a/gtsam/inference/doc/BayesNet.ipynb b/gtsam/inference/doc/BayesNet.ipynb index db64eda3d..c379b32ce 100644 --- a/gtsam/inference/doc/BayesNet.ipynb +++ b/gtsam/inference/doc/BayesNet.ipynb @@ -15,7 +15,7 @@ "id": "bayesnet_desc_md" }, "source": [ - "A `BayesNet` in GTSAM represents a directed graphical model, specifically the result of running sequential variable elimination (like Cholesky or QR factorization) on a `FactorGraph`.\n", + "A `BayesNet` in GTSAM represents a directed graphical model, created by running sequential variable elimination (like Cholesky or QR factorization) on a `FactorGraph` or constructing from scratch.\n", "\n", "It is essentially a collection of `Conditional` objects, ordered according to the elimination order. Each conditional represents $P(\\text{variable} | \\text{parents})$, where the parents are variables that appear later in the elimination ordering.\n", "\n", @@ -57,7 +57,7 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 44, "metadata": { "id": "bayesnet_import_code" }, @@ -69,6 +69,8 @@ "\n", "# We need concrete graph types and elimination to get a BayesNet\n", "from gtsam import GaussianFactorGraph, Ordering, GaussianBayesNet\n", + "# For the Asia example\n", + "from gtsam import DiscreteBayesNet, DiscreteConditional, DiscreteKeys, DiscreteValues, symbol\n", "from gtsam import symbol_shorthand\n", "\n", "X = symbol_shorthand.X\n", @@ -88,7 +90,7 @@ }, { "cell_type": "code", - "execution_count": 14, + "execution_count": 45, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -144,7 +146,7 @@ }, { "cell_type": "code", - "execution_count": 15, + "execution_count": 46, "metadata": {}, "outputs": [ { @@ -199,7 +201,7 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 47, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -250,7 +252,7 @@ }, { "cell_type": "code", - "execution_count": 11, + "execution_count": 48, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -297,7 +299,7 @@ }, { "cell_type": "code", - "execution_count": 12, + "execution_count": 49, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -353,10 +355,10 @@ "\n" ], "text/plain": [ - "" + "" ] }, - "execution_count": 12, + "execution_count": 49, "metadata": {}, "output_type": "execute_result" } @@ -364,6 +366,364 @@ "source": [ "graphviz.Source(bayes_net.dot())" ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bayesnet_discrete_md" + }, + "source": [ + "## Example: DiscreteBayesNet (Asia Network)\n", + "\n", + "While the previous examples focused on `GaussianBayesNet`, GTSAM also supports `DiscreteBayesNet` for representing probability distributions over discrete variables. Here we construct the classic 'Asia' network example directly by adding `DiscreteConditional` objects." + ] + }, + { + "cell_type": "code", + "execution_count": 50, + "metadata": { + "id": "bayesnet_discrete_imports_code" + }, + "outputs": [], + "source": [ + "# Define keys for the Asia network variables\n", + "A = symbol('A', 8) # Visit to Asia?\n", + "S = symbol('S', 7) # Smoker?\n", + "T = symbol('T', 6) # Tuberculosis?\n", + "L = symbol('L', 5) # Lung Cancer?\n", + "B = symbol('B', 4) # Bronchitis?\n", + "E = symbol('E', 3) # Tuberculosis or Lung Cancer?\n", + "X = symbol('X', 2) # Positive X-Ray?\n", + "D = symbol('D', 1) # Dyspnea (Shortness of breath)?\n", + "\n", + "# Define cardinalities (all are binary in this case)\n", + "cardinalities = { A: 2, S: 2, T: 2, L: 2, B: 2, E: 2, X: 2, D: 2 }\n", + "\n", + "# Helper to create DiscreteKeys object\n", + "def make_keys(keys_list):\n", + " dk = DiscreteKeys()\n", + " for k in keys_list:\n", + " dk.push_back((k, cardinalities[k]))\n", + " return dk" + ] + }, + { + "cell_type": "code", + "execution_count": 51, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayesnet_discrete_build_code", + "outputId": "456789ab-cdef-0123-4567-89abcdef0123" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Asia Bayes Net:\n", + "DiscreteBayesNet\n", + " \n", + "size: 8\n", + "conditional 0: P( D1 | E3 B4 ):\n", + " Choice(E3) \n", + " 0 Choice(D1) \n", + " 0 0 Choice(B4) \n", + " 0 0 0 Leaf 0.9\n", + " 0 0 1 Leaf 0.2\n", + " 0 1 Choice(B4) \n", + " 0 1 0 Leaf 0.1\n", + " 0 1 1 Leaf 0.8\n", + " 1 Choice(D1) \n", + " 1 0 Choice(B4) \n", + " 1 0 0 Leaf 0.3\n", + " 1 0 1 Leaf 0.1\n", + " 1 1 Choice(B4) \n", + " 1 1 0 Leaf 0.7\n", + " 1 1 1 Leaf 0.9\n", + "\n", + "conditional 1: P( X2 | E3 ):\n", + " Choice(X2) \n", + " 0 Choice(E3) \n", + " 0 0 Leaf 0.95\n", + " 0 1 Leaf 0.02\n", + " 1 Choice(E3) \n", + " 1 0 Leaf 0.05\n", + " 1 1 Leaf 0.98\n", + "\n", + "conditional 2: P( E3 | T6 L5 ):\n", + " Choice(T6) \n", + " 0 Choice(L5) \n", + " 0 0 Choice(E3) \n", + " 0 0 0 Leaf 1\n", + " 0 0 1 Leaf 0\n", + " 0 1 Choice(E3) \n", + " 0 1 0 Leaf 0\n", + " 0 1 1 Leaf 1\n", + " 1 Choice(L5) \n", + " 1 0 Choice(E3) \n", + " 1 0 0 Leaf 0\n", + " 1 0 1 Leaf 1\n", + " 1 1 Choice(E3) \n", + " 1 1 0 Leaf 0\n", + " 1 1 1 Leaf 1\n", + "\n", + "conditional 3: P( B4 | S7 ):\n", + " Choice(S7) \n", + " 0 Choice(B4) \n", + " 0 0 Leaf 0.7\n", + " 0 1 Leaf 0.3\n", + " 1 Choice(B4) \n", + " 1 0 Leaf 0.4\n", + " 1 1 Leaf 0.6\n", + "\n", + "conditional 4: P( L5 | S7 ):\n", + " Choice(S7) \n", + " 0 Choice(L5) \n", + " 0 0 Leaf 0.99\n", + " 0 1 Leaf 0.01\n", + " 1 Choice(L5) \n", + " 1 0 Leaf 0.9\n", + " 1 1 Leaf 0.1\n", + "\n", + "conditional 5: P( T6 | A8 ):\n", + " Choice(T6) \n", + " 0 Choice(A8) \n", + " 0 0 Leaf 0.99\n", + " 0 1 Leaf 0.95\n", + " 1 Choice(A8) \n", + " 1 0 Leaf 0.01\n", + " 1 1 Leaf 0.05\n", + "\n", + "conditional 6: P( S7 ):\n", + " Leaf 0.5\n", + "\n", + "conditional 7: P( A8 ):\n", + " Choice(A8) \n", + " 0 Leaf 0.99\n", + " 1 Leaf 0.01\n", + "\n" + ] + } + ], + "source": [ + "# Create the DiscreteBayesNet\n", + "asia_net = DiscreteBayesNet()\n", + "\n", + "# Helper function to create parent list in correct format\n", + "def make_parent_tuples(parent_keys):\n", + " return [(pk, cardinalities[pk]) for pk in parent_keys]\n", + "\n", + "# P(D | E, B) - Dyspnea given Either and Bronchitis\n", + "asia_net.add(DiscreteConditional((D, cardinalities[D]), make_parent_tuples([E, B]), \"9/1 2/8 3/7 1/9\"))\n", + "\n", + "# P(X | E) - X-Ray result given Either\n", + "asia_net.add(DiscreteConditional((X, cardinalities[X]), make_parent_tuples([E]), \"95/5 2/98\"))\n", + "\n", + "# P(E | T, L) - Either Tub. or Lung Cancer (OR gate)\n", + "# \"F T T T\" means P(E=1|T=0,L=0)=0, P(E=1|T=0,L=1)=1, P(E=1|T=1,L=0)=1, P(E=1|T=1,L=1)=1\n", + "asia_net.add(DiscreteConditional((E, cardinalities[E]), make_parent_tuples([T, L]), \"F T T T\"))\n", + "\n", + "# P(B | S) - Bronchitis given Smoker\n", + "asia_net.add(DiscreteConditional((B, cardinalities[B]), make_parent_tuples([S]), \"70/30 40/60\"))\n", + "\n", + "# P(L | S) - Lung Cancer given Smoker\n", + "asia_net.add(DiscreteConditional((L, cardinalities[L]), make_parent_tuples([S]), \"99/1 90/10\"))\n", + "\n", + "# P(T | A) - Tuberculosis given Asia\n", + "asia_net.add(DiscreteConditional((T, cardinalities[T]), make_parent_tuples([A]), \"99/1 95/5\"))\n", + "\n", + "# P(S) - Prior on Smoking\n", + "asia_net.add(DiscreteConditional((S, cardinalities[S]), [], \"1/1\")) # or \"50/50\"\n", + "\n", + "# Add conditional probability tables (CPTs) using C++ sugar syntax\n", + "# P(A) - Prior on Asia\n", + "asia_net.add(DiscreteConditional((A, cardinalities[A]), [], \"99/1\"))\n", + "\n", + "print(\"Asia Bayes Net:\")\n", + "asia_net.print()" + ] + }, + { + "cell_type": "code", + "execution_count": 58, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bayesnet_discrete_viz_eval_code", + "outputId": "56789abc-def0-1234-5678-9abcdef01234" + }, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var4683743612465315848\n", + "\n", + "A8\n", + "\n", + "\n", + "\n", + "var6052837899185946630\n", + "\n", + "T6\n", + "\n", + "\n", + "\n", + "var4683743612465315848->var6052837899185946630\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var4755801206503243780\n", + "\n", + "B4\n", + "\n", + "\n", + "\n", + "var4899916394579099649\n", + "\n", + "D1\n", + "\n", + "\n", + "\n", + "var4755801206503243780->var4899916394579099649\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var4971973988617027587\n", + "\n", + "E3\n", + "\n", + "\n", + "\n", + "var4971973988617027587->var4899916394579099649\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var6341068275337658370\n", + "\n", + "X2\n", + "\n", + "\n", + "\n", + "var4971973988617027587->var6341068275337658370\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var5476377146882523141\n", + "\n", + "L5\n", + "\n", + "\n", + "\n", + "var5476377146882523141->var4971973988617027587\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var5980780305148018695\n", + "\n", + "S7\n", + "\n", + "\n", + "\n", + "var5980780305148018695->var4755801206503243780\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var5980780305148018695->var5476377146882523141\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var6052837899185946630->var4971973988617027587\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Log Probability of all zeros: -1.2366269421045588\n", + "Sampled Values (basic print):\n", + "DiscreteValues{4683743612465315848: 0, 4755801206503243780: 1, 4899916394579099649: 1, 4971973988617027587: 0, 5476377146882523141: 0, 5980780305148018695: 1, 6052837899185946630: 0, 6341068275337658370: 0}\n", + "Sampled Values (pretty print):\n", + " A8: 0\n", + " B4: 1\n", + " D1: 1\n", + " E3: 0\n", + " L5: 0\n", + " S7: 1\n", + " T6: 0\n", + " X2: 0\n" + ] + } + ], + "source": [ + "# Visualize the network structure\n", + "dot_string = asia_net.dot()\n", + "display(graphviz.Source(dot_string))\n", + "\n", + "# Evaluate the log probability of a specific assignment\n", + "# Example: Calculate P(A=0, S=0, T=0, L=0, B=0, E=0, X=0, D=0)\n", + "values = DiscreteValues()\n", + "for key, card in cardinalities.items():\n", + " values[key] = 0 # Assign 0 to all variables to start\n", + "\n", + "log_prob_zeros = asia_net.logProbability(values)\n", + "print(f\"Log Probability of all zeros: {log_prob_zeros}\")\n", + "\n", + "# Sample from the Bayes Net\n", + "sample = asia_net.sample()\n", + "print(\"Sampled Values (basic print):\")\n", + "print(sample)\n", + "\n", + "# --- Pretty Print ---\n", + "print(\"Sampled Values (pretty print):\")\n", + "# Create a reverse mapping from integer key to string like 'A8'\n", + "# We defined A=symbol('A',8), S=symbol('S',7), etc. above\n", + "symbol_map = { A: 'A8', S: 'S7', T: 'T6', L: 'L5', B: 'B4', E: 'E3', X: 'X2', D: 'D1' }\n", + "# Iterate through the sampled values and print nicely\n", + "# Sort items by the symbol string for consistent order (optional)\n", + "for key, value in sorted(sample.items(), key=lambda item: symbol_map.get(item[0], str(item[0]))):\n", + " symbol_str = symbol_map.get(key, f\"UnknownKey({key})\") # Get 'A8' from key A\n", + " print(f\" {symbol_str}: {value}\")" + ] } ], "metadata": {