Fix Matlab wrapper

gtsam/geometry/SO3.h

@@ -165,8 +165,8 @@ struct GTSAM_EXPORT ExpmapFunctor {
 /// Math extends Ethan theme of elegant I + aW + bWW expressions.
 /// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83).
 struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
-  const Vector3 omega;  ///< The rotation vector.
-  bool nearPi{false};   ///< Flag indicating if theta is near pi.
+  Vector3 omega;       ///< The rotation vector.
+  bool nearPi{false};  ///< Flag indicating if theta is near pi.
 
   /// Constructor with element of Lie algebra so(3)
   explicit DexpFunctor(const Vector3& omega);

gtsam/geometry/SimpleCamera.h

@@ -23,7 +23,6 @@
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
-#include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
 

gtsam/geometry/geometry.i

@@ -221,11 +221,11 @@ namespace so3 {
   };
 
   class ExpmapFunctor {
-    const double theta2;
-    const double theta;
-    const Matrix3 W;
-    const Matrix3 WW;
-    const bool nearZero;
+    double theta2;
+    double theta;
+    gtsam::Matrix3 W;
+    gtsam::Matrix3 WW;
+    bool nearZero;
     double A;  // A = sin(theta) / theta
     double B;  // B = (1 - cos(theta))
     ExpmapFunctor(const gtsam::Vector3& omega);
@@ -235,7 +235,7 @@ namespace so3 {
   };
 
   virtual class DexpFunctor : gtsam::so3::ExpmapFunctor {
-    const gtsam::Vector3 omega;
+    gtsam::Vector3 omega;
 
     DexpFunctor(const gtsam::Vector3& omega);
     DexpFunctor(const gtsam::Vector3& omega, double nearZeroThresholdSq, double nearPiThresholdSq);
@@ -291,8 +291,6 @@ class SO3 {
   static gtsam::Matrix3 LogmapDerivative(const gtsam::Vector3& omega);
   gtsam::SO3 expmap(gtsam::Vector3 v);
   gtsam::Vector3 logmap(const gtsam::SO3& g);
-  static gtsam::Matrix3 Hat(const gtsam::Vector3& xi);
-  static gtsam::Vector3 Vee(const gtsam::Matrix3& xi);
 
   // Matrix Lie Group
   gtsam::Vector vec() const;

gtsam/inference/inference.i

@@ -120,15 +120,20 @@ class Ordering {
   Ordering(const std::vector<size_t>& keys);
 
   template <
-      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
-
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
+                      gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph,
+                      gtsam::GaussianFactorGraph,
+                      gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
   static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex);
 
   template <
-      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
+                      gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph,
+                      gtsam::GaussianFactorGraph,
+                      gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering ColamdConstrainedLast(
       const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
       bool forceOrder = false);

gtsam/linear/linear.i

@@ -684,7 +684,7 @@ virtual class GaussianBayesTree {
                                 gtsam::DefaultKeyFormatter);
   size_t size() const;
   bool empty() const;
-  const GaussianBayesTree::Roots& roots() const;
+  const gtsam::GaussianBayesTree::Roots& roots() const;
   const gtsam::GaussianBayesTreeClique* operator[](size_t j) const;
   size_t numCachedSeparatorMarginals() const;
 

gtsam/nonlinear/nonlinear.i

@@ -710,7 +710,7 @@ template <T = {double,
                gtsam::Pose3,
                gtsam::Similarity2,
                gtsam::Similarity3}>
-class ConcentratedGaussian : gtsam::ExtendedPriorFactor<T> {
+virtual class ConcentratedGaussian : gtsam::ExtendedPriorFactor<T> {
   ConcentratedGaussian();
   // Constructors mirroring header (origin terminology)
   ConcentratedGaussian(gtsam::Key key, const T& origin, const gtsam::noiseModel::Gaussian::shared_ptr& noiseModel);
@@ -726,10 +726,10 @@ class ConcentratedGaussian : gtsam::ExtendedPriorFactor<T> {
   double evaluate(const T& x) const;
   double evaluate(const gtsam::Values& values) const;
   // Chart transport / reset operations
-  ConcentratedGaussian reset() const;
-  ConcentratedGaussian transportTo(const T& x_hat) const;
+  This reset() const;
+  This transportTo(const T& x_hat) const;
   // Fusion operator
-  ConcentratedGaussian operator*(const ConcentratedGaussian& other) const;
+  This operator*(const This& other) const;
 };
 
 #include <gtsam/nonlinear/NonlinearEquality.h>

gtsam/slam/dataset.h

@@ -20,25 +20,26 @@
 
 #pragma once
 
-#include <gtsam/sfm/BinaryMeasurement.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/sfm/SfmData.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/types.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/SL4.h>
+#include <gtsam/linear/NoiseModel.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/linear/NoiseModel.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/types.h>
+#include <gtsam/sfm/BinaryMeasurement.h>
+#include <gtsam/sfm/SfmData.h>
+#include <gtsam/slam/BetweenFactor.h>
 
-#include <string>
-#include <utility> // for pair
-#include <vector>
 #include <iosfwd>
 #include <map>
 #include <optional>
+#include <string>
+#include <utility>  // for pair
+#include <vector>
 
 namespace gtsam {
 
@@ -221,6 +222,8 @@ parse3DFactors(const std::string &filename,
                const noiseModel::Diagonal::shared_ptr &model = nullptr,
                size_t maxIndex = 0);
 
+using BetweenFactorSL4s = std::vector<BetweenFactor<SL4>::shared_ptr>;
+
 using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
 using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
 using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;

gtsam/slam/slam.i

@@ -4,13 +4,14 @@
 
 namespace gtsam {
 
-#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/SO4.h>
 #include <gtsam/geometry/SL4.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/geometry/Similarity2.h>
 #include <gtsam/geometry/Similarity3.h>
 #include <gtsam/geometry/Gal3.h>
+// Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified}
+#include <gtsam/geometry/SimpleCamera.h>
 
 // ######
 
@@ -154,9 +155,6 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
-// Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified}
-#include <gtsam/geometry/SimpleCamera.h>
-
 #include <gtsam/slam/SmartFactorBase.h>
 
 // Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3
@@ -170,8 +168,8 @@ virtual class SmartFactorBase : gtsam::NonlinearFactor {
   void add(const gtsam::Point2& measured, gtsam::Key key);
   void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& cameraKeys);
   size_t dim() const;
-  const std::vector<gtsam::Point2>& measured() const;
-  std::vector<CAMERA> cameras(const gtsam::Values& values) const;
+  const gtsam::Point2Vector& measured() const;
+  gtsam::CameraSet<CAMERA> cameras(const gtsam::Values& values) const;
 
   void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter =
     gtsam::DefaultKeyFormatter) const;
@@ -219,27 +217,27 @@ virtual class SmartProjectionFactor : gtsam::SmartFactorBase<CAMERA> {
   gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet<CAMERA>& cameras) const;
   bool triangulateForLinearize(const gtsam::CameraSet<CAMERA>& cameras) const;
 
-  gtsam::HessianFactor createHessianFactor(
+  gtsam::HessianFactor* createHessianFactor(
       const gtsam::CameraSet<CAMERA>& cameras, const double lambda = 0.0,
       bool diagonalDamping = false) const;
-  gtsam::JacobianFactor createJacobianQFactor(
+  gtsam::JacobianFactor* createJacobianQFactor(
       const gtsam::CameraSet<CAMERA>& cameras, double lambda) const;
-  gtsam::JacobianFactor createJacobianQFactor(
+  gtsam::JacobianFactor* createJacobianQFactor(
       const gtsam::Values& values, double lambda) const;
-  gtsam::JacobianFactor createJacobianSVDFactor(
+  gtsam::JacobianFactor* createJacobianSVDFactor(
       const gtsam::CameraSet<CAMERA>& cameras, double lambda) const;
-  gtsam::HessianFactor linearizeToHessian(
+  gtsam::HessianFactor* linearizeToHessian(
       const gtsam::Values& values, double lambda = 0.0) const;
-  gtsam::JacobianFactor linearizeToJacobian(
+  gtsam::JacobianFactor* linearizeToJacobian(
       const gtsam::Values& values, double lambda = 0.0) const;
 
-  gtsam::GaussianFactor linearizeDamped(const gtsam::CameraSet<CAMERA>& cameras,
+  gtsam::GaussianFactor* linearizeDamped(const gtsam::CameraSet<CAMERA>& cameras,
       const double lambda = 0.0) const;
 
-  gtsam::GaussianFactor linearizeDamped(const gtsam::Values& values,
+  gtsam::GaussianFactor* linearizeDamped(const gtsam::Values& values,
       const double lambda = 0.0) const;
 
-  gtsam::GaussianFactor linearize(
+  gtsam::GaussianFactor* linearize(
       const gtsam::Values& values) const;
 
   bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::CameraSet<CAMERA>& cameras) const;
@@ -312,7 +310,7 @@ virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor<CAMERA> {
            const gtsam::FastVector<size_t>& cameraIds = gtsam::FastVector<size_t>());
 
   const gtsam::KeyVector& nonUniqueKeys() const;
-  const gtsam::CameraSet<CAMERA>& cameraRig() const;
+  const gtsam::CameraSet<CAMERA>* cameraRig() const;
   const gtsam::FastVector<size_t>& cameraIds() const;
 };
 
@@ -367,6 +365,7 @@ class RotateFactor : gtsam::NoiseModelFactor {
 
   gtsam::Vector evaluateError(const gtsam::Rot3& R) const;
 };
+
 class RotateDirectionsFactor : gtsam::NoiseModelFactor {
   RotateDirectionsFactor(gtsam::Key key, const gtsam::Unit3& i_p, const gtsam::Unit3& c_z,
     const gtsam::noiseModel::Base* model);
@@ -391,6 +390,7 @@ class OrientedPlane3Factor : gtsam::NoiseModelFactor {
   gtsam::Vector evaluateError(
       const gtsam::Pose3& pose, const gtsam::OrientedPlane3& plane) const;
 };
+
 class OrientedPlane3DirectionPrior : gtsam::NoiseModelFactor {
   OrientedPlane3DirectionPrior();
   OrientedPlane3DirectionPrior(gtsam::Key key, const gtsam::Vector& z,

gtsam/symbolic/symbolic.i

@@ -206,7 +206,7 @@ class SymbolicBayesTree {
   // Standard Interface
   bool empty() const;
   size_t size() const;
-  const SymbolicBayesTree::Roots& roots() const;
+  const gtsam::SymbolicBayesTree::Roots& roots() const;
   const gtsam::SymbolicBayesTreeClique* operator[](size_t j) const;
 
   void saveGraph(string s,

matlab/CMakeLists.txt

@@ -23,7 +23,7 @@ set(GTSAM_TOOLBOX_INSTALL_PATH
       "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox"
 )
 if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
-  set(GTSAM_TOOLBOX_INSTALL_PATH "gtsam_toolbox")
+  set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
 endif()
 
 set(WRAP_MEX_BUILD_STATIC_MODULE ${GTSAM_MEX_BUILD_STATIC_MODULE})

wrap/.github/workflows/linux-ci.yml

@@ -5,35 +5,33 @@ on: [pull_request]
 jobs:
   build:
     name: Tests for 🐍 ${{ matrix.python-version }}
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-latest
 
     strategy:
       fail-fast: false
       matrix:
-        python-version: ["3.9", "3.10", "3.11", "3.12"]
+        python-version: ["3.10", "3.11", "3.12", "3.13"]
 
     steps:
       - name: Checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v5
 
       - name: Install Dependencies
         run: |
           sudo apt-get -y update
-          sudo apt install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev
+          sudo apt install cmake
 
-      - name: Set up Python ${{ matrix.python-version }}
-        uses: actions/setup-python@v2
+      - name: Install uv and set the ${{ matrix.python-version }}
+        uses: astral-sh/setup-uv@v6
         with:
           python-version: ${{ matrix.python-version }}
 
       - name: Python Dependencies
-        run: |
-          sudo pip3 install -U pip setuptools
-          sudo pip3 install -r requirements.txt
+        run: uv sync --locked --all-extras --dev
 
       - name: Build and Test
         run: |
+          # Build
           cmake .
-          cd tests
           # Use Pytest to run all the tests.
-          pytest
+          uv run pytest tests

wrap/.github/workflows/macos-ci.yml

@@ -5,36 +5,32 @@ on: [pull_request]
 jobs:
   build:
     name: Tests for 🐍 ${{ matrix.python-version }}
-    runs-on: macos-14
+    runs-on: macos-latest
 
     strategy:
       fail-fast: false
       matrix:
-        python-version: ["3.9", "3.10", "3.11", "3.12"]
+        python-version: ["3.10", "3.11", "3.12", "3.13"]
 
     steps:
       - name: Checkout
         uses: actions/checkout@v2
 
       - name: Install Dependencies
         run: |
-          brew install cmake ninja boost
+          brew install cmake ninja
 
-      - name: Set up Python ${{ matrix.python-version }}
-        uses: actions/setup-python@v2
+      - name: Install uv and set the ${{ matrix.python-version }}
+        uses: astral-sh/setup-uv@v6
         with:
           python-version: ${{ matrix.python-version }}
 
       - name: Python Dependencies
-        run: |
-          pip3 install -U pip setuptools
-          pip3 install -r requirements.txt
+        run: uv sync --locked --all-extras --dev
 
       - name: Build and Test
         run: |
           # Build
           cmake .
-          cd tests
           # Use Pytest to run all the tests.
-          pytest
-
+          uv run pytest tests

wrap/.gitignore

@@ -11,3 +11,5 @@ __pycache__/
 **/.coverage
 
 gtwrap/matlab_wrapper/matlab_wrapper.tpl
+
+.python-version

wrap/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.9)
+cmake_minimum_required(VERSION 3.22)
 
 # Set the project name and version
 project(gtwrap VERSION 1.0)
@@ -34,14 +34,13 @@ configure_package_config_file(
   ${CMAKE_CURRENT_BINARY_DIR}/cmake/gtwrapConfig.cmake
   INSTALL_DESTINATION "${INSTALL_CMAKE_DIR}"
   PATH_VARS INSTALL_CMAKE_DIR INSTALL_LIB_DIR INSTALL_BIN_DIR
-            INSTALL_INCLUDE_DIR
-  INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
+            INSTALL_INCLUDE_DIR)
 
 # Set all the install paths
-set(GTWRAP_CMAKE_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR})
-set(GTWRAP_LIB_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${INSTALL_LIB_DIR})
-set(GTWRAP_BIN_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${INSTALL_BIN_DIR})
-set(GTWRAP_INCLUDE_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${INSTALL_INCLUDE_DIR})
+set(GTWRAP_CMAKE_INSTALL_DIR $${INSTALL_CMAKE_DIR})
+set(GTWRAP_LIB_INSTALL_DIR ${INSTALL_LIB_DIR})
+set(GTWRAP_BIN_INSTALL_DIR ${INSTALL_BIN_DIR})
+set(GTWRAP_INCLUDE_INSTALL_DIR ${INSTALL_INCLUDE_DIR})
 
 # ##############################################################################
 # Install the package