BayesTree cleanup
							parent
							
								
									241d6dc07a
								
							
						
					
					
						commit
						b54ad4e3f0
					
				|  | @ -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", | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue