unstable/slam

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h

@@ -106,8 +106,8 @@ private:
 
   Matrix Jacobian_wrt_t0_Overall_;
 
-  boost::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
+  std::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
-  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
 
 public:
 	
@@ -126,7 +126,7 @@ public:
       double dt12, const Vector world_g, const Vector world_rho,
       const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
       const Matrix& Jacobian_wrt_t0_Overall,
-      boost::optional<IMUBIAS> Bias_initial = boost::none, boost::optional<POSE> body_P_sensor = boost::none) :
+      std::optional<IMUBIAS> Bias_initial = {}, std::optional<POSE> body_P_sensor = {}) :
         Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2),
         delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
         dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
@@ -385,7 +385,7 @@ public:
       const Vector& delta_pos_in_t0, const Vector3& delta_angles,
       double dt12, const Vector world_g, const Vector world_rho,
       const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
-      const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
+      const std::optional<IMUBIAS>& Bias_initial = {}) {
 
 
     // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
@@ -414,7 +414,7 @@ public:
   static inline VELOCITY PredictVelocityFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1,
       const Vector& delta_vel_in_t0, double dt12, const Vector world_g, const Vector world_rho,
       const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
-      const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
+      const std::optional<IMUBIAS>& Bias_initial = {}) {
 
     // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
     Vector delta_BiasAcc  = Bias1.accelerometer();
@@ -436,7 +436,7 @@ public:
       const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
       double dt12, const Vector world_g, const Vector world_rho,
       const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
-      const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
+      const std::optional<IMUBIAS>& Bias_initial = {}) {
 
     Pose2 = PredictPoseFromPreIntegration(Pose1, Vel1, Bias1, delta_pos_in_t0, delta_angles, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial);
     Vel2  = PredictVelocityFromPreIntegration(Pose1, Vel1, Bias1, delta_vel_in_t0, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial);
@@ -447,7 +447,7 @@ public:
       Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
       const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
       Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(),
-      boost::optional<POSE> p_body_P_sensor = boost::none){
+      std::optional<POSE> p_body_P_sensor = {}){
     // Note: all delta terms refer to an IMU\sensor system at t0
     // Note: Earth-related terms are not accounted here but are incorporated in predict functions.
 

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h

@@ -105,7 +105,7 @@ private:
 
   Matrix Jacobian_wrt_t0_Overall_;
 
-  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
 
 public:
 
@@ -124,7 +124,7 @@ public:
       double dt12, const Vector world_g, const Vector world_rho,
       const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
       const Matrix& Jacobian_wrt_t0_Overall,
-      boost::optional<POSE> body_P_sensor = boost::none) :
+      std::optional<POSE> body_P_sensor = {}) :
         Base(model_equivalent, Pose1, Vel1, Pose2, Vel2),
         delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
         dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
@@ -352,7 +352,7 @@ public:
       Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
       const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
       Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall,
-      boost::optional<POSE> p_body_P_sensor = boost::none){
+      std::optional<POSE> p_body_P_sensor = {}){
     // Note: all delta terms refer to an IMU\sensor system at t0
     // Note: Earth-related terms are not accounted here but are incorporated in predict functions.
 

gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h

@@ -92,7 +92,7 @@ private:
   Vector world_rho_;
   Vector world_omega_earth_;
 
-  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
 
 public:
 
@@ -108,7 +108,7 @@ public:
   /** Constructor */
   InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
       const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho,
-      const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional<POSE> body_P_sensor = boost::none) :
+      const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, std::optional<POSE> body_P_sensor = {}) :
         Base(calc_descrete_noise_model(model_continuous, measurement_dt ),
             Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro),
             dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) {  }

gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp

