Merge branch 'develop' into model-selection-bayestree

.github/workflows/build-python.yml

@@ -29,10 +29,10 @@ jobs:
         name:
           [
             ubuntu-20.04-gcc-9,
-            ubuntu-22.04-gcc-9-tbb,
+            ubuntu-20.04-gcc-9-tbb,
             ubuntu-20.04-clang-9,
             macOS-11-xcode-13.4.1,
-            windows-2019-msbuild,
+            windows-2022-msbuild,
           ]
 
         build_type: [Release]
@@ -43,8 +43,8 @@ jobs:
             compiler: gcc
             version: "9"
 
-          - name: ubuntu-22.04-gcc-9-tbb
-            os: ubuntu-22.04
+          - name: ubuntu-20.04-gcc-9-tbb
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: tbb
@@ -59,8 +59,8 @@ jobs:
             compiler: xcode
             version: "13.4.1"
 
-          - name: windows-2019-msbuild
-            os: windows-2019
+          - name: windows-2022-msbuild
+            os: windows-2022
             platform: 64
 
     steps:
@@ -109,6 +109,12 @@ jobs:
         uses: ilammy/msvc-dev-cmd@v1
         with:
           arch: x${{matrix.platform}}
+          toolset: 14.38
+
+      - name: cl version (Windows)
+        if: runner.os == 'Windows'
+        shell: cmd
+        run: cl
 
       - name: Setup python (Windows)
         uses: actions/setup-python@v4
@@ -145,6 +151,12 @@ jobs:
           echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
           echo "GTSAM Uses TBB"
 
+      - name: Set Swap Space (Linux)
+        if: runner.os == 'Linux'
+        uses: pierotofy/set-swap-space@master
+        with:
+          swap-size-gb: 6
+
       - name: Install System Dependencies (Linux, macOS)
         if: runner.os != 'Windows'
         run: |

.github/workflows/build-windows.yml

@@ -27,7 +27,7 @@ jobs:
         # 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: [
-            windows-2019-cl,
+            windows-2022-cl,
           ]
 
         build_type: [
@@ -37,12 +37,25 @@ jobs:
 
         build_unstable: [ON]
         include:
-          - name: windows-2019-cl
-            os: windows-2019
+          - name: windows-2022-cl
+            os: windows-2022
             compiler: cl
             platform: 64
 
     steps:
+      - name: Checkout
+        uses: actions/checkout@v3
+
+      - name: Setup msbuild
+        uses: ilammy/msvc-dev-cmd@v1
+        with:
+          arch: x${{ matrix.platform }}
+          toolset: 14.38
+
+      - name: cl version
+        shell: cmd
+        run: cl
+
       - name: Install Dependencies
         shell: powershell
         run: |
@@ -91,14 +104,6 @@ jobs:
           # Set the BOOST_ROOT variable
           echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV
 
-      - name: Checkout
-        uses: actions/checkout@v3
-
-      - name: Setup msbuild
-        uses: ilammy/msvc-dev-cmd@v1
-        with:
-          arch: x${{ matrix.platform }}
-
       - name: Configuration
         shell: bash
         run: |

gtsam/base/std_optional_serialization.h

@@ -8,10 +8,10 @@
 * Functionality to serialize std::optional<T> to boost::archive
 * Inspired from this PR: https://github.com/boostorg/serialization/pull/163
 * ---------------------------------------------------------------------------- */
+#pragma once
 
 // Defined only if boost serialization is enabled
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
-#pragma once
 #include <optional>
 #include <boost/config.hpp>
 
@@ -55,9 +55,14 @@ namespace std { template<> struct is_trivially_move_constructible<boost::seriali
 #endif
 #endif
 
-
-// Only for old boost
-#if BOOST_VERSION < 108000
+/*
+ * PR https://github.com/boostorg/serialization/pull/163 was merged
+ * on September 3rd 2023,
+ * and so the below code is now a part of Boost 1.84.
+ * We include it for posterity, hence the check for BOOST_VERSION being less
+ * than 1.84.
+ */
+#if BOOST_VERSION < 108400
 // function specializations must be defined in the appropriate
 // namespace - boost::serialization
 namespace boost {
@@ -100,5 +105,5 @@ void serialize(Archive& ar, std::optional<T>& t, const unsigned int version) {
 
 }  // namespace serialization
 }  // namespace boost
-#endif
-#endif
+#endif // BOOST_VERSION < 108400
+#endif // GTSAM_ENABLE_BOOST_SERIALIZATION

gtsam/geometry/CameraSet.h

@@ -327,12 +327,16 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
    * g = F' * (b - E * P * E' * b)
    * Fixed size version
    */
+#ifdef _WIN32
+#if _MSC_VER < 1937
   template <int N>  // N = 2 or 3
   static SymmetricBlockMatrix SchurComplement(
       const FBlocks& Fs, const Matrix& E, const Eigen::Matrix<double, N, N>& P,
       const Vector& b) {
     return SchurComplement<N, D>(Fs, E, P, b);
   }
+#endif
+#endif
 
   /// Computes Point Covariance P, with lambda parameter
   template <int N>  // N = 2 or 3 (point dimension)

gtsam/geometry/Similarity3.h

@@ -202,6 +202,18 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /// @{
 
  private:
+
+  #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template<class Archive>
+    void serialize(Archive & ar, const unsigned int /*version*/) {
+      ar & BOOST_SERIALIZATION_NVP(R_);
+      ar & BOOST_SERIALIZATION_NVP(t_);
+      ar & BOOST_SERIALIZATION_NVP(s_);
+    }
+  #endif
+
   /// Calculate expmap and logmap coefficients.
   static Matrix3 GetV(Vector3 w, double lambda);
 

gtsam/navigation/BarometricFactor.h

@@ -91,7 +91,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
                -0.00649;
     }
 
