From 9fe58e7502b8d7ed52704acfd30df2b181bd58e6 Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Tue, 15 Apr 2025 17:49:25 -0400 Subject: [PATCH 01/17] Trigger doxygen generation in cibuildwheel --- .github/scripts/python_wheels/cibw_before_all.sh | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/.github/scripts/python_wheels/cibw_before_all.sh b/.github/scripts/python_wheels/cibw_before_all.sh index 2398877a8..1906ede89 100644 --- a/.github/scripts/python_wheels/cibw_before_all.sh +++ b/.github/scripts/python_wheels/cibw_before_all.sh @@ -14,7 +14,7 @@ export PYTHON="python${PYTHON_VERSION}" if [ "$(uname)" == "Linux" ]; then # manylinux2014 is based on CentOS 7, so use yum to install dependencies - yum install -y wget + yum install -y wget doxygen # Install Boost from source wget https://archives.boost.io/release/1.87.0/source/boost_1_87_0.tar.gz --quiet @@ -24,7 +24,7 @@ if [ "$(uname)" == "Linux" ]; then ./b2 install --prefix=/opt/boost --with=all cd .. elif [ "$(uname)" == "Darwin" ]; then - brew install wget cmake boost + brew install wget cmake boost doxygen fi $(which $PYTHON) -m pip install -r $PROJECT_DIR/python/dev_requirements.txt @@ -48,11 +48,14 @@ cmake $PROJECT_DIR \ -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF \ - -DCMAKE_INSTALL_PREFIX=$PROJECT_DIR/gtsam_install + -DCMAKE_INSTALL_PREFIX=$PROJECT_DIR/gtsam_install \ + -DGTSAM_GENERATE_DOC_XML=1 -cd $PROJECT_DIR/build/python +# Generate Doxygen XML documentation +doxygen build/doc/Doxyfile # Install the Python wrapper module and generate Python stubs +cd $PROJECT_DIR/build/python if [ "$(uname)" == "Linux" ]; then make -j $(nproc) install make -j $(nproc) python-stubs From 2d043579528d3abab7d6308d5d6f37ebcff0c5a7 Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Wed, 16 Apr 2025 12:06:23 -0400 Subject: [PATCH 02/17] Set GTWRAP_ADD_DOCSTRINGS to ON --- .github/scripts/python_wheels/cibw_before_all.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/scripts/python_wheels/cibw_before_all.sh b/.github/scripts/python_wheels/cibw_before_all.sh index 1906ede89..eab6481c6 100644 --- a/.github/scripts/python_wheels/cibw_before_all.sh +++ b/.github/scripts/python_wheels/cibw_before_all.sh @@ -49,7 +49,8 @@ cmake $PROJECT_DIR \ -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF \ -DCMAKE_INSTALL_PREFIX=$PROJECT_DIR/gtsam_install \ - -DGTSAM_GENERATE_DOC_XML=1 + -DGTSAM_GENERATE_DOC_XML=1 \ + -DGTWRAP_ADD_DOCSTRINGS=ON # Generate Doxygen XML documentation doxygen build/doc/Doxyfile From 0db2a0d16bea88680573ca748daaa1e6ef2cbead Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Wed, 16 Apr 2025 12:35:10 -0400 Subject: [PATCH 03/17] Supress boost output --- .github/scripts/python_wheels/cibw_before_all.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/scripts/python_wheels/cibw_before_all.sh b/.github/scripts/python_wheels/cibw_before_all.sh index eab6481c6..d0cfc996d 100644 --- a/.github/scripts/python_wheels/cibw_before_all.sh +++ b/.github/scripts/python_wheels/cibw_before_all.sh @@ -21,7 +21,7 @@ if [ "$(uname)" == "Linux" ]; then tar -xzf boost_1_87_0.tar.gz cd boost_1_87_0 ./bootstrap.sh --prefix=/opt/boost - ./b2 install --prefix=/opt/boost --with=all + ./b2 install --prefix=/opt/boost --with=all -d0 cd .. elif [ "$(uname)" == "Darwin" ]; then brew install wget cmake boost doxygen From 7f62e8c65e8df0b1bcd9287161f49866fe7d2bda Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Wed, 16 Apr 2025 13:48:24 -0400 Subject: [PATCH 04/17] Fix errors in xml_parser and rerun --- wrap/gtwrap/xml_parser/xml_parser.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/wrap/gtwrap/xml_parser/xml_parser.py b/wrap/gtwrap/xml_parser/xml_parser.py index 9920027ac..e133b4882 100644 --- a/wrap/gtwrap/xml_parser/xml_parser.py +++ b/wrap/gtwrap/xml_parser/xml_parser.py @@ -56,9 +56,14 @@ class XMLDocParser: documenting_index = self.determine_documenting_index( cpp_class, cpp_method, method_args_names, member_defs) + if member_defs: + self.print_if_verbose( + f"Extracted {len(member_defs)} member definitions for {cpp_class}.{cpp_method}." + ) + # Extract the docs for the function that matches cpp_class.cpp_method(*method_args_names). return self.get_formatted_docstring(member_defs[documenting_index], - ignored_params) + ignored_params) if member_defs else "" def get_member_defs(self, xml_folder: str, cpp_class: str, cpp_method: str): @@ -95,7 +100,7 @@ class XMLDocParser: return "" # Create the path to the file with the documentation for cpp_class. - xml_class_file = xml_folder_path / class_index.attrib['refid'] + '.xml' + xml_class_file = xml_folder_path / f"{class_index.attrib['refid']}.xml" # Parse the class file class_tree = self.parse_xml(xml_class_file) From b661d0d5db8770f7bfa858a86dcdd0f6d9746639 Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Wed, 16 Apr 2025 14:13:48 -0400 Subject: [PATCH 05/17] Changes to build macOS wheels for 10.15 or newer --- .../scripts/python_wheels/cibw_before_all.sh | 51 +++++++++++++++---- .github/workflows/build-cibw.yml | 47 +++++++++++------ 2 files changed, 71 insertions(+), 27 deletions(-) diff --git a/.github/scripts/python_wheels/cibw_before_all.sh b/.github/scripts/python_wheels/cibw_before_all.sh index 2398877a8..98179ad14 100644 --- a/.github/scripts/python_wheels/cibw_before_all.sh +++ b/.github/scripts/python_wheels/cibw_before_all.sh @@ -15,16 +15,46 @@ export PYTHON="python${PYTHON_VERSION}" if [ "$(uname)" == "Linux" ]; then # manylinux2014 is based on CentOS 7, so use yum to install dependencies yum install -y wget - - # Install Boost from source - wget https://archives.boost.io/release/1.87.0/source/boost_1_87_0.tar.gz --quiet - tar -xzf boost_1_87_0.tar.gz - cd boost_1_87_0 - ./bootstrap.sh --prefix=/opt/boost - ./b2 install --prefix=/opt/boost --with=all - cd .. elif [ "$(uname)" == "Darwin" ]; then - brew install wget cmake boost + brew install cmake + + # If MACOSX_DEPLOYMENT_TARGET is not explicitly set, default to the version of the host system. + if [[ -z "${MACOSX_DEPLOYMENT_TARGET}" ]]; then + export MACOSX_DEPLOYMENT_TARGET="$(sw_vers -productVersion | cut -d '.' -f 1-2)" + fi +fi + +# Install Boost from source +wget https://archives.boost.io/release/1.87.0/source/boost_1_87_0.tar.gz --quiet +tar -xzf boost_1_87_0.tar.gz +cd boost_1_87_0 + +BOOST_PREFIX="$HOME/opt/boost" +./bootstrap.sh --prefix=${BOOST_PREFIX} + +if [ "$(uname)" == "Linux" ]; then + ./b2 install --prefix=${BOOST_PREFIX} --with=all -d0 +elif [ "$(uname)" == "Darwin" ]; then + ./b2 install --prefix=${BOOST_PREFIX} --with=all -d0 \ + cxxflags="-mmacosx-version-min=${MACOSX_DEPLOYMENT_TARGET}" \ + linkflags="-mmacosx-version-min=${MACOSX_DEPLOYMENT_TARGET}" +fi +cd .. + +# Export paths so CMake or build system can find Boost +export BOOST_ROOT="${BOOST_PREFIX}" +export BOOST_INCLUDEDIR="${BOOST_PREFIX}/include" +export BOOST_LIBRARYDIR="${BOOST_PREFIX}/lib" + +# Ensure runtime linker can find Boost libraries +export LD_LIBRARY_PATH="${BOOST_LIBRARYDIR}:$LD_LIBRARY_PATH" # For Linux +export REPAIR_LIBRARY_PATH="${BOOST_LIBRARYDIR}:$DYLD_LIBRARY_PATH" # For macOS, REPAIR_LIBRARY_PATH is used by delocate + +if [ "$(uname)" == "Darwin" ]; then + # Explicitly add rpath to Boost dylibs so delocate can find them + for dylib in ${BOOST_LIBRARYDIR}/*.dylib; do + install_name_tool -add_rpath "@loader_path" "$dylib" + done fi $(which $PYTHON) -m pip install -r $PROJECT_DIR/python/dev_requirements.txt @@ -50,9 +80,8 @@ cmake $PROJECT_DIR \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF \ -DCMAKE_INSTALL_PREFIX=$PROJECT_DIR/gtsam_install -cd $PROJECT_DIR/build/python - # Install the Python wrapper module and generate Python stubs +cd $PROJECT_DIR/build/python if [ "$(uname)" == "Linux" ]; then make -j $(nproc) install make -j $(nproc) python-stubs diff --git a/.github/workflows/build-cibw.yml b/.github/workflows/build-cibw.yml index f0b93aef1..95a2e8dfd 100644 --- a/.github/workflows/build-cibw.yml +++ b/.github/workflows/build-cibw.yml @@ -76,22 +76,22 @@ jobs: manylinux_image: manylinux2014 # MacOS x86_64 - # - os: macos-13 - # python_version: "3.10" - # cibw_python_version: 310 - # platform_id: macosx_x86_64 - # - os: macos-13 - # python_version: "3.11" - # cibw_python_version: 311 - # platform_id: macosx_x86_64 - # - os: macos-13 - # python_version: "3.12" - # cibw_python_version: 312 - # platform_id: macosx_x86_64 - # - os: macos-13 - # python_version: "3.13" - # cibw_python_version: 313 - # platform_id: macosx_x86_64 + - os: macos-13 + python_version: "3.10" + cibw_python_version: 310 + platform_id: macosx_x86_64 + - os: macos-13 + python_version: "3.11" + cibw_python_version: 311 + platform_id: macosx_x86_64 + - os: macos-13 + python_version: "3.12" + cibw_python_version: 312 + platform_id: macosx_x86_64 + - os: macos-13 + python_version: "3.13" + cibw_python_version: 313 + platform_id: macosx_x86_64 steps: - name: Checkout @@ -130,6 +130,14 @@ jobs: run: | cmake . -B build -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=${{ matrix.python_version }} + # If on macOS, we previously installed boost using homebrew for the first build. + # We need to uninstall it before building the wheels with cibuildwheel, which will + # install boost from source. + - name: Uninstall Boost (MacOS) + if: runner.os == 'macOS' + run: | + brew uninstall boost + - name: Build and test wheels env: # Generate the platform identifier. See https://cibuildwheel.pypa.io/en/stable/options/#build-skip. @@ -139,6 +147,13 @@ jobs: CIBW_ARCHS: all CIBW_ENVIRONMENT_PASS_LINUX: DEVELOP TIMESTAMP + # Set the minimum required MacOS version for the wheels. + MACOSX_DEPLOYMENT_TARGET: 10.15 + + # Set DYLD_LIBRARY_PATH to REPAIR_LIBRARY_PATH, which is set in cibw_before_all.sh. REPAIR_LIBRARY_PATH + # simply appends BOOST_LIBRARYDIR to the path, which is required during during link-time repair. + CIBW_REPAIR_WHEEL_COMMAND_MACOS: DYLD_LIBRARY_PATH=$REPAIR_LIBRARY_PATH delocate-wheel --require-archs {delocate_archs} -w {dest_dir} -v {wheel} + # Use build instead of pip wheel to build the wheels. This is recommended by PyPA. # See https://cibuildwheel.pypa.io/en/stable/options/#build-frontend. CIBW_BUILD_FRONTEND: "build" From f6ff9745a120276b490964cd1f30f8bb0e0e855d Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Wed, 16 Apr 2025 20:13:01 -0400 Subject: [PATCH 06/17] Remove unnecessary change --- wrap/gtwrap/xml_parser/xml_parser.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/wrap/gtwrap/xml_parser/xml_parser.py b/wrap/gtwrap/xml_parser/xml_parser.py index e133b4882..c73b80c4e 100644 --- a/wrap/gtwrap/xml_parser/xml_parser.py +++ b/wrap/gtwrap/xml_parser/xml_parser.py @@ -56,11 +56,6 @@ class XMLDocParser: documenting_index = self.determine_documenting_index( cpp_class, cpp_method, method_args_names, member_defs) - if member_defs: - self.print_if_verbose( - f"Extracted {len(member_defs)} member definitions for {cpp_class}.{cpp_method}." - ) - # Extract the docs for the function that matches cpp_class.cpp_method(*method_args_names). return self.get_formatted_docstring(member_defs[documenting_index], ignored_params) if member_defs else "" From ab3243fd5833f6cf7734be12d737a60aa920505e Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 21:12:10 -0400 Subject: [PATCH 07/17] Clarify Symbol --- gtsam/inference/doc/Symbol.ipynb | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/inference/doc/Symbol.ipynb b/gtsam/inference/doc/Symbol.ipynb index 20d9fd7e2..c9de75957 100644 --- a/gtsam/inference/doc/Symbol.ipynb +++ b/gtsam/inference/doc/Symbol.ipynb @@ -43,7 +43,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 6, "metadata": { "id": "symbol_import_code" }, @@ -66,7 +66,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 7, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -110,7 +110,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 8, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -151,12 +151,12 @@ "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)`." + "GTSAM provides convenient shorthand functions `X(j)`, `L(j)`, etc., which are equivalent to `gtsam.Symbol('x', j)`, `gtsam.Symbol('l', j)`. To use these, first import with `from gtsam import symbol_shorthand`, then set up the variables you want to use with statements like `X = symbol_shorthand.X`." ] }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 9, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -169,19 +169,19 @@ "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" + "Symbol('x', 0).key() == X(0): True\n", + "Symbol('l', 1).key() == 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", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L\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}\")" + "print(f\"Symbol('x', 0).key() == X(0): {Symbol('x', 0).key() == X(0)}\")\n", + "print(f\"Symbol('l', 1).key() == L(1): {Symbol('l', 1).key() == L(1)}\")" ] } ], From 768076615a84af4bbfc3caa1a98bee3e6946c75f Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 21:12:43 -0400 Subject: [PATCH 08/17] COLAMD vs METIS details --- gtsam/inference/doc/Ordering.ipynb | 2636 +++++++++++++++++++++++++++- 1 file changed, 2603 insertions(+), 33 deletions(-) diff --git a/gtsam/inference/doc/Ordering.ipynb b/gtsam/inference/doc/Ordering.ipynb index 80dbc58f9..1648e8b45 100644 --- a/gtsam/inference/doc/Ordering.ipynb +++ b/gtsam/inference/doc/Ordering.ipynb @@ -17,7 +17,7 @@ "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." + "GTSAM provides several algorithms to compute good orderings automatically, such as COLAMD and METIS (if available), or allows you to specify a custom ordering." ] }, { @@ -45,7 +45,7 @@ }, { "cell_type": "code", - "execution_count": 15, + "execution_count": 1, "metadata": { "id": "ordering_import_code" }, @@ -53,9 +53,10 @@ "source": [ "import gtsam\n", "from gtsam import Ordering, VariableIndex\n", - "# Need a graph type for automatic ordering\n", - "from gtsam import SymbolicFactorGraph\n", + "# Need graph types\n", + "from gtsam import SymbolicFactorGraph, SymbolicBayesNet\n", "from gtsam import symbol_shorthand\n", + "import graphviz\n", "\n", "X = symbol_shorthand.X\n", "L = symbol_shorthand.L" @@ -74,7 +75,7 @@ }, { "cell_type": "code", - "execution_count": 16, + "execution_count": 44, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -99,21 +100,14 @@ }, { "cell_type": "code", - "execution_count": 18, - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "ordering_auto_code", - "outputId": "789abcde-f012-3456-789a-bcdef0123456" - }, + "execution_count": 45, + "metadata": {}, "outputs": [ { "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" + "COLAMD Ordering: Position 0: l1, x0, x1, l2, x2\n" ] } ], @@ -129,13 +123,2589 @@ "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\n", "colamd_ordering = Ordering.ColamdSymbolicFactorGraph(graph)\n", - "colamd_ordering.print(\"COLAMD Ordering: \")\n", + "colamd_ordering.print(\"COLAMD Ordering: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_auto_intro_md" + }, + "source": [ + "## Automatic Ordering Algorithms: COLAMD vs METIS\n", "\n", - "# Constrained COLAMD (force x0 and x2 to be eliminated last)\n", - "constrained_ordering = Ordering.ColamdConstrainedLastSymbolicFactorGraph(graph, gtsam.KeyVector([X(0), X(2)]))\n", - "constrained_ordering.print(\"Constrained COLAMD (x0, x2 last): \")" + "GTSAM provides algorithms to automatically compute an elimination ordering from a factor graph. Two common algorithms are:\n", + "\n", + "1. **COLAMD (Column Approximate Minimum Degree):** A greedy algorithm that aims to minimize *fill-in* at each elimination step. It typically produces orderings that are good for sparse direct methods executed sequentially.\n", + "2. **METIS:** A graph partitioning algorithm (requires METIS library to be installed when compiling GTSAM). It aims to find orderings that partition the graph well, often leading to more balanced elimination trees which can be beneficial for parallel computation and sometimes reduce overall fill-in compared to purely local greedy methods like COLAMD, especially on large, structured problems.\n", + "\n", + "Let's illustrate the difference using a 2D grid factor graph." + ] + }, + { + "cell_type": "code", + "execution_count": 28, + "metadata": { + "id": "ordering_grid_setup_code" + }, + "outputs": [], + "source": [ + "# Create a 5x5 grid graph\n", + "ROWS, COLS = 5, 5\n", + "\n", + "# Use 'x' symbols for grid nodes\n", + "X_grid = lambda r, c: gtsam.symbol('x', r * COLS + c)\n", + "\n", + "def create_grid_graph():\n", + " \"\"\"Creates a SymbolicFactorGraph representing a 2D grid.\"\"\"\n", + " graph = SymbolicFactorGraph()\n", + " keys = []\n", + "\n", + " for r in range(ROWS):\n", + " for c in range(COLS):\n", + " key = X_grid(r, c)\n", + " keys.append(key)\n", + " # Add a unary factor (like a prior or measurement)\n", + " graph.push_factor(key)\n", + " # Add binary factors connecting to right and down neighbors\n", + " if c + 1 < COLS:\n", + " key_right = X_grid(r, c + 1)\n", + " graph.push_factor(key, key_right)\n", + " if r + 1 < ROWS:\n", + " key_down = X_grid(r + 1, c)\n", + " graph.push_factor(key, key_down)\n", + " return graph, keys\n", + "\n", + "grid_graph, grid_keys = create_grid_graph()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_grid_viz_md" + }, + "source": [ + "Here's the structure of our 5x5 grid graph. Edges represent factors connecting variables (nodes)." + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": { + "id": "ordering_grid_viz_code" + }, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor3\n", + "\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor4\n", + "\n", + "\n", + "\n", + "\n", + "factor5\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor5\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor4\n", + "\n", + "\n", + "\n", + "\n", + "factor6\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor6\n", + "\n", + "\n", + "\n", + "\n", + "factor7\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor7\n", + "\n", + "\n", + "\n", + "\n", + "factor8\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor8\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323\n", + "\n", + "x3\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor7\n", + "\n", + "\n", + "\n", + "\n", + "factor9\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor9\n", + "\n", + "\n", + "\n", + "\n", + "factor10\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor10\n", + "\n", + "\n", + "\n", + "\n", + "factor11\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor11\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352324\n", + "\n", + "x4\n", + "\n", + "\n", + "\n", + "var8646911284551352324--factor10\n", + "\n", + "\n", + "\n", + "\n", + "factor12\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352324--factor12\n", + "\n", + "\n", + "\n", + "\n", + "factor13\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352324--factor13\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352325\n", + "\n", + "x5\n", + "\n", + "\n", + "\n", + "var8646911284551352325--factor2\n", + "\n", + "\n", + "\n", + "\n", + "factor14\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352325--factor14\n", + "\n", + "\n", + "\n", + "\n", + "factor15\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352325--factor15\n", + "\n", + "\n", + "\n", + "\n", + "factor16\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352325--factor16\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326\n", + "\n", + "x6\n", + "\n", + "\n", + "\n", + "var8646911284551352326--factor5\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326--factor15\n", + "\n", + "\n", + "\n", + "\n", + "factor17\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326--factor17\n", + "\n", + "\n", + "\n", + "\n", + "factor18\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326--factor18\n", + "\n", + "\n", + "\n", + "\n", + "factor19\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326--factor19\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327\n", + "\n", + "x7\n", + "\n", + "\n", + "\n", + "var8646911284551352327--factor8\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327--factor18\n", + "\n", + "\n", + "\n", + "\n", + "factor20\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327--factor20\n", + "\n", + "\n", + "\n", + "\n", + "factor21\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327--factor21\n", + "\n", + "\n", + "\n", + "\n", + "factor22\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327--factor22\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352328\n", + "\n", + "x8\n", + "\n", + "\n", + "\n", + "var8646911284551352328--factor11\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352328--factor21\n", + "\n", + "\n", + "\n", + "\n", + "factor23\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352328--factor23\n", + "\n", + "\n", + "\n", + "\n", + "factor24\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352328--factor24\n", + "\n", + "\n", + "\n", + "\n", + "factor25\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352328--factor25\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329\n", + "\n", + "x9\n", + "\n", + "\n", + "\n", + "var8646911284551352329--factor13\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329--factor24\n", + "\n", + "\n", + "\n", + "\n", + "factor26\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329--factor26\n", + "\n", + "\n", + "\n", + "\n", + "factor27\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329--factor27\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330\n", + "\n", + "x10\n", + "\n", + "\n", + "\n", + "var8646911284551352330--factor16\n", + "\n", + "\n", + "\n", + "\n", + "factor28\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330--factor28\n", + "\n", + "\n", + "\n", + "\n", + "factor29\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330--factor29\n", + "\n", + "\n", + "\n", + "\n", + "factor30\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330--factor30\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331\n", + "\n", + "x11\n", + "\n", + "\n", + "\n", + "var8646911284551352331--factor19\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331--factor29\n", + "\n", + "\n", + "\n", + "\n", + "factor31\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331--factor31\n", + "\n", + "\n", + "\n", + "\n", + "factor32\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331--factor32\n", + "\n", + "\n", + "\n", + "\n", + "factor33\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331--factor33\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332\n", + "\n", + "x12\n", + "\n", + "\n", + "\n", + "var8646911284551352332--factor22\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332--factor32\n", + "\n", + "\n", + "\n", + "\n", + "factor34\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332--factor34\n", + "\n", + "\n", + "\n", + "\n", + "factor35\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332--factor35\n", + "\n", + "\n", + "\n", + "\n", + "factor36\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332--factor36\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333\n", + "\n", + "x13\n", + "\n", + "\n", + "\n", + "var8646911284551352333--factor25\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333--factor35\n", + "\n", + "\n", + "\n", + "\n", + "factor37\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333--factor37\n", + "\n", + "\n", + "\n", + "\n", + "factor38\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333--factor38\n", + "\n", + "\n", + "\n", + "\n", + "factor39\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333--factor39\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334\n", + "\n", + "x14\n", + "\n", + "\n", + "\n", + "var8646911284551352334--factor27\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334--factor38\n", + "\n", + "\n", + "\n", + "\n", + "factor40\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334--factor40\n", + "\n", + "\n", + "\n", + "\n", + "factor41\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334--factor41\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352335\n", + "\n", + "x15\n", + "\n", + "\n", + "\n", + "var8646911284551352335--factor30\n", + "\n", + "\n", + "\n", + "\n", + "factor42\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352335--factor42\n", + "\n", + "\n", + "\n", + "\n", + "factor43\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352335--factor43\n", + "\n", + "\n", + "\n", + "\n", + "factor44\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352335--factor44\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336\n", + "\n", + "x16\n", + "\n", + "\n", + "\n", + "var8646911284551352336--factor33\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336--factor43\n", + "\n", + "\n", + "\n", + "\n", + "factor45\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336--factor45\n", + "\n", + "\n", + "\n", + "\n", + "factor46\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336--factor46\n", + "\n", + "\n", + "\n", + "\n", + "factor47\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336--factor47\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337\n", + "\n", + "x17\n", + "\n", + "\n", + "\n", + "var8646911284551352337--factor36\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337--factor46\n", + "\n", + "\n", + "\n", + "\n", + "factor48\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337--factor48\n", + "\n", + "\n", + "\n", + "\n", + "factor49\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337--factor49\n", + "\n", + "\n", + "\n", + "\n", + "factor50\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337--factor50\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352338\n", + "\n", + "x18\n", + "\n", + "\n", + "\n", + "var8646911284551352338--factor39\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352338--factor49\n", + "\n", + "\n", + "\n", + "\n", + "factor51\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352338--factor51\n", + "\n", + "\n", + "\n", + "\n", + "factor52\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352338--factor52\n", + "\n", + "\n", + "\n", + "\n", + "factor53\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352338--factor53\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339\n", + "\n", + "x19\n", + "\n", + "\n", + "\n", + "var8646911284551352339--factor41\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339--factor52\n", + "\n", + "\n", + "\n", + "\n", + "factor54\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339--factor54\n", + "\n", + "\n", + "\n", + "\n", + "factor55\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339--factor55\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352340\n", + "\n", + "x20\n", + "\n", + "\n", + "\n", + "var8646911284551352340--factor44\n", + "\n", + "\n", + "\n", + "\n", + "factor56\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352340--factor56\n", + "\n", + "\n", + "\n", + "\n", + "factor57\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352340--factor57\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352341\n", + "\n", + "x21\n", + "\n", + "\n", + "\n", + "var8646911284551352341--factor47\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352341--factor57\n", + "\n", + "\n", + "\n", + "\n", + "factor58\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352341--factor58\n", + "\n", + "\n", + "\n", + "\n", + "factor59\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352341--factor59\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342\n", + "\n", + "x22\n", + "\n", + "\n", + "\n", + "var8646911284551352342--factor50\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342--factor59\n", + "\n", + "\n", + "\n", + "\n", + "factor60\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342--factor60\n", + "\n", + "\n", + "\n", + "\n", + "factor61\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342--factor61\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343\n", + "\n", + "x23\n", + "\n", + "\n", + "\n", + "var8646911284551352343--factor53\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343--factor61\n", + "\n", + "\n", + "\n", + "\n", + "factor62\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343--factor62\n", + "\n", + "\n", + "\n", + "\n", + "factor63\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343--factor63\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352344\n", + "\n", + "x24\n", + "\n", + "\n", + "\n", + "var8646911284551352344--factor55\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352344--factor63\n", + "\n", + "\n", + "\n", + "\n", + "factor64\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352344--factor64\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "display(graphviz.Source(grid_graph.dot()))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_colamd_md" + }, + "source": [ + "### COLAMD Ordering and Resulting Bayes Net\n", + "\n", + "Now, we compute the COLAMD ordering and eliminate the variables according to this order. We then visualize the resulting Symbolic Bayes Net. The structure of the Bayes Net (specifically, the cliques formed by the conditional dependencies) reflects the structure of the elimination tree (or Bayes Tree)." + ] + }, + { + "cell_type": "code", + "execution_count": 47, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "ordering_colamd_code", + "outputId": "789abcde-f012-3456-789a-bcdef0123456" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "COLAMD Ordering: \n", + "Position 0: x0, x5, x1, x6, x4, x9, x3, x8, x12, x24\n", + "Position 10: x23, x19, x18, x20, x21, x15, x16, x22, x17, x14\n", + "Position 20: x13, x10, x11, x2, x7\n", + "\n", + "COLAMD Bayes Net Structure:\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", + "var8646911284551352325\n", + "\n", + "x5\n", + "\n", + "\n", + "\n", + "var8646911284551352321->var8646911284551352325\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323\n", + "\n", + "x3\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352323\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326\n", + "\n", + "x6\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352328\n", + "\n", + "x8\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352328\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330\n", + "\n", + "x10\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352330\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331\n", + "\n", + "x11\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352331\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333\n", + "\n", + "x13\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352333\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334\n", + "\n", + "x14\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352334\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352324\n", + "\n", + "x4\n", + "\n", + "\n", + "\n", + "var8646911284551352323->var8646911284551352324\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329\n", + "\n", + "x9\n", + "\n", + "\n", + "\n", + "var8646911284551352323->var8646911284551352329\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352325->var8646911284551352320\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326->var8646911284551352325\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327\n", + "\n", + "x7\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352322\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352328\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352330\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352331\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332\n", + "\n", + "x12\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352332\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352333\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352334\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337\n", + "\n", + "x17\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352337\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352328->var8646911284551352323\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352328->var8646911284551352329\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329->var8646911284551352324\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352325\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352333\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352334\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352335\n", + "\n", + "x15\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352335\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336\n", + "\n", + "x16\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352336\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352337\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342\n", + "\n", + "x22\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352342\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331->var8646911284551352330\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331->var8646911284551352332\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331->var8646911284551352333\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331->var8646911284551352334\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331->var8646911284551352336\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331->var8646911284551352337\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331->var8646911284551352342\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352328\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352332\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352334\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352337\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352338\n", + "\n", + "x18\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352338\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352342\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334->var8646911284551352323\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334->var8646911284551352328\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334->var8646911284551352329\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334->var8646911284551352337\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334->var8646911284551352338\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339\n", + "\n", + "x19\n", + "\n", + "\n", + "\n", + "var8646911284551352334->var8646911284551352339\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334->var8646911284551352342\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352340\n", + "\n", + "x20\n", + "\n", + "\n", + "\n", + "var8646911284551352335->var8646911284551352340\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352341\n", + "\n", + "x21\n", + "\n", + "\n", + "\n", + "var8646911284551352335->var8646911284551352341\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336->var8646911284551352335\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336->var8646911284551352341\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352332\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352336\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352338\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352342\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352338->var8646911284551352339\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343\n", + "\n", + "x23\n", + "\n", + "\n", + "\n", + "var8646911284551352338->var8646911284551352343\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339->var8646911284551352343\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352344\n", + "\n", + "x24\n", + "\n", + "\n", + "\n", + "var8646911284551352339->var8646911284551352344\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352341->var8646911284551352340\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352335\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352336\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352338\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352339\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352341\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352343\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343->var8646911284551352344\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# COLAMD (Column Approximate Minimum Degree) ordering\n", + "colamd_ordering = Ordering.ColamdSymbolicFactorGraph(grid_graph)\n", + "print(\"COLAMD Ordering: \")\n", + "colamd_ordering.print()\n", + "\n", + "# Eliminate using COLAMD ordering\n", + "bayes_net_colamd = grid_graph.eliminateSequential(colamd_ordering)\n", + "\n", + "# Visualize the resulting Bayes Net\n", + "print(\"\\nCOLAMD Bayes Net Structure:\")\n", + "display(graphviz.Source(bayes_net_colamd.dot()))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_metis_md" + }, + "source": [ + "### METIS Ordering and Resulting Bayes Net\n", + "\n", + "Next, we compute the METIS ordering and visualize its resulting Bayes Net. Compare its structure to the one generated by COLAMD." + ] + }, + { + "cell_type": "code", + "execution_count": 49, + "metadata": { + "id": "ordering_metis_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "METIS Ordering: \n", + "Position 0: x21, x1, x15, x11, x5, x20, x0, x16, x6, x10\n", + "Position 10: x24, x4, x18, x14, x8, x23, x3, x19, x9, x13\n", + "Position 20: x7, x17, x2, x22, x12\n", + "\n", + "METIS Bayes Net Structure:\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352320->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352325\n", + "\n", + "x5\n", + "\n", + "\n", + "\n", + "var8646911284551352320->var8646911284551352325\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352320\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323\n", + "\n", + "x3\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352323\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326\n", + "\n", + "x6\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327\n", + "\n", + "x7\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352327\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329\n", + "\n", + "x9\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352329\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330\n", + "\n", + "x10\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352330\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333\n", + "\n", + "x13\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352333\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337\n", + "\n", + "x17\n", + "\n", + "\n", + "\n", + "var8646911284551352322->var8646911284551352337\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352324\n", + "\n", + "x4\n", + "\n", + "\n", + "\n", + "var8646911284551352323->var8646911284551352324\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352328\n", + "\n", + "x8\n", + "\n", + "\n", + "\n", + "var8646911284551352323->var8646911284551352328\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326->var8646911284551352320\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326->var8646911284551352321\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352326->var8646911284551352325\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352331\n", + "\n", + "x11\n", + "\n", + "\n", + "\n", + "var8646911284551352326->var8646911284551352331\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336\n", + "\n", + "x16\n", + "\n", + "\n", + "\n", + "var8646911284551352326->var8646911284551352336\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352323\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352328\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352329\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352330\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352327->var8646911284551352333\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329->var8646911284551352323\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329->var8646911284551352324\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352329->var8646911284551352328\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352334\n", + "\n", + "x14\n", + "\n", + "\n", + "\n", + "var8646911284551352329->var8646911284551352334\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339\n", + "\n", + "x19\n", + "\n", + "\n", + "\n", + "var8646911284551352329->var8646911284551352339\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352320\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352325\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352331\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352335\n", + "\n", + "x15\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352335\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352336\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352340\n", + "\n", + "x20\n", + "\n", + "\n", + "\n", + "var8646911284551352330->var8646911284551352340\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332\n", + "\n", + "x12\n", + "\n", + "\n", + "\n", + "var8646911284551352332->var8646911284551352322\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332->var8646911284551352327\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332->var8646911284551352330\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332->var8646911284551352331\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332->var8646911284551352333\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332->var8646911284551352336\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352332->var8646911284551352337\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342\n", + "\n", + "x22\n", + "\n", + "\n", + "\n", + "var8646911284551352332->var8646911284551352342\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352323\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352328\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352329\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352334\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352338\n", + "\n", + "x18\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352338\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352339\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343\n", + "\n", + "x23\n", + "\n", + "\n", + "\n", + "var8646911284551352333->var8646911284551352343\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336->var8646911284551352331\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336->var8646911284551352335\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352336->var8646911284551352340\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352341\n", + "\n", + "x21\n", + "\n", + "\n", + "\n", + "var8646911284551352336->var8646911284551352341\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352327\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352329\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352330\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352333\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352336\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352338\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352339\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352337->var8646911284551352343\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339->var8646911284551352334\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339->var8646911284551352338\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352339->var8646911284551352343\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352344\n", + "\n", + "x24\n", + "\n", + "\n", + "\n", + "var8646911284551352339->var8646911284551352344\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352340->var8646911284551352335\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352340->var8646911284551352341\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352322\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352326\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352327\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352329\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352330\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352333\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352336\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352337\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352339\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352340\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352341\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342->var8646911284551352343\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343->var8646911284551352338\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343->var8646911284551352344\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "metis_ordering = Ordering.MetisSymbolicFactorGraph(grid_graph)\n", + "print(\"METIS Ordering: \")\n", + "metis_ordering.print()\n", + "\n", + "# Eliminate using METIS ordering\n", + "bayes_net_metis = grid_graph.eliminateSequential(metis_ordering)\n", + "\n", + "# Visualize the resulting Bayes Net\n", + "print(\"\\nMETIS Bayes Net Structure:\")\n", + "display(graphviz.Source(bayes_net_metis.dot()))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_comparison_md" + }, + "source": [ + "### Comparison\n", + "\n", + "Observe the differences in the Bayes Net structures produced by COLAMD and METIS:\n", + "\n", + "* **COLAMD:** Often produces a more 'stringy' or deeper elimination tree/Bayes Net. The cliques (conditionals in the Bayes Net) might be smaller initially but can grow larger towards the root (variables eliminated last).\n", + "* **METIS:** Tends to produce a more 'bushy' or balanced tree. It tries to partition the graph, eliminating variables within partitions first, leading to potentially larger initial cliques but often a shallower overall structure and smaller separators (variables connecting cliques high up in the tree). \n", + "\n", + "When should you choose one over the other? The best choice often depends on the specific problem structure and computational goals:\n", + "\n", + "* **Use COLAMD when:**\n", + " * You need a good, general-purpose ordering quickly. COLAMD is typically much faster *to compute the ordering itself* than METIS.\n", + " * You are primarily using a *sequential* solver (running on a single CPU core). COLAMD's greedy strategy is often well-suited for minimizing fill-in in this scenario.\n", + " * The factor graph is relatively small or doesn't have a highly regular structure where complex partitioning would yield significant benefits.\n", + "\n", + "* **Use METIS when:**\n", + " * You are aiming for maximum performance with a *parallel* solver (e.g., using GTSAM's multifrontal solvers with TBB). METIS's graph partitioning approach tends to create more balanced elimination trees, which allows for better workload distribution across multiple CPU cores.\n", + " * You are dealing with very large-scale problems, especially those with a regular structure (like large grids, meshes from finite element analysis, or extensive SLAM maps). On such problems, METIS can sometimes find an ordering with significantly less *total fill-in* than COLAMD, leading to faster factorization, even if computing the ordering itself takes longer.\n", + " * The cost of computing the ordering is negligible compared to the cost of the subsequent factorization and solve steps (e.g., you compute the ordering once for a structure that is solved repeatedly).\n", + "\n", + "**In summary:** COLAMD is a robust and fast default. METIS is often the preferred choice for large-scale problems and parallel execution, potentially offering better final factorization performance at the cost of a slower ordering computation. Experimentation on your specific problem type might be necessary to determine the optimal choice.\n", + "\n", + "For more information on COLAMD and METIS, see [Factor Graphs for Robot Perception](https://www.cs.cmu.edu/~kaess/pub/Dellaert17fnt.pdf)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ordering_constrained_md" + }, + "source": [ + "### Constrained Ordering\n", + "\n", + "Sometimes, we want to force certain variables to be eliminated last (e.g., the current robot pose in SLAM). `Ordering.ColamdConstrainedLast` allows this for COLAMD." + ] + }, + { + "cell_type": "code", + "execution_count": 51, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "ordering_constrained_code", + "outputId": "89abcdef-0123-4567-89ab-cdef01234567" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Constrained COLAMD (x0, x24 last):\n", + "Position 0: x5, x1, x6, x23, x20, x21, x15, x16, x22, x12\n", + "Position 10: x19, x4, x9, x3, x8, x14, x13, x18, x17, x2\n", + "Position 20: x7, x10, x11, x24, x0\n" + ] + } + ], + "source": [ + "# Example: Constrained COLAMD forcing corners (x0, x24) to be eliminated last\n", + "# Note: We use the grid_graph defined earlier\n", + "corner_keys = gtsam.KeyVector([X_grid(0, 0), X_grid(ROWS-1, COLS-1)])\n", + "constrained_ordering = Ordering.ColamdConstrainedLastSymbolicFactorGraph(grid_graph, corner_keys)\n", + "print(f\"Constrained COLAMD ({gtsam.DefaultKeyFormatter(corner_keys[0])}, {gtsam.DefaultKeyFormatter(corner_keys[1])} last):\")\n", + "constrained_ordering.print()" ] }, { @@ -151,7 +2721,7 @@ }, { "cell_type": "code", - "execution_count": 19, + "execution_count": 52, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -164,17 +2734,17 @@ "name": "stdout", "output_type": "stream", "text": [ - "Ordering size: 5\n", - "Key at position 0: l1\n" + "COLAMD Ordering size: 25\n", + "Key at position 0 (COLAMD): x0\n" ] } ], "source": [ - "print(f\"Ordering size: {colamd_ordering.size()}\")\n", + "print(f\"COLAMD 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)}\")" + "print(f\"Key at position 0 (COLAMD): {gtsam.DefaultKeyFormatter(key_at_0)}\")" ] }, { @@ -185,12 +2755,12 @@ "source": [ "## Appending Keys\n", "\n", - "You can append keys using `push_back`." + "You can append keys to an existing ordering using `push_back`." ] }, { "cell_type": "code", - "execution_count": 20, + "execution_count": 53, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -203,23 +2773,23 @@ "name": "stdout", "output_type": "stream", "text": [ - "Appended Ordering: Position 0: l1, x0, x1, l2, x2, l0, x3\n" + "Appended Ordering: Position 0: x0, x5, x1, x6, x4, x9, x3, x8, x12, x24\n", + "Position 10: x23, x19, x18, x20, x21, x15, x16, x22, x17, x14\n", + "Position 20: x13, x10, x11, x2, x7, l0, x25\n" ] } ], "source": [ + "# Use the COLAMD ordering from the grid example\n", "appended_ordering = Ordering(colamd_ordering)\n", - "appended_ordering.push_back(L(0))\n", - "appended_ordering.push_back(X(3))\n", + "appended_ordering.push_back(L(0)) # Append a landmark key\n", + "appended_ordering.push_back(X(ROWS*COLS)) # Append a new pose key x25\n", "\n", "appended_ordering.print(\"Appended Ordering: \")" ] } ], "metadata": { - "colab": { - "provenance": [] - }, "kernelspec": { "display_name": "gtsam", "language": "python", From eee10eb75dfe42b647c7ec9e04c9362bd6840fa4 Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 23:41:41 -0400 Subject: [PATCH 09/17] Ameliorate visuals --- gtsam/inference/doc/Ordering.ipynb | 2614 ++++------------------------ 1 file changed, 348 insertions(+), 2266 deletions(-) diff --git a/gtsam/inference/doc/Ordering.ipynb b/gtsam/inference/doc/Ordering.ipynb index 1648e8b45..9f4a595ed 100644 --- a/gtsam/inference/doc/Ordering.ipynb +++ b/gtsam/inference/doc/Ordering.ipynb @@ -45,16 +45,16 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 21, "metadata": { "id": "ordering_import_code" }, "outputs": [], "source": [ "import gtsam\n", - "from gtsam import Ordering, VariableIndex\n", + "from gtsam import Ordering\n", "# Need graph types\n", - "from gtsam import SymbolicFactorGraph, SymbolicBayesNet\n", + "from gtsam import SymbolicFactorGraph\n", "from gtsam import symbol_shorthand\n", "import graphviz\n", "\n", @@ -75,7 +75,7 @@ }, { "cell_type": "code", - "execution_count": 44, + "execution_count": 22, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -100,7 +100,7 @@ }, { "cell_type": "code", - "execution_count": 45, + "execution_count": 23, "metadata": {}, "outputs": [ { @@ -146,29 +146,29 @@ }, { "cell_type": "code", - "execution_count": 28, + "execution_count": 24, "metadata": { "id": "ordering_grid_setup_code" }, "outputs": [], "source": [ - "# Create a 5x5 grid graph\n", - "ROWS, COLS = 5, 5\n", + "# Create a 3x4 grid graph\n", + "ROWS, COLS = 3, 4\n", "\n", "# Use 'x' symbols for grid nodes\n", - "X_grid = lambda r, c: gtsam.symbol('x', r * COLS + c)\n", + "X_grid = lambda r, c: X(10 * (r + 1) + c + 1)\n", + "\n", "\n", "def create_grid_graph():\n", " \"\"\"Creates a SymbolicFactorGraph representing a 2D grid.\"\"\"\n", " graph = SymbolicFactorGraph()\n", " keys = []\n", - "\n", + " positions = {}\n", " for r in range(ROWS):\n", " for c in range(COLS):\n", " key = X_grid(r, c)\n", + " positions[key] = gtsam.Point2(c, COLS-r)\n", " keys.append(key)\n", - " # Add a unary factor (like a prior or measurement)\n", - " graph.push_factor(key)\n", " # Add binary factors connecting to right and down neighbors\n", " if c + 1 < COLS:\n", " key_right = X_grid(r, c + 1)\n", @@ -176,9 +176,10 @@ " if r + 1 < ROWS:\n", " key_down = X_grid(r + 1, c)\n", " graph.push_factor(key, key_down)\n", - " return graph, keys\n", + " return graph, keys, positions\n", "\n", - "grid_graph, grid_keys = create_grid_graph()" + "\n", + "grid_graph, grid_keys, positions = create_grid_graph()" ] }, { @@ -187,15 +188,13 @@ "id": "ordering_grid_viz_md" }, "source": [ - "Here's the structure of our 5x5 grid graph. Edges represent factors connecting variables (nodes)." + "Here's the structure of our grid graph. Edges represent factors connecting variables (nodes)." ] }, { "cell_type": "code", - "execution_count": 22, - "metadata": { - "id": "ordering_grid_viz_code" - }, + "execution_count": 25, + "metadata": {}, "outputs": [ { "data": { @@ -206,1015 +205,172 @@ "\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", - "factor2\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352320--factor2\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352321\n", - "\n", - "x1\n", - "\n", - "\n", - "\n", - "var8646911284551352321--factor1\n", - "\n", - "\n", - "\n", - "\n", - "factor3\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352321--factor3\n", - "\n", - "\n", - "\n", - "\n", - "factor4\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352321--factor4\n", - "\n", - "\n", - "\n", - "\n", - "factor5\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352321--factor5\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352322\n", - "\n", - "x2\n", - "\n", - "\n", - "\n", - "var8646911284551352322--factor4\n", - "\n", - "\n", - "\n", - "\n", - "factor6\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352322--factor6\n", - "\n", - "\n", - "\n", - "\n", - "factor7\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352322--factor7\n", - "\n", - "\n", - "\n", - "\n", - "factor8\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352322--factor8\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352323\n", - "\n", - "x3\n", - "\n", - "\n", - "\n", - "var8646911284551352323--factor7\n", - "\n", - "\n", - "\n", - "\n", - "factor9\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352323--factor9\n", - "\n", - "\n", - "\n", - "\n", - "factor10\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352323--factor10\n", - "\n", - "\n", - "\n", - "\n", - "factor11\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352323--factor11\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352324\n", - "\n", - "x4\n", - "\n", - "\n", - "\n", - "var8646911284551352324--factor10\n", - "\n", - "\n", - "\n", - "\n", - "factor12\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352324--factor12\n", - "\n", - "\n", - "\n", - "\n", - "factor13\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352324--factor13\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352325\n", - "\n", - "x5\n", - "\n", - "\n", - "\n", - "var8646911284551352325--factor2\n", - "\n", - "\n", - "\n", - "\n", - "factor14\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352325--factor14\n", - "\n", - "\n", - "\n", - "\n", - "factor15\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352325--factor15\n", - "\n", - "\n", - "\n", - "\n", - "factor16\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352325--factor16\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326\n", - "\n", - "x6\n", - "\n", - "\n", - "\n", - "var8646911284551352326--factor5\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326--factor15\n", - "\n", - "\n", - "\n", - "\n", - "factor17\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326--factor17\n", - "\n", - "\n", - "\n", - "\n", - "factor18\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326--factor18\n", - "\n", - "\n", - "\n", - "\n", - "factor19\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326--factor19\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327\n", - "\n", - "x7\n", - "\n", - "\n", - "\n", - "var8646911284551352327--factor8\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327--factor18\n", - "\n", - "\n", - "\n", - "\n", - "factor20\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327--factor20\n", - "\n", - "\n", - "\n", - "\n", - "factor21\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327--factor21\n", - "\n", - "\n", - "\n", - "\n", - "factor22\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327--factor22\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352328\n", - "\n", - "x8\n", - "\n", - "\n", - "\n", - "var8646911284551352328--factor11\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352328--factor21\n", - "\n", - "\n", - "\n", - "\n", - "factor23\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352328--factor23\n", - "\n", - "\n", - "\n", - "\n", - "factor24\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352328--factor24\n", - "\n", - "\n", - "\n", - "\n", - "factor25\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352328--factor25\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329\n", - "\n", - "x9\n", - "\n", - "\n", - "\n", - "var8646911284551352329--factor13\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329--factor24\n", - "\n", - "\n", - "\n", - "\n", - "factor26\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329--factor26\n", - "\n", - "\n", - "\n", - "\n", - "factor27\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329--factor27\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330\n", - "\n", - "x10\n", - "\n", - "\n", - "\n", - "var8646911284551352330--factor16\n", - "\n", - "\n", - "\n", - "\n", - "factor28\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330--factor28\n", - "\n", - "\n", - "\n", - "\n", - "factor29\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330--factor29\n", - "\n", - "\n", - "\n", - "\n", - "factor30\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330--factor30\n", - "\n", - "\n", + "\n", + "\n", + "\n", "\n", - "\n", + "\n", "var8646911284551352331\n", - "\n", - "x11\n", - "\n", - "\n", - "\n", - "var8646911284551352331--factor19\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352331--factor29\n", - "\n", - "\n", - "\n", - "\n", - "factor31\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352331--factor31\n", - "\n", - "\n", - "\n", - "\n", - "factor32\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352331--factor32\n", - "\n", - "\n", - "\n", - "\n", - "factor33\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352331--factor33\n", - "\n", + "\n", + "x11\n", "\n", "\n", - "\n", + "\n", "var8646911284551352332\n", - "\n", - "x12\n", + "\n", + "x12\n", "\n", - "\n", - "\n", - "var8646911284551352332--factor22\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332--factor32\n", - "\n", - "\n", - "\n", - "\n", - "factor34\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332--factor34\n", - "\n", - "\n", - "\n", - "\n", - "factor35\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332--factor35\n", - "\n", - "\n", - "\n", - "\n", - "factor36\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332--factor36\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333\n", - "\n", - "x13\n", - "\n", - "\n", - "\n", - "var8646911284551352333--factor25\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333--factor35\n", - "\n", - "\n", - "\n", - "\n", - "factor37\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333--factor37\n", - "\n", - "\n", - "\n", - "\n", - "factor38\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333--factor38\n", - "\n", - "\n", - "\n", - "\n", - "factor39\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333--factor39\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334\n", - "\n", - "x14\n", - "\n", - "\n", - "\n", - "var8646911284551352334--factor27\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334--factor38\n", - "\n", - "\n", - "\n", - "\n", - "factor40\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334--factor40\n", - "\n", - "\n", - "\n", - "\n", - "factor41\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334--factor41\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352335\n", - "\n", - "x15\n", - "\n", - "\n", - "\n", - "var8646911284551352335--factor30\n", - "\n", - "\n", - "\n", - "\n", - "factor42\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352335--factor42\n", - "\n", - "\n", - "\n", - "\n", - "factor43\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352335--factor43\n", - "\n", - "\n", - "\n", - "\n", - "factor44\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352335--factor44\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336\n", - "\n", - "x16\n", - "\n", - "\n", - "\n", - "var8646911284551352336--factor33\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336--factor43\n", - "\n", - "\n", - "\n", - "\n", - "factor45\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336--factor45\n", - "\n", - "\n", - "\n", - "\n", - "factor46\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336--factor46\n", - "\n", - "\n", - "\n", - "\n", - "factor47\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336--factor47\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337\n", - "\n", - "x17\n", - "\n", - "\n", - "\n", - "var8646911284551352337--factor36\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337--factor46\n", - "\n", - "\n", - "\n", - "\n", - "factor48\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337--factor48\n", - "\n", - "\n", - "\n", - "\n", - "factor49\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337--factor49\n", - "\n", - "\n", - "\n", - "\n", - "factor50\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337--factor50\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352338\n", - "\n", - "x18\n", - "\n", - "\n", - "\n", - "var8646911284551352338--factor39\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352338--factor49\n", - "\n", - "\n", - "\n", - "\n", - "factor51\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352338--factor51\n", - "\n", - "\n", - "\n", - "\n", - "factor52\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352338--factor52\n", - "\n", - "\n", - "\n", - "\n", - "factor53\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352338--factor53\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339\n", - "\n", - "x19\n", - "\n", - "\n", - "\n", - "var8646911284551352339--factor41\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339--factor52\n", - "\n", - "\n", - "\n", - "\n", - "factor54\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339--factor54\n", - "\n", - "\n", - "\n", - "\n", - "factor55\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339--factor55\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352340\n", - "\n", - "x20\n", - "\n", - "\n", - "\n", - "var8646911284551352340--factor44\n", - "\n", - "\n", - "\n", - "\n", - "factor56\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352340--factor56\n", - "\n", - "\n", - "\n", - "\n", - "factor57\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352340--factor57\n", - "\n", + "\n", + "\n", + "var8646911284551352331--var8646911284551352332\n", + "\n", "\n", "\n", - "\n", + "\n", "var8646911284551352341\n", - "\n", - "x21\n", + "\n", + "x21\n", "\n", - "\n", - "\n", - "var8646911284551352341--factor47\n", - "\n", + "\n", + "\n", + "var8646911284551352331--var8646911284551352341\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352341--factor57\n", - "\n", + "\n", + "\n", + "var8646911284551352333\n", + "\n", + "x13\n", "\n", - "\n", - "\n", - "factor58\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352341--factor58\n", - "\n", - "\n", - "\n", - "\n", - "factor59\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352341--factor59\n", - "\n", + "\n", + "\n", + "var8646911284551352332--var8646911284551352333\n", + "\n", "\n", "\n", - "\n", + "\n", "var8646911284551352342\n", - "\n", - "x22\n", + "\n", + "x22\n", "\n", - "\n", - "\n", - "var8646911284551352342--factor50\n", - "\n", + "\n", + "\n", + "var8646911284551352332--var8646911284551352342\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352342--factor59\n", - "\n", + "\n", + "\n", + "var8646911284551352334\n", + "\n", + "x14\n", "\n", - "\n", - "\n", - "factor60\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342--factor60\n", - "\n", - "\n", - "\n", - "\n", - "factor61\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342--factor61\n", - "\n", + "\n", + "\n", + "var8646911284551352333--var8646911284551352334\n", + "\n", "\n", "\n", - "\n", + "\n", "var8646911284551352343\n", - "\n", - "x23\n", + "\n", + "x23\n", "\n", - "\n", - "\n", - "var8646911284551352343--factor53\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352343--factor61\n", - "\n", - "\n", - "\n", - "\n", - "factor62\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352343--factor62\n", - "\n", - "\n", - "\n", - "\n", - "factor63\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352343--factor63\n", - "\n", + "\n", + "\n", + "var8646911284551352333--var8646911284551352343\n", + "\n", "\n", "\n", - "\n", + "\n", "var8646911284551352344\n", - "\n", - "x24\n", + "\n", + "x24\n", "\n", - "\n", - "\n", - "var8646911284551352344--factor55\n", - "\n", + "\n", + "\n", + "var8646911284551352334--var8646911284551352344\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352344--factor63\n", - "\n", + "\n", + "\n", + "var8646911284551352341--var8646911284551352342\n", + "\n", "\n", - "\n", - "\n", - "factor64\n", - "\n", + "\n", + "\n", + "var8646911284551352351\n", + "\n", + "x31\n", "\n", - "\n", - "\n", - "var8646911284551352344--factor64\n", - "\n", + "\n", + "\n", + "var8646911284551352341--var8646911284551352351\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352342--var8646911284551352343\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352352\n", + "\n", + "x32\n", + "\n", + "\n", + "\n", + "var8646911284551352342--var8646911284551352352\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352343--var8646911284551352344\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352353\n", + "\n", + "x33\n", + "\n", + "\n", + "\n", + "var8646911284551352343--var8646911284551352353\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352354\n", + "\n", + "x34\n", + "\n", + "\n", + "\n", + "var8646911284551352344--var8646911284551352354\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352351--var8646911284551352352\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352352--var8646911284551352353\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352353--var8646911284551352354\n", + "\n", "\n", "\n", "\n" ], "text/plain": [ - "" + "" ] }, "metadata": {}, @@ -1222,7 +378,9 @@ } ], "source": [ - "display(graphviz.Source(grid_graph.dot()))" + "writer = gtsam.DotWriter(binaryEdges = True)\n", + "writer.variablePositions = positions\n", + "display(graphviz.Source(grid_graph.dot(writer=writer), engine='neato'))" ] }, { @@ -1238,7 +396,7 @@ }, { "cell_type": "code", - "execution_count": 47, + "execution_count": 26, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -1252,9 +410,14 @@ "output_type": "stream", "text": [ "COLAMD Ordering: \n", - "Position 0: x0, x5, x1, x6, x4, x9, x3, x8, x12, x24\n", - "Position 10: x23, x19, x18, x20, x21, x15, x16, x22, x17, x14\n", - "Position 20: x13, x10, x11, x2, x7\n", + "Position 0: x11, x31, x14, x34, x33, x24, x23, x32, x13, x22\n", + "Position 10: x21, x12\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ "\n", "COLAMD Bayes Net Structure:\n" ] @@ -1267,628 +430,119 @@ " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n", "\n", - "\n", - "\n", - "\n", - "\n", - "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", "\n", - "var8646911284551352320\n", - "\n", - "x0\n", + "0\n", + "\n", + "x11, x21, x12\n", "\n", - "\n", + "\n", "\n", - "var8646911284551352321\n", - "\n", - "x1\n", + "1\n", + "\n", + "x13, x22 : x12, x21\n", "\n", - "\n", - "\n", - "var8646911284551352321->var8646911284551352320\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352325\n", - "\n", - "x5\n", - "\n", - "\n", - "\n", - "var8646911284551352321->var8646911284551352325\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352322\n", - "\n", - "x2\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352321\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352323\n", - "\n", - "x3\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352323\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326\n", - "\n", - "x6\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352326\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352328\n", - "\n", - "x8\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352328\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330\n", - "\n", - "x10\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352330\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352331\n", - "\n", - "x11\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352331\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333\n", - "\n", - "x13\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352333\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334\n", - "\n", - "x14\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352334\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352324\n", - "\n", - "x4\n", - "\n", - "\n", - "\n", - "var8646911284551352323->var8646911284551352324\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329\n", - "\n", - "x9\n", - "\n", - "\n", - "\n", - "var8646911284551352323->var8646911284551352329\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352325->var8646911284551352320\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326->var8646911284551352321\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326->var8646911284551352325\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327\n", - "\n", - "x7\n", - "\n", - "\n", + "\n", "\n", - "var8646911284551352327->var8646911284551352322\n", - "\n", - "\n", + "0->1\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352326\n", - "\n", - "\n", + "\n", + "\n", + "2\n", + "\n", + "x32 : x13, x21, x22\n", "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352328\n", - "\n", - "\n", + "\n", + "\n", + "1->2\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352330\n", - "\n", - "\n", + "\n", + "\n", + "3\n", + "\n", + "x31 : x21, x32\n", "\n", - "\n", + "\n", "\n", - "var8646911284551352327->var8646911284551352331\n", - "\n", - "\n", + "2->3\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352332\n", - "\n", - "x12\n", + "\n", + "\n", + "4\n", + "\n", + "x23 : x13, x22, x32\n", "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352332\n", - "\n", - "\n", + "\n", + "\n", + "2->4\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352333\n", - "\n", - "\n", + "\n", + "\n", + "5\n", + "\n", + "x24 : x13, x23, x32\n", "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352334\n", - "\n", - "\n", + "\n", + "\n", + "4->5\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352337\n", - "\n", - "x17\n", + "\n", + "\n", + "6\n", + "\n", + "x14 : x13, x24\n", "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352337\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352328->var8646911284551352323\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352328->var8646911284551352329\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329->var8646911284551352324\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352321\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352325\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352326\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352333\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352334\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352335\n", - "\n", - "x15\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352335\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336\n", - "\n", - "x16\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352336\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352337\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342\n", - "\n", - "x22\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352342\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352331->var8646911284551352326\n", - "\n", - "\n", - "\n", - "\n", + "\n", "\n", - "var8646911284551352331->var8646911284551352330\n", - "\n", - "\n", + "5->6\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352331->var8646911284551352332\n", - "\n", - "\n", + "\n", + "\n", + "7\n", + "\n", + "x33 : x23, x24, x32\n", "\n", - "\n", - "\n", - "var8646911284551352331->var8646911284551352333\n", - "\n", - "\n", + "\n", + "\n", + "5->7\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352331->var8646911284551352334\n", - "\n", - "\n", + "\n", + "\n", + "8\n", + "\n", + "x34 : x24, x33\n", "\n", - "\n", - "\n", - "var8646911284551352331->var8646911284551352336\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352331->var8646911284551352337\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352331->var8646911284551352342\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352328\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352332\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352334\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352337\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352338\n", - "\n", - "x18\n", - "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352338\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352342\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334->var8646911284551352323\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334->var8646911284551352328\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334->var8646911284551352329\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334->var8646911284551352337\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334->var8646911284551352338\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339\n", - "\n", - "x19\n", - "\n", - "\n", - "\n", - "var8646911284551352334->var8646911284551352339\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334->var8646911284551352342\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352340\n", - "\n", - "x20\n", - "\n", - "\n", - "\n", - "var8646911284551352335->var8646911284551352340\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352341\n", - "\n", - "x21\n", - "\n", - "\n", - "\n", - "var8646911284551352335->var8646911284551352341\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336->var8646911284551352335\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336->var8646911284551352341\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352332\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352336\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352338\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352342\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352338->var8646911284551352339\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352343\n", - "\n", - "x23\n", - "\n", - "\n", - "\n", - "var8646911284551352338->var8646911284551352343\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339->var8646911284551352343\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352344\n", - "\n", - "x24\n", - "\n", - "\n", - "\n", - "var8646911284551352339->var8646911284551352344\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352341->var8646911284551352340\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352335\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352336\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352338\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352339\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352341\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352343\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352343->var8646911284551352344\n", - "\n", - "\n", + "\n", + "\n", + "7->8\n", + "\n", + "\n", "\n", "\n", "\n" ], "text/plain": [ - "" + "" ] }, "metadata": {}, @@ -1902,11 +556,11 @@ "colamd_ordering.print()\n", "\n", "# Eliminate using COLAMD ordering\n", - "bayes_net_colamd = grid_graph.eliminateSequential(colamd_ordering)\n", + "bayes_tree_colamd = grid_graph.eliminateMultifrontal(colamd_ordering)\n", "\n", "# Visualize the resulting Bayes Net\n", "print(\"\\nCOLAMD Bayes Net Structure:\")\n", - "display(graphviz.Source(bayes_net_colamd.dot()))" + "display(graphviz.Source(bayes_tree_colamd.dot()))" ] }, { @@ -1922,7 +576,7 @@ }, { "cell_type": "code", - "execution_count": 49, + "execution_count": 27, "metadata": { "id": "ordering_metis_code" }, @@ -1932,9 +586,8 @@ "output_type": "stream", "text": [ "METIS Ordering: \n", - "Position 0: x21, x1, x15, x11, x5, x20, x0, x16, x6, x10\n", - "Position 10: x24, x4, x18, x14, x8, x23, x3, x19, x9, x13\n", - "Position 20: x7, x17, x2, x22, x12\n", + "Position 0: x34, x23, x14, x24, x13, x32, x11, x31, x21, x12\n", + "Position 10: x33, x22\n", "\n", "METIS Bayes Net Structure:\n" ] @@ -1947,676 +600,107 @@ " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n", "\n", - "\n", - "\n", - "\n", - "\n", - "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", "\n", - "var8646911284551352320\n", - "\n", - "x0\n", + "8\n", + "\n", + "x21, x12, x33, x22\n", "\n", - "\n", + "\n", "\n", - "var8646911284551352321\n", - "\n", - "x1\n", + "9\n", + "\n", + "x11 : x12, x21\n", "\n", - "\n", - "\n", - "var8646911284551352320->var8646911284551352321\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352325\n", - "\n", - "x5\n", - "\n", - "\n", - "\n", - "var8646911284551352320->var8646911284551352325\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352322\n", - "\n", - "x2\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352320\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352321\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352323\n", - "\n", - "x3\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352323\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326\n", - "\n", - "x6\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352326\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327\n", - "\n", - "x7\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352327\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329\n", - "\n", - "x9\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352329\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330\n", - "\n", - "x10\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352330\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333\n", - "\n", - "x13\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352333\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337\n", - "\n", - "x17\n", - "\n", - "\n", - "\n", - "var8646911284551352322->var8646911284551352337\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352324\n", - "\n", - "x4\n", - "\n", - "\n", - "\n", - "var8646911284551352323->var8646911284551352324\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352328\n", - "\n", - "x8\n", - "\n", - "\n", - "\n", - "var8646911284551352323->var8646911284551352328\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326->var8646911284551352320\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326->var8646911284551352321\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352326->var8646911284551352325\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352331\n", - "\n", - "x11\n", - "\n", - "\n", - "\n", - "var8646911284551352326->var8646911284551352331\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336\n", - "\n", - "x16\n", - "\n", - "\n", - "\n", - "var8646911284551352326->var8646911284551352336\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352323\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352326\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352328\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352329\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352330\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352327->var8646911284551352333\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329->var8646911284551352323\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329->var8646911284551352324\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352329->var8646911284551352328\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352334\n", - "\n", - "x14\n", - "\n", - "\n", - "\n", - "var8646911284551352329->var8646911284551352334\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339\n", - "\n", - "x19\n", - "\n", - "\n", - "\n", - "var8646911284551352329->var8646911284551352339\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352320\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352325\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352326\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352331\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352335\n", - "\n", - "x15\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352335\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352336\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352340\n", - "\n", - "x20\n", - "\n", - "\n", - "\n", - "var8646911284551352330->var8646911284551352340\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332\n", - "\n", - "x12\n", - "\n", - "\n", - "\n", - "var8646911284551352332->var8646911284551352322\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332->var8646911284551352326\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332->var8646911284551352327\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332->var8646911284551352330\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332->var8646911284551352331\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332->var8646911284551352333\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332->var8646911284551352336\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352332->var8646911284551352337\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342\n", - "\n", - "x22\n", - "\n", - "\n", + "\n", "\n", - "var8646911284551352332->var8646911284551352342\n", - "\n", - "\n", + "8->9\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352323\n", - "\n", - "\n", + "\n", + "\n", + "10\n", + "\n", + "x31 : x21, x22, x33\n", "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352328\n", - "\n", - "\n", + "\n", + "\n", + "8->10\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352329\n", - "\n", - "\n", + "\n", + "\n", + "12\n", + "\n", + "x13 : x12, x22, x33\n", "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352334\n", - "\n", - "\n", + "\n", + "\n", + "8->12\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352338\n", - "\n", - "x18\n", + "\n", + "\n", + "11\n", + "\n", + "x32 : x22, x31, x33\n", "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352338\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352339\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352343\n", - "\n", - "x23\n", - "\n", - "\n", - "\n", - "var8646911284551352333->var8646911284551352343\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336->var8646911284551352331\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336->var8646911284551352335\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352336->var8646911284551352340\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352341\n", - "\n", - "x21\n", - "\n", - "\n", - "\n", - "var8646911284551352336->var8646911284551352341\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352326\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352327\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352329\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352330\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352333\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352336\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352338\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352339\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352337->var8646911284551352343\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339->var8646911284551352334\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339->var8646911284551352338\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352339->var8646911284551352343\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352344\n", - "\n", - "x24\n", - "\n", - "\n", - "\n", - "var8646911284551352339->var8646911284551352344\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352340->var8646911284551352335\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352340->var8646911284551352341\n", - "\n", - "\n", - "\n", - "\n", + "\n", "\n", - "var8646911284551352342->var8646911284551352322\n", - "\n", - "\n", + "10->11\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352326\n", - "\n", - "\n", + "\n", + "\n", + "13\n", + "\n", + "x23, x24 : x13, x22, x33\n", "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352327\n", - "\n", - "\n", + "\n", + "\n", + "12->13\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352329\n", - "\n", - "\n", + "\n", + "\n", + "14\n", + "\n", + "x14 : x13, x24\n", "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352330\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352333\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352336\n", - "\n", - "\n", - "\n", - "\n", + "\n", "\n", - "var8646911284551352342->var8646911284551352337\n", - "\n", - "\n", + "13->14\n", + "\n", + "\n", "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352339\n", - "\n", - "\n", + "\n", + "\n", + "15\n", + "\n", + "x34 : x24, x33\n", "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352340\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352341\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352342->var8646911284551352343\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352343->var8646911284551352338\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "var8646911284551352343->var8646911284551352344\n", - "\n", - "\n", + "\n", + "\n", + "13->15\n", + "\n", + "\n", "\n", "\n", "\n" ], "text/plain": [ - "" + "" ] }, "metadata": {}, @@ -2629,11 +713,11 @@ "metis_ordering.print()\n", "\n", "# Eliminate using METIS ordering\n", - "bayes_net_metis = grid_graph.eliminateSequential(metis_ordering)\n", + "bayes_tree_metis = grid_graph.eliminateMultifrontal(metis_ordering)\n", "\n", "# Visualize the resulting Bayes Net\n", "print(\"\\nMETIS Bayes Net Structure:\")\n", - "display(graphviz.Source(bayes_net_metis.dot()))" + "display(graphviz.Source(bayes_tree_metis.dot()))" ] }, { @@ -2644,9 +728,9 @@ "source": [ "### Comparison\n", "\n", - "Observe the differences in the Bayes Net structures produced by COLAMD and METIS:\n", + "Observe the differences in the Bayes tree structures produced by COLAMD and METIS:\n", "\n", - "* **COLAMD:** Often produces a more 'stringy' or deeper elimination tree/Bayes Net. The cliques (conditionals in the Bayes Net) might be smaller initially but can grow larger towards the root (variables eliminated last).\n", + "* **COLAMD:** Often produces a more 'stringy' or deeper Bayes tree. The cliques (conditionals in the Bayes tree) might be smaller initially but can grow larger towards the root (variables eliminated last).\n", "* **METIS:** Tends to produce a more 'bushy' or balanced tree. It tries to partition the graph, eliminating variables within partitions first, leading to potentially larger initial cliques but often a shallower overall structure and smaller separators (variables connecting cliques high up in the tree). \n", "\n", "When should you choose one over the other? The best choice often depends on the specific problem structure and computational goals:\n", @@ -2657,7 +741,7 @@ " * The factor graph is relatively small or doesn't have a highly regular structure where complex partitioning would yield significant benefits.\n", "\n", "* **Use METIS when:**\n", - " * You are aiming for maximum performance with a *parallel* solver (e.g., using GTSAM's multifrontal solvers with TBB). METIS's graph partitioning approach tends to create more balanced elimination trees, which allows for better workload distribution across multiple CPU cores.\n", + " * You are aiming for maximum performance with a *parallel* solver (e.g., using GTSAM's multifrontal solvers with TBB). METIS's graph partitioning approach tends to create more balanced Bayes trees, which allows for better workload distribution across multiple CPU cores.\n", " * You are dealing with very large-scale problems, especially those with a regular structure (like large grids, meshes from finite element analysis, or extensive SLAM maps). On such problems, METIS can sometimes find an ordering with significantly less *total fill-in* than COLAMD, leading to faster factorization, even if computing the ordering itself takes longer.\n", " * The cost of computing the ordering is negligible compared to the cost of the subsequent factorization and solve steps (e.g., you compute the ordering once for a structure that is solved repeatedly).\n", "\n", @@ -2679,7 +763,7 @@ }, { "cell_type": "code", - "execution_count": 51, + "execution_count": 28, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -2692,10 +776,9 @@ "name": "stdout", "output_type": "stream", "text": [ - "Constrained COLAMD (x0, x24 last):\n", - "Position 0: x5, x1, x6, x23, x20, x21, x15, x16, x22, x12\n", - "Position 10: x19, x4, x9, x3, x8, x14, x13, x18, x17, x2\n", - "Position 20: x7, x10, x11, x24, x0\n" + "Constrained COLAMD (x11, x34 last):\n", + "Position 0: x31, x32, x21, x14, x24, x13, x23, x33, x22, x12\n", + "Position 10: x34, x11\n" ] } ], @@ -2721,7 +804,7 @@ }, { "cell_type": "code", - "execution_count": 52, + "execution_count": 29, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -2734,8 +817,8 @@ "name": "stdout", "output_type": "stream", "text": [ - "COLAMD Ordering size: 25\n", - "Key at position 0 (COLAMD): x0\n" + "COLAMD Ordering size: 12\n", + "Key at position 0 (COLAMD): x11\n" ] } ], @@ -2760,7 +843,7 @@ }, { "cell_type": "code", - "execution_count": 53, + "execution_count": 30, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -2773,9 +856,8 @@ "name": "stdout", "output_type": "stream", "text": [ - "Appended Ordering: Position 0: x0, x5, x1, x6, x4, x9, x3, x8, x12, x24\n", - "Position 10: x23, x19, x18, x20, x21, x15, x16, x22, x17, x14\n", - "Position 20: x13, x10, x11, x2, x7, l0, x25\n" + "Appended Ordering: Position 0: x11, x31, x14, x34, x33, x24, x23, x32, x13, x22\n", + "Position 10: x21, x12, l0, x12\n" ] } ], @@ -2791,7 +873,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -2805,7 +887,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, From 2f0bff14b78c245fa7eca3e070c3bd901a79f291 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 17 Apr 2025 10:28:17 -0400 Subject: [PATCH 10/17] Ordering minor fixes --- gtsam/inference/doc/Ordering.ipynb | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/doc/Ordering.ipynb b/gtsam/inference/doc/Ordering.ipynb index 9f4a595ed..e021677e8 100644 --- a/gtsam/inference/doc/Ordering.ipynb +++ b/gtsam/inference/doc/Ordering.ipynb @@ -17,7 +17,7 @@ "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, such as COLAMD and METIS (if available), or allows you to specify a custom ordering." + "GTSAM provides several algorithms to compute good orderings automatically, such as COLAMD and METIS, or allows you to specify a custom ordering." ] }, { @@ -40,7 +40,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { @@ -139,7 +139,7 @@ "GTSAM provides algorithms to automatically compute an elimination ordering from a factor graph. Two common algorithms are:\n", "\n", "1. **COLAMD (Column Approximate Minimum Degree):** A greedy algorithm that aims to minimize *fill-in* at each elimination step. It typically produces orderings that are good for sparse direct methods executed sequentially.\n", - "2. **METIS:** A graph partitioning algorithm (requires METIS library to be installed when compiling GTSAM). It aims to find orderings that partition the graph well, often leading to more balanced elimination trees which can be beneficial for parallel computation and sometimes reduce overall fill-in compared to purely local greedy methods like COLAMD, especially on large, structured problems.\n", + "2. **METIS:** A graph partitioning algorithm. It aims to find orderings that partition the graph well, often leading to more balanced elimination trees which can be beneficial for parallel computation and sometimes reduce overall fill-in compared to purely local greedy methods like COLAMD, especially on large, structured problems.\n", "\n", "Let's illustrate the difference using a 2D grid factor graph." ] @@ -391,7 +391,7 @@ "source": [ "### COLAMD Ordering and Resulting Bayes Net\n", "\n", - "Now, we compute the COLAMD ordering and eliminate the variables according to this order. We then visualize the resulting Symbolic Bayes Net. The structure of the Bayes Net (specifically, the cliques formed by the conditional dependencies) reflects the structure of the elimination tree (or Bayes Tree)." + "Now, we compute the COLAMD ordering and eliminate the variables according to this order." ] }, { From 515127cbe90148e62b660f7e4ae7e679e4182272 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 17 Apr 2025 10:28:41 -0400 Subject: [PATCH 11/17] %pip install --quiet gtsam-develop --- gtsam/geometry/doc/Pose2.ipynb | 2 +- gtsam/geometry/doc/Pose3.ipynb | 2 +- gtsam/geometry/doc/Rot2.ipynb | 408 +++++----- gtsam/geometry/doc/Rot3.ipynb | 896 +++++++++++----------- gtsam/inference/doc/BayesNet.ipynb | 2 +- gtsam/inference/doc/BayesTree.ipynb | 2 +- gtsam/inference/doc/Conditional.ipynb | 2 +- gtsam/inference/doc/DotWriter.ipynb | 2 +- gtsam/inference/doc/EdgeKey.ipynb | 2 +- gtsam/inference/doc/EliminationTree.ipynb | 2 +- gtsam/inference/doc/Factor.ipynb | 2 +- gtsam/inference/doc/FactorGraph.ipynb | 2 +- gtsam/inference/doc/ISAM.ipynb | 2 +- gtsam/inference/doc/JunctionTree.ipynb | 2 +- gtsam/inference/doc/Key.ipynb | 2 +- gtsam/inference/doc/LabeledSymbol.ipynb | 2 +- gtsam/inference/doc/Symbol.ipynb | 2 +- gtsam/inference/doc/VariableIndex.ipynb | 2 +- gtsam/nonlinear/doc/CustomFactor.ipynb | 14 +- 19 files changed, 670 insertions(+), 680 deletions(-) diff --git a/gtsam/geometry/doc/Pose2.ipynb b/gtsam/geometry/doc/Pose2.ipynb index 660c78ac6..8dc7ffccc 100644 --- a/gtsam/geometry/doc/Pose2.ipynb +++ b/gtsam/geometry/doc/Pose2.ipynb @@ -39,7 +39,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/geometry/doc/Pose3.ipynb b/gtsam/geometry/doc/Pose3.ipynb index 25a45fafb..2a925fb6d 100644 --- a/gtsam/geometry/doc/Pose3.ipynb +++ b/gtsam/geometry/doc/Pose3.ipynb @@ -30,7 +30,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/geometry/doc/Rot2.ipynb b/gtsam/geometry/doc/Rot2.ipynb index 610727090..f4eb642a2 100644 --- a/gtsam/geometry/doc/Rot2.ipynb +++ b/gtsam/geometry/doc/Rot2.ipynb @@ -1,45 +1,31 @@ { - "nbformat": 4, - "nbformat_minor": 0, - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "name": "python3", - "display_name": "Python 3" - }, - "language_info": { - "name": "python" - } - }, "cells": [ { "cell_type": "markdown", - "source": [ - "# Rot2" - ], "metadata": { "id": "-3NPWeM5nKTz" - } + }, + "source": [ + "# Rot2" + ] }, { "cell_type": "markdown", - "source": [ - "A `gtsam.Rot2` represents rotation in 2D space. It models a 2D rotation in the Special Orthogonal Group $\\text{SO}(2)$." - ], "metadata": { "id": "zKQLwRQWvRAW" - } + }, + "source": [ + "A `gtsam.Rot2` represents rotation in 2D space. It models a 2D rotation in the Special Orthogonal Group $\\text{SO}(2)$." + ] }, { "cell_type": "markdown", - "source": [ - "\"Open" - ], "metadata": { "id": "1MUA6xip5fG4" - } + }, + "source": [ + "\"Open" + ] }, { "cell_type": "code", @@ -49,41 +35,70 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { "cell_type": "code", - "source": [ - "from gtsam import Rot2, Point2\n", - "import numpy as np" - ], + "execution_count": 6, "metadata": { "id": "-dp28DoR7WsD" }, - "execution_count": 6, - "outputs": [] + "outputs": [], + "source": [ + "from gtsam import Rot2, Point2\n", + "import numpy as np" + ] }, { "cell_type": "markdown", - "source": [ - "## Initialization and properties" - ], "metadata": { "id": "gZRXZTrJ7mqJ" - } + }, + "source": [ + "## Initialization and properties" + ] }, { "cell_type": "markdown", - "source": [ - "A `Rot2` can be initialized with no arguments, which yields the identity rotation, or it can be constructed from an angle in radians, degrees, cos-sin form, or the bearing or arctangent of a 2D point. `Rot2` uses radians to communicate angle by default." - ], "metadata": { "id": "PK-HWTDm7sU4" - } + }, + "source": [ + "A `Rot2` can be initialized with no arguments, which yields the identity rotation, or it can be constructed from an angle in radians, degrees, cos-sin form, or the bearing or arctangent of a 2D point. `Rot2` uses radians to communicate angle by default." + ] }, { "cell_type": "code", + "execution_count": 58, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "oIakIOAB9afi", + "outputId": "c2cb005d-056f-4a4f-b5ff-2226be1ba3e7" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Identities:\n", + "0.0\n", + "0.0\n", + "Radians:\n", + "1.5707963267948966\n", + "1.5707963267948966\n", + "Degrees:\n", + "1.5707963267948966\n", + "Cos-Sin:\n", + "0.5235987755982988\n", + "Bearing:\n", + "0.7853981633974483\n", + "0.7853981633974483\n" + ] + } + ], "source": [ "# The identity rotation has theta = 0.\n", "identity = Rot2()\n", @@ -118,39 +133,13 @@ "# Or with atan2(y, x), which accomplishes the same thing.\n", "atan = Rot2.atan2(p[1], p[0])\n", "print(atan.theta())" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "oIakIOAB9afi", - "outputId": "c2cb005d-056f-4a4f-b5ff-2226be1ba3e7" - }, - "execution_count": 58, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "Identities:\n", - "0.0\n", - "0.0\n", - "Radians:\n", - "1.5707963267948966\n", - "1.5707963267948966\n", - "Degrees:\n", - "1.5707963267948966\n", - "Cos-Sin:\n", - "0.5235987755982988\n", - "Bearing:\n", - "0.7853981633974483\n", - "0.7853981633974483\n" - ] - } ] }, { "cell_type": "markdown", + "metadata": { + "id": "rHovUXbUys5r" + }, "source": [ "The following properties are available from the standard interface:\n", "- `theta()` (in radians)\n", @@ -161,25 +150,11 @@ "\\cos\\theta & -\\sin\\theta \\\\\n", "\\sin\\theta & \\cos\\theta\n", "\\end{bmatrix}$)" - ], - "metadata": { - "id": "rHovUXbUys5r" - } + ] }, { "cell_type": "code", - "source": [ - "example_rot = Rot2(3 * np.pi / 4)\n", - "\n", - "# The default print statement includes 'theta: ' and a newline at the end.\n", - "print(example_rot)\n", - "\n", - "print(f\"Radians: {example_rot.theta()}\")\n", - "print(f\"Degrees: {example_rot.degrees()}\")\n", - "print(f\"Cosine: {example_rot.c()}\")\n", - "print(f\"Sine: {example_rot.s()}\")\n", - "print(f\"Matrix:\\n{example_rot.matrix()}\")\n" - ], + "execution_count": 18, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -187,11 +162,10 @@ "id": "P5OXTjFu2DeX", "outputId": "70848419-c055-44bc-de11-08e8f93fe3bf" }, - "execution_count": 18, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "theta: 2.35619\n", "\n", @@ -204,28 +178,59 @@ " [ 0.70710678 -0.70710678]]\n" ] } + ], + "source": [ + "example_rot = Rot2(3 * np.pi / 4)\n", + "\n", + "# The default print statement includes 'theta: ' and a newline at the end.\n", + "print(example_rot)\n", + "\n", + "print(f\"Radians: {example_rot.theta()}\")\n", + "print(f\"Degrees: {example_rot.degrees()}\")\n", + "print(f\"Cosine: {example_rot.c()}\")\n", + "print(f\"Sine: {example_rot.s()}\")\n", + "print(f\"Matrix:\\n{example_rot.matrix()}\")\n" ] }, { "cell_type": "markdown", - "source": [ - "## Basic operations" - ], "metadata": { "id": "PpqHUDnl5rTW" - } + }, + "source": [ + "## Basic operations" + ] }, { "cell_type": "markdown", - "source": [ - "For basic use, a `Rot2` can rotate and unrotate a point." - ], "metadata": { "id": "sa4qx58n5tG9" - } + }, + "source": [ + "For basic use, a `Rot2` can rotate and unrotate a point." + ] }, { "cell_type": "code", + "execution_count": 25, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "yaBKjGn05_-c", + "outputId": "fb89fe09-b2b8-496d-e835-f379d197a4eb" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Rotated: [-2.82842712e+00 2.22044605e-16]\n", + "Unrotated: [-2. 2.]\n", + "Unrotated again: [-2.22044605e-16 2.82842712e+00]\n" + ] + } + ], "source": [ "rot = Rot2.fromDegrees(45)\n", "p = Point2(-2, 2)\n", @@ -237,38 +242,37 @@ "print(f\"Unrotated: {rot.unrotate(rotated)}\")\n", "# Of course, unrotating a point you didn't rotate just rotates it backwards.\n", "print(f\"Unrotated again: {rot.unrotate(p)}\")" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "yaBKjGn05_-c", - "outputId": "fb89fe09-b2b8-496d-e835-f379d197a4eb" - }, - "execution_count": 25, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "Rotated: [-2.82842712e+00 2.22044605e-16]\n", - "Unrotated: [-2. 2.]\n", - "Unrotated again: [-2.22044605e-16 2.82842712e+00]\n" - ] - } ] }, { "cell_type": "markdown", - "source": [ - "Also, the `equals()` function allows for comparison of two `Rot2` objects with a tolerance." - ], "metadata": { "id": "RVFCoBpW6Bvh" - } + }, + "source": [ + "Also, the `equals()` function allows for comparison of two `Rot2` objects with a tolerance." + ] }, { "cell_type": "code", + "execution_count": 31, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "m74YbK5h6CPU", + "outputId": "1b16695e-cdfe-4348-875f-c41d8b4a3b26" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "True\n", + "False\n" + ] + } + ], "source": [ "eq_rads = Rot2(np.pi / 4)\n", "eq_degs = Rot2.fromDegrees(45)\n", @@ -277,48 +281,60 @@ "\n", "# Direct comparison does not work for Rot2.\n", "print(eq_rads == eq_degs)" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "m74YbK5h6CPU", - "outputId": "1b16695e-cdfe-4348-875f-c41d8b4a3b26" - }, - "execution_count": 31, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "True\n", - "False\n" - ] - } ] }, { "cell_type": "markdown", - "source": [ - "## Lie group $\\text{SO}(2)$" - ], "metadata": { "id": "ko9KSZgd4bCp" - } + }, + "source": [ + "## Lie group $\\text{SO}(2)$" + ] }, { "cell_type": "markdown", + "metadata": { + "id": "76D2KkX241zX" + }, "source": [ "### Group operations\n", "\n", "`Rot2` implements the group operations `inverse`, `compose`, `between` and `identity`. For more information on groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)." - ], - "metadata": { - "id": "76D2KkX241zX" - } + ] }, { "cell_type": "code", + "execution_count": 57, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "QZ_NTeXK87Wq", + "outputId": "12af920d-8f86-473d-88ab-ee57d316cb72" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Inverse:\n", + "theta: -0.523599\n", + "\n", + "Compose:\n", + "theta: 1.5708\n", + "\n", + "theta: 1.5708\n", + "\n", + "Between:\n", + "theta: 0.523599\n", + "\n", + "Identity:\n", + "theta: 0\n", + "\n" + ] + } + ], "source": [ "a = Rot2(np.pi / 6)\n", "b = Rot2(np.pi / 3)\n", @@ -340,51 +356,44 @@ "# The identity is theta = 0, as above.\n", "print(\"Identity:\")\n", "print(Rot2.Identity())" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "QZ_NTeXK87Wq", - "outputId": "12af920d-8f86-473d-88ab-ee57d316cb72" - }, - "execution_count": 57, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "Inverse:\n", - "theta: -0.523599\n", - "\n", - "Compose:\n", - "theta: 1.5708\n", - "\n", - "theta: 1.5708\n", - "\n", - "Between:\n", - "theta: 0.523599\n", - "\n", - "Identity:\n", - "theta: 0\n", - "\n" - ] - } ] }, { "cell_type": "markdown", + "metadata": { + "id": "YfiYuVpL-cgq" + }, "source": [ "## Lie group operations\n", "\n", "`Rot2` implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)." - ], - "metadata": { - "id": "YfiYuVpL-cgq" - } + ] }, { "cell_type": "code", + "execution_count": 54, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "4JiDEGqG-van", + "outputId": "56047f8d-3349-4ee6-e73c-cd2fa1efb95d" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "theta: 1.5708\n", + "\n", + "theta: 3.14159\n", + "\n", + "[1.57079633]\n", + "[-0.78539816]\n", + "[-0.78539816]\n" + ] + } + ], "source": [ "r = Rot2(np.pi / 2)\n", "w = Rot2(np.pi / 4)\n", @@ -405,30 +414,21 @@ "# logmap is the same as calculating the coordinate of the second Rot2 in the\n", "# local frame of the first, which localCoordinates (inherited from LieGroup) does.\n", "print(r.localCoordinates(w))\n" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "4JiDEGqG-van", - "outputId": "56047f8d-3349-4ee6-e73c-cd2fa1efb95d" - }, - "execution_count": 54, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "theta: 1.5708\n", - "\n", - "theta: 3.14159\n", - "\n", - "[1.57079633]\n", - "[-0.78539816]\n", - "[-0.78539816]\n" - ] - } ] } - ] -} \ No newline at end of file + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/geometry/doc/Rot3.ipynb b/gtsam/geometry/doc/Rot3.ipynb index 937679528..0eb21ddae 100644 --- a/gtsam/geometry/doc/Rot3.ipynb +++ b/gtsam/geometry/doc/Rot3.ipynb @@ -1,45 +1,31 @@ { - "nbformat": 4, - "nbformat_minor": 0, - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "name": "python3", - "display_name": "Python 3" - }, - "language_info": { - "name": "python" - } - }, "cells": [ { "cell_type": "markdown", - "source": [ - "# Rot3" - ], "metadata": { "id": "Wy0JIcGioHI9" - } + }, + "source": [ + "# Rot3" + ] }, { "cell_type": "markdown", - "source": [ - "A `gtsam.Rot3` represents an orientation or attitude in 3D space. It can be manipulated and presented as a rotation matrix $ R \\in \\mathbb{R}^{3 \\times 3} $, a unit quaternion, roll-pitch-yaw (Euler) angles $ (\\phi, \\theta, \\psi) $, or as an axis-angle representation $ (\\hat{\\omega}, \\theta) $ with $ \\hat{\\omega} \\in \\mathbb{R}^3 $ and $ \\theta \\in \\mathbb{R} $. It models a 3D orientation as both a manifold in $ \\mathcal{SO}(3) $ and as a Lie group in $ \\text{SO}(3) $. Internally, it is stored as a $ 3 \\times 3 $ rotation matrix but can be configured to use quaternions at build time for efficiency." - ], "metadata": { "id": "YqaxPKyloJG_" - } + }, + "source": [ + "A `gtsam.Rot3` represents an orientation or attitude in 3D space. It can be manipulated and presented as a rotation matrix $ R \\in \\mathbb{R}^{3 \\times 3} $, a unit quaternion, roll-pitch-yaw (Euler) angles $ (\\phi, \\theta, \\psi) $, or as an axis-angle representation $ (\\hat{\\omega}, \\theta) $ with $ \\hat{\\omega} \\in \\mathbb{R}^3 $ and $ \\theta \\in \\mathbb{R} $. It models a 3D orientation as both a manifold in $ \\mathcal{SO}(3) $ and as a Lie group in $ \\text{SO}(3) $. Internally, it is stored as a $ 3 \\times 3 $ rotation matrix but can be configured to use quaternions at build time for efficiency." + ] }, { "cell_type": "markdown", - "source": [ - "\"Open" - ], "metadata": { "id": "Hmwbhz75pcQT" - } + }, + "source": [ + "\"Open" + ] }, { "cell_type": "code", @@ -49,79 +35,54 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "_fWy46Mepoxh" + }, + "outputs": [], "source": [ "import gtsam\n", "from gtsam import Rot3, Point3, Quaternion\n", "import numpy as np" - ], - "metadata": { - "id": "_fWy46Mepoxh" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", - "source": [ - "## Initialization" - ], "metadata": { "id": "3DkkBKyAqGnY" - } + }, + "source": [ + "## Initialization" + ] }, { "cell_type": "markdown", - "source": [ - "A `Rot3` can be initialized in many different ways, which are detailed in this section. Note that printing a `Rot3` displays its 3x3 rotation matrix representation, which in general is a 3x3 matrix where the columns are unit vectors that define the orientation's coordinate frame." - ], "metadata": { "id": "RrJ5ZEdhqJPU" - } + }, + "source": [ + "A `Rot3` can be initialized in many different ways, which are detailed in this section. Note that printing a `Rot3` displays its 3x3 rotation matrix representation, which in general is a 3x3 matrix where the columns are unit vectors that define the orientation's coordinate frame." + ] }, { "cell_type": "markdown", + "metadata": { + "id": "AqEy5JLe5X1t" + }, "source": [ "### Constructor\n", "\n", "The `Rot3` constructor provides for initialization with no arguments, yielding the identity rotation (equivalent to $I_3$), initialization with a precalculated rotation matrix (either as a 3x3 `np.ndarray`, as three 3-vectors, or as 9 floats), and initialization with a quaternion's $w, x, y, z$." - ], - "metadata": { - "id": "AqEy5JLe5X1t" - } + ] }, { "cell_type": "code", - "source": [ - "# No-argument constructor\n", - "a = Rot3()\n", - "print(a)\n", - "\n", - "# Construct from a rotation matrix\n", - "theta = np.pi / 2\n", - "b = Rot3(np.array([ # Rotate around X axis by PI / 2\n", - " [1, 0, 0],\n", - " [0, np.cos(theta), -np.sin(theta)],\n", - " [0, np.sin(theta), np.cos(theta)]\n", - "]))\n", - "print(b)\n", - "\n", - "# Construct from three column vectors\n", - "c = Rot3([11, 21, 31], [12, 22, 32], [13, 23, 33])\n", - "print(c)\n", - "\n", - "# Construct from 9 floats\n", - "d = Rot3(1, 2, 3, 4, 5, 6, 7, 8, 9)\n", - "print(d)\n", - "\n", - "# Construct from quaternion values\n", - "e = Rot3(0, 0, 0, 1) # Rotate around Z axis by pi\n", - "print(e)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -129,11 +90,10 @@ "id": "mj8X-wIdq6GR", "outputId": "48a6921d-df39-4fd8-aaf8-76d4bcdb70b1" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -167,33 +127,57 @@ "\n" ] } + ], + "source": [ + "# No-argument constructor\n", + "a = Rot3()\n", + "print(a)\n", + "\n", + "# Construct from a rotation matrix\n", + "theta = np.pi / 2\n", + "b = Rot3(np.array([ # Rotate around X axis by PI / 2\n", + " [1, 0, 0],\n", + " [0, np.cos(theta), -np.sin(theta)],\n", + " [0, np.sin(theta), np.cos(theta)]\n", + "]))\n", + "print(b)\n", + "\n", + "# Construct from three column vectors\n", + "c = Rot3([11, 21, 31], [12, 22, 32], [13, 23, 33])\n", + "print(c)\n", + "\n", + "# Construct from 9 floats\n", + "d = Rot3(1, 2, 3, 4, 5, 6, 7, 8, 9)\n", + "print(d)\n", + "\n", + "# Construct from quaternion values\n", + "e = Rot3(0, 0, 0, 1) # Rotate around Z axis by pi\n", + "print(e)" ] }, { "cell_type": "markdown", + "metadata": { + "id": "EMaB3yVoJ_qv" + }, "source": [ "### Named constructors\n", "\n", "In addition to its constructors, `Rot3` has several named constructors, or factory functions, that allow instantiation from a wide variety of methods." - ], - "metadata": { - "id": "EMaB3yVoJ_qv" - } + ] }, { "cell_type": "markdown", - "source": [ - "`Rot3.Identity()` returns the 3x3 rotation identity matrix." - ], "metadata": { "id": "3s9Ym_6BaE_r" - } + }, + "source": [ + "`Rot3.Identity()` returns the 3x3 rotation identity matrix." + ] }, { "cell_type": "code", - "source": [ - "print(Rot3.Identity())" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -201,11 +185,10 @@ "id": "GcAB8GtVaLjK", "outputId": "b9e701cd-6a3f-4171-a518-158a2f7b60fd" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -215,19 +198,68 @@ "\n" ] } + ], + "source": [ + "print(Rot3.Identity())" ] }, { "cell_type": "markdown", - "source": [ - "`Rx`, `Ry`, `Rz`, and `RzRyRx` create rotations around these axes." - ], "metadata": { "id": "F2MXz29VXqLR" - } + }, + "source": [ + "`Rx`, `Ry`, `Rz`, and `RzRyRx` create rotations around these axes." + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "w-qh5dX6VWAW", + "outputId": "8fe29ae6-eb47-4460-c27b-3acd8ee5e5bf" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 6.12323e-17, -1;\n", + "\t0, 1, 6.12323e-17\n", + "]\n", + "\n", + "R: [\n", + "\t0.707107, 0, 0.707107;\n", + "\t0, 1, 0;\n", + "\t-0.707107, 0, 0.707107\n", + "]\n", + "\n", + "R: [\n", + "\t0.866025, -0.5, 0;\n", + "\t0.5, 0.866025, 0;\n", + "\t0, 0, 1\n", + "]\n", + "\n", + "R: [\n", + "\t0.612372, 0.612372, 0.5;\n", + "\t0.353553, 0.353553, -0.866025;\n", + "\t-0.707107, 0.707107, 4.32978e-17\n", + "]\n", + "\n", + "R: [\n", + "\t0.612372, 0.612372, 0.5;\n", + "\t0.353553, 0.353553, -0.866025;\n", + "\t-0.707107, 0.707107, 4.32978e-17\n", + "]\n", + "\n" + ] + } + ], "source": [ "# Rotation around X axis\n", "x = Rot3.Rx(np.pi / 2)\n", @@ -250,24 +282,36 @@ "print(zyx)\n", "# Of course, zyx is the same as z * y * x, since we fed the same angles to each.\n", "print(z * y * x)" - ], + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "c-_H7XmUYAd_" + }, + "source": [ + "Similarly, `Yaw`, `Pitch`, `Roll`, and `Ypr` are available." + ] + }, + { + "cell_type": "code", + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" }, - "id": "w-qh5dX6VWAW", - "outputId": "8fe29ae6-eb47-4460-c27b-3acd8ee5e5bf" + "id": "bGEMGXkpYT9t", + "outputId": "31655b9f-045f-4b51-dff2-42de4382427f" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", - "\t1, 0, 0;\n", - "\t0, 6.12323e-17, -1;\n", - "\t0, 1, 6.12323e-17\n", + "\t0.866025, -0.5, 0;\n", + "\t0.5, 0.866025, 0;\n", + "\t0, 0, 1\n", "]\n", "\n", "R: [\n", @@ -277,15 +321,9 @@ "]\n", "\n", "R: [\n", - "\t0.866025, -0.5, 0;\n", - "\t0.5, 0.866025, 0;\n", - "\t0, 0, 1\n", - "]\n", - "\n", - "R: [\n", - "\t0.612372, 0.612372, 0.5;\n", - "\t0.353553, 0.353553, -0.866025;\n", - "\t-0.707107, 0.707107, 4.32978e-17\n", + "\t1, 0, 0;\n", + "\t0, 6.12323e-17, -1;\n", + "\t0, 1, 6.12323e-17\n", "]\n", "\n", "R: [\n", @@ -296,19 +334,7 @@ "\n" ] } - ] - }, - { - "cell_type": "markdown", - "source": [ - "Similarly, `Yaw`, `Pitch`, `Roll`, and `Ypr` are available." ], - "metadata": { - "id": "c-_H7XmUYAd_" - } - }, - { - "cell_type": "code", "source": [ "# Yaw around Z axis (positive yaw is to the right, as in aircraft heading)\n", "y = Rot3.Yaw(np.pi / 6)\n", @@ -327,65 +353,20 @@ "# Ypr is not overloaded to support an array.\n", "ypr = Rot3.Ypr(np.pi / 6, np.pi / 4, np.pi / 2)\n", "print(ypr)" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "bGEMGXkpYT9t", - "outputId": "31655b9f-045f-4b51-dff2-42de4382427f" - }, - "execution_count": null, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "R: [\n", - "\t0.866025, -0.5, 0;\n", - "\t0.5, 0.866025, 0;\n", - "\t0, 0, 1\n", - "]\n", - "\n", - "R: [\n", - "\t0.707107, 0, 0.707107;\n", - "\t0, 1, 0;\n", - "\t-0.707107, 0, 0.707107\n", - "]\n", - "\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 6.12323e-17, -1;\n", - "\t0, 1, 6.12323e-17\n", - "]\n", - "\n", - "R: [\n", - "\t0.612372, 0.612372, 0.5;\n", - "\t0.353553, 0.353553, -0.866025;\n", - "\t-0.707107, 0.707107, 4.32978e-17\n", - "]\n", - "\n" - ] - } ] }, { "cell_type": "markdown", - "source": [ - "`Rot3.Quaternion` is identical to the four-argument `Rot3` constructor." - ], "metadata": { "id": "_ks-ohhZZ5Ap" - } + }, + "source": [ + "`Rot3.Quaternion` is identical to the four-argument `Rot3` constructor." + ] }, { "cell_type": "code", - "source": [ - "# Create from quaternion w, x, y, z\n", - "q = Rot3.Quaternion(0, 0, 0, 1)\n", - "print(q)\n", - "print(q.equals(Rot3(0, 0, 0, 1), 1e-8))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -393,11 +374,10 @@ "id": "uO9hb2RBaG3g", "outputId": "5409ef2e-3651-439b-af9f-6ed7888aa9d5" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t-1, 0, 0;\n", @@ -408,23 +388,26 @@ "True\n" ] } + ], + "source": [ + "# Create from quaternion w, x, y, z\n", + "q = Rot3.Quaternion(0, 0, 0, 1)\n", + "print(q)\n", + "print(q.equals(Rot3(0, 0, 0, 1), 1e-8))" ] }, { "cell_type": "markdown", - "source": [ - "`Rot3.AxisAngle` creates a `Rot3` from an axis and an angle around that axis." - ], "metadata": { "id": "jy7dn6_vabsK" - } + }, + "source": [ + "`Rot3.AxisAngle` creates a `Rot3` from an axis and an angle around that axis." + ] }, { "cell_type": "code", - "source": [ - "aa = Rot3.AxisAngle([0, 1, 0], np.pi / 2)\n", - "print(aa)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -432,11 +415,10 @@ "id": "M_OOSKgAaqhF", "outputId": "cbb875b7-9204-4a90-c225-dbea46718c6f" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t2.22045e-16, 0, 1;\n", @@ -446,25 +428,24 @@ "\n" ] } + ], + "source": [ + "aa = Rot3.AxisAngle([0, 1, 0], np.pi / 2)\n", + "print(aa)" ] }, { "cell_type": "markdown", - "source": [ - "`Rot3.Rodrigues` creates a `Rot3` from incremental roll, pitch, and yaw values. It is identical to the exponential map at identity." - ], "metadata": { "id": "ruyunRlUclFX" - } + }, + "source": [ + "`Rot3.Rodrigues` creates a `Rot3` from incremental roll, pitch, and yaw values. It is identical to the exponential map at identity." + ] }, { "cell_type": "code", - "source": [ - "rod = Rot3.Rodrigues(np.pi / 6, np.pi / 4, np.pi / 2)\n", - "# Rodrigues is overloaded to support an array.\n", - "# e.g. Rot3.Rodrigues([np.pi / 6, np.pi / 4, np.pi / 2])\n", - "print(rod)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -472,11 +453,10 @@ "id": "NUgeRQzIcqYp", "outputId": "8c189748-267b-4c25-e0cf-4eed8721bc4a" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t-0.156058, -0.673795, 0.72225;\n", @@ -486,31 +466,30 @@ "\n" ] } + ], + "source": [ + "rod = Rot3.Rodrigues(np.pi / 6, np.pi / 4, np.pi / 2)\n", + "# Rodrigues is overloaded to support an array.\n", + "# e.g. Rot3.Rodrigues([np.pi / 6, np.pi / 4, np.pi / 2])\n", + "print(rod)" ] }, { "cell_type": "markdown", + "metadata": { + "id": "INihgtF6fI22" + }, "source": [ "`Rot3.ClosestTo` finds the closest valid `Rot3` to the input matrix which minimizes the Frobenius norm. The Frobenius norm is a measure of matrix difference:\n", "\n", "$$\n", "||A - B||_F = \\sqrt{\\sum_{i,j} (A_{ij} - B_{ij})^2}\n", "$$" - ], - "metadata": { - "id": "INihgtF6fI22" - } + ] }, { "cell_type": "code", - "source": [ - "closest = Rot3.ClosestTo([\n", - " [1, 0, 0],\n", - " [0, 2, 0],\n", - " [0, 0, 3]\n", - "])\n", - "print(closest)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -518,11 +497,10 @@ "id": "EFMbwiTKfLfJ", "outputId": "72bf7784-6a8c-4052-c444-d85d6d9014e7" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -532,19 +510,30 @@ "\n" ] } + ], + "source": [ + "closest = Rot3.ClosestTo([\n", + " [1, 0, 0],\n", + " [0, 2, 0],\n", + " [0, 0, 3]\n", + "])\n", + "print(closest)" ] }, { "cell_type": "markdown", - "source": [ - "## Properties" - ], "metadata": { "id": "Sm3oUTObqxJl" - } + }, + "source": [ + "## Properties" + ] }, { "cell_type": "markdown", + "metadata": { + "id": "0Gax04WXqyeE" + }, "source": [ "The following properties are available from the standard interface:\n", "- `matrix()`: Returns the 3x3 rotation matrix.\n", @@ -559,31 +548,11 @@ "- `toQuaternion()`: Returns the quaternion representation. The quaternion's attributes can then be accessed either individually with `w()`, `x()`, `y()`, `z()` or together with `coeffs()`.\n", "\n", "Note that accessing `roll()`, `pitch()`, and `yaw()` separately is less efficient than calling `rpy()` or `ypr()`." - ], - "metadata": { - "id": "0Gax04WXqyeE" - } + ] }, { "cell_type": "code", - "source": [ - "props = Rot3.RzRyRx(0, np.pi / 6, np.pi / 2)\n", - "\n", - "print(\"Matrix:\\n\", props.matrix())\n", - "print(\"Transpose:\\n\", props.transpose())\n", - "print()\n", - "print(\"x, y, z:\", props.xyz())\n", - "print(\"y, p, r:\", props.ypr())\n", - "print(\"r, p, y:\", props.rpy())\n", - "print()\n", - "print(\"Roll: \", props.roll())\n", - "print(\"Pitch: \", props.pitch())\n", - "print(\"Yaw: \", props.yaw())\n", - "print()\n", - "print(\"Axis-angle:\\n\", props.axisAngle())\n", - "print()\n", - "print(\"Quaternion:\", props.toQuaternion().coeffs())" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -591,11 +560,10 @@ "id": "zbNPBHiwDAE2", "outputId": "716f9db1-f7c7-4418-f7c7-5c6bae0b6a2c" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "Matrix:\n", " [[ 5.30287619e-17 -1.00000000e+00 3.06161700e-17]\n", @@ -623,28 +591,65 @@ "Quaternion: [-0.1830127 0.1830127 0.6830127 0.6830127]\n" ] } + ], + "source": [ + "props = Rot3.RzRyRx(0, np.pi / 6, np.pi / 2)\n", + "\n", + "print(\"Matrix:\\n\", props.matrix())\n", + "print(\"Transpose:\\n\", props.transpose())\n", + "print()\n", + "print(\"x, y, z:\", props.xyz())\n", + "print(\"y, p, r:\", props.ypr())\n", + "print(\"r, p, y:\", props.rpy())\n", + "print()\n", + "print(\"Roll: \", props.roll())\n", + "print(\"Pitch: \", props.pitch())\n", + "print(\"Yaw: \", props.yaw())\n", + "print()\n", + "print(\"Axis-angle:\\n\", props.axisAngle())\n", + "print()\n", + "print(\"Quaternion:\", props.toQuaternion().coeffs())" ] }, { "cell_type": "markdown", - "source": [ - "## Basic operations" - ], "metadata": { "id": "-XnTJ-psGeJf" - } + }, + "source": [ + "## Basic operations" + ] }, { "cell_type": "markdown", - "source": [ - "`Rot3` can rotate and unrotate a 3D point or vector. Rotation is calculated by the simple matrix product $Rx$, and unrotation by $R^{-1}x$." - ], "metadata": { "id": "gj3OlBlGGfjj" - } + }, + "source": [ + "`Rot3` can rotate and unrotate a 3D point or vector. Rotation is calculated by the simple matrix product $Rx$, and unrotation by $R^{-1}x$." + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "rtbkHyp3GgWx", + "outputId": "43b1178c-39d3-4df2-face-9e8cef162cdd" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[1.2246468e-16 2.0000000e+00 0.0000000e+00]\n", + "[2. 0. 0.]\n", + "[ 1.2246468e-16 -2.0000000e+00 0.0000000e+00]\n" + ] + } + ], "source": [ "z90 = Rot3.Rz(np.pi / 2)\n", "point = [2, 0, 0]\n", @@ -656,38 +661,39 @@ "print(z90.unrotate(rotated))\n", "# Rotate backwards by 90 degrees around the Z axis\n", "print(z90.unrotate(point))" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "rtbkHyp3GgWx", - "outputId": "43b1178c-39d3-4df2-face-9e8cef162cdd" - }, - "execution_count": null, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "[1.2246468e-16 2.0000000e+00 0.0000000e+00]\n", - "[2. 0. 0.]\n", - "[ 1.2246468e-16 -2.0000000e+00 0.0000000e+00]\n" - ] - } ] }, { "cell_type": "markdown", - "source": [ - "Check whether two `Rot3` instances are equal within a certain tolerance using `equals()`. Be careful with the `==` operator; it does not compare rotational equivalence, it compares object reference. If you wish to use more fine-grained equality comparison, convert to `np.ndarray` with `matrix()`." - ], "metadata": { "id": "d0bQ-tmwHmZ5" - } + }, + "source": [ + "Check whether two `Rot3` instances are equal within a certain tolerance using `equals()`. Be careful with the `==` operator; it does not compare rotational equivalence, it compares object reference. If you wish to use more fine-grained equality comparison, convert to `np.ndarray` with `matrix()`." + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "MYiKxq4vItz3", + "outputId": "e8f8fdd0-c539-476f-f233-2f78f98ca671" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "xyz.equals(ypr, 1e-8): True\n", + "xyz == ypr: False\n", + "xyz == xyz: True\n", + "xyz.matrix() == ypr.matrix(): True\n" + ] + } + ], "source": [ "xyz = Rot3.RzRyRx(np.pi / 2, np.pi / 4, np.pi / 6)\n", "ypr = Rot3.Ypr(np.pi / 6, np.pi / 4, np.pi / 2)\n", @@ -696,30 +702,13 @@ "print(\"xyz == ypr:\", xyz == ypr)\n", "print(\"xyz == xyz:\", xyz == xyz)\n", "print(\"xyz.matrix() == ypr.matrix():\", np.all(xyz.matrix() == ypr.matrix()))" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "MYiKxq4vItz3", - "outputId": "e8f8fdd0-c539-476f-f233-2f78f98ca671" - }, - "execution_count": null, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "xyz.equals(ypr, 1e-8): True\n", - "xyz == ypr: False\n", - "xyz == xyz: True\n", - "xyz.matrix() == ypr.matrix(): True\n" - ] - } ] }, { "cell_type": "markdown", + "metadata": { + "id": "bQzlRJ_2HWNz" + }, "source": [ "Use SLERP (spherical linear interpolation) to interpolate between two `Rot3` instances. In terms of the Lie algebra (see below), SLERP can be calculated by scaling the log mapped relative rotation by the interpolation term $t$, then converting back to $\\text{SO}(3)$ using the exponential map. The formula is thus:\n", "\n", @@ -728,19 +717,11 @@ "$$\n", "\n", "where $R_1$ and $R_2$ are the start `Rot3` and end `Rot3` of the interpolation and $t$ is the interpolation term, usually but not necessarily in the range $[0, 1]$." - ], - "metadata": { - "id": "bQzlRJ_2HWNz" - } + ] }, { "cell_type": "code", - "source": [ - "a = Rot3.RzRyRx(0, np.pi / 4, 0)\n", - "b = Rot3.RzRyRx(np.pi / 6, 0, 0)\n", - "\n", - "print(a.slerp(0.5, b))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -748,11 +729,10 @@ "id": "y45qZPivHkRR", "outputId": "e2320424-004f-47bf-e844-bf04df63a916" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.922613, 0.0523387, 0.382159;\n", @@ -762,36 +742,37 @@ "\n" ] } + ], + "source": [ + "a = Rot3.RzRyRx(0, np.pi / 4, 0)\n", + "b = Rot3.RzRyRx(np.pi / 6, 0, 0)\n", + "\n", + "print(a.slerp(0.5, b))" ] }, { "cell_type": "markdown", - "source": [ - "## Lie group $\\text{SO}(3)$" - ], "metadata": { "id": "XlmNuuxSGgoj" - } + }, + "source": [ + "## Lie group $\\text{SO}(3)$" + ] }, { "cell_type": "markdown", + "metadata": { + "id": "XtUmE-QSGsh9" + }, "source": [ "### Group operations\n", "\n", "`Rot3` implements the group operations `inverse`, `compose`, `between` and `identity`. For more information on groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)." - ], - "metadata": { - "id": "XtUmE-QSGsh9" - } + ] }, { "cell_type": "code", - "source": [ - "a = Rot3.Rz(np.pi / 4)\n", - "b = Rot3.Roll(np.pi / 2)\n", - "\n", - "print(\"a:\\n\", a.matrix(), \"\\nb:\\n\", b.matrix())" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -799,11 +780,10 @@ "id": "axvFPtxYdGru", "outputId": "977f9582-5b23-43c7-a6f5-a7bfce6d5cea" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "a:\n", " [[ 0.70710678 -0.70710678 0. ]\n", @@ -815,24 +795,26 @@ " [ 0.000000e+00 1.000000e+00 6.123234e-17]]\n" ] } + ], + "source": [ + "a = Rot3.Rz(np.pi / 4)\n", + "b = Rot3.Roll(np.pi / 2)\n", + "\n", + "print(\"a:\\n\", a.matrix(), \"\\nb:\\n\", b.matrix())" ] }, { "cell_type": "markdown", - "source": [ - "The inverse of an $\\text{SO}(3)$ rotation matrix is the same as its transpose." - ], "metadata": { "id": "3eH9K5VH9jTb" - } + }, + "source": [ + "The inverse of an $\\text{SO}(3)$ rotation matrix is the same as its transpose." + ] }, { "cell_type": "code", - "source": [ - "print(a.inverse())\n", - "# The inverse is the same as the transpose.\n", - "print(a.inverse().equals(Rot3(a.transpose()), 1e-8))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -840,11 +822,10 @@ "id": "ffVBzuOhGugd", "outputId": "ff207bed-850c-422a-d9a6-a0b59e801989" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.707107, 0.707107, 0;\n", @@ -855,28 +836,25 @@ "True\n" ] } + ], + "source": [ + "print(a.inverse())\n", + "# The inverse is the same as the transpose.\n", + "print(a.inverse().equals(Rot3(a.transpose()), 1e-8))" ] }, { "cell_type": "markdown", - "source": [ - "The product of the composition operation $A * B$ is the rotation matrix which applies the rotation of $A$ and then the rotation of $B$. The composition of two rotation matrices is just the product of the two matrices." - ], "metadata": { "id": "P1bYYGkGdjxm" - } + }, + "source": [ + "The product of the composition operation $A * B$ is the rotation matrix which applies the rotation of $A$ and then the rotation of $B$. The composition of two rotation matrices is just the product of the two matrices." + ] }, { "cell_type": "code", - "source": [ - "print(a.compose(b))\n", - "\n", - "# The * operator is syntactic sugar for the compose operation.\n", - "print(a.compose(b).equals(a * b, 1e-8))\n", - "\n", - "# The composition of two rotation matrices is just the product of the matrices.\n", - "print(np.all(a.compose(b).matrix() == a.matrix() @ b.matrix()))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -884,11 +862,10 @@ "id": "4zXJJ77FdLBB", "outputId": "c5875121-0d97-475c-8669-93b92ac37c1b" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.707107, -4.32978e-17, 0.707107;\n", @@ -900,27 +877,33 @@ "True\n" ] } + ], + "source": [ + "print(a.compose(b))\n", + "\n", + "# The * operator is syntactic sugar for the compose operation.\n", + "print(a.compose(b).equals(a * b, 1e-8))\n", + "\n", + "# The composition of two rotation matrices is just the product of the matrices.\n", + "print(np.all(a.compose(b).matrix() == a.matrix() @ b.matrix()))" ] }, { "cell_type": "markdown", + "metadata": { + "id": "TIuwUygjfECu" + }, "source": [ "The between operation calculates the rotation from one `Rot3` to another. It is defined as simply:\n", "\n", "$$\n", "R_{relative} = R_1^{-1}R_2\n", "$$" - ], - "metadata": { - "id": "TIuwUygjfECu" - } + ] }, { "cell_type": "code", - "source": [ - "print(a.between(b))\n", - "print(a.between(b).equals(a.inverse() * b, 1e-8))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -928,11 +911,10 @@ "id": "_qGyDV15dgmU", "outputId": "be748925-3425-41c7-b06b-57ab87955699" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.707107, 4.32978e-17, -0.707107;\n", @@ -943,22 +925,24 @@ "True\n" ] } + ], + "source": [ + "print(a.between(b))\n", + "print(a.between(b).equals(a.inverse() * b, 1e-8))" ] }, { "cell_type": "markdown", - "source": [ - "The identity is $I_3$, as described above." - ], "metadata": { "id": "6_jR6zhMfa1l" - } + }, + "source": [ + "The identity is $I_3$, as described above." + ] }, { "cell_type": "code", - "source": [ - "print(Rot3.Identity())" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -966,11 +950,10 @@ "id": "SchtjDIPfXtb", "outputId": "7d199a1e-b9a7-4775-81e7-9c1328012b89" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -980,26 +963,25 @@ "\n" ] } + ], + "source": [ + "print(Rot3.Identity())" ] }, { "cell_type": "markdown", + "metadata": { + "id": "fjfUIqKHflXY" + }, "source": [ "#### Group operation invariants\n", "\n", "See that the following group invariants hold:" - ], - "metadata": { - "id": "fjfUIqKHflXY" - } + ] }, { "cell_type": "code", - "source": [ - "print(\"Compose(a, Inverse(a)) == Identity: \", (a * a.inverse()).equals(Rot3.Identity(), 1e-8))\n", - "print(\"Compose(a, Between(a, b)) == b:\", (a * a.between(b)).equals(b, 1e-8))\n", - "print(\"Between(a, b) == Compose(Inverse(a), b):\", a.between(b).equals(a.inverse() * b, 1e-8))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -1007,32 +989,39 @@ "id": "ysQSPxuwfnen", "outputId": "4a7d8404-fc2a-46ca-ba18-236fd417382b" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "Compose(a, Inverse(a)) == Identity: True\n", "Compose(a, Between(a, b)) == b: True\n", "Between(a, b) == Compose(Inverse(a), b): True\n" ] } + ], + "source": [ + "print(\"Compose(a, Inverse(a)) == Identity: \", (a * a.inverse()).equals(Rot3.Identity(), 1e-8))\n", + "print(\"Compose(a, Between(a, b)) == b:\", (a * a.between(b)).equals(b, 1e-8))\n", + "print(\"Between(a, b) == Compose(Inverse(a), b):\", a.between(b).equals(a.inverse() * b, 1e-8))" ] }, { "cell_type": "markdown", + "metadata": { + "id": "nKxTJy8YGuxg" + }, "source": [ "### Lie group operations\n", "\n", "`Rot3` implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)." - ], - "metadata": { - "id": "nKxTJy8YGuxg" - } + ] }, { "cell_type": "markdown", + "metadata": { + "id": "MmBfK0ad1KZ6" + }, "source": [ "The exponential map for $\\text{SO}(3)$ converts a 3D rotation vector (Lie algebra element in $\\mathfrak{so}(3)$) into a rotation matrix (Lie group element in $\\text{SO}(3)$). This is used to map a rotation vector $\\boldsymbol{\\omega} \\in \\mathbb{R}^3$ to a rotation matrix $R \\in \\text{SO}(3)$.\n", "\n", @@ -1079,38 +1068,22 @@ "$$\n", "\n", "since $ \\sin\\theta \\approx \\theta$ and $ 1 - \\cos\\theta \\approx \\frac{\\theta^2}{2} $." - ], - "metadata": { - "id": "MmBfK0ad1KZ6" - } + ] }, { "cell_type": "code", - "source": [ - "r1 = Rot3.RzRyRx(np.pi / 6, np.pi / 2, 0)\n", - "r2 = Rot3.RzRyRx(0, 0, np.pi / 4)\n", - "p1 = [np.pi / 2, 0, 0]\n", - "\n", - "# The exponential map at identity creates a rotation using Rodrigues' formula.\n", - "print(Rot3.Expmap(p1))\n", - "# The retract function takes the exponential map of the supplied vector and\n", - "# composes it with the calling Rot3. In other words, it maps from the tangent\n", - "# space to the manifold.\n", - "print(r1)\n", - "print(r1.retract(p1))" - ], + "execution_count": null, "metadata": { - "id": "yA5wO-5jGw2u", "colab": { "base_uri": "https://localhost:8080/" }, + "id": "yA5wO-5jGw2u", "outputId": "e0c75e07-2b6d-4f84-a90d-f1cceb3ad9fa" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -1132,10 +1105,26 @@ "\n" ] } + ], + "source": [ + "r1 = Rot3.RzRyRx(np.pi / 6, np.pi / 2, 0)\n", + "r2 = Rot3.RzRyRx(0, 0, np.pi / 4)\n", + "p1 = [np.pi / 2, 0, 0]\n", + "\n", + "# The exponential map at identity creates a rotation using Rodrigues' formula.\n", + "print(Rot3.Expmap(p1))\n", + "# The retract function takes the exponential map of the supplied vector and\n", + "# composes it with the calling Rot3. In other words, it maps from the tangent\n", + "# space to the manifold.\n", + "print(r1)\n", + "print(r1.retract(p1))" ] }, { "cell_type": "markdown", + "metadata": { + "id": "Yk2nazsK6ixV" + }, "source": [ "The logarithm map for $ \\text{SO}(3) $ is the inverse of the exponential map It converts a rotation matrix $ R \\in SO(3) $ into a 3D rotation vector (a Lie algebra element in $ \\mathfrak{so}(3) $).\n", "\n", @@ -1183,13 +1172,29 @@ "$$\n", "\n", "where $ R_{ij} $ are the elements of $ R $." - ], - "metadata": { - "id": "Yk2nazsK6ixV" - } + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "0V2oQQ0lxS2-", + "outputId": "62b40acb-799e-4a91-dacd-c9e0266665c3" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[ 0.41038024 1.53155991 -0.41038024]\n", + "[-1.01420581 -1.32173874 1.01420581]\n", + "[-1.01420581 -1.32173874 1.01420581]\n" + ] + } + ], "source": [ "# Calculate the log map of r at identity. Returns the coordinates of the rotation\n", "# in the tangent space.\n", @@ -1200,37 +1205,11 @@ "# logmap is the same as calculating the coordinate of the second Rot3 in the\n", "# local frame of the first, which localCoordinates (inherited from LieGroup) does.\n", "print(r1.localCoordinates(r2))" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "0V2oQQ0lxS2-", - "outputId": "62b40acb-799e-4a91-dacd-c9e0266665c3" - }, - "execution_count": null, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "[ 0.41038024 1.53155991 -0.41038024]\n", - "[-1.01420581 -1.32173874 1.01420581]\n", - "[-1.01420581 -1.32173874 1.01420581]\n" - ] - } ] }, { "cell_type": "code", - "source": [ - "# Applying localCoordinates and then retract cancels out, returning r2 given any\n", - "# r1. This is because it transforms r2 from the manifold to the tangent space\n", - "# using the log map, then transforms that result back into the manifold using\n", - "# the exponential map.\n", - "print(r2)\n", - "print(r1.retract(r1.localCoordinates(r2)))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -1238,11 +1217,10 @@ "id": "-kTgSGJS06EC", "outputId": "97051c49-284e-4ee8-d806-53f0d842fc31" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.707107, -0.707107, 0;\n", @@ -1258,34 +1236,30 @@ "\n" ] } + ], + "source": [ + "# Applying localCoordinates and then retract cancels out, returning r2 given any\n", + "# r1. This is because it transforms r2 from the manifold to the tangent space\n", + "# using the log map, then transforms that result back into the manifold using\n", + "# the exponential map.\n", + "print(r2)\n", + "print(r1.retract(r1.localCoordinates(r2)))" ] }, { "cell_type": "markdown", + "metadata": { + "id": "s_qu2bGt1-vr" + }, "source": [ "## Serialization\n", "\n", "A `Rot3` can be serialized to a string for saving, then later used by deserializing the string." - ], - "metadata": { - "id": "s_qu2bGt1-vr" - } + ] }, { "cell_type": "code", - "source": [ - "a = Rot3.Rx(np.pi / 2)\n", - "print(\"Before serialization:\", a)\n", - "\n", - "str_val = a.serialize()\n", - "print(str_val)\n", - "print(\"The serialized value is a string:\", type(str_val))\n", - "# Save to file, etc...\n", - "\n", - "b = Rot3()\n", - "b.deserialize(str_val)\n", - "print(\"After deserialization:\", b)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -1293,11 +1267,10 @@ "id": "m6ku7L_768Ta", "outputId": "8cb6fe04-6759-4cd9-8145-42f4fc2a72dd" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "Before serialization: R: [\n", "\t1, 0, 0;\n", @@ -1317,7 +1290,34 @@ "\n" ] } + ], + "source": [ + "a = Rot3.Rx(np.pi / 2)\n", + "print(\"Before serialization:\", a)\n", + "\n", + "str_val = a.serialize()\n", + "print(str_val)\n", + "print(\"The serialized value is a string:\", type(str_val))\n", + "# Save to file, etc...\n", + "\n", + "b = Rot3()\n", + "b.deserialize(str_val)\n", + "print(\"After deserialization:\", b)" ] } - ] -} \ No newline at end of file + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/inference/doc/BayesNet.ipynb b/gtsam/inference/doc/BayesNet.ipynb index c379b32ce..d6c75e9d6 100644 --- a/gtsam/inference/doc/BayesNet.ipynb +++ b/gtsam/inference/doc/BayesNet.ipynb @@ -52,7 +52,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index a9452f4d4..324c8a56b 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -52,7 +52,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/Conditional.ipynb b/gtsam/inference/doc/Conditional.ipynb index 9f6a53665..c0fd371d3 100644 --- a/gtsam/inference/doc/Conditional.ipynb +++ b/gtsam/inference/doc/Conditional.ipynb @@ -63,7 +63,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/DotWriter.ipynb b/gtsam/inference/doc/DotWriter.ipynb index 6ad795add..97a73d476 100644 --- a/gtsam/inference/doc/DotWriter.ipynb +++ b/gtsam/inference/doc/DotWriter.ipynb @@ -40,7 +40,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/EdgeKey.ipynb b/gtsam/inference/doc/EdgeKey.ipynb index ca55d6619..df5d39684 100644 --- a/gtsam/inference/doc/EdgeKey.ipynb +++ b/gtsam/inference/doc/EdgeKey.ipynb @@ -38,7 +38,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/EliminationTree.ipynb b/gtsam/inference/doc/EliminationTree.ipynb index 75378e7d7..5e42040a3 100644 --- a/gtsam/inference/doc/EliminationTree.ipynb +++ b/gtsam/inference/doc/EliminationTree.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/Factor.ipynb b/gtsam/inference/doc/Factor.ipynb index 6e6ce1515..352bb95a5 100644 --- a/gtsam/inference/doc/Factor.ipynb +++ b/gtsam/inference/doc/Factor.ipynb @@ -51,7 +51,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/FactorGraph.ipynb b/gtsam/inference/doc/FactorGraph.ipynb index a48d56ef7..13acae2f5 100644 --- a/gtsam/inference/doc/FactorGraph.ipynb +++ b/gtsam/inference/doc/FactorGraph.ipynb @@ -53,7 +53,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/ISAM.ipynb b/gtsam/inference/doc/ISAM.ipynb index f4daa82a8..fcc5ba1f3 100644 --- a/gtsam/inference/doc/ISAM.ipynb +++ b/gtsam/inference/doc/ISAM.ipynb @@ -42,7 +42,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/JunctionTree.ipynb b/gtsam/inference/doc/JunctionTree.ipynb index b5835f9df..5aa0058a8 100644 --- a/gtsam/inference/doc/JunctionTree.ipynb +++ b/gtsam/inference/doc/JunctionTree.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/Key.ipynb b/gtsam/inference/doc/Key.ipynb index c8ad5c8c3..cd8cc3382 100644 --- a/gtsam/inference/doc/Key.ipynb +++ b/gtsam/inference/doc/Key.ipynb @@ -38,7 +38,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/LabeledSymbol.ipynb b/gtsam/inference/doc/LabeledSymbol.ipynb index c383cbbaa..a5051ea41 100644 --- a/gtsam/inference/doc/LabeledSymbol.ipynb +++ b/gtsam/inference/doc/LabeledSymbol.ipynb @@ -38,7 +38,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/Symbol.ipynb b/gtsam/inference/doc/Symbol.ipynb index c9de75957..67c71972d 100644 --- a/gtsam/inference/doc/Symbol.ipynb +++ b/gtsam/inference/doc/Symbol.ipynb @@ -38,7 +38,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/VariableIndex.ipynb b/gtsam/inference/doc/VariableIndex.ipynb index 080e03fa1..ef1c9f41b 100644 --- a/gtsam/inference/doc/VariableIndex.ipynb +++ b/gtsam/inference/doc/VariableIndex.ipynb @@ -40,7 +40,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam-develop" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/nonlinear/doc/CustomFactor.ipynb b/gtsam/nonlinear/doc/CustomFactor.ipynb index beb2961bb..43d674919 100644 --- a/gtsam/nonlinear/doc/CustomFactor.ipynb +++ b/gtsam/nonlinear/doc/CustomFactor.ipynb @@ -18,7 +18,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "id": "5ccb48e4", "metadata": { "tags": [ @@ -28,17 +28,7 @@ "languageId": "markdown" } }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\u001b[31mERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)\u001b[0m\u001b[31m\n", - "\u001b[0m\u001b[31mERROR: No matching distribution found for gtsam-develop\u001b[0m\u001b[31m\n", - "\u001b[0mNote: you may need to restart the kernel to use updated packages.\n" - ] - } - ], + "outputs": [], "source": [ "%pip install --quiet gtsam-develop" ] From 721a24fabca9001fcac082a6b4af8f4c7e8dc77b Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Thu, 17 Apr 2025 17:34:44 -0400 Subject: [PATCH 12/17] Add targets for Mac ARM 64 --- .github/workflows/build-cibw.yml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/.github/workflows/build-cibw.yml b/.github/workflows/build-cibw.yml index 95a2e8dfd..2671ea041 100644 --- a/.github/workflows/build-cibw.yml +++ b/.github/workflows/build-cibw.yml @@ -93,6 +93,24 @@ jobs: cibw_python_version: 313 platform_id: macosx_x86_64 + # MacOS arm64 + - os: macos-14 + python_version: "3.10" + cibw_python_version: 310 + platform_id: macosx_arm64 + - os: macos-14 + python_version: "3.11" + cibw_python_version: 311 + platform_id: macosx_arm64 + - os: macos-14 + python_version: "3.12" + cibw_python_version: 312 + platform_id: macosx_arm64 + - os: macos-14 + python_version: "3.13" + cibw_python_version: 313 + platform_id: macosx_arm64 + steps: - name: Checkout uses: actions/checkout@v4 From bfde2b3a3738702a792dba9a0f4ab0043bd6e068 Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 16 Apr 2025 21:21:22 -0400 Subject: [PATCH 13/17] Update navbar --- myst.yml | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/myst.yml b/myst.yml index 5db7be6ff..2a3a7547f 100644 --- a/myst.yml +++ b/myst.yml @@ -28,17 +28,15 @@ project: - file: ./doc/expressions.md site: nav: - - title: Getting started - children: - - title: Overview - url: /readme - - title: Install guide - url: /install + - title: GTSAM.org + url: https://gtsam.org - title: C++ reference url: https://gtsam.org/doxygen/ + options: + logo_text: GTSAM template: book-theme - # TODO: Graphics for favicon and to replace "Made with MyST" in the top left + # TODO: Graphics for favicon, site logo # options: # favicon: favicon.ico # logo: site_logo.png From 4cfae388887779a0cc8d051d7970973447025f1d Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 17 Apr 2025 22:04:42 -0400 Subject: [PATCH 14/17] Rerun CI --- myst.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/myst.yml b/myst.yml index 2a3a7547f..80d190996 100644 --- a/myst.yml +++ b/myst.yml @@ -33,7 +33,7 @@ site: - title: C++ reference url: https://gtsam.org/doxygen/ options: - logo_text: GTSAM + logo_text: GTSAM template: book-theme # TODO: Graphics for favicon, site logo From bbaacc26c7c1d22f1511e313dddba1b4d8166978 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 19 Apr 2025 23:39:39 -0400 Subject: [PATCH 15/17] Wrap BayesTree traversal --- gtsam/linear/linear.i | 17 +++++++++++++++++ gtsam/symbolic/symbolic.i | 4 +++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 4b6498424..44d0a06a2 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -657,6 +657,21 @@ virtual class GaussianBayesNet { }; #include +class GaussianBayesTreeClique { + GaussianBayesTreeClique(); + GaussianBayesTreeClique(const gtsam::GaussianConditional* conditional); + bool equals(const gtsam::GaussianBayesTreeClique& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter); + const gtsam::GaussianConditional* conditional() const; + bool isRoot() const; + gtsam::GaussianBayesTreeClique* parent() const; + size_t nrChildren() const; + gtsam::GaussianBayesTreeClique* operator[](size_t j) const; + size_t treeSize() const; + size_t numCachedSeparatorMarginals() const; + void deleteCachedShortcuts(); +}; virtual class GaussianBayesTree { // Standard Constructors and Named Constructors GaussianBayesTree(); @@ -666,6 +681,8 @@ virtual class GaussianBayesTree { gtsam::DefaultKeyFormatter); size_t size() const; bool empty() const; + const GaussianBayesTree::Roots& roots() const; + const gtsam::GaussianBayesTreeClique* operator[](size_t j) const; size_t numCachedSeparatorMarginals() const; string dot(const gtsam::KeyFormatter& keyFormatter = diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i index 4da59dfa9..dd5289e01 100644 --- a/gtsam/symbolic/symbolic.i +++ b/gtsam/symbolic/symbolic.i @@ -185,6 +185,8 @@ class SymbolicBayesTreeClique { const gtsam::SymbolicConditional* conditional() const; bool isRoot() const; gtsam::SymbolicBayesTreeClique* parent() const; + size_t nrChildren() const; + gtsam::SymbolicBayesTreeClique* operator[](size_t j) const; size_t treeSize() const; size_t numCachedSeparatorMarginals() const; void deleteCachedShortcuts(); @@ -204,7 +206,7 @@ class SymbolicBayesTree { // Standard Interface bool empty() const; size_t size() const; - + const SymbolicBayesTree::Roots& roots() const; const gtsam::SymbolicBayesTreeClique* operator[](size_t j) const; void saveGraph(string s, From 3c26d1f6137ffaee607cf3f4beaf8cc652c740d5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 19 Apr 2025 23:39:56 -0400 Subject: [PATCH 16/17] Symbolic docs --- gtsam/symbolic/doc/SymbolicBayesNet.ipynb | 231 +++++++++ gtsam/symbolic/doc/SymbolicBayesTree.ipynb | 325 ++++++++++++ .../doc/SymbolicBayesTreeClique.ipynb | 262 ++++++++++ gtsam/symbolic/doc/SymbolicConditional.ipynb | 142 ++++++ .../doc/SymbolicEliminationTree.ipynb | 138 +++++ gtsam/symbolic/doc/SymbolicFactor.ipynb | 131 +++++ gtsam/symbolic/doc/SymbolicFactorGraph.ipynb | 479 ++++++++++++++++++ gtsam/symbolic/doc/SymbolicJunctionTree.ipynb | 133 +++++ gtsam/symbolic/symbolic.md | 40 ++ 9 files changed, 1881 insertions(+) create mode 100644 gtsam/symbolic/doc/SymbolicBayesNet.ipynb create mode 100644 gtsam/symbolic/doc/SymbolicBayesTree.ipynb create mode 100644 gtsam/symbolic/doc/SymbolicBayesTreeClique.ipynb create mode 100644 gtsam/symbolic/doc/SymbolicConditional.ipynb create mode 100644 gtsam/symbolic/doc/SymbolicEliminationTree.ipynb create mode 100644 gtsam/symbolic/doc/SymbolicFactor.ipynb create mode 100644 gtsam/symbolic/doc/SymbolicFactorGraph.ipynb create mode 100644 gtsam/symbolic/doc/SymbolicJunctionTree.ipynb create mode 100644 gtsam/symbolic/symbolic.md diff --git a/gtsam/symbolic/doc/SymbolicBayesNet.ipynb b/gtsam/symbolic/doc/SymbolicBayesNet.ipynb new file mode 100644 index 000000000..49d6d4b43 --- /dev/null +++ b/gtsam/symbolic/doc/SymbolicBayesNet.ipynb @@ -0,0 +1,231 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# SymbolicBayesNet" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `SymbolicBayesNet` is a directed acyclic graph (DAG) composed of `SymbolicConditional` objects. It represents the structure of a factorized probability distribution P(X) = Π P(Xi | Parents(Xi)) purely in terms of variable connectivity.\n", + "\n", + "It is typically the result of running sequential variable elimination on a `SymbolicFactorGraph`." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Note: you may need to restart the kernel to use updated packages.\n" + ] + } + ], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import SymbolicConditional, SymbolicFactorGraph, Ordering\n", + "from gtsam.symbol_shorthand import X, L\n", + "import graphviz" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating a SymbolicBayesNet\n", + "\n", + "SymbolicBayesNets are usually created by eliminating a [SymbolicFactorGraph](SymbolicFactorGraph.ipynb). But you can also build them directly:" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Directly Built Symbolic Bayes Net:\n", + " \n", + "size: 5\n", + "conditional 0: P( l1 | x0)\n", + "conditional 1: P( x0 | x1)\n", + "conditional 2: P( l2 | x1)\n", + "conditional 3: P( x1 | x2)\n", + "conditional 4: P( x2)\n" + ] + } + ], + "source": [ + "from gtsam import SymbolicBayesNet\n", + "\n", + "# Create a new Bayes Net\n", + "symbolic_bayes_net = SymbolicBayesNet()\n", + "\n", + "# Add conditionals directly\n", + "symbolic_bayes_net.push_back(SymbolicConditional(L(1), X(0))) # P(l1 | x0)\n", + "symbolic_bayes_net.push_back(SymbolicConditional(X(0), X(1))) # P(x0 | x1)\n", + "symbolic_bayes_net.push_back(SymbolicConditional(L(2), X(1))) # P(l2 | x1)\n", + "symbolic_bayes_net.push_back(SymbolicConditional(X(1), X(2))) # P(x1 | x2)\n", + "symbolic_bayes_net.push_back(SymbolicConditional(X(2))) # P(x2)\n", + "\n", + "symbolic_bayes_net.print(\"Directly Built Symbolic Bayes Net:\\n\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Accessing Conditionals and Visualization" + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Conditional at index 1: P( x0 | x1)\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089\n", + "\n", + "l1\n", + "\n", + "\n", + "\n", + "var7782220156096217090\n", + "\n", + "l2\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "var8646911284551352320->var7782220156096217089\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321->var7782220156096217090\n", + "\n", + "\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": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# Access a conditional by index\n", + "conditional_1 = bayes_net.at(1) # P(x0 | l1)\n", + "conditional_1.print(\"Conditional at index 1: \")\n", + "\n", + "# Visualize the Bayes Net structure\n", + "display(graphviz.Source(bayes_net.dot()))" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/gtsam/symbolic/doc/SymbolicBayesTree.ipynb b/gtsam/symbolic/doc/SymbolicBayesTree.ipynb new file mode 100644 index 000000000..c6c7aea55 --- /dev/null +++ b/gtsam/symbolic/doc/SymbolicBayesTree.ipynb @@ -0,0 +1,325 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# SymbolicBayesTree" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `SymbolicBayesTree` is a tree structure where each node (`SymbolicBayesTreeClique`) represents a clique of variables that were eliminated together during multifrontal elimination. Unlike a `SymbolicJunctionTree` which stores factors, a `SymbolicBayesTree` stores the resulting `SymbolicConditional` for each clique.\n", + "\n", + "It represents the factored structure $P(X) = Π P(C_j | S_j)$, where $C_j$ are the frontal variables of clique $j$ and $S_j$ are its separator variables (parents in the Bayes Tree). This structure is computationally advantageous for inference, especially for calculating marginals or performing incremental updates (like in iSAM)." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Note: you may need to restart the kernel to use updated packages.\n" + ] + } + ], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import SymbolicFactorGraph, Ordering\n", + "from gtsam.symbol_shorthand import X, L\n", + "import graphviz # For visualization" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating a SymbolicBayesTree\n", + "\n", + "A Bayes tree is typically created by eliminating a `SymbolicFactorGraph` using the multifrontal method." + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "metadata": {}, + "outputs": [], + "source": [ + "# Create a factor graph from a GTSFM problem\n", + "graph = SymbolicFactorGraph()\n", + "\n", + "edges = [(2, 4), (2, 5), (2, 27), (4, 6), (5, 6), (5, 7), (5, 8), (5, 9), (6, 7), (6, 8), (6, 9), \n", + " (7, 8), (7, 9), (8, 9), (9, 12), (9, 24), (9, 28), (10, 12), (10, 29), (20, 21), (20, 22), \n", + " (20, 23), (20, 24), (21, 22), (21, 23), (21, 24), (22, 23), (22, 24), (23, 24), (25, 26), \n", + " (25, 27), (25, 28), (25, 29), (26, 27), (26, 28), (26, 29), (27, 28), (27, 29), (28, 29)]\n", + "\n", + "for i,j in edges:\n", + " graph.push_factor(X(i), X(j))\n", + "\n", + "# Define an elimination ordering\n", + "ordering = Ordering.MetisSymbolicFactorGraph(graph) # Use METIS for this example\n", + "\n", + "# Eliminate using Multifrontal method\n", + "bayes_tree = graph.eliminateMultifrontal(ordering)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Visualization and Properties\n", + "\n", + "The Bayes tree structure can be visualized, and we can query its properties." + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Bayes Tree size (number of cliques): 8\n", + "Is the Bayes Tree empty? False\n" + ] + } + ], + "source": [ + "print(f\"\\nBayes Tree size (number of cliques): {bayes_tree.size()}\")\n", + "print(f\"Is the Bayes Tree empty? {bayes_tree.empty()}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "20\n", + "\n", + "x28, x25, x26, x9, x29, x27\n", + "\n", + "\n", + "\n", + "21\n", + "\n", + "x5, x4 : x9, x27\n", + "\n", + "\n", + "\n", + "20->21\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "25\n", + "\n", + "x12 : x9, x29\n", + "\n", + "\n", + "\n", + "20->25\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "27\n", + "\n", + "x24, x23, x22, x21, x20 : x9\n", + "\n", + "\n", + "\n", + "20->27\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "22\n", + "\n", + "x2 : x4, x5, x27\n", + "\n", + "\n", + "\n", + "21->22\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "23\n", + "\n", + "x6 : x4, x5, x9\n", + "\n", + "\n", + "\n", + "21->23\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "24\n", + "\n", + "x8, x7 : x5, x6, x9\n", + "\n", + "\n", + "\n", + "23->24\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "26\n", + "\n", + "x10 : x12, x29\n", + "\n", + "\n", + "\n", + "25->26\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# Visualize the Bayes tree using graphviz\n", + "display(graphviz.Source(bayes_tree.dot()))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Traversing a Bayes Tree" + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Number of roots: 1\n", + "\n", + "Clique:\n", + " P( x28 x25 x26 x9 x29 x27)\n", + "\n", + "Clique:\n", + " P( x5 x4 | x9 x27)\n", + "\n", + "Clique:\n", + " P( x2 | x4 x5 x27)\n", + "\n", + "Clique:\n", + " P( x6 | x4 x5 x9)\n", + "\n", + "Clique:\n", + " P( x8 x7 | x5 x6 x9)\n", + "\n", + "Clique:\n", + " P( x12 | x9 x29)\n", + "\n", + "Clique:\n", + " P( x10 | x12 x29)\n", + "\n", + "Clique:\n", + " P( x24 x23 x22 x21 x20 | x9)\n" + ] + } + ], + "source": [ + "roots = bayes_tree.roots()\n", + "print(f\"Number of roots: {len(roots)}\")\n", + "\n", + "def traverse_clique(clique):\n", + " if clique:\n", + " clique.print(\"\\nClique:\\n\")\n", + " for j in range(clique.nrChildren()):\n", + " traverse_clique(clique[j])\n", + "\n", + "for root in roots:\n", + " traverse_clique(root)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/gtsam/symbolic/doc/SymbolicBayesTreeClique.ipynb b/gtsam/symbolic/doc/SymbolicBayesTreeClique.ipynb new file mode 100644 index 000000000..ba3ed1a8d --- /dev/null +++ b/gtsam/symbolic/doc/SymbolicBayesTreeClique.ipynb @@ -0,0 +1,262 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# SymbolicBayesTreeClique" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `SymbolicBayesTreeClique` represents a single node within a `SymbolicBayesTree`. Each clique corresponds to a set of variables (frontal variables) that were eliminated together during the multifrontal elimination process.\n", + "\n", + "Key aspects of a clique:\n", + "* **Conditional:** It stores the `SymbolicConditional` P(Frontals | Parents/Separator) that results from eliminating the frontal variables.\n", + "* **Tree Structure:** It maintains pointers to its parent and children cliques within the Bayes Tree, defining the tree's topology.\n", + "* **Frontal and Separator Variables:** Implicitly defines the frontal variables (eliminated in this clique) and separator variables (parents in the Bayes Tree, connecting it to its parent clique).\n", + "\n", + "Users typically interact with the `SymbolicBayesTree` as a whole, but understanding the clique structure is helpful for comprehending how the Bayes Tree represents the factored distribution and facilitates efficient inference." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import SymbolicFactorGraph, Ordering\n", + "# SymbolicBayesTreeClique is accessed *through* a SymbolicBayesTree\n", + "from gtsam.symbol_shorthand import X, L\n", + "import graphviz" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Accessing and Inspecting Cliques\n", + "\n", + "Cliques are obtained by first creating a `SymbolicBayesTree` (usually via elimination) and then accessing its nodes." + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Bayes Tree has 4 cliques.\n", + "Number of roots: 1\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "6\n", + "\n", + "x1, x2\n", + "\n", + "\n", + "\n", + "7\n", + "\n", + "x0 : x1\n", + "\n", + "\n", + "\n", + "6->7\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "9\n", + "\n", + "l2 : x1\n", + "\n", + "\n", + "\n", + "6->9\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "8\n", + "\n", + "l1 : x0\n", + "\n", + "\n", + "\n", + "7->8\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# Create a factor graph\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(2))\n", + "\n", + "# Eliminate to get a Bayes Tree\n", + "ordering = Ordering.ColamdSymbolicFactorGraph(graph)\n", + "bayes_tree = graph.eliminateMultifrontal(ordering)\n", + "\n", + "print(f\"Bayes Tree has {bayes_tree.size()} cliques.\")\n", + "\n", + "roots = bayes_tree.roots()\n", + "print(f\"Number of roots: {len(roots)}\")\n", + "\n", + "# Visualize the Bayes tree using graphviz\n", + "display(graphviz.Source(bayes_tree.dot()))" + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Inspecting Clique 0:\n", + " Clique Structure: P( x1 x2)\n", + " Associated Conditional: P( P( x1 x2)\n", + " Is Root? True\n", + " Parent Clique is None (likely a root clique).\n", + " Number of Children: 2\n", + "\n", + "Inspecting Clique 0:\n", + " Clique Structure: P( x0 | x1)\n", + " Associated Conditional: P( P( x0 | x1)\n", + " Is Root? False\n", + " Parent Clique exists.\n", + " Number of Children: 1\n", + "\n", + "Inspecting Clique 0:\n", + " Clique Structure: P( l1 | x0)\n", + " Associated Conditional: P( P( l1 | x0)\n", + " Is Root? False\n", + " Parent Clique exists.\n", + " Number of Children: 0\n", + "\n", + "Inspecting Clique 0:\n", + " Clique Structure: P( l2 | x1)\n", + " Associated Conditional: P( P( l2 | x1)\n", + " Is Root? False\n", + " Parent Clique exists.\n", + " Number of Children: 0\n" + ] + } + ], + "source": [ + "def inspect(clique):\n", + " print(\"\\nInspecting Clique 0:\")\n", + " clique.print(\" Clique Structure: \")\n", + " \n", + " # Get the conditional stored in the clique\n", + " conditional = clique.conditional()\n", + " if conditional:\n", + " conditional.print(\" Associated Conditional: P(\")\n", + " else:\n", + " print(\" Clique has no associated conditional (might be empty root)\")\n", + " \n", + " # Check properties\n", + " print(f\" Is Root? {clique.isRoot()}\")\n", + " # Accessing parent/children is possible in C++ but might be less direct or typical in Python wrapper usage\n", + " # Parent clique (careful, might be null for root)\n", + " parent_clique = clique.parent() \n", + " if parent_clique:\n", + " print(\" Parent Clique exists.\")\n", + " else:\n", + " print(\" Parent Clique is None (likely a root clique).\")\n", + " \n", + " print(f\" Number of Children: {clique.nrChildren()}\") # Example if method existed\n", + "\n", + "def traverse_clique(clique):\n", + " inspect(clique)\n", + " for j in range(clique.nrChildren()):\n", + " traverse_clique(clique[j])\n", + "\n", + "for root in roots:\n", + " traverse_clique(root)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/gtsam/symbolic/doc/SymbolicConditional.ipynb b/gtsam/symbolic/doc/SymbolicConditional.ipynb new file mode 100644 index 000000000..e43881ee0 --- /dev/null +++ b/gtsam/symbolic/doc/SymbolicConditional.ipynb @@ -0,0 +1,142 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# SymbolicConditional" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `SymbolicConditional` represents a conditional probability distribution P(Frontals | Parents) in purely symbolic form. It only stores the keys of the frontal variables and the parent variables, without any associated numerical probability function.\n", + "\n", + "`SymbolicConditional` objects are typically produced as the result of symbolic elimination on a `SymbolicFactorGraph`. A collection of these forms a `SymbolicBayesNet`." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Note: you may need to restart the kernel to use updated packages.\n" + ] + } + ], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import SymbolicConditional\n", + "from gtsam.symbol_shorthand import X, L" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating SymbolicConditionals\n", + "\n", + "Conditionals specify frontal (conditioned) variables and parent variables." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "P(X(0)): P( x0)\n", + " Nr Frontals: 1, Nr Parents: 0\n", + "\n", + "P(X(1) | X(0)): P( x1 | x0)\n", + " Nr Frontals: 1, Nr Parents: 1\n", + "\n", + "P(X(2) | X(0), X(1)): P( x2 | x0 x1)\n", + " Nr Frontals: 1, Nr Parents: 2\n", + "\n", + "P(L(1) | X(0), X(1)): P( l1 | x0 x1)\n", + " Nr Frontals: 1, Nr Parents: 2\n", + "\n", + "P(X(2), L(1) | X(0), X(1)): P( x2 l1 | x0 x1)\n", + " Nr Frontals: 2, Nr Parents: 2\n" + ] + } + ], + "source": [ + "# P(X(0))\n", + "c0 = SymbolicConditional(X(0))\n", + "c0.print(\"P(X(0)): \")\n", + "print(f\" Nr Frontals: {c0.nrFrontals()}, Nr Parents: {c0.nrParents()}\\n\")\n", + "\n", + "# P(X(1) | X(0))\n", + "c1 = SymbolicConditional(X(1), X(0))\n", + "c1.print(\"P(X(1) | X(0)): \")\n", + "print(f\" Nr Frontals: {c1.nrFrontals()}, Nr Parents: {c1.nrParents()}\\n\")\n", + "\n", + "# P(X(2) | X(0), X(1))\n", + "c2 = SymbolicConditional(X(2), X(0), X(1))\n", + "c2.print(\"P(X(2) | X(0), X(1)): \")\n", + "print(f\" Nr Frontals: {c2.nrFrontals()}, Nr Parents: {c2.nrParents()}\\n\")\n", + "\n", + "# P(L(1) | X(0), X(1))\n", + "c3 = SymbolicConditional(L(1), X(0), X(1))\n", + "c3.print(\"P(L(1) | X(0), X(1)): \")\n", + "print(f\" Nr Frontals: {c3.nrFrontals()}, Nr Parents: {c3.nrParents()}\\n\")\n", + "\n", + "# Create from keys and number of frontals, e.g. P(X(2), L(1) | X(0), X(1))\n", + "# Keys = [Frontals..., Parents...]\n", + "c4 = SymbolicConditional.FromKeys([X(2), L(1), X(0), X(1)], 2)\n", + "c4.print(\"P(X(2), L(1) | X(0), X(1)): \")\n", + "print(f\" Nr Frontals: {c4.nrFrontals()}, Nr Parents: {c4.nrParents()}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/gtsam/symbolic/doc/SymbolicEliminationTree.ipynb b/gtsam/symbolic/doc/SymbolicEliminationTree.ipynb new file mode 100644 index 000000000..ae068282d --- /dev/null +++ b/gtsam/symbolic/doc/SymbolicEliminationTree.ipynb @@ -0,0 +1,138 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# SymbolicEliminationTree" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `SymbolicEliminationTree` represents the computational structure used in variable elimination, particularly in sparse Cholesky or QR factorization. Each node in the tree corresponds to the elimination of a single variable.\n", + "\n", + "The tree structure reveals dependencies: the elimination of a variable (node) depends on the results from its children in the tree. The root of the tree corresponds to the last variable eliminated. This structure is closely related to the resulting Bayes net or Bayes tree." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import SymbolicEliminationTree, SymbolicFactorGraph, Ordering\n", + "from gtsam.symbol_shorthand import X, L" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating a SymbolicEliminationTree\n", + "\n", + "An elimination tree is constructed from a `SymbolicFactorGraph` and an `Ordering`." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbolic Elimination Tree:\n", + "-(x2)\n", + "Symbolic Elimination Tree:\n", + "| -(x1)\n", + "Symbolic Elimination Tree:\n", + "| - x1 x2\n", + "Symbolic Elimination Tree:\n", + "| | -(x0)\n", + "Symbolic Elimination Tree:\n", + "| | - x0\n", + "Symbolic Elimination Tree:\n", + "| | - x0 x1\n", + "Symbolic Elimination Tree:\n", + "| | | -(l1)\n", + "Symbolic Elimination Tree:\n", + "| | | - x0 l1\n", + "Symbolic Elimination Tree:\n", + "| | -(l2)\n", + "Symbolic Elimination Tree:\n", + "| | - x1 l2\n" + ] + } + ], + "source": [ + "# Create a factor graph\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(2))\n", + "\n", + "# Define an elimination ordering\n", + "ordering = Ordering([L(1), L(2), X(0), X(1), X(2)]) # Eliminate L(1) first, then X(0), X(1), X(2) last\n", + "\n", + "# Construct the elimination tree\n", + "elimination_tree = SymbolicEliminationTree(graph, ordering)\n", + "\n", + "# Print the tree structure (text representation)\n", + "elimination_tree.print(\"Symbolic Elimination Tree:\\n\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "*(Note: Direct visualization of the elimination tree structure isn't available via a simple `.dot()` method like factor graphs or Bayes nets/trees in the Python wrapper, but the print output shows the parent-child relationships.)*" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/gtsam/symbolic/doc/SymbolicFactor.ipynb b/gtsam/symbolic/doc/SymbolicFactor.ipynb new file mode 100644 index 000000000..06141920b --- /dev/null +++ b/gtsam/symbolic/doc/SymbolicFactor.ipynb @@ -0,0 +1,131 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# SymbolicFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `SymbolicFactor` represents the connectivity (topology) between variables in a factor graph without any specific numerical function associated with it. It's primarily to *illustrate* symbolic elimination. Internally, GTSAM does analyze the structure of other factor graph types without explicitly converting to a symbolic factor graph.\n", + "\n", + "It inherits from `gtsam.Factor` and stores the keys (indices) of the variables it connects." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Note: you may need to restart the kernel to use updated packages.\n" + ] + } + ], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import SymbolicFactor\n", + "from gtsam.symbol_shorthand import X\n", + "\n", + "# Example Keys\n", + "x0, x1, x2 = X(0), X(1), X(2)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating SymbolicFactors\n", + "\n", + "SymbolicFactors can be created by specifying the keys they involve." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Unary Factor f0: \n", + " x0\n", + "Binary Factor f1: \n", + " x0 x1\n", + "Ternary Factor f2: \n", + " x1 x2 x0\n", + "Factor f3 from KeyVector: \n", + " x0 x1 x2\n" + ] + } + ], + "source": [ + "# Unary factor\n", + "f0 = SymbolicFactor(x0)\n", + "f0.print(\"Unary Factor f0: \\n\")\n", + "\n", + "# Binary factor\n", + "f1 = SymbolicFactor(x0, x1)\n", + "f1.print(\"Binary Factor f1: \\n\")\n", + "\n", + "# Ternary factor\n", + "f2 = SymbolicFactor(x1, x2, x0)\n", + "f2.print(\"Ternary Factor f2: \\n\")\n", + "\n", + "# From a list of keys\n", + "keys = gtsam.KeyVector([x0, x1, x2])\n", + "f3 = SymbolicFactor.FromKeys(keys)\n", + "f3.print(\"Factor f3 from KeyVector: \\n\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/gtsam/symbolic/doc/SymbolicFactorGraph.ipynb b/gtsam/symbolic/doc/SymbolicFactorGraph.ipynb new file mode 100644 index 000000000..10c90e321 --- /dev/null +++ b/gtsam/symbolic/doc/SymbolicFactorGraph.ipynb @@ -0,0 +1,479 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# SymbolicFactorGraph" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `SymbolicFactorGraph` is a factor graph consisting purely of `SymbolicFactor` objects. It represents the structure or topology of a probabilistic graphical model (like a Markov Random Field) without the numerical details.\n", + "\n", + "It's primarily used to *illustrate* symbolic elimination, which determines the structure of the resulting Bayes net or Bayes tree and finds an efficient variable elimination ordering (e.g., using COLAMD or METIS)." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Note: you may need to restart the kernel to use updated packages.\n" + ] + } + ], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import SymbolicFactorGraph, Ordering\n", + "from gtsam.symbol_shorthand import X, L\n", + "import graphviz" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating and Manipulating Symbolic Factor Graphs" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbolic Factor Graph:\n", + " \n", + "size: 5\n", + "factor 0: x0\n", + "factor 1: x0 x1\n", + "factor 2: x1 x2\n", + "factor 3: x0 l1\n", + "factor 4: x1 l2\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", + "var7782220156096217090\n", + "\n", + "l2\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090--factor4\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", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor2\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 12, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Create an empty graph\n", + "graph = SymbolicFactorGraph()\n", + "\n", + "# Add factors (using convenience methods)\n", + "graph.push_factor(X(0)) # Unary\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(2))\n", + "\n", + "# Print the graph structure\n", + "graph.print(\"Symbolic Factor Graph:\\n\")\n", + "\n", + "# Visualize the graph using Graphviz\n", + "graphviz.Source(graph.dot())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Symbolic Elimination\n", + "\n", + "We can perform symbolic elimination to get the structure of the resulting Bayes net or Bayes tree." + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [], + "source": [ + "# Define an elimination ordering (can also be computed automatically)\n", + "ordering = Ordering([L(1), L(2), X(0), X(1), X(2)])" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Resulting Symbolic Bayes Net:\n", + " \n", + "size: 5\n", + "conditional 0: P( l1 | x0)\n", + "conditional 1: P( x0 | x1)\n", + "conditional 2: P( l2 | x1)\n", + "conditional 3: P( x1 | x2)\n", + "conditional 4: P( x2)\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089\n", + "\n", + "l1\n", + "\n", + "\n", + "\n", + "var7782220156096217090\n", + "\n", + "l2\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "var8646911284551352320->var7782220156096217089\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321->var7782220156096217090\n", + "\n", + "\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": 18, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Eliminate sequentially to get a Bayes Net structure\n", + "bayes_net = graph.eliminateSequential(ordering)\n", + "bayes_net.print(\"\\nResulting Symbolic Bayes Net:\\n\")\n", + "\n", + "# Visualize the Bayes Net using Graphviz\n", + "graphviz.Source(bayes_net.dot())" + ] + }, + { + "cell_type": "code", + "execution_count": 19, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Resulting Symbolic Bayes Tree:\n", + ": cliques: 4, variables: 5\n", + "\n", + "Resulting Symbolic Bayes Tree:\n", + "- P( x1 x2)\n", + "\n", + "Resulting Symbolic Bayes Tree:\n", + "| - P( x0 | x1)\n", + "\n", + "Resulting Symbolic Bayes Tree:\n", + "| | - P( l1 | x0)\n", + "\n", + "Resulting Symbolic Bayes Tree:\n", + "| - P( l2 | x1)\n" + ] + }, + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "0\n", + "\n", + "x1, x2\n", + "\n", + "\n", + "\n", + "1\n", + "\n", + "x0 : x1\n", + "\n", + "\n", + "\n", + "0->1\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "3\n", + "\n", + "l2 : x1\n", + "\n", + "\n", + "\n", + "0->3\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "2\n", + "\n", + "l1 : x0\n", + "\n", + "\n", + "\n", + "1->2\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 19, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "# Eliminate using Multifrontal method to get a Bayes Tree structure\n", + "bayes_tree = graph.eliminateMultifrontal(ordering)\n", + "bayes_tree.print(\"\\nResulting Symbolic Bayes Tree:\\n\")\n", + "\n", + "# Visualize the Bayes Tree using Graphviz\n", + "graphviz.Source(bayes_tree.dot())" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/gtsam/symbolic/doc/SymbolicJunctionTree.ipynb b/gtsam/symbolic/doc/SymbolicJunctionTree.ipynb new file mode 100644 index 000000000..3f600b42b --- /dev/null +++ b/gtsam/symbolic/doc/SymbolicJunctionTree.ipynb @@ -0,0 +1,133 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# SymbolicJunctionTree" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A `SymbolicJunctionTree` (often used interchangeably with Clique Tree in GTSAM documentation) is a data structure used in multifrontal variable elimination. It's derived from an `EliminationTree`.\n", + "\n", + "Key differences from an Elimination Tree:\n", + "* **Nodes are Cliques:** Each node in a Junction Tree represents a *clique* (a group of variables eliminated together), not just a single variable.\n", + "* **Stores Factors:** Nodes (`SymbolicCluster` objects) store the symbolic factors associated with the variables in that clique.\n", + "\n", + "The Junction Tree organizes the factors and variables for efficient multifrontal elimination, which processes variables in these larger cliques simultaneously. The result of eliminating a Junction Tree is a `SymbolicBayesTree`." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import SymbolicJunctionTree, SymbolicEliminationTree, SymbolicFactorGraph, Ordering\n", + "from gtsam.symbol_shorthand import X, L" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Creating a SymbolicJunctionTree\n", + "\n", + "A Junction Tree is constructed from a `SymbolicEliminationTree`." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Symbolic Junction Tree:\n", + "- (6) x1 x2 \n", + "Symbolic Junction Tree:\n", + "| - (6) x0 \n", + "Symbolic Junction Tree:\n", + "| | - (2) l1 \n", + "Symbolic Junction Tree:\n", + "| - (2) l2 \n" + ] + } + ], + "source": [ + "# Create a factor graph\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(2))\n", + "\n", + "# Define an elimination ordering\n", + "ordering = Ordering([L(1), L(2), X(0), X(1), X(2)]) \n", + "\n", + "# Construct the elimination tree first\n", + "elimination_tree = SymbolicEliminationTree(graph, ordering)\n", + "\n", + "# Construct the junction tree from the elimination tree\n", + "junction_tree = SymbolicJunctionTree(elimination_tree)\n", + "\n", + "# Print the tree structure (text representation)\n", + "junction_tree.print(\"Symbolic Junction Tree:\\n\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "*(Note: The `SymbolicCluster` class represents the nodes within the Junction Tree, containing the factors and frontal/separator keys for that clique. Direct visualization is usually done via the resulting Bayes Tree.)*" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/gtsam/symbolic/symbolic.md b/gtsam/symbolic/symbolic.md new file mode 100644 index 000000000..bfae60842 --- /dev/null +++ b/gtsam/symbolic/symbolic.md @@ -0,0 +1,40 @@ +# Symbolic Module + +The `symbolic` module in GTSAM deals with the *structure* of factor graphs and Bayesian networks, independent of the specific numerical types of factors (like Gaussian or discrete). It allows for analyzing graph connectivity, determining optimal variable elimination orders, and understanding the sparsity structure of the resulting inference objects. + +This is crucial for efficient inference, as the symbolic elimination steps determine the computational complexity and memory requirements of the numerical factorization. + +The classes here are used primarily to *illustrate* symbolic elimination. Internally, GTSAM does analyze the structure of other factor graph types without explicitly converting to a symbolic factor graph. + + +## Classes + +Here's an overview of the key classes in the `symbolic` module: + +### Factor Graph + +- **[SymbolicFactor](doc/SymbolicFactor.ipynb)**: Represents the connectivity between a set of variables (keys) in a factor graph, without any specific numerical function associated with it. It defines the hyperedges of the graph. +- **[SymbolicFactorGraph](doc/SymbolicFactorGraph.ipynb)**: A collection of `SymbolicFactor` objects, representing the overall structure of a factor graph. + +### Elimination Products + +These classes represent the results of symbolic variable elimination: + +- **[SymbolicConditional](doc/SymbolicConditional.ipynb)**: Represents the structure of a conditional probability distribution P(Frontals | Parents). It stores the keys of the frontal (conditioned) and parent variables resulting from eliminating one or more variables from a set of factors. +- **[SymbolicBayesNet](doc/SymbolicBayesNet.ipynb)**: A directed acyclic graph composed of `SymbolicConditional` objects, representing the structure of a factorized distribution P(X) = Π P(Xi | Parents(Xi)). Typically results from sequential elimination. +- **[SymbolicBayesTree](doc/SymbolicBayesTree.ipynb)**: A tree structure where each node (`SymbolicBayesTreeClique`) represents a `SymbolicConditional` P(Frontals | Separator). This is the result of multifrontal elimination and is the underlying structure for efficient incremental updates (iSAM) and exact marginal computation. +- **[SymbolicBayesTreeClique](doc/SymbolicBayesTreeClique.ipynb)**: Represents a single clique (node) within a `SymbolicBayesTree`, containing a `SymbolicConditional`. + +### Elimination Structures + +These classes represent intermediate structures used during the elimination process: + +- **[SymbolicEliminationTree](doc/SymbolicEliminationTree.ipynb)**: Represents the dependency structure of sequential variable elimination. Each node corresponds to eliminating a single variable. +- **[SymbolicJunctionTree](doc/SymbolicJunctionTree.ipynb)** (Clique Tree): An intermediate structure used in multifrontal elimination, derived from an elimination tree. Each node (`SymbolicCluster`) represents a clique of variables eliminated together and stores the *factors* associated with that clique. + +**Importance** +Performing symbolic analysis can be used for: +1. Choosing an Optimal Ordering: Finding an ordering that minimizes fill-in (new connections created during elimination) is key to efficient numerical factorization. +2. Predicting Computational Cost: The structure of the Bayes net or Bayes tree determines the complexity of subsequent numerical operations like solving or marginalization. +3. Memory Allocation: Knowing the structure allows for pre-allocation of memory for the numerical factorization. +The symbolic module provides the foundation for GTSAM's efficient inference algorithms. From 8df8741a6fc03492a048e992d6e0d815d373b72e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 19 Apr 2025 23:42:20 -0400 Subject: [PATCH 17/17] Add to toc --- myst.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/myst.yml b/myst.yml index 80d190996..5fbe34010 100644 --- a/myst.yml +++ b/myst.yml @@ -19,6 +19,9 @@ project: - file: ./gtsam/nonlinear/nonlinear.md children: - pattern: ./gtsam/nonlinear/doc/* + - file: ./gtsam/symbolic/symbolic.md + children: + - pattern: ./gtsam/symbolic/doc/* - file: ./gtsam/navigation/navigation.md children: - pattern: ./gtsam/navigation/doc/*