Fixing compatibility issues

Author: nkhedekar

gtsam/navigation/ImuFactor.cpp

@@ -266,7 +266,7 @@ ImuFactorWithGravity::ImuFactorWithGravity(Key pose_i, Key vel_i, Key pose_j, Ke
 
 //------------------------------------------------------------------------------
 NonlinearFactor::shared_ptr ImuFactorWithGravity::clone() const {
-return boost::static_pointer_cast<NonlinearFactor>(
+return std::static_pointer_cast<NonlinearFactor>(
     NonlinearFactor::shared_ptr(new This(*this)));
 }
 
@@ -297,9 +297,9 @@ return e != nullptr && base && pim;
 //------------------------------------------------------------------------------
 Vector ImuFactorWithGravity::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
   const Pose3& pose_j, const Vector3& vel_j,
-  const imuBias::ConstantBias& bias_i, const Vector3& gravity, boost::optional<Matrix&> H1,
-  boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
-  boost::optional<Matrix&> H4, boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
+  const imuBias::ConstantBias& bias_i, const Vector3& gravity, OptionalMatrixType H1,
+  OptionalMatrixType H2, OptionalMatrixType H3,
+  OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const {
   std::cout << "Evaluating error" << std::endl;
     return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, gravity,
     H1, H2, H3, H4, H5, H6);

gtsam/navigation/ImuFactor.h

@@ -403,11 +403,12 @@ class GTSAM_EXPORT ImuFactorWithGravity: public NoiseModelFactorN<Pose3, Vector3
 
   /// vector of errors
   Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
-      const Pose3& pose_j, const Vector3& vel_j,
-      const imuBias::ConstantBias& bias_i, const Vector3& gravity, boost::optional<Matrix&> H1 =
-          boost::none, boost::optional<Matrix&> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
-          boost::none, boost::optional<Matrix&> H5 = boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
+                       const Pose3& pose_j, const Vector3& vel_j,
+                       const imuBias::ConstantBias& bias_i,
+                       const Vector3& gravity, OptionalMatrixType H1,
+                       OptionalMatrixType H2, OptionalMatrixType H3,
+                       OptionalMatrixType H4, OptionalMatrixType H5,
+                       OptionalMatrixType H6) const override;
 
 #ifdef GTSAM_TANGENT_PREINTEGRATION
   /// Merge two pre-integrated measurement classes

gtsam/navigation/PreintegrationBase.cpp

@@ -146,7 +146,7 @@ NavState PreintegrationBase::predict(const NavState& state_i,
 NavState PreintegrationBase::predict(const NavState& state_i,
   const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
   OptionalJacobian<9, 6> H2) const {
-    return predict(state_i, bias_i, p().n_gravity, H1, H2, boost::none);
+    return predict(state_i, bias_i, p().n_gravity, H1, H2, nullptr);
 }
 
 //------------------------------------------------------------------------------
@@ -186,7 +186,7 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i,
   OptionalJacobian<9, 9> H1,
   OptionalJacobian<9, 9> H2,
   OptionalJacobian<9, 6> H3) const {
-    return computeError(state_i, state_j, bias_i, p().n_gravity, H1, H2, H3, boost::none);
+    return computeError(state_i, state_j, bias_i, p().n_gravity, H1, H2, H3, nullptr);
 }
 
 //------------------------------------------------------------------------------
@@ -227,7 +227,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
   const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1,
   OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3,
   OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const {
-    return computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, p().n_gravity, H1, H2, H3, H4, H5, boost::none);
+    return computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, p().n_gravity, H1, H2, H3, H4, H5, nullptr);
   }
 
 }  // namespace gtsam

gtsam/navigation/PreintegrationBase.h

@@ -152,9 +152,9 @@ class GTSAM_EXPORT PreintegrationBase {
 
   /// Predict state at time j
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
-                   OptionalJacobian<9, 9> H1 = boost::none,
-                   OptionalJacobian<9, 6> H2 = boost::none,
-                   OptionalJacobian<9, 3> H3 = boost::none) const;
+                   OptionalJacobian<9, 9> H1 = {},
+                   OptionalJacobian<9, 6> H2 = {},
+                   OptionalJacobian<9, 3> H3 = {}) const;
 
   /// Predict state at time j
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
@@ -187,9 +187,9 @@ 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, const Vector3& n_gravity, OptionalJacobian<9, 6> H1 =
-        boost::none, OptionalJacobian<9, 3> H2 = boost::none,
-    OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
-        boost::none, OptionalJacobian<9, 6> H5 = boost::none, OptionalJacobian<9, 3> H6 = boost::none) const;
+        {}, OptionalJacobian<9, 3> H2 = {},
+    OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 =
+        {}, OptionalJacobian<9, 6> H5 = {}, OptionalJacobian<9, 3> H6 = {}) const;
 
  private:
 #if GTSAM_ENABLE_BOOST_SERIALIZATION