@@ -43,7 +43,7 @@ Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi,
 
   // Calculate the error between measured and estimated planes in sensor frame.
   const Vector3 err = measured_p_.errorVector(i_plane,
-    boost::none, (H1 || H2 || H3) ? &error_H_predicted : nullptr);
+    {}, (H1 || H2 || H3) ? &error_H_predicted : nullptr);
 
   // Apply the chain rule to calculate the derivatives.
   if (H1) {

gtsam_unstable/slam/MultiProjectionFactor.h

@@ -39,7 +39,7 @@ namespace gtsam {
     // Keep a copy of measurement and calibration for I/O
     Vector measured_;                    ///< 2D measurement for each of the n views
     boost::shared_ptr<CALIBRATION> K_;  ///< shared pointer to calibration object
-    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
 
 
     // verbosity handling for Cheirality Exceptions
@@ -72,7 +72,7 @@ namespace gtsam {
      */
     MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
         KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
-        boost::optional<POSE> body_P_sensor = boost::none) :
+        std::optional<POSE> body_P_sensor = {}) :
           Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
           throwCheirality_(false), verboseCheirality_(false) {
       keys_.assign(poseKeys.begin(), poseKeys.end());
@@ -94,7 +94,7 @@ namespace gtsam {
     MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
         KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
         bool throwCheirality, bool verboseCheirality,
-        boost::optional<POSE> body_P_sensor = boost::none) :
+        std::optional<POSE> body_P_sensor = {}) :
           Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
           throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
 

gtsam_unstable/slam/PoseBetweenFactor.h

@@ -37,7 +37,7 @@ namespace gtsam {
     typedef NoiseModelFactorN<POSE, POSE> Base;
 
     POSE measured_; /** The measurement */
-    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
 
     /** concept check by type */
     GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
@@ -55,7 +55,7 @@ namespace gtsam {
 
     /** Constructor */
     PoseBetweenFactor(Key key1, Key key2, const POSE& measured,
-        const SharedNoiseModel& model, boost::optional<POSE> body_P_sensor = boost::none) :
+        const SharedNoiseModel& model, std::optional<POSE> body_P_sensor = {}) :
       Base(model, key1, key2), measured_(measured), body_P_sensor_(body_P_sensor) {
     }
 

gtsam_unstable/slam/PosePriorFactor.h

@@ -34,7 +34,7 @@ namespace gtsam {
     typedef NoiseModelFactorN<POSE> Base;
 
     POSE prior_; /** The measurement */
-    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
 
     /** concept check by type */
     GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
@@ -54,7 +54,7 @@ namespace gtsam {
 
     /** Constructor */
     PosePriorFactor(Key key, const POSE& prior, const SharedNoiseModel& model,
-        boost::optional<POSE> body_P_sensor = boost::none) :
+        std::optional<POSE> body_P_sensor = {}) :
       Base(model, key), prior_(prior), body_P_sensor_(body_P_sensor) {
     }
 

gtsam_unstable/slam/ProjectionFactorPPP.h

@@ -130,13 +130,13 @@ namespace gtsam {
           if(H1 || H2 || H3) {
             Matrix H0, H02;
             PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
-            Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
+            Point2 reprojectionError(camera.project(point, H1, H3, {}) - measured_);
             *H2 = *H1 * H02;
             *H1 = *H1 * H0;
             return reprojectionError;
           } else {
             PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
-            return camera.project(point, H1, H3, boost::none) - measured_;
+            return camera.project(point, H1, H3, {}) - measured_;
           }
       } catch( CheiralityException& e) {
         if (H1) *H1 = Matrix::Zero(2,6);

gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp

@@ -31,7 +31,7 @@ Vector ProjectionFactorRollingShutter::evaluateError(
         gtsam::Matrix HbodySensor;
         PinholeCamera<Cal3_S2> camera(
             pose.compose(*body_P_sensor_, HbodySensor), *K_);
-        Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
+        Point2 reprojectionError(camera.project(point, Hprj, H3, {}) -
                                  measured_);
         if (H1) *H1 = Hprj * HbodySensor * (*H1);
         if (H2) *H2 = Hprj * HbodySensor * (*H2);
@@ -42,7 +42,7 @@ Vector ProjectionFactorRollingShutter::evaluateError(
       }
     } else {
       PinholeCamera<Cal3_S2> camera(pose, *K_);
-      Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
+      Point2 reprojectionError(camera.project(point, Hprj, H3, {}) -
                                measured_);
       if (H1) *H1 = Hprj * (*H1);
       if (H2) *H2 = Hprj * (*H2);

gtsam_unstable/slam/ProjectionFactorRollingShutter.h

@@ -49,7 +49,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
   double alpha_;     ///< interpolation parameter in [0,1] corresponding to the
                      ///< point2 measurement
   boost::shared_ptr<Cal3_S2> K_;  ///< shared pointer to calibration object
-  boost::optional<Pose3>
+  std::optional<Pose3>
       body_P_sensor_;  ///< The pose of the sensor in the body frame
 
   // verbosity handling for Cheirality Exceptions
@@ -96,7 +96,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
       const Point2& measured, double alpha, const SharedNoiseModel& model,
       Key poseKey_a, Key poseKey_b, Key pointKey,
       const boost::shared_ptr<Cal3_S2>& K,
-      boost::optional<Pose3> body_P_sensor = boost::none)
+      std::optional<Pose3> body_P_sensor = {})
       : Base(model, poseKey_a, poseKey_b, pointKey),
         measured_(measured),
         alpha_(alpha),
@@ -127,7 +127,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
       Key poseKey_a, Key poseKey_b, Key pointKey,
       const boost::shared_ptr<Cal3_S2>& K, bool throwCheirality,
       bool verboseCheirality,
-      boost::optional<Pose3> body_P_sensor = boost::none)
+      std::optional<Pose3> body_P_sensor = {})
       : Base(model, poseKey_a, poseKey_b, pointKey),
         measured_(measured),
         alpha_(alpha),

gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h

@@ -165,8 +165,8 @@ namespace gtsam {
       T currA_T_currB_pred;
       if (H) {
         Matrix H_compose, H_between1;
-        T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, boost::none);
+        T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, {});
-        currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, boost::none, H_between1);
+        currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, {}, H_between1);
         (*H)[0] = H_compose * H_between1;
       }
       else {

gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp

@@ -342,24 +342,24 @@ TEST(ProjectionFactorRollingShutter, cheirality) {
         std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
             std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                       std::placeholders::_1, std::placeholders::_2,
-                      std::placeholders::_3, boost::none, boost::none,
+                      std::placeholders::_3, {}, {},
-                      boost::none)),
+                      {})),
         pose1, pose2, point);
 
     Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
         std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
             std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                       std::placeholders::_1, std::placeholders::_2,
-                      std::placeholders::_3, boost::none, boost::none,
+                      std::placeholders::_3, {}, {},
-                      boost::none)),
+                      {})),
         pose1, pose2, point);
 
     Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
         std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
             std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                       std::placeholders::_1, std::placeholders::_2,
-                      std::placeholders::_3, boost::none, boost::none,
+                      std::placeholders::_3, {}, {},
-                      boost::none)),
+                      {})),
         pose1, pose2, point);
 
     CHECK(assert_equal(H1Expected, H1Actual, 1e-5));