12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

BayesTree cleanup

release/4.3a0
p-zach 2025-04-14 19:07:42 -04:00
parent 241d6dc07a
commit b54ad4e3f0
1 changed files with 29 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam/inference/doc/BayesTree.ipynb
View File

@ -22,11 +22,6 @@
"$$\n", "$$\n",
"P(X) = \\prod_k P(F_k | S_k)\n", "P(X) = \\prod_k P(F_k | S_k)\n",
"$$\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", "\n",
"Key properties:\n", "Key properties:\n",
"* **Cliques:** Each node (clique) groups variables that are eliminated together.\n", "* **Cliques:** Each node (clique) groups variables that are eliminated together.\n",
@ -167,7 +162,35 @@
"\t1\n", "\t1\n",
"]\n", "]\n",
" b = [ 0 ]\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", "\n",
"Resulting BayesTree:\n", "Resulting BayesTree:\n",
": cliques: 2, variables: 5\n", ": cliques: 2, variables: 5\n",
@ -194,20 +217,6 @@
} }
], ],
"source": [ "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", "# Eliminate multifrontally using COLAMD ordering\n",
"ordering = Ordering.Colamd(VariableIndex(graph))\n", "ordering = Ordering.Colamd(VariableIndex(graph))\n",
"# Note: Multifrontal typically yields multiple roots if graph is disconnected\n", "# Note: Multifrontal typically yields multiple roots if graph is disconnected\n",