Various fixes to compile with latest GTSAM

.github/workflows/linux-ci.yml

@@ -2,10 +2,22 @@ name: Linux CI
 
 on: [pull_request]
 
+# Cancels any in-progress workflow runs for the same PR when a new push is made,
+# allowing the runner to become available more quickly for the latest changes.
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
+  cancel-in-progress: true
+
 jobs:
   build:
     name: ${{ matrix.name }} ${{ matrix.build_type }}
-    runs-on: ${{ matrix.os }}
+    runs-on: ubuntu-latest
+    container:
+      image: borglab/gtsam:latest
+      env:
+        CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
+      volumes:
+        - ${{ github.workspace }}:/gtdynamics
 
     env:
       CTEST_OUTPUT_ON_FAILURE: ON
@@ -17,57 +29,50 @@ jobs:
       matrix:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
-        name: [ubuntu-22.04-gcc-9, ubuntu-22.04-clang-12]
+        name: [ubuntu-24.04-gcc-11, ubuntu-24.04-clang-16]
 
         build_type: [Debug, Release]
         include:
-          - name: ubuntu-22.04-gcc-9
-            os: ubuntu-22.04
+          - name: ubuntu-24.04-gcc-11
             compiler: gcc
-            version: "9"
+            version: "11"
 
-          - name: ubuntu-22.04-clang-12
-            os: ubuntu-22.04
+          - name: ubuntu-24.04-clang-16
             compiler: clang
-            version: "12"
+            version: "16"
 
     steps:
       - name: Setup Compiler
         run: |
-          sudo apt-get -y clean && sudo apt-get -y update
+          apt-get -y clean && apt-get -y update
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
-            sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
+            apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
 
           else
-            sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
+            apt-get install -y clang-${{ matrix.version }} g++-multilib
             echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
             echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
 
       - name: Install Dependencies
         run: |
           # For SDFormat
-          sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-          sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743
-          sudo apt-get -y update
+          apt-get install -y lsb-release
+          sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+          apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743
+          apt-get -y update
 
-          sudo apt-get -y install libtbb-dev libboost-all-dev libsdformat15-dev
+          apt-get -y install libsdformat15-dev
 
           # Install CppUnitLite.
           git clone https://github.com/borglab/CppUnitLite.git
           cd CppUnitLite && mkdir build && cd $_
-          cmake .. && sudo make -j4 install
+          cmake .. && make -j4 install
           cd ../../
 
-      - name: GTSAM
-        run: |
-          sudo add-apt-repository -y ppa:borglab/gtsam-develop
-          sudo apt-get -y update
-          sudo apt-get -y install libgtsam-no-tbb-dev libgtsam-no-tbb-unstable-dev
-
       - name: Checkout
         uses: actions/checkout@master
 
@@ -88,5 +93,5 @@ jobs:
         working-directory: ./build
 
       - name: Install
-        run: sudo make -j$(nproc) install
+        run: make -j$(nproc) install
         working-directory: ./build

.github/workflows/macos-ci.yml

@@ -2,6 +2,12 @@ name: macOS CI
 
 on: [pull_request]
 
+# Cancels any in-progress workflow runs for the same PR when a new push is made,
+# allowing the runner to become available more quickly for the latest changes.
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
+  cancel-in-progress: true
+
 jobs:
   build:
     name: ${{ matrix.name }} ${{ matrix.build_type }}

.github/workflows/python-ci.yml

@@ -2,6 +2,12 @@ name: Python CI
 
 on: [pull_request]
 
+# Cancels any in-progress workflow runs for the same PR when a new push is made,
+# allowing the runner to become available more quickly for the latest changes.
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
+  cancel-in-progress: true
+
 jobs:
   build:
     name: ${{ matrix.name }} ${{ matrix.build_type }} Python ${{ matrix.python_version }}
@@ -17,18 +23,18 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        name: [ubuntu-22.04-gcc-9, ubuntu-22.04-clang-12, macOS-14-xcode-15.4]
+        name: [ubuntu-24.04-gcc-11, ubuntu-24.04-clang-16, macOS-14-xcode-15.4]
         build_type: [Debug, Release]
         python_version: [3]
         include:
