Merge branch 'borglab:develop' into develop

Author: PeterCalifano

.github/scripts/python_wheels/cleanup_gtsam_develop.sh

@@ -0,0 +1,46 @@
+#!/usr/bin/env bash
+
+# This script deletes all but the most recent release from the gtsam-develop project on PyPI
+# and can be used if the project size exceeds the PyPI limit of 10 GB. The user must have
+# owner or maintainer privileges on the project.
+
+set -euo pipefail
+
+usage() {
+  cat <<EOF
+Usage: $(basename "$0") <pypi_username>
+
+Deletes all but the most recent release from the gtsam-develop project on PyPI.
+You must supply the PyPI user name that has owner or maintainer privileges on
+the project. THIS OPERATION IS PERMANENT.
+
+Examples
+  $ $(basename "$0") yambati3
+  $ $(basename "$0")        # will prompt for user name
+EOF
+}
+
+if [[ $# -ge 1 ]]; then
+  PYPI_USER="$1"
+else
+  read -rp "Enter your PyPI user name: " PYPI_USER
+  [[ -z "$PYPI_USER" ]] && { echo "No user name supplied."; usage; exit 1; }
+fi
+
+echo "-----------------------------------------------------------------------"
+echo "WARNING: This WILL permanently delete all but the most recent release"
+echo "         of 'gtsam-develop' on PyPI for user '$PYPI_USER'."
+echo "         This cannot be undone."
+echo "-----------------------------------------------------------------------"
+read -rp "Proceed? [y/N]: " REPLY
+REPLY=${REPLY,,}   # to lowercase
+[[ "$REPLY" != "y" && "$REPLY" != "yes" ]] && { echo "Aborted."; exit 0; }
+
+echo "Running pypi_cleanup for user '$PYPI_USER'..."
+python3 -m pypi_cleanup.__init__ \
+        -p gtsam-develop \
+        --leave-most-recent-only \
+        --do-it \
+        -u "$PYPI_USER"
+
+echo "Done."

.github/workflows/build-cibw.yml

@@ -1,7 +1,7 @@
 # This workflow builds the Python wheels using cibuildwheel and uploads them to TestPyPI.
 # It can be triggered on push to the develop branch or manually via Github Actions.
 
-name: Build Wheels (cibuildwheel)
+name: Build Wheels for Develop
 
 on:
   push:

.github/workflows/build-special.yml

@@ -201,7 +201,7 @@ jobs:
         if: runner.os == 'Linux'
         uses: pierotofy/set-swap-space@master
         with:
-          swap-size-gb: 12
+          swap-size-gb: 8
 
       - name: Build & Test
         run: |

.github/workflows/prod-cibw.yml

@@ -0,0 +1,188 @@
+# This workflow builds the Python wheels using cibuildwheel and uploads them to TestPyPI.
+# It can be triggered on push to the develop branch or manually via Github Actions.
+
+name: Build Wheels for Release
+
+on:
+  release:
+    types: [published]
+  workflow_dispatch:
+
+jobs:
+  build_wheels:
+    name: Build Wheels
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          # Linux x86_64
+          - os: ubuntu-latest
+            python_version: "3.10"
+            cibw_python_version: 310
+            platform_id: manylinux_x86_64
+            manylinux_image: manylinux2014
+          - os: ubuntu-latest
+            python_version: "3.11"
+            cibw_python_version: 311
+            platform_id: manylinux_x86_64
+            manylinux_image: manylinux2014
+          - os: ubuntu-latest
+            python_version: "3.12"
+            cibw_python_version: 312
+            platform_id: manylinux_x86_64
+            manylinux_image: manylinux2014
+          - os: ubuntu-latest
+            python_version: "3.13"
+            cibw_python_version: 313
+            platform_id: manylinux_x86_64
+            manylinux_image: manylinux2014
+
+          # Linux aarch64
+          - os: ubuntu-24.04-arm
+            python_version: "3.10"
+            cibw_python_version: 310
+            platform_id: manylinux_aarch64
+            manylinux_image: manylinux2014
+          - os: ubuntu-24.04-arm
+            python_version: "3.11"
+            cibw_python_version: 311
+            platform_id: manylinux_aarch64
+            manylinux_image: manylinux2014
+          - os: ubuntu-24.04-arm
+            python_version: "3.12"
+            cibw_python_version: 312
+            platform_id: manylinux_aarch64
+            manylinux_image: manylinux2014
+          - os: ubuntu-24.04-arm
+            python_version: "3.13"
+            cibw_python_version: 313
+            platform_id: manylinux_aarch64
+            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
+
+          # 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
+        with:
+          fetch-depth: 0
+          ref: ${{ github.sha }}
+
+      - name: Set up Python ${{ matrix.python_version }}
+        uses: actions/setup-python@v5
+        with:
+          python-version: ${{ matrix.python_version }}
+
+      - name: Install Dependencies
+        run: |
+          python3 -m pip install -r python/dev_requirements.txt
+          if [ "$RUNNER_OS" == "Linux" ]; then
+            sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools libboost-all-dev ninja-build
+          elif [ "$RUNNER_OS" == "macOS" ]; then
+            brew install boost ninja python-setuptools
+          else
+            echo "$RUNNER_OS not supported"
+            exit 1
+          fi
+
+      # We first build the Python wrapper module on the host machine. This is done because cibuildwheel
+      # expects a setup.py file to be present in the project directory.
+      #
+      # The Python wrapper module is then rebuilt within the cibuildwheel container before building
+      # the wheels to ensure platform compatibility.
+      - name: Run CMake
+        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.
+          CIBW_BUILD: cp${{ matrix.cibw_python_version }}-${{ matrix.platform_id }}
+          CIBW_MANYLINUX_X86_64_IMAGE: ${{ matrix.manylinux_image }}
+          CIBW_MANYLINUX_AARCH64_IMAGE: ${{ matrix.manylinux_image }}
+          CIBW_ARCHS: all
+
+          # 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"
+          CIBW_BEFORE_ALL: bash .github/scripts/python_wheels/cibw_before_all.sh ${{ matrix.python_version }} {project}
+
+          CIBW_BUILD_VERBOSITY: 1
+
+        run: bash .github/scripts/python_wheels/build_wheels.sh
+
+      - name: Store artifacts
+        uses: actions/upload-artifact@v4
+        with:
+          name: cibw-wheels-cp${{ matrix.cibw_python_version }}-${{ matrix.platform_id }}
+          path: wheelhouse/*.whl
+
+  upload_all:
+    name: Upload All
+    needs: build_wheels
+    runs-on: ubuntu-latest
+    permissions:
+      id-token: write
+    steps:
+      - name: Download Artifacts
+        uses: actions/download-artifact@v4
+        with:
+          path: dist/
+          merge-multiple: true
+
+      - name: Publish to PyPI
+        uses: pypa/gh-action-pypi-publish@release/v1
+        with:
+          verbose: true
+          packages-dir: dist/
+          repository-url: https://test.pypi.org/legacy/

README.md

@@ -55,6 +55,9 @@ Optional prerequisites - used automatically if findable by CMake:
 
 GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. It also introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect.
 
+ There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V43` for newly deprecated methods since the 4.3 release, which is on by default, allowing anyone to just pull version 4.3 and compile.
+
+
 ## Wrappers
 
 We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.

examples/GEKF_Rot3Example.cpp

@@ -0,0 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file GEKF_Rot3Example.cpp
+ * @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control
+ * and a single magnetometer update.
+ * @date April 25, 2025
+ * @authors Scott Baker, Matt Kielo, Frank Dellaert
+ */
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/navigation/LieGroupEKF.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+// --- 1) Closed‐loop dynamics f(X): xi = –k·[φx,φy,0], H = ∂xi/∂φ·Dφ ---
+static constexpr double k = 0.5;
+Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
+  // φ = Logmap(R), Dφ = ∂φ/∂δR
+  Matrix3 D_phi;
+  Vector3 phi = Rot3::Logmap(X, D_phi);
+  // zero out yaw
+  phi[2] = 0.0;
+  D_phi.row(2).setZero();
+
+  if (H) *H = -k * D_phi;  // ∂(–kφ)/∂δR
+  return -k * phi;         // xi ∈ 𝔰𝔬(3)
+}
+
+// --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× ---
+static const Vector3 m_world(0, 0, -1);
+Vector3 h_mag(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
+  Vector3 z = X.inverse().rotate(m_world);
+  if (H) *H = -skewSymmetric(z);
+  return z;
+}
+
+int main() {
+  // Initial estimate (identity) and covariance
+  const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3);
+  const Matrix3 P0 = Matrix3::Identity() * 0.1;
+  LieGroupEKF<Rot3> ekf(R0, P0);
+
+  // Timestep, process noise, measurement noise
+  double dt = 0.1;
+  Matrix3 Q = Matrix3::Identity() * 0.01;
+  Matrix3 Rm = Matrix3::Identity() * 0.05;
+
+  cout << "=== Init ===\nR:\n"
+       << ekf.state().matrix() << "\nP:\n"
+       << ekf.covariance() << "\n\n";
+
+  // Predict using state‐dependent f
+  ekf.predict(dynamicsSO3, dt, Q);
+  cout << "--- After predict ---\nR:\n" << ekf.state().matrix() << "\n\n";
+
+  // Magnetometer measurement = body‐frame reading of m_world
+  Vector3 z = h_mag(R0);
+  ekf.update(h_mag, z, Rm);
+  cout << "--- After update ---\nR:\n" << ekf.state().matrix() << "\n";
+
+  return 0;
+}