Merge pull request #2 from borglab/develop

Update from develop

Author: demul

.github/workflows/build-python.yml

@@ -145,6 +145,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: |

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,7 +55,14 @@ namespace std { template<> struct is_trivially_move_constructible<boost::seriali
 #endif
 #endif
 
-
+/*
+ * 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 {
@@ -98,4 +105,5 @@ void serialize(Archive& ar, std::optional<T>& t, const unsigned int version) {
 
 }  // namespace serialization
 }  // namespace boost
-#endif
+#endif // BOOST_VERSION < 108400
+#endif // GTSAM_ENABLE_BOOST_SERIALIZATION

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/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,