-          - name: ubuntu-22.04-gcc-9
-            os: ubuntu-22.04
+          - name: ubuntu-24.04-gcc-11
+            os: ubuntu-24.04
             compiler: gcc
-            version: "9"
-          - name: ubuntu-22.04-clang-12
-            os: ubuntu-22.04
+            version: "11"
+          - name: ubuntu-24.04-clang-16
+            os: ubuntu-24.04
             compiler: clang
-            version: "12"
+            version: "16"
           - name: macOS-14-xcode-15.4
             os: macOS-14
             compiler: xcode

examples/example_constraint_manifold/main_quadruped.cpp

@@ -26,7 +26,6 @@
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/LevenbergMarquardtParams.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/expressions.h>

examples/scripts/constrainedExample.h

@@ -18,7 +18,6 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/expressions.h>
 
 namespace gtdynamics {

gtdynamics.i

@@ -679,7 +679,7 @@ gtsam::Key ContactWrenchKey(int i, int k, int t=0);
 gtsam::Key PhaseKey(int k);
 gtsam::Key TimeKey(int t);
 
-///////////////////// Key Methods /////////////////////
+/******************** Key Methods ********************/
 void InsertJointAngle(gtsam::Values@ values, int j, int t, double value);
 
 void InsertJointAngle(gtsam::Values @values, int j, double value);

gtdynamics/cablerobot/factors/CableLengthFactor.h

@@ -11,7 +11,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
 #include <iostream>
 #include <string>

gtdynamics/cablerobot/factors/CableTensionFactor.cpp

@@ -12,7 +12,6 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
 
 #include <iostream>
 #include <string>

gtdynamics/cablerobot/factors/CableTensionFactor.h

@@ -12,7 +12,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
 #include <iostream>
 #include <string>

gtdynamics/cablerobot/factors/CableVelocityFactor.h

@@ -12,7 +12,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
 #include <iostream>
 #include <string>

gtdynamics/factors/CollocationFactors.h

@@ -17,7 +17,7 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
 #include <string>
 
@@ -96,7 +96,7 @@ class EulerPoseCollocationFactor
     return error;
   }
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
@@ -180,7 +180,7 @@ class TrapezoidalPoseCollocationFactor
     return error;
   }
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
@@ -261,7 +261,7 @@ class FixTimeTrapezoidalPoseCollocationFactor
     return error;
   }
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
@@ -338,7 +338,7 @@ class EulerTwistCollocationFactor
     return error;
   }
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
@@ -423,7 +423,7 @@ class TrapezoidalTwistCollocationFactor
     return error;
   }
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
@@ -505,7 +505,7 @@ class FixTimeTrapezoidalTwistCollocationFactor
     return error;
   }
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));

gtdynamics/factors/ContactDynamicsFrictionConeFactor.h

@@ -17,7 +17,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
 #include <iostream>
 #include <optional>
@@ -140,7 +140,7 @@ class ContactDynamicsFrictionConeFactor
     return error;
   }
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));

gtdynamics/factors/ContactDynamicsMomentFactor.h

@@ -94,7 +94,7 @@ class ContactDynamicsMomentFactor
 
   virtual ~ContactDynamicsMomentFactor() {}
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));

gtdynamics/factors/ContactEqualityFactor.h

@@ -19,7 +19,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
 #include <memory>
 #include <string>

gtdynamics/factors/ContactHeightFactor.h

@@ -72,7 +72,7 @@ class ContactHeightFactor : public gtsam::ExpressionFactor<double> {
 
   virtual ~ContactHeightFactor() {}
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));

gtdynamics/factors/ContactKinematicsAccelFactor.h

@@ -71,7 +71,7 @@ class ContactKinematicsAccelFactor
 
   virtual ~ContactKinematicsAccelFactor() {}
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));

gtdynamics/factors/ContactKinematicsTwistFactor.h

@@ -70,7 +70,7 @@ class ContactKinematicsTwistFactor
              ContactKinematicsTwistConstraint(twist_key, cTcom)) {}
   virtual ~ContactKinematicsTwistFactor() {}
 
-  //// @return a deep copy of this factor
+  /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
     return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));