linear, navigation, std::optional serialization tests

examples/SFMExample_SmartFactor.cpp

@@ -114,10 +114,10 @@ int main(int argc, char* argv[]) {
     // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
     SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
     if (smart) {
-      // The output of point() is in boost::optional<Point3>, as sometimes
+      // The output of point() is in std::optional<Point3>, as sometimes
       // the triangulation operation inside smart factor will encounter degeneracy.
       auto point = smart->point(result);
-      if (point) // ignore if boost::optional return nullptr
+      if (point) // ignore if std::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }

examples/SFMExample_SmartFactorPCG.cpp

@@ -110,8 +110,8 @@ int main(int argc, char* argv[]) {
   for (size_t j = 0; j < points.size(); ++j) {
     auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
     if (smart) {
-      boost::optional<Point3> point = smart->point(result);
+      std::optional<Point3> point = smart->point(result);
-      if (point)  // ignore if boost::optional return nullptr
+      if (point)  // ignore if std::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }

examples/TriangulationLOSTExample.cpp

@@ -29,6 +29,7 @@
 #include <chrono>
 #include <iostream>
 #include <random>
+#include <optional>
 
 using namespace std;
 using namespace gtsam;
@@ -145,9 +146,9 @@ int main(int argc, char* argv[]) {
         cameras, noisyMeasurements, rank_tol, true, measurementNoise, false);
     durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart;
 
-    errorsLOST.row(i) = *estimateLOST - landmark;
+    errorsLOST.row(i) = estimateLOST - landmark;
-    errorsDLT.row(i) = *estimateDLT - landmark;
+    errorsDLT.row(i) = estimateDLT - landmark;
-    errorsDLTOpt.row(i) = *estimateDLTOpt - landmark;
+    errorsDLTOpt.row(i) = estimateDLTOpt - landmark;
   }
   PrintCovarianceStats(errorsLOST, "LOST");
   PrintCovarianceStats(errorsDLT, "DLT");

gtsam/base/serializationTestHelpers.h

@@ -24,6 +24,7 @@
 #include <string>
 
 #include <gtsam/base/serialization.h>
+#include <gtsam/base/TestableAssertions.h>
 
 #include <boost/serialization/serialization.hpp>
 #include <boost/filesystem.hpp>

gtsam/base/std_optional_serialization.h

@@ -26,24 +26,11 @@ void save(Archive& ar, const std::optional<T>& t, const unsigned int /*version*/
 ) {
   // It is an inherent limitation to the serialization of optional.hpp
   // that the underlying type must be either a pointer or must have a
-  // default constructor.  It's possible that this could change sometime
+  // default constructor.
-  // in the future, but for now, one will have to work around it.  This can
-  // be done by serialization the optional<T> as optional<T *>
   BOOST_STATIC_ASSERT(boost::serialization::detail::is_default_constructible<T>::value || boost::is_pointer<T>::value);
   const bool tflag = t.has_value();
   ar << boost::serialization::make_nvp("initialized", tflag);
   if (tflag) {
-    const boost::serialization::item_version_type item_version(version<T>::value);
-#if 0
-        const boost::archive::library_version_type library_version(
-            ar.get_library_version()
-        };
-        if(boost::archive::library_version_type(3) < library_version){
-            ar << BOOST_SERIALIZATION_NVP(item_version);
-        }
-#else
-    ar << BOOST_SERIALIZATION_NVP(item_version);
-#endif
     ar << boost::serialization::make_nvp("value", *t);
   }
 }
@@ -58,16 +45,11 @@ void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/
     return;
   }
 
-  boost::serialization::item_version_type item_version(0);
+  if (!t.has_value()) {
-  boost::archive::library_version_type library_version(ar.get_library_version());
+    // Need to be default constructible
-  if (boost::archive::library_version_type(3) < library_version) {
+    t.emplace();
-    ar >> BOOST_SERIALIZATION_NVP(item_version);
   }
-  detail::stack_allocate<T> tp;
+  ar >> boost::serialization::make_nvp("value", *t);
-  ar >> boost::serialization::make_nvp("value", tp.reference());
-  t = std::move(tp.reference());
-  // hack because std::optional does not have get_ptr fn
-  ar.reset_object_address(&(t.value()), &tp.reference());
 }
 
 template <class Archive, class T>
@@ -75,6 +57,8 @@ void serialize(Archive& ar, std::optional<T>& t, const unsigned int version) {
   boost::serialization::split_free(ar, t, version);
 }
 
+// derive boost::xml_archive_impl for archiving std::optional<T> with xml
+
 }  // namespace serialization
 }  // namespace boost
 

gtsam/base/tests/testStdOptionalSerialization.cpp

@@ -0,0 +1,110 @@
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+#include <boost/archive/xml_oarchive.hpp>
+
+#include "gtsam/base/serializationTestHelpers.h"
+#include "gtsam/base/std_optional_serialization.h"
+
+using namespace gtsam;
+
+// A test to check that the serialization of std::optional works with boost
+TEST(StdOptionalSerialization, SerializeIntOptional) {
+  // Create an optional
+  std::optional<int> opt(42);
+
+  // Serialize it
+  std::stringstream ss;
+  boost::archive::text_oarchive oa(ss);
+  oa << opt;
+
+  // Deserialize it
+  std::optional<int> opt2;
+  boost::archive::text_iarchive ia(ss);
+  ia >> opt2;
+
+  // Check that it worked
+  EXPECT(opt2.has_value());
+  EXPECT(*opt2 == 42);
+}
+
+// A test to check if we serialize an empty optional, it is deserialized as an
+// empty optional
+TEST(StdOptionalSerialization, SerializeEmptyOptional) {
+  // Create an optional
+  std::optional<int> opt;
+
+  // Serialize it
+  std::stringstream ss;
+  boost::archive::text_oarchive oa(ss);
+  oa << opt;
+
+  // Deserialize it
+  std::optional<int> opt2 = 43;
+  boost::archive::text_iarchive ia(ss);
+  ia >> opt2;
+
+  // Check that it worked
+  EXPECT(!opt2.has_value());
+}
+
+/* ************************************************************************* */
+// Implement the equals trait for TestOptionalStruct
+namespace gtsam {
+// A struct which contains an optional
+class TestOptionalStruct {
+public:
+  std::optional<int> opt;
+  TestOptionalStruct() = default;
+  TestOptionalStruct(const int& opt)
+      : opt(opt) {}
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(opt);
+  }
+};
+
+template <>
+struct traits<TestOptionalStruct> {
+  static bool Equals(const TestOptionalStruct& q1, const TestOptionalStruct& q2, double) {
+    // if both have values then compare the values
+    if (q1.opt.has_value() && q2.opt.has_value()) {
+      return *q1.opt == *q2.opt;
+    }
+    // otherwise check that both are empty
+    return q1.opt == q2.opt;
+  }
+
+  static void Print(const TestOptionalStruct& m, const std::string& s = "") {
+    /* std::cout << s << "opt: " << m.opt << std::endl; */
+    if (m.opt.has_value()) {
+      std::cout << s << "opt: " << *m.opt << std::endl;
+    } else {
+      std::cout << s << "opt: empty" << std::endl;
+    }
+  }
+};
+}  // namespace gtsam
+
+// Test for a struct with an initialized optional
+TEST(StdOptionalSerialization, SerializTestOptionalStruct) {
+  TestOptionalStruct optStruct(10);
+  EXPECT(serializationTestHelpers::equalsObj(optStruct));
+  EXPECT(serializationTestHelpers::equalsXML(optStruct));
+  EXPECT(serializationTestHelpers::equalsBinary(optStruct));
+}
+
+// Test for a struct with an uninitialized optional
+TEST(StdOptionalSerialization, SerializTestOptionalStructUninitialized) {
+  TestOptionalStruct optStruct;
+  EXPECT(serializationTestHelpers::equalsObj(optStruct));
+  EXPECT(serializationTestHelpers::equalsXML(optStruct));
+  EXPECT(serializationTestHelpers::equalsBinary(optStruct));
+}
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}

