From d7f60353c9789590e5801759bf73c8e4dfd3583f Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 14:02:31 -0800 Subject: [PATCH] unstable/slam --- .../slam/EquivInertialNavFactor_GlobalVel.h | 14 +++++++------- .../slam/EquivInertialNavFactor_GlobalVel_NoBias.h | 6 +++--- .../slam/InertialNavFactor_GlobalVelocity.h | 4 ++-- gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp | 2 +- gtsam_unstable/slam/MultiProjectionFactor.h | 6 +++--- gtsam_unstable/slam/PoseBetweenFactor.h | 4 ++-- gtsam_unstable/slam/PosePriorFactor.h | 4 ++-- gtsam_unstable/slam/ProjectionFactorPPP.h | 4 ++-- .../slam/ProjectionFactorRollingShutter.cpp | 4 ++-- .../slam/ProjectionFactorRollingShutter.h | 6 +++--- .../slam/TransformBtwRobotsUnaryFactor.h | 4 ++-- .../tests/testProjectionFactorRollingShutter.cpp | 12 ++++++------ 12 files changed, 35 insertions(+), 35 deletions(-) diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 529af878d..f6189cc5a 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -106,8 +106,8 @@ private: Matrix Jacobian_wrt_t0_Overall_; - boost::optional Bias_initial_; // Bias used when pre-integrating IMU measurements - boost::optional body_P_sensor_; // The pose of the sensor in the body frame + std::optional Bias_initial_; // Bias used when pre-integrating IMU measurements + std::optional 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 Bias_initial = boost::none, boost::optional body_P_sensor = boost::none) : + std::optional Bias_initial = {}, std::optional 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& Bias_initial = boost::none) { + const std::optional& 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& Bias_initial = boost::none) { + const std::optional& 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& Bias_initial = boost::none) { + const std::optional& 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 p_body_P_sensor = boost::none){ + std::optional 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. diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index f3e27f7e5..5d251094e 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -105,7 +105,7 @@ private: Matrix Jacobian_wrt_t0_Overall_; - boost::optional body_P_sensor_; // The pose of the sensor in the body frame + std::optional 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 body_P_sensor = boost::none) : + std::optional 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 p_body_P_sensor = boost::none){ + std::optional 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. diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 40fc9da25..a107f84b5 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -92,7 +92,7 @@ private: Vector world_rho_; Vector world_omega_earth_; - boost::optional body_P_sensor_; // The pose of the sensor in the body frame + std::optional 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 body_P_sensor = boost::none) : + const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, std::optional 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) { } diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp index 8854df5c7..010e62218 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp +++ b/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) { diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index f64568118..d7cebcffc 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/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 K_; ///< shared pointer to calibration object - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + std::optional 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& K, - boost::optional body_P_sensor = boost::none) : + std::optional 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& K, bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = boost::none) : + std::optional body_P_sensor = {}) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 14a18d938..9e275116b 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -37,7 +37,7 @@ namespace gtsam { typedef NoiseModelFactorN Base; POSE measured_; /** The measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + std::optional 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 body_P_sensor = boost::none) : + const SharedNoiseModel& model, std::optional body_P_sensor = {}) : Base(model, key1, key2), measured_(measured), body_P_sensor_(body_P_sensor) { } diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index ac199588b..4e55bd649 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -34,7 +34,7 @@ namespace gtsam { typedef NoiseModelFactorN Base; POSE prior_; /** The measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + std::optional 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 body_P_sensor = boost::none) : + std::optional body_P_sensor = {}) : Base(model, key), prior_(prior), body_P_sensor_(body_P_sensor) { } diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 9ef11f022..2b799ac1d 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -130,13 +130,13 @@ namespace gtsam { if(H1 || H2 || H3) { Matrix H0, H02; PinholeCamera 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 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); diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp index 003f2f921..beeafecf4 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -31,7 +31,7 @@ Vector ProjectionFactorRollingShutter::evaluateError( gtsam::Matrix HbodySensor; PinholeCamera 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 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); diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index bbf108ecc..e6c0fd16a 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/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 K_; ///< shared pointer to calibration object - boost::optional + std::optional 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& K, - boost::optional body_P_sensor = boost::none) + std::optional 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& K, bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = boost::none) + std::optional body_P_sensor = {}) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), alpha_(alpha), diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 5745536e0..d2d28ef9b 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/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); - currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, boost::none, H_between1); + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, {}); + currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, {}, H_between1); (*H)[0] = H_compose * H_between1; } else { diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index 3c7c39f48..31a385695 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -342,24 +342,24 @@ TEST(ProjectionFactorRollingShutter, cheirality) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none)), + std::placeholders::_3, {}, {}, + {})), pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none)), + std::placeholders::_3, {}, {}, + {})), pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none)), + std::placeholders::_3, {}, {}, + {})), pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5));