From 841dc6005afe06d98993ac89291e6df5b95f648c Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 5 Jan 2023 14:29:17 -0800 Subject: [PATCH] changed signatures to use OptionalMatrix keyword --- CMakeLists.txt | 2 +- doc/Code/LocalizationFactor.cpp | 2 +- examples/CameraResectioning.cpp | 6 +-- examples/LocalizationExample.cpp | 2 +- gtsam/navigation/AHRSFactor.cpp | 4 +- gtsam/navigation/AHRSFactor.h | 6 +-- gtsam/navigation/AttitudeFactor.h | 4 +- gtsam/navigation/BarometricFactor.cpp | 4 +- gtsam/navigation/BarometricFactor.h | 4 +- gtsam/navigation/CombinedImuFactor.h | 10 +++-- gtsam/navigation/ConstantVelocityFactor.h | 4 +- gtsam/navigation/GPSFactor.cpp | 2 +- gtsam/navigation/GPSFactor.h | 2 +- gtsam/navigation/ImuFactor.cpp | 10 ++--- gtsam/navigation/ImuFactor.h | 14 +++---- gtsam/navigation/MagFactor.h | 18 ++++----- gtsam/navigation/MagPoseFactor.h | 4 +- gtsam/nonlinear/CustomFactor.cpp | 2 +- gtsam/nonlinear/CustomFactor.h | 2 +- gtsam/nonlinear/ExpressionFactor.h | 6 +-- gtsam/nonlinear/FunctorizedFactor.h | 15 ++++---- gtsam/nonlinear/NonlinearEquality.h | 8 ++-- gtsam/nonlinear/NonlinearFactor.h | 10 ++--- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam/sam/BearingFactor.h | 4 +- gtsam/sam/BearingRangeFactor.h | 4 +- gtsam/sam/RangeFactor.h | 8 ++-- gtsam/sfm/TranslationFactor.h | 4 +- gtsam/slam/BetweenFactor.h | 6 +-- gtsam/slam/BoundingConstraint.h | 8 ++-- gtsam/slam/EssentialMatrixConstraint.cpp | 12 ++++-- gtsam/slam/EssentialMatrixConstraint.h | 4 +- gtsam/slam/EssentialMatrixFactor.h | 20 +++++----- gtsam/slam/FrobeniusFactor.h | 10 ++--- gtsam/slam/GeneralSFMFactor.h | 8 ++-- gtsam/slam/OrientedPlane3Factor.cpp | 6 +-- gtsam/slam/PoseRotationPrior.h | 2 +- gtsam/slam/PoseTranslationPrior.h | 2 +- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/ReferenceFrameFactor.h | 9 +++-- gtsam/slam/RotateFactor.h | 4 +- gtsam/slam/StereoFactor.h | 2 +- gtsam/slam/TriangulationFactor.h | 6 +-- gtsam_unstable/slam/MultiProjectionFactor.h | 2 +- tests/simulated2D.h | 10 ++--- tests/simulated2DOriented.h | 8 ++-- tests/simulated3D.h | 6 +-- tests/smallExample.h | 2 +- tests/testExtendedKalmanFilter.cpp | 6 +-- tests/testNonlinearFactor.cpp | 41 ++++++++++----------- 50 files changed, 172 insertions(+), 167 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 90ac89aa8..efa4100a8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,7 @@ add_subdirectory(timing) # Build gtsam_unstable if (GTSAM_BUILD_UNSTABLE) - add_subdirectory(gtsam_unstable) + # add_subdirectory(gtsam_unstable) endif() # This is the new wrapper diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index 2c1f01c43..74be3b924 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -6,7 +6,7 @@ public: NoiseModelFactor1(model, j), mx_(x), my_(y) {} Vector evaluateError(const Pose2& q, - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { const Rot2& R = q.rotation(); if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 1dcca5244..7bd4ebee6 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -46,10 +46,10 @@ public: } /// evaluate the error - Vector evaluateError(const Pose3& pose, boost::optional H = - boost::none) const override { + Vector evaluateError(const Pose3& pose, OptionalMatrixType H = + OptionalNone) const override { PinholeCamera camera(pose, *K_); - return camera.project(P_, H, boost::none, boost::none) - p_; + return camera.project(P_, H, OptionalNone, OptionalNone) - p_; } }; diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 7a39a4af5..a8f2b22c6 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -84,7 +84,7 @@ class UnaryFactor: public NoiseModelFactorN { // The first is the 'evaluateError' function. This function implements the desired measurement // function, returning a vector of errors when evaluated at the provided variable value. It // must also calculate the Jacobians for this measurement function, if requested. - Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const override { + Vector evaluateError(const Pose2& q, OptionalMatrixType H = OptionalNone) const override { // The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta] // The error is then simply calculated as E(q) = h(q) - m: // error_x = q.x - mx diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index afaaee1d4..f00655902 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -119,8 +119,8 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { //------------------------------------------------------------------------------ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, - const Vector3& bias, boost::optional H1, - boost::optional H2, boost::optional H3) const { + const Vector3& bias, OptionalMatrixType H1, + OptionalMatrixType H2, OptionalMatrixType H3) const { // Do bias correction, if (H3) will contain 3*3 derivative used below const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 87fdd2e91..2a3c98c37 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -179,9 +179,9 @@ public: /// vector of errors Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, - const Vector3& bias, boost::optional H1 = boost::none, - boost::optional H2 = boost::none, boost::optional H3 = - boost::none) const override; + const Vector3& bias, OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H3 = + OptionalNone) const override; /// predicted states from IMU /// TODO(frank): relationship with PIM predict ?? diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 93495f227..4da01ec15 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -122,7 +122,7 @@ public: /** vector of errors */ Vector evaluateError(const Rot3& nRb, // - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { return attitudeError(nRb, H); } @@ -197,7 +197,7 @@ public: /** vector of errors */ Vector evaluateError(const Pose3& nTb, // - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp index 2e87986ae..f434179ab 100644 --- a/gtsam/navigation/BarometricFactor.cpp +++ b/gtsam/navigation/BarometricFactor.cpp @@ -43,8 +43,8 @@ bool BarometricFactor::equals(const NonlinearFactor& expected, //*************************************************************************** Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias, - boost::optional H, - boost::optional H2) const { + OptionalMatrixType H, + OptionalMatrixType H2) const { Matrix tH; Vector ret = (Vector(1) << (p.translation(tH).z() + bias - nT_)).finished(); if (H) (*H) = tH.block<1, 6>(2, 0); diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index 2d793c475..be9b84f77 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -78,8 +78,8 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { /// vector of errors Vector evaluateError( const Pose3& p, const double& b, - boost::optional H = boost::none, - boost::optional H2 = boost::none) const override; + OptionalMatrixType H = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override; inline const double& measurementIn() const { return nT_; } diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 3448e7794..91f05489e 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -324,10 +324,12 @@ public: Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none, boost::optional H3 = boost::none, - boost::optional H4 = boost::none, boost::optional H5 = - boost::none, boost::optional H6 = boost::none) const override; + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone, + OptionalMatrixType H4 = OptionalNone, + OptionalMatrixType H5 = OptionalNone, + OptionalMatrixType H6 = OptionalNone) const override; private: /** Serialization function */ diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 4db2b82c0..ccff88cf2 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -48,8 +48,8 @@ class ConstantVelocityFactor : public NoiseModelFactorN { * @return * Vector */ gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { // only used to use update() below static const Vector3 b_accel{0.0, 0.0, 0.0}; static const Vector3 b_omega{0.0, 0.0, 0.0}; diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 1d6b78e13..7a559bb4e 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -38,7 +38,7 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { //*************************************************************************** Vector GPSFactor::evaluateError(const Pose3& p, - boost::optional H) const { + OptionalMatrixType H) const { return p.translation(H) -nT_; } diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index e515d9012..1f488579d 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -79,7 +79,7 @@ public: /// vector of errors Vector evaluateError(const Pose3& p, - boost::optional H = boost::none) const override; + OptionalMatrixType H = OptionalNone) const override; inline const Point3 & measurementIn() const { return nT_; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 1b07991e9..fc74f5034 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -151,9 +151,9 @@ bool ImuFactor::equals(const NonlinearFactor& other, double tol) const { //------------------------------------------------------------------------------ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, boost::optional H1, - boost::optional H2, boost::optional H3, - boost::optional H4, boost::optional H5) const { + const imuBias::ConstantBias& bias_i, OptionalMatrixType H1, + OptionalMatrixType H2, OptionalMatrixType H3, + OptionalMatrixType H4, OptionalMatrixType H5) const { return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); } @@ -247,8 +247,8 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const { Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j, const imuBias::ConstantBias& bias_i, // - boost::optional H1, boost::optional H2, - boost::optional H3) const { + OptionalMatrixType H1, OptionalMatrixType H2, + OptionalMatrixType H3) const { return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index feae1eb14..f00ead70c 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -228,10 +228,10 @@ public: /// 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, boost::optional H1 = - boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none, boost::optional H4 = - boost::none, boost::optional H5 = boost::none) const override; + const imuBias::ConstantBias& bias_i, OptionalMatrixType H1 = + OptionalNone, OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone, OptionalMatrixType H4 = + OptionalNone, OptionalMatrixType H5 = OptionalNone) const override; #ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge two pre-integrated measurement classes @@ -307,9 +307,9 @@ public: /// vector of errors Vector evaluateError(const NavState& state_i, const NavState& state_j, const imuBias::ConstantBias& bias_i, // - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const override; + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone) const override; private: diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 9a718a5e1..95523e18f 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -75,7 +75,7 @@ public: * @brief vector of errors */ Vector evaluateError(const Rot2& nRb, - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { // measured bM = nRb� * nM + b Point3 hx = unrotate(nRb, nM_, H) + bias_; return (hx - measured_); @@ -113,7 +113,7 @@ public: * @brief vector of errors */ Vector evaluateError(const Rot3& nRb, - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { // measured bM = nRb� * nM + b Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; return (hx - measured_); @@ -151,10 +151,10 @@ public: * @param bias (unknown) 3D bias */ Vector evaluateError(const Point3& nM, const Point3& bias, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const override { + OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = + OptionalNone) const override { // measured bM = nRb� * nM + b, where b is unknown bias - Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; + Point3 hx = bRn_.rotate(nM, OptionalNone, H1) + bias; if (H2) *H2 = I_3x3; return (hx - measured_); @@ -192,11 +192,11 @@ public: * @param bias (unknown) 3D bias */ Vector evaluateError(const double& scale, const Unit3& direction, - const Point3& bias, boost::optional H1 = boost::none, - boost::optional H2 = boost::none, boost::optional H3 = - boost::none) const override { + const Point3& bias, OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H3 = + OptionalNone) const override { // measured bM = nRb� * nM + b, where b is unknown bias - Unit3 rotated = bRn_.rotate(direction, boost::none, H2); + Unit3 rotated = bRn_.rotate(direction, OptionalNone, H2); Point3 hx = scale * rotated.point3() + bias; if (H1) *H1 = rotated.point3(); diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index c817e22b4..d5350384c 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -108,11 +108,11 @@ class MagPoseFactor: public NoiseModelFactorN { * Return the factor's error h(x) - z, and the optional Jacobian. Note that * the measurement error is expressed in the body frame. */ - Vector evaluateError(const POSE& nPb, boost::optional H = boost::none) const override { + Vector evaluateError(const POSE& nPb, OptionalMatrixType H = OptionalNone) const override { // Predict the measured magnetic field h(x) in the *body* frame. // If body_P_sensor was given, bias_ will have been rotated into the body frame. Matrix H_rot = Matrix::Zero(MeasDim, RotDim); - const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_; + const Point hx = nPb.rotation().unrotate(nM_, H_rot, OptionalNone) + bias_; if (H) { // Fill in the relevant part of the Jacobian (just rotation columns). diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index e33caed6f..6f0ad3a8b 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -22,7 +22,7 @@ namespace gtsam { /* * Calculates the unwhitened error by invoking the callback functor (i.e. from Python). */ -Vector CustomFactor::unwhitenedError(const Values& x, boost::optional&> H) const { +Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) const { if(this->active(x)) { if(H) { diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 8580a4949..f01a73414 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -75,7 +75,7 @@ public: * Calls the errorFunction closure, which is a std::function object * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array */ - Vector unwhitenedError(const Values &x, boost::optional &> H = boost::none) const override; + Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = boost::none) const override; /** print */ void print(const std::string &s, diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 11bf873e7..0c59a6cb2 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -97,7 +97,7 @@ protected: * both the function evaluation and its derivative(s) in H. */ Vector unwhitenedError(const Values& x, - boost::optional&> H = boost::none) const override { + OptionalMatrixVecType H = OptionalNone) const override { if (H) { const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here @@ -312,8 +312,8 @@ public: /// Backwards compatible evaluateError, to make existing tests compile Vector evaluateError(const A1 &a1, const A2 &a2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const { Values values; values.insert(this->keys_[0], a1); values.insert(this->keys_[1], a2); diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 1dc397c68..987869702 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -62,7 +62,7 @@ class FunctorizedFactor : public NoiseModelFactorN { R measured_; ///< value that is compared with functor return value SharedNoiseModel noiseModel_; ///< noise model - std::function)> func_; ///< functor instance + std::function func_; ///< functor instance public: /** default constructor - only use for serialization */ @@ -76,7 +76,7 @@ class FunctorizedFactor : public NoiseModelFactorN { * @param func: The instance of the functor object */ FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, - const std::function)> func) + const std::function func) : Base(model, key), measured_(z), noiseModel_(model), func_(func) {} ~FunctorizedFactor() override {} @@ -87,8 +87,8 @@ class FunctorizedFactor : public NoiseModelFactorN { NonlinearFactor::shared_ptr(new FunctorizedFactor(*this))); } - Vector evaluateError(const T ¶ms, boost::optional H = - boost::none) const override { + Vector evaluateError(const T ¶ms, OptionalMatrixType H = + OptionalNone) const override { R x = func_(params, H); Vector error = traits::Local(measured_, x); return error; @@ -162,8 +162,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN { R measured_; ///< value that is compared with functor return value SharedNoiseModel noiseModel_; ///< noise model - using FunctionType = std::function, - boost::optional)>; + using FunctionType = std::function; FunctionType func_; ///< functor instance public: @@ -194,8 +193,8 @@ class FunctorizedFactor2 : public NoiseModelFactorN { Vector evaluateError( const T1 ¶ms1, const T2 ¶ms2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { R x = func_(params1, params2, H1, H2); Vector error = traits::Local(measured_, x); return error; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index d1aa58b27..7f63639dc 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -139,7 +139,7 @@ public: /// Error function Vector evaluateError(const T& xj, - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) @@ -249,7 +249,7 @@ public: /// g(x) with optional derivative Vector evaluateError(const X& x1, - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { if (H) (*H) = Matrix::Identity(traits::GetDimension(x1),traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) @@ -324,8 +324,8 @@ class NonlinearEquality2 : public NoiseModelFactorN { /// g(x) with optional derivative2 Vector evaluateError( - const T& x1, const T& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + const T& x1, const T& x2, OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { static const size_t p = traits::dimension; if (H1) *H1 = -Matrix::Identity(p, p); if (H2) *H2 = Matrix::Identity(p, p); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 2d2b7aa4d..f7e3f67fe 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -47,7 +47,7 @@ using OptionalMatrixTypeT = Matrix*; using OptionalMatrixType = Matrix*; // These typedefs and aliases will help with making the unwhitenedError interface // independent of boost -using OptionalMatrixVec = std::vector*; +using OptionalMatrixVecType = std::vector*; #else // creating a none value to use when declaring our interfaces using OptionalNoneType = boost::none_t; @@ -55,7 +55,7 @@ using OptionalNoneType = boost::none_t; template using OptionalMatrixTypeT = boost::optional; using OptionalMatrixType = boost::optional; -using OptionalMatrixVec = boost::optional&>; +using OptionalMatrixVecType = boost::optional&>; #endif /** * Nonlinear factor base class @@ -252,7 +252,7 @@ public: * If the optional arguments is specified, it should compute * both the function evaluation and its derivative(s) in H. */ - virtual Vector unwhitenedError(const Values& x, OptionalMatrixVec H = OptionalNone) const = 0; + virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const = 0; #ifdef NO_BOOST_C17 // support taking in the actual vector instead of the pointer as well Vector unwhitenedError(const Values& x, std::vector& H) { @@ -565,7 +565,7 @@ protected: */ Vector unwhitenedError( const Values& x, - OptionalMatrixVec H = OptionalNone) const override { + OptionalMatrixVecType H = OptionalNone) const override { return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); } @@ -659,7 +659,7 @@ protected: inline Vector unwhitenedError( boost::mp11::index_sequence, // const Values& x, - OptionalMatrixVec H = OptionalNone) const { + OptionalMatrixVecType H = OptionalNone) const { if (this->active(x)) { if (H) { return evaluateError(x.at(keys_[Indices])..., diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index d81ffbd31..f132432f6 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -91,7 +91,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const T& x, boost::optional H = boost::none) const override { + Vector evaluateError(const T& x, OptionalMatrixType H = OptionalNone) const override { if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) return -traits::Local(x, prior_); diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index da624ffbb..cb39604f0 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -62,8 +62,8 @@ struct BearingFactor : public ExpressionFactorN { } Vector evaluateError(const A1& a1, const A2& a2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const { std::vector Hs(2); const auto &keys = Factor::keys(); diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index 0c38a2ea3..f57fc8066 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -74,8 +74,8 @@ class BearingRangeFactor } Vector evaluateError(const A1& a1, const A2& a2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const { std::vector Hs(2); const auto &keys = Factor::keys(); diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index bcd18f372..d96df39eb 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -60,8 +60,8 @@ class RangeFactor : public ExpressionFactorN { } Vector evaluateError(const A1& a1, const A2& a2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const { std::vector Hs(2); const auto &keys = Factor::keys(); @@ -137,8 +137,8 @@ class RangeFactorWithTransform : public ExpressionFactorN { } Vector evaluateError(const A1& a1, const A2& a2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const { std::vector Hs(2); const auto &keys = Factor::keys(); diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 8850d7d2d..3fc0837ba 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -65,8 +65,8 @@ class TranslationFactor : public NoiseModelFactorN { */ Vector evaluateError( const Point3& Ta, const Point3& Tb, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { const Point3 dir = Tb - Ta; Matrix33 H_predicted_dir; const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr); diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index d420e2b54..46f69f14b 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -105,13 +105,13 @@ namespace gtsam { /// @{ /// evaluate error, returns vector of errors size of tangent space - Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const override { + Vector evaluateError(const T& p1, const T& p2, OptionalMatrixType H1 = + OptionalNone, OptionalMatrixType H2 = OptionalNone) const override { T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) #ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR typename traits::ChartJacobian::Jacobian Hlocal; - Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); + Vector rval = traits::Local(measured_, hx, OptionalNone, (H1 || H2) ? &Hlocal : 0); if (H1) *H1 = Hlocal * (*H1); if (H2) *H2 = Hlocal * (*H2); return rval; diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index bd20668d8..dbc27f19c 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -64,8 +64,8 @@ struct BoundingConstraint1: public NoiseModelFactorN { return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } - Vector evaluateError(const X& x, boost::optional H = - boost::none) const override { + Vector evaluateError(const X& x, OptionalMatrixType H = + OptionalNone) const override { Matrix D; double error = value(x, D) - threshold_; if (H) { @@ -134,8 +134,8 @@ struct BoundingConstraint2: public NoiseModelFactorN { } Vector evaluateError(const X1& x1, const X2& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { Matrix D1, D2; double error = value(x1, x2, D1, D2) - threshold_; if (H1) { diff --git a/gtsam/slam/EssentialMatrixConstraint.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp index c1f8b286b..267432e4c 100644 --- a/gtsam/slam/EssentialMatrixConstraint.cpp +++ b/gtsam/slam/EssentialMatrixConstraint.cpp @@ -43,16 +43,20 @@ bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected, /* ************************************************************************* */ Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1, - const Pose3& p2, boost::optional Hp1, - boost::optional Hp2) const { + const Pose3& p2, OptionalMatrixType Hp1, + OptionalMatrixType Hp2) const { // compute relative Pose3 between p1 and p2 Pose3 _1P2_ = p1.between(p2, Hp1, Hp2); // convert to EssentialMatrix Matrix D_hx_1P2; - EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_, - (Hp1 || Hp2) ? boost::optional(D_hx_1P2) : boost::none); + EssentialMatrix hx; + if (Hp1 || Hp2) { + hx = EssentialMatrix::FromPose3(_1P2_, D_hx_1P2); + } else { + hx = EssentialMatrix::FromPose3(_1P2_, OptionalNone); + } // Calculate derivatives if needed if (Hp1) { diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 9d84dfa7b..6d72d34af 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -79,8 +79,8 @@ public: /** vector of errors */ Vector evaluateError(const Pose3& p1, const Pose3& p2, - boost::optional Hp1 = boost::none, // - boost::optional Hp2 = boost::none) const override; + OptionalMatrixType Hp1 = OptionalNone, // + OptionalMatrixType Hp2 = OptionalNone) const override; /** return the measured */ const EssentialMatrix& measured() const { diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 8a0277a45..e1d3395a2 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -91,7 +91,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN { /// vector of errors returns 1D vector Vector evaluateError( const EssentialMatrix& E, - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; @@ -173,8 +173,8 @@ class EssentialMatrixFactor2 */ Vector evaluateError( const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, - boost::optional Dd = boost::none) const override { + OptionalMatrixType DE = OptionalNone, + OptionalMatrixType Dd = OptionalNone) const override { // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d // We then convert to second camera by P2 = 1R2'*(P1-1T2) @@ -284,13 +284,13 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { */ Vector evaluateError( const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, - boost::optional Dd = boost::none) const override { + OptionalMatrixType DE = OptionalNone, + OptionalMatrixType Dd = OptionalNone) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; // Evaluate error - return Base::evaluateError(cameraE, d, boost::none, Dd); + return Base::evaluateError(cameraE, d, OptionalNone, Dd); } else { // Version with derivatives Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 @@ -372,13 +372,13 @@ class EssentialMatrixFactor4 */ Vector evaluateError( const EssentialMatrix& E, const CALIBRATION& K, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { // converting from pixel coordinates to normalized coordinates cA and cB JacobianCalibration cA_H_K; // dcA/dK JacobianCalibration cB_H_K; // dcB/dK - Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none); - Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none); + Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone); + Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone); // convert to homogeneous coordinates Vector3 vA = EssentialMatrix::Homogeneous(cA); diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 2e7e28d03..9521b1fb2 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -65,7 +65,7 @@ class FrobeniusPrior : public NoiseModelFactorN { /// Error is just Frobenius norm between Rot element and vectorized matrix M. Vector evaluateError(const Rot& R, - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { return R.vec(H) - vecM_; // Jacobian is computed only when needed. } }; @@ -86,8 +86,8 @@ class FrobeniusFactor : public NoiseModelFactorN { /// Error is just Frobenius norm between rotation matrices. Vector evaluateError(const Rot& R1, const Rot& R2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { Vector error = R2.vec(H2) - R1.vec(H1); if (H1) *H1 = -*H1; return error; @@ -150,8 +150,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { /// Error is Frobenius norm between R1*R12 and R2. Vector evaluateError(const Rot& R1, const Rot& R2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { const Rot R2hat = R1.compose(R12_); Eigen::Matrix vec_H_R2hat; Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 37296c0d8..a1a78c5d3 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -123,7 +123,7 @@ public: /** h(x)-z */ Vector evaluateError(const CAMERA& camera, const LANDMARK& point, - boost::optional H1=boost::none, boost::optional H2=boost::none) const override { + OptionalMatrixType H1=OptionalNone, OptionalMatrixType H2=OptionalNone) const override { try { return camera.project2(point,H1,H2) - measured_; } @@ -258,9 +258,9 @@ public: /** h(x)-z */ Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const override + OptionalMatrixType H1=OptionalNone, + OptionalMatrixType H2=OptionalNone, + OptionalMatrixType H3=OptionalNone) const override { try { Camera camera(pose3,calib); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 7ead4ad11..468d644cf 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -23,8 +23,8 @@ void OrientedPlane3Factor::print(const string& s, //*************************************************************************** Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, - const OrientedPlane3& plane, boost::optional H1, - boost::optional H2) const { + const OrientedPlane3& plane, OptionalMatrixType H1, + OptionalMatrixType H2) const { Matrix36 predicted_H_pose; Matrix33 predicted_H_plane, error_H_predicted; @@ -64,7 +64,7 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, //*************************************************************************** Vector OrientedPlane3DirectionPrior::evaluateError( - const OrientedPlane3& plane, boost::optional H) const { + const OrientedPlane3& plane, OptionalMatrixType H) const { Unit3 n_hat_p = measured_p_.normal(); Unit3 n_hat_q = plane.normal(); Matrix2 H_p; diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 759e66341..66febaee5 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -75,7 +75,7 @@ public: } /** h(x)-z */ - Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const override { + Vector evaluateError(const Pose& pose, OptionalMatrixType H = OptionalNone) const override { const Rotation& newR = pose.rotation(); if (H) { *H = Matrix::Zero(rDim, xDim); diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index e009ace29..fe1347e3c 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -59,7 +59,7 @@ public: gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** h(x)-z */ - Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const override { + Vector evaluateError(const Pose& pose, OptionalMatrixType H = OptionalNone) const override { const Translation& newTrans = pose.translation(); const Rotation& R = pose.rotation(); const int tDim = traits::GetDimension(newTrans); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index c90fc80d5..fa5e1d5fc 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -133,7 +133,7 @@ namespace gtsam { /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Point3& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override { try { if(body_P_sensor_) { if(H1) { diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 383c81cc4..9258a8e4e 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -95,12 +95,13 @@ public: /** Combined cost and derivative function using boost::optional */ Vector evaluateError(const Point& global, const Transform& trans, const Point& local, - boost::optional Dforeign = boost::none, - boost::optional Dtrans = boost::none, - boost::optional Dlocal = boost::none) const override { + OptionalMatrixType Dforeign = OptionalNone, + OptionalMatrixType Dtrans = OptionalNone, + OptionalMatrixType Dlocal = OptionalNone) const override { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); - if (Dlocal) + if (Dlocal) { *Dlocal = -1* Matrix::Identity(traits::dimension, traits::dimension); + } return traits::Local(local,newlocal); } diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 85539e17e..47727fa85 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -51,7 +51,7 @@ public: /// vector of errors returns 2D vector Vector evaluateError(const Rot3& R, - boost::optional H = boost::none) const override { + OptionalMatrixType H = OptionalNone) const override { // predict p_ as q = R*z_, derivative H will be filled if not none Point3 q = R.rotate(z_,H); // error is just difference, and note derivative of that wrpt q is I3 @@ -102,7 +102,7 @@ public: } /// vector of errors returns 2D vector - Vector evaluateError(const Rot3& iRc, boost::optional H = boost::none) const override { + Vector evaluateError(const Rot3& iRc, OptionalMatrixType H = OptionalNone) const override { Unit3 i_q = iRc * c_z_; Vector error = i_p_.error(i_q, H); if (H) { diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 1013b22b1..5e0f7d168 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -120,7 +120,7 @@ public: /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override { try { if(body_P_sensor_) { if(H1) { diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index f33ba2ca2..34a67d8a9 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -119,10 +119,10 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Point3& point, boost::optional H2 = - boost::none) const override { + Vector evaluateError(const Point3& point, OptionalMatrixType H2 = + OptionalNone) const override { try { - return traits::Local(measured_, camera_.project2(point, boost::none, H2)); + return traits::Local(measured_, camera_.project2(point, OptionalNone, H2)); } catch (CheiralityException& e) { if (H2) *H2 = Matrix::Zero(traits::dimension, 3); diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 34ab51d15..4f3bccd0a 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -129,7 +129,7 @@ namespace gtsam { } /// Evaluate error h(x)-z and optionally derivatives - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const override { Vector a; return a; diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 2cd0d0f39..bce27ac3e 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -139,7 +139,7 @@ namespace simulated2D { } /// Return error and optional derivative - Vector evaluateError(const Pose& x, boost::optional H = boost::none) const override { + Vector evaluateError(const Pose& x, OptionalMatrixType H = OptionalNone) const override { return (prior(x, H) - measured_); } @@ -184,8 +184,8 @@ namespace simulated2D { /// Evaluate error and optionally return derivatives Vector evaluateError(const Pose& x1, const Pose& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { return (odo(x1, x2, H1, H2) - measured_); } @@ -231,8 +231,8 @@ namespace simulated2D { /// Evaluate error and optionally return derivatives Vector evaluateError(const Pose& x1, const Landmark& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { return (mea(x1, x2, H1, H2) - measured_); } diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index 086da7cad..11c2fd9a1 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -90,8 +90,8 @@ namespace simulated2DOriented { } /// Evaluate error and optionally derivative - Vector evaluateError(const Pose2& x, boost::optional H = - boost::none) const { + Vector evaluateError(const Pose2& x, OptionalMatrixType H = + OptionalNone) const { return measured_.localCoordinates(prior(x, H)); } @@ -118,8 +118,8 @@ namespace simulated2DOriented { /// Evaluate error and optionally derivative Vector evaluateError(const VALUE& x1, const VALUE& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { return measured_.localCoordinates(odo(x1, x2, H1, H2)); } diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 4a20acd15..f65e8cfc3 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -89,8 +89,8 @@ struct PointPrior3D: public NoiseModelFactorN { * @param H is an optional Jacobian matrix (Dimension: 3x3) * @return Vector error between prior value and x (Dimension: 3) */ - Vector evaluateError(const Point3& x, boost::optional H = - boost::none) const override { + Vector evaluateError(const Point3& x, OptionalMatrixType H = + OptionalNone) const override { return prior(x, H) - measured_; } }; @@ -121,7 +121,7 @@ struct Simulated3DMeasurement: public NoiseModelFactorN { * @return vector error between measurement and prediction (Dimension: 3) */ Vector evaluateError(const Point3& x1, const Point3& x2, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override { return mea(x1, x2, H1, H2) - measured_; } }; diff --git a/tests/smallExample.h b/tests/smallExample.h index 7439a5436..3eab85b49 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -335,7 +335,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactorN { gtsam::NoiseModelFactorN(model, key), z_(z) { } - Vector evaluateError(const Point2& x, boost::optional A = boost::none) const override { + Vector evaluateError(const Point2& x, OptionalMatrixType A = OptionalNone) const override { if (A) *A = H(x); return (h(x) - z_); } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 88da7006d..f6fcce1e2 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -213,8 +213,8 @@ public: /** vector of errors */ Vector evaluateError(const Point2& p1, const Point2& p2, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const override { + OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = + OptionalNone) const override { // error = p2 - f(p1) // H1 = d error / d p1 = -d f/ d p1 = -F @@ -339,7 +339,7 @@ public: } /** vector of errors */ - Vector evaluateError(const Point2& p, boost::optional H1 = boost::none) const override { + Vector evaluateError(const Point2& p, OptionalMatrixType H1 = OptionalNone) const override { // error = z - h(p) // H = d error / d p = -d h/ d p = -H Vector z_hat = h(p); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index c82361450..43912d703 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -340,8 +340,7 @@ class TestFactor1 : public NoiseModelFactor1 { TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {} using Base::NoiseModelFactor1; // inherit constructors - Vector evaluateError(const double& x1, boost::optional H1 = - boost::none) const override { + Vector evaluateError(const double& x1, OptionalMatrixType H1 = OptionalNone) const override { if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); return (Vector(1) << x1).finished(); } @@ -394,10 +393,10 @@ class TestFactor4 : public NoiseModelFactor4 { Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone, + OptionalMatrixType H4 = OptionalNone) const override { if(H1) { *H1 = (Matrix(1, 1) << 1.0).finished(); *H2 = (Matrix(1, 1) << 2.0).finished(); @@ -484,11 +483,11 @@ public: Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone, + OptionalMatrixType H4 = OptionalNone, + OptionalMatrixType H5 = OptionalNone) const override { if(H1) { *H1 = (Matrix(1, 1) << 1.0).finished(); *H2 = (Matrix(1, 1) << 2.0).finished(); @@ -534,12 +533,12 @@ public: Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone, + OptionalMatrixType H4 = OptionalNone, + OptionalMatrixType H5 = OptionalNone, + OptionalMatrixType H6 = OptionalNone) const override { if(H1) { *H1 = (Matrix(1, 1) << 1.0).finished(); *H2 = (Matrix(1, 1) << 2.0).finished(); @@ -594,10 +593,10 @@ public: Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone, + OptionalMatrixType H4 = OptionalNone) const override { if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); if (H2) *H2 = (Matrix(1, 1) << 2.0).finished(); if (H3) *H3 = (Matrix(1, 1) << 3.0).finished();