gtsam/linear/AcceleratedPowerMethod.h

@@ -60,11 +60,11 @@ class AcceleratedPowerMethod : public PowerMethod<Operator> {
    * vector as ritzVector
    */
   explicit AcceleratedPowerMethod(
-      const Operator &A, const boost::optional<Vector> initial = boost::none,
+      const Operator &A, const std::optional<Vector> initial = {},
       double initialBeta = 0.0)
       : PowerMethod<Operator>(A, initial) {
     // initialize Ritz eigen vector and previous vector
-    this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_);
+    this->ritzVector_ = initial ? *initial : Vector::Random(this->dim_);
     this->ritzVector_.normalize();
     previousVector_ = Vector::Zero(this->dim_);
 

gtsam/linear/IterativeSolver.h

@@ -23,11 +23,11 @@
 
 #include <boost/tuple/tuple.hpp>
 #include <boost/shared_ptr.hpp>
-#include <boost/optional.hpp>
 
 #include <iosfwd>
 #include <string>
 #include <map>
+#include <optional>
 
 namespace gtsam {
 

gtsam/linear/JacobianFactor.cpp

@@ -264,7 +264,7 @@ void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph,
   // Copy the RHS vectors and sigmas
   gttic(copy_vectors);
   bool anyConstrained = false;
-  boost::optional<Vector> sigmas;
+  std::optional<Vector> sigmas;
   // Loop over source jacobians
   DenseIndex nextRow = 0;
   for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) {

gtsam/linear/NoiseModel.cpp

@@ -47,7 +47,7 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
 
 /* ************************************************************************* */
 // check *above the diagonal* for non-zero entries
-boost::optional<Vector> checkIfDiagonal(const Matrix& M) {
+std::optional<Vector> checkIfDiagonal(const Matrix& M) {
   size_t m = M.rows(), n = M.cols();
   assert(m > 0);
   // check all non-diagonal entries
@@ -61,7 +61,7 @@ boost::optional<Vector> checkIfDiagonal(const Matrix& M) {
           break;
         }
   if (full) {
-    return boost::none;
+    return {};
   } else {
     Vector diagonal(n);
     for (j = 0; j < n; j++)
@@ -88,7 +88,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
   if (m != n)
     throw invalid_argument("Gaussian::SqrtInformation: R not square");
   if (smart) {
-    boost::optional<Vector> diagonal = checkIfDiagonal(R);
+    std::optional<Vector> diagonal = checkIfDiagonal(R);
     if (diagonal)
       return Diagonal::Sigmas(diagonal->array().inverse(), true);
   }
@@ -101,7 +101,7 @@ Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart
   size_t m = information.rows(), n = information.cols();
   if (m != n)
     throw invalid_argument("Gaussian::Information: R not square");
-  boost::optional<Vector> diagonal = boost::none;
+  std::optional<Vector> diagonal = {};
   if (smart)
     diagonal = checkIfDiagonal(information);
   if (diagonal)
@@ -119,7 +119,7 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
   size_t m = covariance.rows(), n = covariance.cols();
   if (m != n)
     throw invalid_argument("Gaussian::Covariance: covariance not square");
-  boost::optional<Vector> variances = boost::none;
+  std::optional<Vector> variances = {};
   if (smart)
     variances = checkIfDiagonal(covariance);
   if (variances)
@@ -426,8 +426,8 @@ Constrained::shared_ptr Constrained::unit() const {
 // Check whether column a triggers a constraint and corresponding variable is deterministic
 // Return constraint_row with maximum element in case variable plays in multiple constraints
 template <typename VECTOR>
-boost::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) {
+std::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) {
-  boost::optional<size_t> constraint_row;
+  std::optional<size_t> constraint_row;
   // not zero, so roundoff errors will not be counted
   // TODO(frank): that's a fairly crude way of dealing with roundoff errors :-(
   double max_element = 1e-9;
@@ -437,7 +437,7 @@ boost::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, s
     double abs_ai = std::abs(a(i,0));
     if (abs_ai > max_element) {
       max_element = abs_ai;
-      constraint_row.reset(i);
+      constraint_row = i;
     }
   }
   return constraint_row;
@@ -468,7 +468,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
     Eigen::Block<Matrix> a = Ab.block(0, j, m, 1);
 
     // Check whether we need to handle as a constraint
-    boost::optional<size_t> constraint_row = check_if_constraint(a, invsigmas, m);
+    std::optional<size_t> constraint_row = check_if_constraint(a, invsigmas, m);
 
     if (constraint_row) {
       // Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j)

gtsam/linear/NoiseModel.h

@@ -20,6 +20,7 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/std_optional_serialization.h>
 #include <gtsam/dllexport.h>
 #include <gtsam/linear/LossFunctions.h>
 
@@ -27,7 +28,8 @@
 #include <boost/serialization/extended_type_info.hpp>
 #include <boost/serialization/singleton.hpp>
 #include <boost/serialization/shared_ptr.hpp>
-#include <boost/serialization/optional.hpp>
+
+#include <optional>
 
 namespace gtsam {
 
@@ -164,7 +166,7 @@ namespace gtsam {
     protected:
 
       /** Matrix square root of information matrix (R) */
-      boost::optional<Matrix> sqrt_information_;
+      std::optional<Matrix> sqrt_information_;
 
     private:
 
@@ -184,7 +186,7 @@ namespace gtsam {
 
       /** constructor takes square root information matrix */
       Gaussian(size_t dim = 1,
-               const boost::optional<Matrix>& sqrt_information = boost::none)
+               const std::optional<Matrix>& sqrt_information = {})
           : Base(dim), sqrt_information_(sqrt_information) {}
 
       ~Gaussian() override {}
@@ -732,7 +734,7 @@ namespace gtsam {
     };
 
     // Helper function
-    GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(const Matrix& M);
+    GTSAM_EXPORT std::optional<Vector> checkIfDiagonal(const Matrix& M);
 
   } // namespace noiseModel
 

gtsam/linear/PowerMethod.h

@@ -26,6 +26,7 @@
 #include <Eigen/Sparse>
 #include <random>
 #include <vector>
+#include <optional>
 
 namespace gtsam {
 
@@ -75,10 +76,10 @@ class PowerMethod {
 
   /// Construct from the aim matrix and intial ritz vector
   explicit PowerMethod(const Operator &A,
-                       const boost::optional<Vector> initial = boost::none)
+                       const std::optional<Vector> initial = {})
       : A_(A), dim_(A.rows()), nrIterations_(0) {
     Vector x0;
-    x0 = initial ? initial.get() : Vector::Random(dim_);
+    x0 = initial ? *initial : Vector::Random(dim_);
     x0.normalize();
 
     // initialize Ritz eigen value

gtsam/navigation/AHRSFactor.cpp

@@ -185,7 +185,7 @@ Rot3 AHRSFactor::Predict(const Rot3& rot_i, const Vector3& bias,
 AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
                        const PreintegratedAhrsMeasurements& pim,
                        const Vector3& omegaCoriolis,
-                       const boost::optional<Pose3>& body_P_sensor)
+                       const std::optional<Pose3>& body_P_sensor)
     : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j,
            bias),
       _PIM_(pim) {
@@ -198,7 +198,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
 Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
                          const PreintegratedAhrsMeasurements& pim,
                          const Vector3& omegaCoriolis,
-                         const boost::optional<Pose3>& body_P_sensor) {
+                         const std::optional<Pose3>& body_P_sensor) {
   auto p = boost::make_shared<PreintegratedAhrsMeasurements::Params>(pim.p());
   p->omegaCoriolis = omegaCoriolis;
   p->body_P_sensor = body_P_sensor;

gtsam/navigation/AHRSFactor.h

@@ -24,6 +24,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/Pose3.h>
 
+#include <optional>
+
 namespace gtsam {
 
 /**
@@ -194,13 +196,13 @@ public:
   AHRSFactor(Key rot_i, Key rot_j, Key bias,
              const PreintegratedAhrsMeasurements& pim,
              const Vector3& omegaCoriolis,
-             const boost::optional<Pose3>& body_P_sensor = boost::none);
+             const std::optional<Pose3>& body_P_sensor = {});
 
   /// @deprecated static function, but used in tests.
   static Rot3 predict(
       const Rot3& rot_i, const Vector3& bias,
       const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
-      const boost::optional<Pose3>& body_P_sensor = boost::none);
+      const std::optional<Pose3>& body_P_sensor = {});
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
   /// @deprecated name

gtsam/navigation/ImuBias.cpp

@@ -38,7 +38,7 @@ namespace imuBias {
 //    // H1: Jacobian w.r.t. IMUBias
 //    // H2: Jacobian w.r.t. pose
 //    Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G,
-//        boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
+//        Matrix* H1=nullptr, Matrix* H2=nullptr) const {
 //
 //      Matrix R_G_to_I( pose.rotation().matrix().transpose() );
 //      Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G;

gtsam/navigation/MagPoseFactor.h

@@ -11,9 +11,12 @@
 
 #pragma once
 
+#include <gtsam/base/std_optional_serialization.h>
 #include <gtsam/geometry/concepts.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
+#include <optional>
+
 namespace gtsam {
 
 /**
@@ -35,7 +38,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
   const Point measured_; ///< The measured magnetometer data in the body frame.
   const Point nM_; ///< Local magnetic field (mag output units) in the nav frame.
   const Point bias_; ///< The bias vector (mag output units) in the body frame.
-  boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame.
+  std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame.
 
   static const int MeasDim = Point::RowsAtCompileTime;
   static const int PoseDim = traits<POSE>::dimension;
@@ -74,7 +77,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
                 const Point& direction,
                 const Point& bias,
                 const SharedNoiseModel& model,
-                const boost::optional<POSE>& body_P_sensor)
+                const std::optional<POSE>& body_P_sensor)
       : Base(model, pose_key),
         measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
         nM_(scale * direction.normalized()),

gtsam/navigation/NavState.cpp

@@ -256,7 +256,7 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
 
 //------------------------------------------------------------------------------
 Vector9 NavState::correctPIM(const Vector9& pim, double dt,
-    const Vector3& n_gravity, const boost::optional<Vector3>& omegaCoriolis,
+    const Vector3& n_gravity, const std::optional<Vector3>& omegaCoriolis,
     bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
     OptionalJacobian<9, 9> H2) const {
   const Rot3& nRb = R_;

gtsam/navigation/NavState.h

@@ -79,9 +79,9 @@ public:
   /// @name Component Access
   /// @{
 
-  const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
+  const Rot3& attitude(OptionalJacobian<3, 9> H = {}) const;
-  const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
+  const Point3& position(OptionalJacobian<3, 9> H = {}) const;
-  const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;
+  const Velocity3& velocity(OptionalJacobian<3, 9> H = {}) const;
 
   const Pose3 pose() const {
     return Pose3(attitude(), position());
@@ -108,7 +108,7 @@ public:
     return v_;
   }
   // Return velocity in body frame
-  Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const;
+  Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const;
 
   /// Return matrix group representation, in MATLAB notation:
   /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1]
@@ -156,13 +156,13 @@ public:
 
   /// retract with optional derivatives
   NavState retract(const Vector9& v, //
-      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
+      OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
-          boost::none) const;
+          {}) const;
 
   /// localCoordinates with optional derivatives
   Vector9 localCoordinates(const NavState& g, //
-      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
+      OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
-          boost::none) const;
+          {}) const;
 
   /// @}
   /// @name Dynamics
@@ -176,14 +176,14 @@ public:
 
   /// Compute tangent space contribution due to Coriolis forces
   Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
-      OptionalJacobian<9, 9> H = boost::none) const;
+      OptionalJacobian<9, 9> H = {}) const;
 
   /// Correct preintegrated tangent vector with our velocity and rotated gravity,
   /// taking into account Coriolis forces if omegaCoriolis is given.
   Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
-      const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
+      const std::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
-          false, OptionalJacobian<9, 9> H1 = boost::none,
+          false, OptionalJacobian<9, 9> H1 = {},
-      OptionalJacobian<9, 9> H2 = boost::none) const;
+      OptionalJacobian<9, 9> H2 = {}) const;
 
   /// @}
 

gtsam/navigation/PreintegratedRotation.cpp

@@ -115,8 +115,7 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
 Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
     OptionalJacobian<3, 3> H) const {
   const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
-  const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,
+  const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,{}, H);
-      boost::none, H);
   if (H)
     (*H) *= delRdelBiasOmega_;
   return deltaRij_biascorrected;

gtsam/navigation/PreintegratedRotation.h

@@ -23,6 +23,7 @@
 
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/std_optional_serialization.h>
 
 namespace gtsam {
 
@@ -32,16 +33,17 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
   /// Continuous-time "Covariance" of gyroscope measurements
   /// The units for stddev are σ = rad/s/√Hz
   Matrix3 gyroscopeCovariance;
-  boost::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
+  std::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
-  boost::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
+  std::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
 
   PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
 
   PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
-                              boost::optional<Vector3> omega_coriolis)
+                              std::optional<Vector3> omega_coriolis)
     : gyroscopeCovariance(gyroscope_covariance) {
-      if (omega_coriolis)
+      if (omega_coriolis) {
-        omegaCoriolis.reset(omega_coriolis.get());
+        omegaCoriolis = *omega_coriolis;
+      }
   }
 
   virtual ~PreintegratedRotationParams() {}
@@ -50,12 +52,12 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
   virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
 
   void setGyroscopeCovariance(const Matrix3& cov)   { gyroscopeCovariance = cov;  }
-  void setOmegaCoriolis(const Vector3& omega)       { omegaCoriolis.reset(omega); }
+  void setOmegaCoriolis(const Vector3& omega)       { omegaCoriolis = omega; }
-  void setBodyPSensor(const Pose3& pose)            { body_P_sensor.reset(pose);  }
+  void setBodyPSensor(const Pose3& pose)            { body_P_sensor = pose;  }
 
   const Matrix3& getGyroscopeCovariance()     const { return gyroscopeCovariance; }
-  boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
+  std::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
-  boost::optional<Pose3>   getBodyPSensor()   const { return body_P_sensor; }
+  std::optional<Pose3>   getBodyPSensor()   const { return body_P_sensor; }
 
  private:
   /** Serialization function */
@@ -66,8 +68,8 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
     ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
 
-    // Provide support for Eigen::Matrix in boost::optional
+    // Provide support for Eigen::Matrix in std::optional
-    bool omegaCoriolisFlag = omegaCoriolis.is_initialized();
+    bool omegaCoriolisFlag = omegaCoriolis.has_value();
     ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
     if (omegaCoriolisFlag) {
       ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
@@ -164,12 +166,12 @@ class GTSAM_EXPORT PreintegratedRotation {
   /// Calculate an incremental rotation given the gyro measurement and a time interval,
   /// and update both deltaTij_ and deltaRij_.
   void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
-                            OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
+                            OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
-                            OptionalJacobian<3, 3> F = boost::none);
+                            OptionalJacobian<3, 3> F = {});
 
   /// Return a bias corrected version of the integrated rotation, with optional Jacobian
   Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
-                             OptionalJacobian<3, 3> H = boost::none) const;
+                             OptionalJacobian<3, 3> H = {}) const;
 
   /// Integrate coriolis correction in body frame rot_i
   Vector3 integrateCoriolis(const Rot3& rot_i) const;

gtsam/navigation/PreintegrationBase.h

@@ -129,9 +129,9 @@ class GTSAM_EXPORT PreintegrationBase {
    */
   std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
       const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
-      OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
+      OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = {},
-      OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
+      OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = {},
-      OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
+      OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = {}) const;
 
   /**
    *  Update preintegrated measurements and get derivatives
@@ -148,12 +148,12 @@ class GTSAM_EXPORT PreintegrationBase {
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far
   virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
-      OptionalJacobian<9, 6> H = boost::none) const = 0;
+      OptionalJacobian<9, 6> H = {}) const = 0;
 
   /// Predict state at time j
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
-                   OptionalJacobian<9, 9> H1 = boost::none,
+                   OptionalJacobian<9, 9> H1 = {},
-                   OptionalJacobian<9, 6> H2 = boost::none) const;
+                   OptionalJacobian<9, 6> H2 = {}) const;
 
   /// Calculate error given navStates
   Vector9 computeError(const NavState& state_i, const NavState& state_j,
@@ -167,10 +167,10 @@ class GTSAM_EXPORT PreintegrationBase {
    */
   Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
       const Pose3& pose_j, const Vector3& vel_j,
-      const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
+      const imuBias::ConstantBias& bias_i, 
-          boost::none, OptionalJacobian<9, 3> H2 = boost::none,
+      OptionalJacobian<9, 6> H1 = {}, OptionalJacobian<9, 3> H2 = {},
-      OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
+      OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {}, 
-          boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
+      OptionalJacobian<9, 6> H5 = {}) const;
 
  private:
   /** Serialization function */

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -140,7 +140,7 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
   EXPECT(assert_equal(gyroscopeCovariance,
       actualPim.p().getGyroscopeCovariance(), 1e-6));
   EXPECT(assert_equal(omegaCoriolis,
-      actualPim.p().getOmegaCoriolis().get(), 1e-6));
+      *(actualPim.p().getOmegaCoriolis()), 1e-6));
   EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6));
   DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6);
   EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6));