-    inline double baroOut(const double& meters) {
+    inline double baroOut(const double& meters) const {
         double temp = 15.04 - 0.00649 * meters;
         return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
     }

gtsam/navigation/navigation.i

@@ -294,7 +294,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
   // Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GPSFactor& expected, double tol);
+  bool equals(const gtsam::NonlinearFactor& expected, double tol);
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
@@ -307,12 +307,29 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
   // Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GPSFactor2& expected, double tol);
+  bool equals(const gtsam::NonlinearFactor& expected, double tol);
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
 };
 
+#include <gtsam/navigation/BarometricFactor.h>
+virtual class BarometricFactor : gtsam::NonlinearFactor {
+  BarometricFactor();
+  BarometricFactor(size_t key, size_t baroKey, const double& baroIn,
+                   const gtsam::noiseModel::Base* model);
+
+  // Testable
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::NonlinearFactor& expected, double tol);
+
+  // Standard Interface
+  const double& measurementIn() const;
+  double heightOut(double n) const;
+  double baroOut(const double& meters) const;
+};
+
 #include <gtsam/navigation/Scenario.h>
 virtual class Scenario {
   gtsam::Pose3 pose(double t) const;

gtsam/nonlinear/Values-inl.h

@@ -200,6 +200,7 @@ namespace gtsam {
 // Added this section for compile gtsam python on windows.
 // msvc don't deduct the template arguments correctly, due possible bug in msvc.
 #ifdef _WIN32
+#if _MSC_VER < 1937
    // Handle dynamic matrices
    template <int M, int N>
    struct handle_matrix<Eigen::Matrix<double, M, N, 0, M, N>, true> {
@@ -250,6 +251,7 @@ namespace gtsam {
                             (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
      }
    };
+#endif // #if _MSC_VER < 1937
 #endif // #ifdef _WIN32
 
    }  // internal

gtsam/slam/slam.i

@@ -7,12 +7,13 @@ namespace gtsam {
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/SO4.h>
 #include <gtsam/navigation/ImuBias.h>
+#include <gtsam/geometry/Similarity3.h>
 
 // ######
 
 #include <gtsam/slam/BetweenFactor.h>
 template <T = {double, Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
-               gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3,
+               gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity3,
                gtsam::imuBias::ConstantBias}>
 virtual class BetweenFactor : gtsam::NoiseModelFactor {
   BetweenFactor(size_t key1, size_t key2, const T& relativePose,