@@ -163,7 +163,7 @@ TEST(AHRSFactor, Error) {
   pim.integrateMeasurement(measuredOmega, deltaT);
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, boost::none);
+  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {});
 
   Vector3 errorActual = factor.evaluateError(x1, x2, bias);
 
@@ -458,8 +458,7 @@ TEST (AHRSFactor, predictTest) {
 
   // PreintegratedAhrsMeasurements::predict
   Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
-      std::bind(&PreintegratedAhrsMeasurements::predict,
+      [&pim](const Vector3& b) { return pim.predict(b, {}); }, bias);
-          &pim, std::placeholders::_1, boost::none), bias);
 
   // Actual Jacobians
   Matrix H;

gtsam/navigation/tests/testMagPoseFactor.cpp

@@ -57,8 +57,8 @@ Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)),
 
 // *****************************************************************************
 TEST(MagPoseFactor, Constructors) {
-  MagPoseFactor<Pose2> f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
+  MagPoseFactor<Pose2> f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, {});
-  MagPoseFactor<Pose3> f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
+  MagPoseFactor<Pose3> f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, {});
 
   // Try constructing with a body_P_sensor set.
   MagPoseFactor<Pose2> f2b = MagPoseFactor<Pose2>(
@@ -75,7 +75,7 @@ TEST(MagPoseFactor, JacobianPose2) {
   Matrix H2;
 
   // Error should be zero at the groundtruth pose.
-  MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
+  MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, {});
   CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2>  //
                             ([&f] (const Pose2& p) {return f.evaluateError(p);},
@@ -88,7 +88,7 @@ TEST(MagPoseFactor, JacobianPose3) {
   Matrix H3;
 
   // Error should be zero at the groundtruth pose.
-  MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
+  MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, {});
   CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3>  //
                             ([&f] (const Pose3& p) {return f.evaluateError(p);},

gtsam/sfm/ShonanAveraging.cpp

@@ -554,7 +554,7 @@ static bool PowerMinimumEigenValue(
   }
 
   const Sparse C = pmEigenValue * Matrix::Identity(A.rows(), A.cols()).sparseView() - A;
-  const boost::optional<Vector> initial = perturb(S.row(0));
+  const std::optional<Vector> initial = perturb(S.row(0));
   AcceleratedPowerMethod<Sparse> apmShiftedOperator(C, initial);
 
   const bool minConverged = apmShiftedOperator.compute(