From 9329bddd8a3ab134527a005f9957a8b73506da01 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Fri, 13 Jan 2023 14:11:03 -0800 Subject: [PATCH] OptionalJacobian --- gtsam/base/Lie.h | 30 +++++----- gtsam/base/Matrix.h | 12 ++-- gtsam/base/OptionalJacobian.h | 53 ++++------------ gtsam/base/ProductLieGroup.h | 14 ++--- gtsam/base/TestableAssertions.h | 6 +- gtsam/base/Value.h | 1 + gtsam/base/VectorSpace.h | 60 +++++++++---------- gtsam/base/numericalDerivative.h | 4 +- gtsam/base/tests/testOptionalJacobian.cpp | 13 ++-- gtsam/basis/Basis.h | 28 ++++----- gtsam/basis/tests/testChebyshev2.cpp | 9 +-- gtsam/geometry/BearingRange.h | 12 ++-- gtsam/geometry/Cal3.h | 4 +- gtsam/geometry/Cal3Bundler.cpp | 4 +- gtsam/geometry/Cal3Bundler.h | 8 +-- gtsam/geometry/Cal3DS2_Base.h | 8 +-- gtsam/geometry/Cal3Fisheye.cpp | 2 +- gtsam/geometry/Cal3Fisheye.h | 8 +-- gtsam/geometry/Cal3Unified.h | 8 +-- gtsam/geometry/Cal3_S2.h | 12 ++-- gtsam/geometry/Cal3_S2Stereo.h | 8 +-- gtsam/geometry/CalibratedCamera.cpp | 2 +- gtsam/geometry/CalibratedCamera.h | 36 +++++------ gtsam/geometry/EssentialMatrix.h | 14 ++--- gtsam/geometry/Line3.h | 14 ++--- gtsam/geometry/OrientedPlane3.h | 14 ++--- gtsam/geometry/PinholeCamera.h | 22 +++---- gtsam/geometry/PinholePose.h | 48 +++++++-------- gtsam/geometry/Point2.h | 14 ++--- gtsam/geometry/Point3.h | 20 +++---- gtsam/geometry/Quaternion.h | 14 ++--- gtsam/geometry/Rot2.h | 18 +++--- gtsam/geometry/Rot3.h | 56 ++++++++--------- gtsam/geometry/SO3.h | 10 ++-- gtsam/geometry/SO4.h | 4 +- gtsam/geometry/SOn.h | 10 ++-- gtsam/geometry/Similarity2.h | 8 +-- gtsam/geometry/Similarity3.h | 12 ++-- gtsam/geometry/SphericalCamera.cpp | 2 +- gtsam/geometry/SphericalCamera.h | 16 ++--- gtsam/geometry/StereoCamera.h | 10 ++-- gtsam/geometry/tests/testCal3DS2.cpp | 4 +- gtsam/geometry/tests/testCal3Unified.cpp | 4 +- gtsam/geometry/tests/testCal3_S2.cpp | 8 +-- gtsam/geometry/tests/testCalibratedCamera.cpp | 2 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 36 ++++++----- gtsam/geometry/tests/testOrientedPlane3.cpp | 28 +++++---- gtsam/geometry/tests/testPinholeCamera.cpp | 4 +- gtsam/geometry/tests/testPinholePose.cpp | 2 +- gtsam/geometry/tests/testPoint3.cpp | 12 ++-- gtsam/geometry/tests/testPose2.cpp | 8 +-- gtsam/geometry/tests/testRot3.cpp | 2 +- gtsam/geometry/tests/testRot3Q.cpp | 2 +- gtsam/geometry/tests/testSO3.cpp | 10 ++-- gtsam/geometry/tests/testSimpleCamera.cpp | 2 +- gtsam/geometry/tests/testStereoCamera.cpp | 4 +- gtsam/geometry/tests/testUnit3.cpp | 42 ++++++------- gtsam/inference/EliminateableFactorGraph.h | 5 +- gtsam/linear/GaussianFactorGraph.h | 3 +- gtsam/navigation/AHRSFactor.h | 2 +- gtsam/navigation/AttitudeFactor.h | 2 +- gtsam/navigation/ImuBias.h | 8 +-- gtsam/navigation/MagFactor.h | 2 +- gtsam/navigation/ManifoldPreintegration.cpp | 2 +- gtsam/navigation/ManifoldPreintegration.h | 2 +- gtsam/navigation/TangentPreintegration.h | 12 ++-- gtsam/navigation/tests/testImuBias.cpp | 4 +- gtsam/navigation/tests/testImuFactor.cpp | 20 +++---- .../tests/testManifoldPreintegration.cpp | 4 +- gtsam/navigation/tests/testNavState.cpp | 20 +++---- .../tests/testTangentPreintegration.cpp | 4 +- gtsam/nonlinear/AdaptAutoDiff.h | 4 +- gtsam/nonlinear/Expression-inl.h | 6 +- gtsam/nonlinear/FunctorizedFactor.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.h | 5 +- gtsam/nonlinear/internal/ExpressionNode.h | 10 ++-- gtsam/nonlinear/tests/testExpression.cpp | 2 +- gtsam/nonlinear/tests/testFactorTesting.cpp | 6 +- .../nonlinear/tests/testFunctorizedFactor.cpp | 12 ++-- gtsam/nonlinear/tests/testValues.cpp | 8 +-- gtsam/slam/ProjectionFactor.h | 6 +- gtsam/slam/TriangulationFactor.h | 2 +- gtsam/slam/tests/testTriangulationFactor.cpp | 2 +- gtsam_unstable/dynamics/PoseRTV.h | 8 +-- gtsam_unstable/dynamics/SimpleHelicopter.h | 2 +- gtsam_unstable/geometry/Event.h | 6 +- gtsam_unstable/geometry/InvDepthCamera3.h | 2 +- gtsam_unstable/geometry/Pose3Upright.h | 10 ++-- gtsam_unstable/geometry/tests/testEvent.cpp | 4 +- gtsam_unstable/gtsam_unstable.i | 2 +- tests/testSimulated3D.cpp | 6 +- 91 files changed, 496 insertions(+), 517 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index c4eba5deb..1b574816a 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -54,14 +54,14 @@ struct LieGroup { } Class compose(const Class& g, ChartJacobian H1, - ChartJacobian H2 = boost::none) const { + ChartJacobian H2 = {}) const { if (H1) *H1 = g.inverse().AdjointMap(); if (H2) *H2 = Eigen::Matrix::Identity(); return derived() * g; } Class between(const Class& g, ChartJacobian H1, - ChartJacobian H2 = boost::none) const { + ChartJacobian H2 = {}) const { Class result = derived().inverse() * g; if (H1) *H1 = - result.inverse().AdjointMap(); if (H2) *H2 = Eigen::Matrix::Identity(); @@ -87,7 +87,7 @@ struct LieGroup { /// expmap with optional derivatives Class expmap(const TangentVector& v, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ChartJacobian H1, ChartJacobian H2 = {}) const { Jacobian D_g_v; Class g = Class::Expmap(v,H2 ? &D_g_v : 0); Class h = compose(g); // derivatives inlined below @@ -98,7 +98,7 @@ struct LieGroup { /// logmap with optional derivatives TangentVector logmap(const Class& g, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ChartJacobian H1, ChartJacobian H2 = {}) const { Class h = between(g); // derivatives inlined below Jacobian D_v_h; TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0); @@ -139,7 +139,7 @@ struct LieGroup { /// retract with optional derivatives Class retract(const TangentVector& v, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ChartJacobian H1, ChartJacobian H2 = {}) const { Jacobian D_g_v; Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0); Class h = compose(g); // derivatives inlined below @@ -150,7 +150,7 @@ struct LieGroup { /// localCoordinates with optional derivatives TangentVector localCoordinates(const Class& g, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ChartJacobian H1, ChartJacobian H2 = {}) const { Class h = between(g); // derivatives inlined below Jacobian D_v_h; TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); @@ -188,38 +188,38 @@ struct LieGroupTraits: GetDimensionImpl { typedef OptionalJacobian ChartJacobian; static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { + ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { return origin.localCoordinates(other, Horigin, Hother); } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { + ChartJacobian Horigin = {}, ChartJacobian Hv = {}) { return origin.retract(v, Horigin, Hv); } /// @} /// @name Lie Group /// @{ - static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) { return Class::Logmap(m, Hm); } - static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { return Class::Expmap(v, Hv); } static Class Compose(const Class& m1, const Class& m2, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.compose(m2, H1, H2); } static Class Between(const Class& m1, const Class& m2, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.between(m2, H1, H2); } static Class Inverse(const Class& m, // - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return m.inverse(H); } /// @} @@ -325,8 +325,8 @@ T expm(const Vector& x, int K=7) { */ template T interpolate(const T& X, const T& Y, double t, - typename MakeOptionalJacobian::type Hx = boost::none, - typename MakeOptionalJacobian::type Hy = boost::none) { + typename MakeOptionalJacobian::type Hx = {}, + typename MakeOptionalJacobian::type Hy = {}) { if (Hx || Hy) { typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; const T between = diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index cfedf6d8c..88714d6f6 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -459,8 +459,8 @@ struct MultiplyWithInverse { /// A.inverse() * b, with optional derivatives VectorN operator()(const MatrixN& A, const VectorN& b, - OptionalJacobian H1 = boost::none, - OptionalJacobian H2 = boost::none) const { + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) const { const MatrixN invA = A.inverse(); const VectorN c = invA * b; // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA] @@ -495,16 +495,16 @@ struct MultiplyWithInverseFunction { /// f(a).inverse() * b, with optional derivatives VectorN operator()(const T& a, const VectorN& b, - OptionalJacobian H1 = boost::none, - OptionalJacobian H2 = boost::none) const { + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) const { MatrixN A; - phi_(a, b, boost::none, A); // get A = f(a) by calling f once + phi_(a, b, {}, A); // get A = f(a) by calling f once const MatrixN invA = A.inverse(); const VectorN c = invA * b; if (H1) { Eigen::Matrix H; - phi_(a, c, H, boost::none); // get derivative H of forward mapping + phi_(a, c, H, {}); // get derivative H of forward mapping *H1 = -invA* H; } if (H2) *H2 = invA; diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 4bde4e5e7..3d5af97fd 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -18,23 +18,18 @@ */ #pragma once +#include #include // Configuration from CMake #include #include #include -#ifndef OPTIONALJACOBIAN_NOBOOST -#include -#endif - namespace gtsam { /** * OptionalJacobian is an Eigen::Ref like class that can take be constructed using * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic - * matrix will be resized. Finally, there is a constructor that takes - * boost::none, the default constructor acts like boost::none, and - * boost::optional is also supported for backwards compatibility. + * matrix will be resized. * Below this class, a dynamic version is also implemented. */ template @@ -66,11 +61,18 @@ private: public: - /// Default constructor acts like boost::none + /// Default constructor OptionalJacobian() : map_(nullptr) { } + /// Default constructor with nullptr_t + /// To guide the compiler when nullptr + /// is passed to args of the type OptionalJacobian + OptionalJacobian(std::nullptr_t /*unused*/) : + map_(nullptr) { + } + /// Constructor that will usurp data of a fixed-size matrix OptionalJacobian(Jacobian& fixed) : map_(nullptr) { @@ -118,24 +120,6 @@ public: } } -#ifndef OPTIONALJACOBIAN_NOBOOST - - /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t /*none*/) : - map_(nullptr) { - } - - /// Constructor compatible with old-style derivatives - OptionalJacobian(const boost::optional optional) : - map_(nullptr) { - if (optional) { - optional->resize(Rows, Cols); - usurp(optional->data()); - } - } - -#endif - /// Constructor that will usurp data of a block expression /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible // template @@ -200,7 +184,7 @@ private: public: - /// Default constructor acts like boost::none + /// Default constructor acts like OptionalJacobian() : pointer_(nullptr) { } @@ -211,21 +195,6 @@ public: /// Construct from refrence to dynamic matrix OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {} -#ifndef OPTIONALJACOBIAN_NOBOOST - - /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t /*none*/) : - pointer_(nullptr) { - } - - /// Constructor compatible with old-style derivatives - OptionalJacobian(const boost::optional optional) : - pointer_(nullptr) { - if (optional) pointer_ = &(*optional); - } - -#endif - /// Return true if allocated, false if default constructor was used operator bool() const { return pointer_!=nullptr; diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 6689c11d6..831968bc8 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -75,14 +75,14 @@ public: typedef OptionalJacobian ChartJacobian; ProductLieGroup retract(const TangentVector& v, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) const { if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet"); G g = traits::Retract(this->first, v.template head()); H h = traits::Retract(this->second, v.template tail()); return ProductLieGroup(g,h); } TangentVector localCoordinates(const ProductLieGroup& g, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) const { if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet"); typename traits::TangentVector v1 = traits::Local(this->first, g.first); typename traits::TangentVector v2 = traits::Local(this->second, g.second); @@ -101,7 +101,7 @@ protected: public: ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = boost::none) const { + ChartJacobian H2 = {}) const { Jacobian1 D_g_first; Jacobian2 D_h_second; G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); @@ -114,7 +114,7 @@ public: return ProductLieGroup(g,h); } ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = boost::none) const { + ChartJacobian H2 = {}) const { Jacobian1 D_g_first; Jacobian2 D_h_second; G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); @@ -137,7 +137,7 @@ public: } return ProductLieGroup(g,h); } - static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {}) { Jacobian1 D_g_first; Jacobian2 D_h_second; G g = traits::Expmap(v.template head(), Hv ? &D_g_first : 0); H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); @@ -148,7 +148,7 @@ public: } return ProductLieGroup(g,h); } - static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}) { Jacobian1 D_g_first; Jacobian2 D_h_second; typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? &D_g_first : 0); typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? &D_h_second : 0); @@ -161,7 +161,7 @@ public: } return v; } - static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = {}) { return Logmap(p, Hp); } ProductLieGroup expmap(const TangentVector& v) const { diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 0bf3cf21f..3ade1cc2e 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -50,11 +50,11 @@ template bool assert_equal(const std::optional& expected, const std::optional& actual, double tol = 1e-9) { if (!expected && actual) { - std::cout << "expected is boost::none, while actual is not" << std::endl; + std::cout << "expected is {}, while actual is not" << std::endl; return false; } if (expected && !actual) { - std::cout << "actual is boost::none, while expected is not" << std::endl; + std::cout << "actual is {}, while expected is not" << std::endl; return false; } if (!expected && !actual) @@ -65,7 +65,7 @@ bool assert_equal(const std::optional& expected, template bool assert_equal(const V& expected, const std::optional& actual, double tol = 1e-9) { if (!actual) { - std::cout << "actual is boost::none" << std::endl; + std::cout << "actual is {}" << std::endl; return false; } return assert_equal(expected, *actual, tol); diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 697c4f3be..0d2c601cd 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -21,6 +21,7 @@ #include // Configuration from CMake #include +#include #include #include #include diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index f7809f596..9fbd581bb 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -32,7 +32,7 @@ struct VectorSpaceImpl { static int GetDimension(const Class&) { return N;} static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); Class v = other-origin; @@ -40,7 +40,7 @@ struct VectorSpaceImpl { } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return origin + v; @@ -51,31 +51,31 @@ struct VectorSpaceImpl { /// @name Lie Group /// @{ - static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) { if (Hm) *Hm = Jacobian::Identity(); return m.vector(); } - static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { if (Hv) *Hv = Jacobian::Identity(); return Class(v); } - static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, - ChartJacobian H2 = boost::none) { + static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v1 + v2; } - static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, - ChartJacobian H2 = boost::none) { + static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v2 - v1; } - static Class Inverse(const Class& v, ChartJacobian H = boost::none) { + static Class Inverse(const Class& v, ChartJacobian H = {}) { if (H) *H = - Jacobian::Identity(); return -v; } @@ -106,7 +106,7 @@ struct VectorSpaceImpl { } static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Eye(origin); if (H2) *H2 = Eye(other); Class v = other-origin; @@ -114,7 +114,7 @@ struct VectorSpaceImpl { } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = Eye(origin); if (H2) *H2 = Eye(origin); return origin + v; @@ -125,12 +125,12 @@ struct VectorSpaceImpl { /// @name Lie Group /// @{ - static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) { if (Hm) *Hm = Eye(m); return m.vector(); } - static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { Class result(v); if (Hv) *Hv = Eye(v); @@ -138,14 +138,14 @@ struct VectorSpaceImpl { } static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2 = boost::none) { + ChartJacobian H2 = {}) { if (H1) *H1 = Eye(v1); if (H2) *H2 = Eye(v2); return v1 + v2; } static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2 = boost::none) { + ChartJacobian H2 = {}) { if (H1) *H1 = - Eye(v1); if (H2) *H2 = Eye(v2); return v2 - v1; @@ -237,7 +237,7 @@ struct ScalarTraits : VectorSpaceImpl { typedef OptionalJacobian<1, 1> ChartJacobian; static TangentVector Local(Scalar origin, Scalar other, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) (*H1)[0] = -1.0; if (H2) (*H2)[0] = 1.0; TangentVector result; @@ -246,7 +246,7 @@ struct ScalarTraits : VectorSpaceImpl { } static Scalar Retract(Scalar origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) (*H1)[0] = 1.0; if (H2) (*H2)[0] = 1.0; return origin + v[0]; @@ -255,12 +255,12 @@ struct ScalarTraits : VectorSpaceImpl { /// @name Lie Group /// @{ - static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) { + static TangentVector Logmap(Scalar m, ChartJacobian H = {}) { if (H) (*H)[0] = 1.0; return Local(0, m); } - static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + static Scalar Expmap(const TangentVector& v, ChartJacobian H = {}) { if (H) (*H)[0] = 1.0; return v[0]; } @@ -312,7 +312,7 @@ struct traits > : typedef OptionalJacobian ChartJacobian; static TangentVector Local(const Fixed& origin, const Fixed& other, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) (*H1) = -Jacobian::Identity(); if (H2) (*H2) = Jacobian::Identity(); TangentVector result; @@ -321,7 +321,7 @@ struct traits > : } static Fixed Retract(const Fixed& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) (*H1) = Jacobian::Identity(); if (H2) (*H2) = Jacobian::Identity(); return origin + Eigen::Map(v.data()); @@ -330,14 +330,14 @@ struct traits > : /// @name Lie Group /// @{ - static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) { + static TangentVector Logmap(const Fixed& m, ChartJacobian H = {}) { if (H) *H = Jacobian::Identity(); TangentVector result; Eigen::Map(result.data()) = m; return result; } - static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + static Fixed Expmap(const TangentVector& v, ChartJacobian H = {}) { Fixed m; m.setZero(); if (H) *H = Jacobian::Identity(); @@ -393,7 +393,7 @@ struct DynamicTraits { } static TangentVector Local(const Dynamic& m, const Dynamic& other, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = -Eye(m); if (H2) *H2 = Eye(m); TangentVector v(GetDimension(m)); @@ -402,7 +402,7 @@ struct DynamicTraits { } static Dynamic Retract(const Dynamic& m, const TangentVector& v, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = Eye(m); if (H2) *H2 = Eye(m); return m + Eigen::Map(v.data(), m.rows(), m.cols()); @@ -411,32 +411,32 @@ struct DynamicTraits { /// @name Lie Group /// @{ - static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) { + static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) { if (H) *H = Eye(m); TangentVector result(GetDimension(m)); Eigen::Map(result.data(), m.cols(), m.rows()) = m; return result; } - static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) { + static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = {}) { static_cast(H); throw std::runtime_error("Expmap not defined for dynamic types"); } - static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) { + static Dynamic Inverse(const Dynamic& m, ChartJacobian H = {}) { if (H) *H = -Eye(m); return -m; } static Dynamic Compose(const Dynamic& v1, const Dynamic& v2, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = Eye(v1); if (H2) *H2 = Eye(v1); return v1 + v2; } static Dynamic Between(const Dynamic& v1, const Dynamic& v2, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = -Eye(v1); if (H2) *H2 = Eye(v1); return v2 - v1; diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index e4255bc63..8c9934d77 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -37,8 +37,8 @@ namespace gtsam { * for a function with one relevant param and an optional derivative: * Foo bar(const Obj& a, OptionalMatrixType H1) * Use boost.bind to restructure: - * std::bind(bar, std::placeholders::_1, boost::none) - * This syntax will fix the optional argument to boost::none, while using the first argument provided + * std::bind(bar, std::placeholders::_1, {}) + * This syntax will fix the optional argument to {}, while using the first argument provided * * For member functions, such as below, with an instantiated copy instanceOfSomeClass * Foo SomeClass::bar(const Obj& a) diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index ae91642f4..7f1197584 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -32,7 +32,6 @@ using namespace gtsam; TEST( OptionalJacobian, Constructors ) { Matrix23 fixed; Matrix dynamic; - boost::optional optional(dynamic); OptionalJacobian<2, 3> H; EXPECT(!H); @@ -41,22 +40,18 @@ TEST( OptionalJacobian, Constructors ) { TEST_CONSTRUCTOR(2, 3, &fixed, true); TEST_CONSTRUCTOR(2, 3, dynamic, true); TEST_CONSTRUCTOR(2, 3, &dynamic, true); - TEST_CONSTRUCTOR(2, 3, boost::none, false); - TEST_CONSTRUCTOR(2, 3, optional, true); // Test dynamic OptionalJacobian<-1, -1> H7; EXPECT(!H7); TEST_CONSTRUCTOR(-1, -1, dynamic, true); - TEST_CONSTRUCTOR(-1, -1, boost::none, false); - TEST_CONSTRUCTOR(-1, -1, optional, true); } //****************************************************************************** Matrix kTestMatrix = (Matrix23() << 11,12,13,21,22,23).finished(); -void test(OptionalJacobian<2, 3> H = boost::none) { +void test(OptionalJacobian<2, 3> H = {}) { if (H) *H = kTestMatrix; } @@ -116,7 +111,7 @@ TEST( OptionalJacobian, Fixed) { } //****************************************************************************** -void test2(OptionalJacobian<-1,-1> H = boost::none) { +void test2(OptionalJacobian<-1,-1> H = {}) { if (H) *H = kTestMatrix; // resizes } @@ -145,12 +140,12 @@ TEST( OptionalJacobian, Dynamic) { } //****************************************************************************** -void test3(double add, OptionalJacobian<2,1> H = boost::none) { +void test3(double add, OptionalJacobian<2,1> H = {}) { if (H) *H << add + 10, add + 20; } // This function calls the above function three times, one for each column -void test4(OptionalJacobian<2, 3> H = boost::none) { +void test4(OptionalJacobian<2, 3> H = {}) { if (H) { test3(1, H.cols<1>(0)); test3(2, H.cols<1>(1)); diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index d517078f1..97704dab4 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -152,14 +152,14 @@ class Basis { /// Regular 1D evaluation double apply(const typename DERIVED::Parameters& p, - OptionalJacobian<-1, -1> H = boost::none) const { + OptionalJacobian<-1, -1> H = {}) const { if (H) *H = weights_; return (weights_ * p)(0); } /// c++ sugar double operator()(const typename DERIVED::Parameters& p, - OptionalJacobian<-1, -1> H = boost::none) const { + OptionalJacobian<-1, -1> H = {}) const { return apply(p, H); // might call apply in derived } @@ -212,14 +212,14 @@ class Basis { /// M-dimensional evaluation VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar VectorM operator()(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -271,14 +271,14 @@ class Basis { /// Calculate component of component rowIndex_ of P double apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = H_; return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); } /// c++ sugar double operator()(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -315,7 +315,7 @@ class Basis { /// Manifold evaluation T apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { // Interpolate the M-dimensional vector to yield a vector in tangent space Eigen::Matrix xi = Base::operator()(P, H); @@ -334,7 +334,7 @@ class Basis { /// c++ sugar T operator()(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); // might call apply in derived } }; @@ -377,13 +377,13 @@ class Basis { : DerivativeFunctorBase(N, x, a, b) {} double apply(const typename DERIVED::Parameters& p, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = this->weights_; return (this->weights_ * p)(0); } /// c++ sugar double operator()(const typename DERIVED::Parameters& p, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(p, H); // might call apply in derived } }; @@ -433,14 +433,14 @@ class Basis { } VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar VectorM operator()( const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -491,13 +491,13 @@ class Basis { } /// Calculate derivative of component rowIndex_ of F double apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = H_; return P.row(rowIndex_) * this->weights_.transpose(); } /// c++ sugar double operator()(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); } }; diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 73339e0f1..177ea5b49 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -17,6 +17,7 @@ * methods */ +#include #include #include #include @@ -119,7 +120,7 @@ TEST(Chebyshev2, InterpolateVector) { // Check derivative std::function)> f = boost::bind( - &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); + &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, nullptr); Matrix numericalH = numericalDerivative11, 2 * N>(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); @@ -377,7 +378,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { // Test Jacobian Matrix expectedH = numericalDerivative11, M * N>( - boost::bind(&VecD::operator(), fx, _1, boost::none), X); + boost::bind(&VecD::operator(), fx, _1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -409,7 +410,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { VecD vecd(N, points(0), 0, T); vecd(X, actualH); Matrix expectedH = numericalDerivative11, M * N>( - boost::bind(&VecD::operator(), vecd, _1, boost::none), X); + boost::bind(&VecD::operator(), vecd, _1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-6)); } @@ -426,7 +427,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); Matrix expectedH = numericalDerivative11, M * N>( - boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); + boost::bind(&CompFunc::operator(), fx, _1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 95b0232f0..ca767c1f1 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -77,8 +77,8 @@ public: /// Prediction function that stacks measurements static BearingRange Measure( const A1& a1, const A2& a2, - OptionalJacobian::dimension> H1 = boost::none, - OptionalJacobian::dimension> H2 = boost::none) { + OptionalJacobian::dimension> H1 = {}, + OptionalJacobian::dimension> H2 = {}) { typename MakeJacobian::type HB1; typename MakeJacobian::type HB2; typename MakeJacobian::type HR1; @@ -181,8 +181,8 @@ struct HasBearing { typedef RT result_type; RT operator()( const A1& a1, const A2& a2, - OptionalJacobian::dimension, traits::dimension> H1=boost::none, - OptionalJacobian::dimension, traits::dimension> H2=boost::none) { + OptionalJacobian::dimension, traits::dimension> H1={}, + OptionalJacobian::dimension, traits::dimension> H2={}) { return a1.bearing(a2, H1, H2); } }; @@ -195,8 +195,8 @@ struct HasRange { typedef RT result_type; RT operator()( const A1& a1, const A2& a2, - OptionalJacobian::dimension, traits::dimension> H1=boost::none, - OptionalJacobian::dimension, traits::dimension> H2=boost::none) { + OptionalJacobian::dimension, traits::dimension> H1={}, + OptionalJacobian::dimension, traits::dimension> H2={}) { return a1.range(a2, H1, H2); } }; diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 25acc11f8..6f446c8b3 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -45,8 +45,8 @@ namespace gtsam { */ template void calibrateJacobians(const Cal& calibration, const Point2& pn, - OptionalJacobian<2, Dim> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) { + OptionalJacobian<2, Dim> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) { if (Dcal || Dp) { Eigen::Matrix H_uncal_K; Matrix22 H_uncal_pn, H_uncal_pn_inv; diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 3ae624bbc..cfaea61a9 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -131,14 +131,14 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, /* ************************************************************************* */ Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { Matrix2 Dp; - uncalibrate(p, boost::none, Dp); + uncalibrate(p, {}, Dp); return Dp; } /* ************************************************************************* */ Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const { Matrix23 Dcal; - uncalibrate(p, Dcal, boost::none); + uncalibrate(p, Dcal, {}); return Dcal; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index c8e0fe381..f333c2f02 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -116,8 +116,8 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert a pixel coordinate to ideal coordinate xy @@ -127,8 +127,8 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index f901de9a9..c8c76dcb9 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -122,12 +122,12 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in (distorted) image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy - Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// Derivative of uncalibrate wrpt intrinsic coordinates Matrix2 D2d_intrinsic(const Point2& p) const; diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index fd2c7ab65..fac09e465 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -134,7 +134,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, Matrix2 jac; // Calculate the current estimate (uv_hat) and the jacobian - const Point2 uv_hat = uncalibrate(pi, boost::none, jac); + const Point2 uv_hat = uncalibrate(pi, {}, jac); // Test convergence if ((uv_hat - uv).norm() < tol_) break; diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index a314acb5e..2530231c5 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -121,8 +121,8 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) * @return point in (distorted) image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, @@ -132,8 +132,8 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// @} /// @name Testable diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 83024c0ce..cf25c8f00 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -106,12 +106,12 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { * @return point in image coordinates */ Point2 uncalibrate(const Point2& p, - OptionalJacobian<2, 10> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + OptionalJacobian<2, 10> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// Conver a pixel coordinate to ideal coordinate - Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// Convert a 3D point to normalized unit plane Point2 spaceToNPlane(const Point2& p) const; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 10061fae8..b8d776538 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -66,8 +66,8 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert image coordinates uv to intrinsic coordinates xy @@ -76,8 +76,8 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert homogeneous image coordinates to intrinsic coordinates @@ -102,8 +102,8 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) inline Cal3_S2 between(const Cal3_S2& q, - OptionalJacobian<5, 5> H1 = boost::none, - OptionalJacobian<5, 5> H2 = boost::none) const { + OptionalJacobian<5, 5> H1 = {}, + OptionalJacobian<5, 5> H2 = {}) const { if (H1) *H1 = -I_5x5; if (H2) *H2 = I_5x5; return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_, diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index ddd96bf17..574b5d7b5 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -64,8 +64,8 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert image coordinates uv to intrinsic coordinates xy @@ -74,8 +74,8 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert homogeneous image coordinates to intrinsic coordinates diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index dd88f9f69..34deb5c84 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -117,7 +117,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { Matrix3 Rt; // calculated by transformTo if needed - const Point3 q = pose().transformTo(point, boost::none, Dpoint ? &Rt : 0); + const Point3 q = pose().transformTo(point, {}, Dpoint ? &Rt : 0); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (q.z() <= 0) throw CheiralityException(); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 941ed3ffc..c8e27444e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -176,7 +176,7 @@ public: * @param pc point in camera coordinates */ static Point2 Project(const Point3& pc, // - OptionalJacobian<2, 3> Dpoint = boost::none); + OptionalJacobian<2, 3> Dpoint = {}); /** * Project from 3D point at infinity in camera coordinates into image @@ -184,7 +184,7 @@ public: * @param pc point in camera coordinates */ static Point2 Project(const Unit3& pc, // - OptionalJacobian<2, 2> Dpoint = boost::none); + OptionalJacobian<2, 2> Dpoint = {}); /// Project a point into the image and check depth std::pair projectSafe(const Point3& pw) const; @@ -195,7 +195,7 @@ public: * @return the intrinsic coordinates of the projected point */ Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + {}, OptionalJacobian<2, 3> Dpoint = {}) const; /** Project point at infinity into the image * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION @@ -203,13 +203,13 @@ public: * @return the intrinsic coordinates of the projected point */ Point2 project2(const Unit3& point, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const; + OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth static Point3 BackprojectFromCamera(const Point2& p, const double depth, - OptionalJacobian<3, 2> Dpoint = boost::none, - OptionalJacobian<3, 1> Ddepth = boost::none); + OptionalJacobian<3, 2> Dpoint = {}, + OptionalJacobian<3, 1> Ddepth = {}); /// @} /// @name Advanced interface @@ -270,7 +270,7 @@ public: // Create CalibratedCamera, with derivatives static CalibratedCamera Create(const Pose3& pose, - OptionalJacobian H1 = boost::none) { + OptionalJacobian H1 = {}) { if (H1) *H1 << I_6x6; return CalibratedCamera(pose); @@ -346,13 +346,13 @@ public: * Use project2, which is more consistently named across Pinhole cameras */ Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + {}, OptionalJacobian<2, 3> Dpoint = {}) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth Point3 backproject(const Point2& pn, double depth, - OptionalJacobian<3, 6> Dresult_dpose = boost::none, - OptionalJacobian<3, 2> Dresult_dp = boost::none, - OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const { + OptionalJacobian<3, 6> Dresult_dpose = {}, + OptionalJacobian<3, 2> Dresult_dp = {}, + OptionalJacobian<3, 1> Dresult_ddepth = {}) const { Matrix32 Dpoint_dpn; Matrix31 Dpoint_ddepth; @@ -379,8 +379,8 @@ public: * @return range (double) */ double range(const Point3& point, - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { + OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 3> Dpoint = {}) const { return pose().range(point, Dcamera, Dpoint); } @@ -389,8 +389,8 @@ public: * @param pose Other SO(3) pose * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 6> Dpose = {}) const { return this->pose().range(pose, Dcamera, Dpose); } @@ -400,8 +400,8 @@ public: * @return range (double) */ double range(const CalibratedCamera& camera, // - OptionalJacobian<1, 6> H1 = boost::none, // - OptionalJacobian<1, 6> H2 = boost::none) const { + OptionalJacobian<1, 6> H1 = {}, // + OptionalJacobian<1, 6> H2 = {}) const { return pose().range(camera.pose(), H1, H2); } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 909576aa0..ea980d667 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -49,12 +49,12 @@ class EssentialMatrix { /// Named constructor with derivatives GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb, - OptionalJacobian<5, 3> H1 = boost::none, - OptionalJacobian<5, 2> H2 = boost::none); + OptionalJacobian<5, 3> H1 = {}, + OptionalJacobian<5, 2> H2 = {}); /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) GTSAM_EXPORT static EssentialMatrix FromPose3(const Pose3& _1P2_, - OptionalJacobian<5, 6> H = boost::none); + OptionalJacobian<5, 6> H = {}); /// Random, using Rot3::Random and Unit3::Random template @@ -139,8 +139,8 @@ class EssentialMatrix { * @return point in pose coordinates */ GTSAM_EXPORT Point3 transformTo(const Point3& p, - OptionalJacobian<3, 5> DE = boost::none, - OptionalJacobian<3, 3> Dpoint = boost::none) const; + OptionalJacobian<3, 5> DE = {}, + OptionalJacobian<3, 3> Dpoint = {}) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -148,7 +148,7 @@ class EssentialMatrix { * @param E essential matrix E in camera frame C */ GTSAM_EXPORT EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE = - boost::none, OptionalJacobian<5, 3> HR = boost::none) const; + {}, OptionalJacobian<5, 3> HR = {}) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -161,7 +161,7 @@ class EssentialMatrix { /// epipolar error, algebraic GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, - OptionalJacobian<1, 5> H = boost::none) const; + OptionalJacobian<1, 5> H = {}) const; /// @} diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index 08b81eaef..b1a1e4f6e 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -32,8 +32,8 @@ class Line3; * @return Transformed line in camera frame */ GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL, - OptionalJacobian<4, 6> Dpose = boost::none, - OptionalJacobian<4, 4> Dline = boost::none); + OptionalJacobian<4, 6> Dpose = {}, + OptionalJacobian<4, 4> Dline = {}); /** @@ -69,8 +69,8 @@ class GTSAM_EXPORT Line3 { * @return q: resulting line after adding the increment and mapping to the manifold */ Line3 retract(const Vector4 &v, - OptionalJacobian<4, 4> Dp = boost::none, - OptionalJacobian<4, 4> Dv = boost::none) const; + OptionalJacobian<4, 4> Dp = {}, + OptionalJacobian<4, 4> Dv = {}) const; /** * The localCoordinates method is the inverse of retract and finds the difference @@ -82,8 +82,8 @@ class GTSAM_EXPORT Line3 { * @return v: difference in the tangent space */ Vector4 localCoordinates(const Line3 &q, - OptionalJacobian<4, 4> Dp = boost::none, - OptionalJacobian<4, 4> Dq = boost::none) const; + OptionalJacobian<4, 4> Dp = {}, + OptionalJacobian<4, 4> Dq = {}) const; /** * Print R, a, b @@ -105,7 +105,7 @@ class GTSAM_EXPORT Line3 { * @return Unit3 - projected line in image plane, in homogenous coordinates. * We use Unit3 since it is a manifold with the right dimension. */ - Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const; + Unit3 project(OptionalJacobian<2, 4> Dline = {}) const; /** * Returns point on the line that is at a certain distance starting from the diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index d0d912ee9..07c8445fe 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -87,8 +87,8 @@ public: * @return the transformed plane */ OrientedPlane3 transform(const Pose3& xr, - OptionalJacobian<3, 3> Hp = boost::none, - OptionalJacobian<3, 6> Hr = boost::none) const; + OptionalJacobian<3, 3> Hp = {}, + OptionalJacobian<3, 6> Hr = {}) const; /** Computes the error between the two planes, with derivatives. * This uses Unit3::errorVector, as opposed to the other .error() in this @@ -98,8 +98,8 @@ public: * @param other the other plane */ Vector3 errorVector(const OrientedPlane3& other, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Dimensionality of tangent space = 3 DOF inline static size_t Dim() { @@ -113,7 +113,7 @@ public: /// The retract function OrientedPlane3 retract(const Vector3& v, - OptionalJacobian<3, 3> H = boost::none) const; + OptionalJacobian<3, 3> H = {}) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; @@ -125,13 +125,13 @@ public: } /// Return the normal - inline Unit3 normal(OptionalJacobian<2, 3> H = boost::none) const { + inline Unit3 normal(OptionalJacobian<2, 3> H = {}) const { if (H) *H << I_2x2, Z_2x1; return n_; } /// Return the perpendicular distance to the origin - inline double distance(OptionalJacobian<1, 3> H = boost::none) const { + inline double distance(OptionalJacobian<1, 3> H = {}) const { if (H) *H << 0,0,1; return d_; } diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 48233c1ed..8f299f854 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -109,8 +109,8 @@ public: // Create PinholeCamera, with derivatives static PinholeCamera Create(const Pose3& pose, const Calibration &K, - OptionalJacobian H1 = boost::none, // - OptionalJacobian H2 = boost::none) { + OptionalJacobian H1 = {}, // + OptionalJacobian H2 = {}) { typedef Eigen::Matrix MatrixK6; if (H1) *H1 << I_6x6, MatrixK6::Zero(); @@ -237,19 +237,19 @@ public: *Dcamera << Dpose, Dcal; return pi; } else { - return Base::project(pw, boost::none, Dpoint, boost::none); + return Base::project(pw, {}, Dpoint, {}); } } /// project a 3D point from world coordinates into the image Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { + {}, OptionalJacobian<2, 3> Dpoint = {}) const { return _project2(pw, Dcamera, Dpoint); } /// project a point at infinity from world coordinates into the image Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera = - boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const { + {}, OptionalJacobian<2, 2> Dpoint = {}) const { return _project2(pw, Dcamera, Dpoint); } @@ -259,7 +259,7 @@ public: * @return range (double) */ double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera = - boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { + {}, OptionalJacobian<1, 3> Dpoint = {}) const { Matrix16 Dpose_; double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint); if (Dcamera) @@ -273,7 +273,7 @@ public: * @return range (double) */ double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera = - boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { + {}, OptionalJacobian<1, 6> Dpose = {}) const { Matrix16 Dpose_; double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose); if (Dcamera) @@ -288,8 +288,8 @@ public: */ template double range(const PinholeCamera& camera, - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const { + OptionalJacobian<1, dimension> Dcamera = {}, + OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = {}) const { Matrix16 Dcamera_, Dother_; double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0, Dother ? &Dother_ : 0); @@ -309,8 +309,8 @@ public: * @return range (double) */ double range(const CalibratedCamera& camera, - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { + OptionalJacobian<1, dimension> Dcamera = {}, + OptionalJacobian<1, 6> Dother = {}) const { return range(camera.pose(), Dcamera, Dother); } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index b2de4fb11..e9d3859a1 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -115,32 +115,32 @@ public: } /// project a 3D point from world coordinates into the image - Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}, + OptionalJacobian<2, DimK> Dcal = {}) const { return _project(pw, Dpose, Dpoint, Dcal); } /// project a 3D point from world coordinates into the image - Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { + Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}, + OptionalJacobian<2, DimK> Dcal = {}) const { return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured); } /// project a point at infinity from world coordinates into the image - Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { + Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}, + OptionalJacobian<2, DimK> Dcal = {}) const { return _project(pw, Dpose, Dpoint, Dcal); } /// backproject a 2-dimensional point to a 3-dimensional point at given depth Point3 backproject(const Point2& p, double depth, - OptionalJacobian<3, 6> Dresult_dpose = boost::none, - OptionalJacobian<3, 2> Dresult_dp = boost::none, - OptionalJacobian<3, 1> Dresult_ddepth = boost::none, - OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const { + OptionalJacobian<3, 6> Dresult_dpose = {}, + OptionalJacobian<3, 2> Dresult_dp = {}, + OptionalJacobian<3, 1> Dresult_ddepth = {}, + OptionalJacobian<3, DimK> Dresult_dcal = {}) const { typedef Eigen::Matrix Matrix2K; Matrix2K Dpn_dcal; Matrix22 Dpn_dp; @@ -179,8 +179,8 @@ public: * @return range (double) */ double range(const Point3& point, - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { + OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 3> Dpoint = {}) const { return pose().range(point, Dcamera, Dpoint); } @@ -189,8 +189,8 @@ public: * @param pose Other SO(3) pose * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 6> Dpose = {}) const { return this->pose().range(pose, Dcamera, Dpose); } @@ -200,7 +200,7 @@ public: * @return range (double) */ double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera = - boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { + {}, OptionalJacobian<1, 6> Dother = {}) const { return pose().range(camera.pose(), Dcamera, Dother); } @@ -211,8 +211,8 @@ public: */ template double range(const PinholeBaseK& camera, - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { + OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 6> Dother = {}) const { return pose().range(camera.pose(), Dcamera, Dother); } @@ -376,14 +376,14 @@ public: * @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose) * @param Dpoint is the Jacobian w.r.t. point3 */ - Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const { + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}) const { return Base::project(pw, Dpose, Dpoint); } /// project2 version for point at infinity - Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const { + Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const { return Base::project(pw, Dpose, Dpoint); } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 2b4f48197..6d5940584 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -36,12 +36,12 @@ GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair using Point2Pairs = std::vector; /// Distance of the point from the origin, with Jacobian -GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); +GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = {}); /// distance between two points GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q, - OptionalJacobian<1, 2> H1 = boost::none, - OptionalJacobian<1, 2> H2 = boost::none); + OptionalJacobian<1, 2> H1 = {}, + OptionalJacobian<1, 2> H2 = {}); // For MATLAB wrapper typedef std::vector > Point2Vector; @@ -56,14 +56,14 @@ inline Point2 operator*(double s, const Point2& p) { * Calculate f and h, respectively the parallel and perpendicular distance of * the intersections of two circles along and from the line connecting the centers. * Both are dimensionless fractions of the distance d between the circle centers. - * If the circles do not intersect or they are identical, returns boost::none. + * If the circles do not intersect or they are identical, returns {}. * If one solution (touching circles, as determined by tol), h will be exactly zero. * h is a good measure for how accurate the intersection will be, as when circles touch * or nearly touch, the intersection is ill-defined with noisy radius measurements. * @param R_d : R/d, ratio of radius of first circle to distance between centers * @param r_d : r/d, ratio of radius of second circle to distance between centers * @param tol: absolute tolerance below which we consider touching circles - * @return optional Point2 with f and h, boost::none if no solution. + * @return optional Point2 with f and h, {} if no solution. */ GTSAM_EXPORT std::optional circleCircleIntersection(double R_d, double r_d, double tol = 1e-9); @@ -97,8 +97,8 @@ template <> struct Range { typedef double result_type; double operator()(const Point2& p, const Point2& q, - OptionalJacobian<1, 2> H1 = boost::none, - OptionalJacobian<1, 2> H2 = boost::none) { + OptionalJacobian<1, 2> H1 = {}, + OptionalJacobian<1, 2> H2 = {}) { return distance2(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 13a435c24..b1fd26bb2 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -44,24 +44,24 @@ using Point3Pairs = std::vector; /// distance between two points GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none); + OptionalJacobian<1, 3> H1 = {}, + OptionalJacobian<1, 3> H2 = {}); /// Distance of the point from the origin, with Jacobian -GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none); +GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {}); /// normalize, with optional Jacobian -GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); +GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {}); /// cross product @return this x q GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, - OptionalJacobian<3, 3> H_p = boost::none, - OptionalJacobian<3, 3> H_q = boost::none); + OptionalJacobian<3, 3> H_p = {}, + OptionalJacobian<3, 3> H_q = {}); /// dot product GTSAM_EXPORT double dot(const Point3& p, const Point3& q, - OptionalJacobian<1, 3> H_p = boost::none, - OptionalJacobian<1, 3> H_q = boost::none); + OptionalJacobian<1, 3> H_p = {}, + OptionalJacobian<1, 3> H_q = {}); /// mean template @@ -82,8 +82,8 @@ template <> struct Range { typedef double result_type; double operator()(const Point3& p, const Point3& q, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) { + OptionalJacobian<1, 3> H1 = {}, + OptionalJacobian<1, 3> H2 = {}) { return distance3(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 2ef47d58e..f9ed2a383 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -55,14 +55,14 @@ struct traits { /// @name Lie group traits /// @{ static Q Compose(const Q &g, const Q & h, - ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { + ChartJacobian Hg = {}, ChartJacobian Hh = {}) { if (Hg) *Hg = h.toRotationMatrix().transpose(); if (Hh) *Hh = I_3x3; return g * h; } static Q Between(const Q &g, const Q & h, - ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { + ChartJacobian Hg = {}, ChartJacobian Hh = {}) { Q d = g.inverse() * h; if (Hg) *Hg = -d.toRotationMatrix().transpose(); if (Hh) *Hh = I_3x3; @@ -70,14 +70,14 @@ struct traits { } static Q Inverse(const Q &g, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { if (H) *H = -g.toRotationMatrix(); return g.inverse(); } /// Exponential map, using the inlined code from Eigen's conversion from axis/angle static Q Expmap(const Eigen::Ref& omega, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { using std::cos; using std::sin; if (H) *H = SO3::ExpmapDerivative(omega.template cast()); @@ -95,7 +95,7 @@ struct traits { } /// We use our own Logmap, as there is a slight bug in Eigen - static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) { + static TangentVector Logmap(const Q& q, ChartJacobian H = {}) { using std::acos; using std::sqrt; @@ -145,7 +145,7 @@ struct traits { /// @{ static TangentVector Local(const Q& g, const Q& h, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { Q b = Between(g, h, H1, H2); Matrix3 D_v_b; TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0); @@ -155,7 +155,7 @@ struct traits { } static Q Retract(const Q& g, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { Matrix3 D_h_v; Q b = Expmap(v,H2 ? &D_h_v : 0); Q h = Compose(g, b, H1, H2); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 2e9e91cd8..fbf2d82e4 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -80,7 +80,7 @@ namespace gtsam { * @return 2D rotation \f$ \in SO(2) \f$ */ static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H = - boost::none); + {}); /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ static Rot2 atan2(double y, double x); @@ -123,10 +123,10 @@ namespace gtsam { /// @{ /// Exponential map at identity - create a rotation from canonical coordinates - static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none); + static Rot2 Expmap(const Vector1& v, ChartJacobian H = {}); /// Log map at identity - return the canonical coordinates of this rotation - static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none); + static Vector1 Logmap(const Rot2& r, ChartJacobian H = {}); /** Calculate Adjoint map */ Matrix1 AdjointMap() const { return I_1x1; } @@ -143,10 +143,10 @@ namespace gtsam { // Chart at origin simply uses exponential map and its inverse struct ChartAtOrigin { - static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) { + static Rot2 Retract(const Vector1& v, ChartJacobian H = {}) { return Expmap(v, H); } - static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) { + static Vector1 Local(const Rot2& r, ChartJacobian H = {}) { return Logmap(r, H); } }; @@ -160,8 +160,8 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, - OptionalJacobian<2, 2> H2 = boost::none) const; + Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = {}, + OptionalJacobian<2, 2> H2 = {}) const; /** syntactic sugar for rotate */ inline Point2 operator*(const Point2& p) const { @@ -171,8 +171,8 @@ namespace gtsam { /** * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ */ - Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, - OptionalJacobian<2, 2> H2 = boost::none) const; + Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = {}, + OptionalJacobian<2, 2> H2 = {}) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 65f3d4e5f..00beeb515 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -152,13 +152,13 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. static Rot3 RzRyRx(double x, double y, double z, - OptionalJacobian<3, 1> Hx = boost::none, - OptionalJacobian<3, 1> Hy = boost::none, - OptionalJacobian<3, 1> Hz = boost::none); + OptionalJacobian<3, 1> Hx = {}, + OptionalJacobian<3, 1> Hy = {}, + OptionalJacobian<3, 1> Hz = {}); /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. inline static Rot3 RzRyRx(const Vector& xyz, - OptionalJacobian<3, 3> H = boost::none) { + OptionalJacobian<3, 3> H = {}) { assert(xyz.size() == 3); Rot3 out; if (H) { @@ -194,9 +194,9 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * Positive roll is to right (decreasing yaw in aircraft). */ static Rot3 Ypr(double y, double p, double r, - OptionalJacobian<3, 1> Hy = boost::none, - OptionalJacobian<3, 1> Hp = boost::none, - OptionalJacobian<3, 1> Hr = boost::none) { + OptionalJacobian<3, 1> Hy = {}, + OptionalJacobian<3, 1> Hp = {}, + OptionalJacobian<3, 1> Hr = {}) { return RzRyRx(r, p, y, Hr, Hp, Hy); } @@ -347,8 +347,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup { // Cayley chart around origin struct CayleyChart { - static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none); - static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none); + static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = {}); + static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = {}); }; /// Retraction from R^3 to Rot3 manifold using the Cayley transform @@ -371,7 +371,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ - static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) { + static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {}) { if(H) *H = Rot3::ExpmapDerivative(v); #ifdef GTSAM_USE_QUATERNIONS return traits::Expmap(v); @@ -384,7 +384,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * Log map at identity - returns the canonical coordinates * \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none); + static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = {}); /// Derivative of Expmap static Matrix3 ExpmapDerivative(const Vector3& x); @@ -397,8 +397,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup { // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE struct ChartAtOrigin { - static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none); - static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none); + static Rot3 Retract(const Vector3& v, ChartJacobian H = {}); + static Vector3 Local(const Rot3& r, ChartJacobian H = {}); }; using LieGroup::inverse; // version with derivative @@ -410,27 +410,27 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, - OptionalJacobian<3,3> H2 = boost::none) const; + Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = {}, + OptionalJacobian<3,3> H2 = {}) const; /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, - OptionalJacobian<3,3> H2=boost::none) const; + Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = {}, + OptionalJacobian<3,3> H2={}) const; /// @} /// @name Group Action on Unit3 /// @{ /// rotate 3D direction from rotated coordinate frame to world frame - Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, - OptionalJacobian<2,2> Hp = boost::none) const; + Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = {}, + OptionalJacobian<2,2> Hp = {}) const; /// unrotate 3D direction from world frame to rotated coordinate frame - Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, - OptionalJacobian<2,2> Hp = boost::none) const; + Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = {}, + OptionalJacobian<2,2> Hp = {}) const; /// rotate 3D direction from rotated coordinate frame to world frame Unit3 operator*(const Unit3& p) const; @@ -458,19 +458,19 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * Use RQ to calculate xyz angle representation * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) */ - Vector3 xyz(OptionalJacobian<3, 3> H = boost::none) const; + Vector3 xyz(OptionalJacobian<3, 3> H = {}) const; /** * Use RQ to calculate yaw-pitch-roll angle representation * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) */ - Vector3 ypr(OptionalJacobian<3, 3> H = boost::none) const; + Vector3 ypr(OptionalJacobian<3, 3> H = {}) const; /** * Use RQ to calculate roll-pitch-yaw angle representation * @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r) */ - Vector3 rpy(OptionalJacobian<3, 3> H = boost::none) const; + Vector3 rpy(OptionalJacobian<3, 3> H = {}) const; /** * Accessor to get to component of angle representations @@ -478,7 +478,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - double roll(OptionalJacobian<1, 3> H = boost::none) const; + double roll(OptionalJacobian<1, 3> H = {}) const; /** * Accessor to get to component of angle representations @@ -486,7 +486,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - double pitch(OptionalJacobian<1, 3> H = boost::none) const; + double pitch(OptionalJacobian<1, 3> H = {}) const; /** * Accessor to get to component of angle representations @@ -494,7 +494,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - double yaw(OptionalJacobian<1, 3> H = boost::none) const; + double yaw(OptionalJacobian<1, 3> H = {}) const; /// @} /// @name Advanced Interface @@ -583,7 +583,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * @return a vector [thetax, thetay, thetaz] in radians. */ GTSAM_EXPORT std::pair RQ( - const Matrix3& A, OptionalJacobian<3, 9> H = boost::none); + const Matrix3& A, OptionalJacobian<3, 9> H = {}); template<> struct traits : public internal::LieGroup {}; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 0463bc2e8..6f78bed4e 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -121,7 +121,7 @@ namespace so3 { * We only provide the 9*9 derivative in the first argument M. */ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, - OptionalJacobian<9, 9> H = boost::none); + OptionalJacobian<9, 9> H = {}); /// (constant) Jacobian of compose wrpt M GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); @@ -170,13 +170,13 @@ class DexpFunctor : public ExpmapFunctor { const Matrix3& dexp() const { return dexp_; } /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with dexp().inverse(), with optional derivatives GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; }; } // namespace so3 diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 7503b012b..529935006 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -70,13 +70,13 @@ Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H); /** * Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3). */ -GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none); +GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = {}); /** * Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) * -> \f$ S \in St(3,4) \f$. */ -GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none); +GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = {}); /** Serialization function */ template diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 2c8261668..b8b9deb3b 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -242,12 +242,12 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * Retract uses Cayley map. See note about xi element order in Hat. * Deafault implementation has no Jacobian implemented */ - static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none); + static SO Retract(const TangentVector& xi, ChartJacobian H = {}); /** * Inverse of Retract. See note about xi element order in Hat. */ - static TangentVector Local(const SO& R, ChartJacobian H = boost::none); + static TangentVector Local(const SO& R, ChartJacobian H = {}); }; // Return dynamic identity DxD Jacobian for given SO(n) @@ -267,7 +267,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /** * Exponential map at identity - create a rotation from canonical coordinates */ - static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none); + static SO Expmap(const TangentVector& omega, ChartJacobian H = {}); /// Derivative of Expmap, currently only defined for SO3 static MatrixDD ExpmapDerivative(const TangentVector& omega); @@ -275,7 +275,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /** * Log map at identity - returns the canonical coordinates of this rotation */ - static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none); + static TangentVector Logmap(const SO& R, ChartJacobian H = {}); /// Derivative of Logmap, currently only defined for SO3 static MatrixDD LogmapDerivative(const TangentVector& omega); @@ -293,7 +293,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * X and fixed-size Jacobian if dimension is known at compile time. * */ VectorN2 vec(OptionalJacobian H = - boost::none) const; + {}) const; /// Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N) template > diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h index ad4b82e09..47281b383 100644 --- a/gtsam/geometry/Similarity2.h +++ b/gtsam/geometry/Similarity2.h @@ -143,20 +143,20 @@ class GTSAM_EXPORT Similarity2 : public LieGroup { * \f$ [t_x, t_y, \delta, \lambda] \f$ */ static Vector4 Logmap(const Similarity2& S, // - OptionalJacobian<4, 4> Hm = boost::none); + OptionalJacobian<4, 4> Hm = {}); /// Exponential map at the identity static Similarity2 Expmap(const Vector4& v, // - OptionalJacobian<4, 4> Hm = boost::none); + OptionalJacobian<4, 4> Hm = {}); /// Chart at the origin struct ChartAtOrigin { static Similarity2 Retract(const Vector4& v, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return Similarity2::Expmap(v, H); } static Vector4 Local(const Similarity2& other, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return Similarity2::Logmap(other, H); } }; diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 41d9d9965..aa0f3a62a 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -98,8 +98,8 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { /// Action on a point p is s*(R*p+t) Point3 transformFrom(const Point3& p, // - OptionalJacobian<3, 7> H1 = boost::none, // - OptionalJacobian<3, 3> H2 = boost::none) const; + OptionalJacobian<3, 7> H1 = {}, // + OptionalJacobian<3, 3> H2 = {}) const; /** * Action on a pose T. @@ -142,21 +142,21 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ */ static Vector7 Logmap(const Similarity3& s, // - OptionalJacobian<7, 7> Hm = boost::none); + OptionalJacobian<7, 7> Hm = {}); /** Exponential map at the identity */ static Similarity3 Expmap(const Vector7& v, // - OptionalJacobian<7, 7> Hm = boost::none); + OptionalJacobian<7, 7> Hm = {}); /// Chart at the origin struct ChartAtOrigin { static Similarity3 Retract(const Vector7& v, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return Similarity3::Expmap(v, H); } static Vector7 Local(const Similarity3& other, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return Similarity3::Logmap(other, H); } }; diff --git a/gtsam/geometry/SphericalCamera.cpp b/gtsam/geometry/SphericalCamera.cpp index 58a29dc09..5c6a2b3a4 100644 --- a/gtsam/geometry/SphericalCamera.cpp +++ b/gtsam/geometry/SphericalCamera.cpp @@ -96,7 +96,7 @@ Vector2 SphericalCamera::reprojectionError( Matrix23 H_project_point; Matrix22 H_error; Unit3 projected = project2(point, H_project_pose, H_project_point); - Vector2 error = measured.errorVector(projected, boost::none, H_error); + Vector2 error = measured.errorVector(projected, {}, H_error); if (Dpose) *Dpose = H_error * H_project_pose; if (Dpoint) *Dpoint = H_error * H_project_point; return error; diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index 53b67241f..dd3bc7420 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -152,16 +152,16 @@ class GTSAM_EXPORT SphericalCamera { * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point */ - Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const; + Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}) const; /** Project point into the image * (note: there is no CheiralityException for a spherical camera) * @param point 3D direction in world coordinates * @return the intrinsic coordinates of the projected point */ - Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const; + Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth Point3 backproject(const Unit3& p, const double depth) const; @@ -174,16 +174,16 @@ class GTSAM_EXPORT SphericalCamera { * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point */ - Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const; + Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}) const; /** Compute reprojection error for a given 3D point in world coordinates * @param point 3D point in world coordinates * @return the tangent space error between the projection and the measurement */ Vector2 reprojectionError(const Point3& point, const Unit3& measured, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const; + OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}) const; /// @} /// move a cameras according to d diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 603476278..6ccebd8c2 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -143,7 +143,7 @@ public: * @param H2 derivative with respect to point */ StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 = - boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; + {}, OptionalJacobian<3, 3> H2 = {}) const; /// back-project a measurement Point3 backproject(const StereoPoint2& z) const; @@ -153,8 +153,8 @@ public: * @param H2 derivative with respect to point */ Point3 backproject2(const StereoPoint2& z, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + OptionalJacobian<3, 6> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// @} /// @name Deprecated @@ -167,8 +167,8 @@ public: * @param H3 IGNORED (for calibration) */ StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1, - OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = - boost::none) const; + OptionalJacobian<3, 3> H2 = {}, OptionalJacobian<3, 0> H3 = + {}) const; /// for Nonlinear Triangulation Vector defaultErrorWhenTriangulatingBehindCamera() const { diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 7ef6e5001..e9ee39836 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -57,7 +57,7 @@ Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { /* ************************************************************************* */ TEST(Cal3DS2, Duncalibrate1) { Matrix computed; - K.uncalibrate(p, computed, boost::none); + K.uncalibrate(p, computed, {}); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical, computed, 1e-5)); Matrix separate = K.D2d_calibration(p); @@ -67,7 +67,7 @@ TEST(Cal3DS2, Duncalibrate1) { /* ************************************************************************* */ TEST(Cal3DS2, Duncalibrate2) { Matrix computed; - K.uncalibrate(p, boost::none, computed); + K.uncalibrate(p, {}, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical, computed, 1e-5)); Matrix separate = K.D2d_intrinsic(p); diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index 648bb358c..0f58bd82a 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -68,7 +68,7 @@ Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { /* ************************************************************************* */ TEST(Cal3Unified, Duncalibrate1) { Matrix computed; - K.uncalibrate(p, computed, boost::none); + K.uncalibrate(p, computed, {}); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical, computed, 1e-6)); } @@ -76,7 +76,7 @@ TEST(Cal3Unified, Duncalibrate1) { /* ************************************************************************* */ TEST(Cal3Unified, Duncalibrate2) { Matrix computed; - K.uncalibrate(p, boost::none, computed); + K.uncalibrate(p, {}, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical, computed, 1e-6)); } diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 41be5ea8e..68d5e238b 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -64,7 +64,7 @@ Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { TEST(Cal3_S2, Duncalibrate1) { Matrix25 computed; - K.uncalibrate(p, computed, boost::none); + K.uncalibrate(p, computed, {}); Matrix numerical = numericalDerivative21(uncalibrate_, K, p); CHECK(assert_equal(numerical, computed, 1e-8)); } @@ -72,7 +72,7 @@ TEST(Cal3_S2, Duncalibrate1) { /* ************************************************************************* */ TEST(Cal3_S2, Duncalibrate2) { Matrix computed; - K.uncalibrate(p, boost::none, computed); + K.uncalibrate(p, {}, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p); CHECK(assert_equal(numerical, computed, 1e-9)); } @@ -84,7 +84,7 @@ Point2 calibrate_(const Cal3_S2& k, const Point2& pt) { /* ************************************************************************* */ TEST(Cal3_S2, Dcalibrate1) { Matrix computed; - Point2 expected = K.calibrate(p_uv, computed, boost::none); + Point2 expected = K.calibrate(p_uv, computed, {}); Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); CHECK(assert_equal(expected, p_xy, 1e-8)); CHECK(assert_equal(numerical, computed, 1e-8)); @@ -93,7 +93,7 @@ TEST(Cal3_S2, Dcalibrate1) { /* ************************************************************************* */ TEST(Cal3_S2, Dcalibrate2) { Matrix computed; - Point2 expected = K.calibrate(p_uv, boost::none, computed); + Point2 expected = K.calibrate(p_uv, {}, computed); Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); CHECK(assert_equal(expected, p_xy, 1e-8)); CHECK(assert_equal(numerical, computed, 1e-8)); diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 0fb0754fe..27f882ff5 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -54,7 +54,7 @@ TEST(CalibratedCamera, Create) { // Check derivative std::function f = // - std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none); + std::bind(CalibratedCamera::Create, std::placeholders::_1, nullptr); Matrix numericalH = numericalDerivative11(f, kDefaultPose); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index ff8c61f35..bd4568118 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -40,17 +40,21 @@ TEST(EssentialMatrix, FromRotationAndDirection) { trueE, EssentialMatrix::FromRotationAndDirection(trueRotation, trueDirection, actualH1, actualH2), 1e-8)); - Matrix expectedH1 = numericalDerivative11( - std::bind(EssentialMatrix::FromRotationAndDirection, - std::placeholders::_1, trueDirection, boost::none, boost::none), - trueRotation); - EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); + { + std::function fn = [](const Rot3& R) { + return EssentialMatrix::FromRotationAndDirection(R, trueDirection); + }; + Matrix expectedH1 = numericalDerivative11(fn, trueRotation); + EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); + } - Matrix expectedH2 = numericalDerivative11( - std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, - std::placeholders::_1, boost::none, boost::none), - trueDirection); - EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); + { + std::function fn = [](const Unit3& t) { + return EssentialMatrix::FromRotationAndDirection(trueRotation, t); + }; + Matrix expectedH2 = numericalDerivative11(fn, trueDirection); + EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); + } } //************************************************************************* @@ -174,8 +178,10 @@ TEST (EssentialMatrix, FromPose3_a) { Matrix actualH; Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); - Matrix expectedH = numericalDerivative11( - std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); + std::function fn = [](const Pose3& pose) { + return EssentialMatrix::FromPose3(pose); + }; + Matrix expectedH = numericalDerivative11(fn, pose); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -187,8 +193,10 @@ TEST (EssentialMatrix, FromPose3_b) { EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); - Matrix expectedH = numericalDerivative11( - std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); + std::function fn = [](const Pose3& pose) { + return EssentialMatrix::FromPose3(pose); + }; + Matrix expectedH = numericalDerivative11(fn, pose); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 41b152557..f27b5070b 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -23,7 +23,6 @@ using namespace std::placeholders; using namespace gtsam; -using boost::none; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) @@ -57,17 +56,17 @@ TEST(OrientedPlane3, transform) { gtsam::Point3(2.0, 3.0, 4.0)); OrientedPlane3 plane(-1, 0, 0, 5); OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); - OrientedPlane3 transformedPlane = plane.transform(pose, none, none); + OrientedPlane3 transformedPlane = plane.transform(pose, {}, {}); EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5)); // Test the jacobians of transform Matrix actualH1, expectedH1, actualH2, expectedH2; expectedH1 = numericalDerivative21(transform_, plane, pose); - plane.transform(pose, actualH1, none); + plane.transform(pose, actualH1, {}); EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); expectedH2 = numericalDerivative22(transform_, plane, pose); - plane.transform(pose, none, actualH2); + plane.transform(pose, {}, actualH2); EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } @@ -135,8 +134,9 @@ TEST(OrientedPlane3, errorVector) { EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); std::function f = - std::bind(&OrientedPlane3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + [](const OrientedPlane3& p1, const OrientedPlane3& p2) { + return p1.errorVector(p2); + }; expectedH1 = numericalDerivative21(f, plane1, plane2); expectedH2 = numericalDerivative22(f, plane1, plane2); EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); @@ -147,8 +147,10 @@ TEST(OrientedPlane3, errorVector) { TEST(OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); Matrix33 H_actual; - std::function f = std::bind( - &OrientedPlane3::retract, plane, std::placeholders::_1, boost::none); + std::function f = [&plane](const Vector3& v) { + return plane.retract(v); + }; + { Vector3 v(-0.1, 0.2, 0.3); plane.retract(v, H_actual); @@ -168,8 +170,9 @@ TEST(OrientedPlane3, jacobian_normal) { Matrix23 H_actual, H_expected; OrientedPlane3 plane(-1, 0.1, 0.2, 5); - std::function f = std::bind( - &OrientedPlane3::normal, std::placeholders::_1, boost::none); + std::function f = [](const OrientedPlane3& p) { + return p.normal(); + }; H_expected = numericalDerivative11(f, plane); plane.normal(H_actual); @@ -181,8 +184,9 @@ TEST(OrientedPlane3, jacobian_distance) { Matrix13 H_actual, H_expected; OrientedPlane3 plane(-1, 0.1, 0.2, 5); - std::function f = std::bind( - &OrientedPlane3::distance, std::placeholders::_1, boost::none); + std::function f = [](const OrientedPlane3& p) { + return p.distance(); + }; H_expected = numericalDerivative11(f, plane); plane.distance(H_actual); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 0679a4609..6b5d93627 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -67,7 +67,7 @@ TEST(PinholeCamera, Create) { // Check derivative std::function f = // std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none); + nullptr, nullptr); Matrix numericalH1 = numericalDerivative21(f,pose,K); EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); Matrix numericalH2 = numericalDerivative22(f,pose,K); @@ -82,7 +82,7 @@ TEST(PinholeCamera, Pose) { // Check derivative std::function f = // - std::bind(&Camera::getPose, std::placeholders::_1, boost::none); + std::bind(&Camera::getPose, std::placeholders::_1, nullptr); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index acfcd9f39..4fccc18a1 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -66,7 +66,7 @@ TEST(PinholeCamera, Pose) { // Check derivative std::function f = // - std::bind(&Camera::getPose,_1,boost::none); + std::bind(&Camera::getPose,_1,{}); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index e373e1d52..ad9f29620 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -121,9 +121,8 @@ TEST( Point3, dot) { /* ************************************************************************* */ TEST(Point3, cross) { Matrix aH1, aH2; - std::function f = - std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none); + std::function f = + [](const Point3& p, const Point3& q) { return gtsam::cross(p, q); }; const Point3 omega(0, 1, 0), theta(4, 6, 8); cross(omega, theta, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); @@ -142,8 +141,7 @@ TEST( Point3, cross2) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; std::function f = - std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, // - boost::none, boost::none); + [](const Point3& p, const Point3& q) { return gtsam::cross(p, q); }; { gtsam::cross(p, q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); @@ -162,8 +160,8 @@ TEST (Point3, normalize) { Point3 point(1, -2, 3); // arbitrary point Point3 expected(point / sqrt(14.0)); EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8)); - Matrix expectedH = numericalDerivative11( - std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point); + std::function fn = [](const Point3& p) { return normalize(p); }; + Matrix expectedH = numericalDerivative11(fn, point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 34ce3932d..e38bfbd75 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -228,7 +228,7 @@ TEST( Pose2, ExpmapDerivative1) { Vector3 w(0.1, 0.27, -0.3); Pose2::Expmap(w,actualH); Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w, boost::none, 1e-2); + OptionalJacobian<3, 3> >(&Pose2::Expmap, w, {}, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } @@ -238,7 +238,7 @@ TEST( Pose2, ExpmapDerivative2) { Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 Pose2::Expmap(w0,actualH); Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w0, boost::none, 1e-2); + OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, {}, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } @@ -249,7 +249,7 @@ TEST( Pose2, LogmapDerivative1) { Pose2 p = Pose2::Expmap(w); EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5)); Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); + OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } @@ -260,7 +260,7 @@ TEST( Pose2, LogmapDerivative2) { Pose2 p = Pose2::Expmap(w0); EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5)); Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); + OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 1df342d57..0e7967715 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -360,7 +360,7 @@ TEST( Rot3, rotate_derivatives) { Matrix actualDrotate1a, actualDrotate1b, actualDrotate2; R.rotate(P, actualDrotate1a, actualDrotate2); - R.inverse().rotate(P, actualDrotate1b, boost::none); + R.inverse().rotate(P, actualDrotate1b, {}); Matrix numerical1 = numericalDerivative21(testing::rotate, R, P); Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); Matrix numerical3 = numericalDerivative22(testing::rotate, R, P); diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index be159ed58..bd2ad4562 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -30,7 +30,7 @@ using namespace gtsam; //****************************************************************************** TEST(Rot3Q , Compare) { - using boost::none; + using {}; // We set up expected values with quaternions typedef Quaternion Q; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 96c1cce32..1afe9ff21 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -210,7 +210,7 @@ TEST(SO3, ExpmapDerivative) { TEST(SO3, ExpmapDerivative2) { const Vector3 theta(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -221,7 +221,7 @@ TEST(SO3, ExpmapDerivative2) { TEST(SO3, ExpmapDerivative3) { const Vector3 theta(10, 20, 30); const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -276,7 +276,7 @@ TEST(SO3, ExpmapDerivative5) { TEST(SO3, ExpmapDerivative6) { const Vector3 thetahat(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat); + std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), thetahat); Matrix3 Jactual; SO3::Expmap(thetahat, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); @@ -287,7 +287,7 @@ TEST(SO3, LogmapDerivative) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R); const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } @@ -297,7 +297,7 @@ TEST(SO3, JacobianLogmap) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R); Matrix3 Jactual; SO3::Logmap(R, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 173ccf05b..f4f69a137 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -123,7 +123,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) { TEST( SimpleCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; - Point2 result = camera.project(point1, Dpose, Dpoint, boost::none); + Point2 result = camera.project(point1, Dpose, Dpoint, {}); Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, pose1, point1); CHECK(assert_equal(result, Point2(-100, 100) )); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index bc432cad3..d23b3c740 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -96,11 +96,11 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_ TEST( StereoCamera, Dproject) { Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); - Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); + Matrix actual1; stereoCam.project2(landmark, actual1, {}); CHECK(assert_equal(expected1,actual1,1e-7)); Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K); - Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); + Matrix actual2; stereoCam.project2(landmark, {}, actual2); CHECK(assert_equal(expected2,actual2,1e-7)); } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 96a5522fb..66801971f 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -74,12 +74,12 @@ TEST(Unit3, rotate) { // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(rotate_, R, p); - R.rotate(p, actualH, boost::none); + R.rotate(p, actualH, nullptr); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } { expectedH = numericalDerivative22(rotate_, R, p); - R.rotate(p, boost::none, actualH); + R.rotate(p, nullptr, actualH); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -100,12 +100,12 @@ TEST(Unit3, unrotate) { // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(unrotate_, R, p); - R.unrotate(p, actualH, boost::none); + R.unrotate(p, actualH, nullptr); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } { expectedH = numericalDerivative22(unrotate_, R, p); - R.unrotate(p, boost::none, actualH); + R.unrotate(p, nullptr, actualH); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -124,7 +124,7 @@ TEST(Unit3, dot) { Matrix H1, H2; std::function f = std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, // - boost::none, boost::none); + nullptr, nullptr); { p.dot(q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-5)); @@ -154,13 +154,13 @@ TEST(Unit3, error) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q); + std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalDerivative11( - std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r); + std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -182,33 +182,33 @@ TEST(Unit3, error2) { { expected = numericalDerivative21( std::bind(&Unit3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), + std::placeholders::_2, nullptr, nullptr), p, q); - p.errorVector(q, actual, boost::none); + p.errorVector(q, actual, nullptr); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative21( std::bind(&Unit3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), + std::placeholders::_2, nullptr, nullptr), p, r); - p.errorVector(r, actual, boost::none); + p.errorVector(r, actual, nullptr); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( std::bind(&Unit3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), + std::placeholders::_2, nullptr, nullptr), p, q); - p.errorVector(q, boost::none, actual); + p.errorVector(q, nullptr, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( std::bind(&Unit3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), + std::placeholders::_2, nullptr, nullptr), p, r); - p.errorVector(r, boost::none, actual); + p.errorVector(r, nullptr, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } } @@ -225,13 +225,13 @@ TEST(Unit3, distance) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalGradient( - std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q); + std::bind(&Unit3::distance, &p, std::placeholders::_1, nullptr), q); p.distance(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalGradient( - std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r); + std::bind(&Unit3::distance, &p, std::placeholders::_1, nullptr), r); p.distance(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -323,7 +323,7 @@ TEST(Unit3, basis) { Matrix62 actualH; Matrix62 expectedH = numericalDerivative11( - std::bind(BasisTest, std::placeholders::_1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, nullptr), p); // without H, first time EXPECT(assert_equal(expected, p.basis(), 1e-6)); @@ -352,7 +352,7 @@ TEST(Unit3, basis_derivatives) { p.basis(actualH); Matrix62 expectedH = numericalDerivative11( - std::bind(BasisTest, std::placeholders::_1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, nullptr), p); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -381,7 +381,7 @@ TEST (Unit3, jacobian_retract) { Matrix22 H; Unit3 p; std::function f = - std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none); + std::bind(&Unit3::retract, p, std::placeholders::_1, nullptr); { Vector2 v (-0.2, 0.1); p.retract(v, H); @@ -444,7 +444,7 @@ TEST (Unit3, FromPoint3) { Unit3 expected(point); EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5)); Matrix expectedH = numericalDerivative11( - std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point); + std::bind(Unit3::FromPoint3, std::placeholders::_1, nullptr), point); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 1ee990807..9aaab8278 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include #include @@ -326,7 +327,7 @@ namespace gtsam { /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesNet( boost::variant variables, - boost::none_t, + std::nullptr_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const { return marginalMultifrontalBayesNet(variables, function, variableIndex); @@ -335,7 +336,7 @@ namespace gtsam { /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesTree( boost::variant variables, - boost::none_t, + std::nullptr_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const { return marginalMultifrontalBayesTree(variables, function, variableIndex); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 8b268558a..4f3a89226 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -21,6 +21,7 @@ #pragma once +#include #include #include #include // Included here instead of fw-declared so we can use Errors::iterator @@ -414,7 +415,7 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** @deprecated */ VectorValues GTSAM_DEPRECATED - optimize(boost::none_t, const Eliminate& function = + optimize(std::nullptr_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const { return optimize(function); } diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 576874555..e99fa7ae5 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -103,7 +103,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation /// Predict bias-corrected incremental rotation /// TODO: The matrix Hbias is the derivative of predict? Unit-test? - Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const; + Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = {}) const; // This function is only used for test purposes // (compare numerical derivatives wrt analytic ones) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index e353def6a..4dc5ecc0a 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -54,7 +54,7 @@ public: /** vector of errors */ Vector attitudeError(const Rot3& p, - OptionalJacobian<2,3> H = boost::none) const; + OptionalJacobian<2,3> H = {}) const; const Unit3& nZ() const { return nZ_; diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index d9080eb27..ab210a62a 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -72,8 +72,8 @@ public: /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ Vector3 correctAccelerometer(const Vector3& measurement, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { + OptionalJacobian<3, 6> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const { if (H1) (*H1) << -I_3x3, Z_3x3; if (H2) (*H2) << I_3x3; return measurement - biasAcc_; @@ -81,8 +81,8 @@ public: /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ Vector3 correctGyroscope(const Vector3& measurement, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { + OptionalJacobian<3, 6> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const { if (H1) (*H1) << Z_3x3, -I_3x3; if (H2) (*H2) << I_3x3; return measurement - biasGyro_; diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 142d5a957..d0c62d6c4 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -65,7 +65,7 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, OptionalMatrixType HR = OptionalNone) { - Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none); + Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, {}); if (HR) { // assign to temporary first to avoid error in Win-Debug mode Matrix H = HR->col(2); diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp index 165acdaf0..baddb73e3 100644 --- a/gtsam/navigation/ManifoldPreintegration.cpp +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -113,7 +113,7 @@ Vector9 ManifoldPreintegration::biasCorrectedDelta( const imuBias::ConstantBias biasIncr = bias_i - biasHat_; Matrix3 D_correctedRij_bias; const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); - const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, {}, H ? &D_correctedRij_bias : 0); if (H) D_correctedRij_bias *= delRdelBiasOmega_; diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index a290972e4..b9135e3e4 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -103,7 +103,7 @@ public: /// summarizing the preintegrated IMU measurements so far /// NOTE(frank): implementation is different in two versions Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const override; + OptionalJacobian<9, 6> H = {}) const override; /** Dummy clone for MATLAB */ virtual boost::shared_ptr clone() const { diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 56b0a1c75..80a39e0f2 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -91,9 +91,9 @@ public: static Vector9 UpdatePreintegrated(const Vector3& a_body, const Vector3& w_body, const double dt, const Vector9& preintegrated, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); + OptionalJacobian<9, 9> A = {}, + OptionalJacobian<9, 3> B = {}, + OptionalJacobian<9, 3> C = {}); /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame @@ -106,13 +106,13 @@ public: /// summarizing the preintegrated IMU measurements so far /// NOTE(frank): implementation is different in two versions Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const override; + OptionalJacobian<9, 6> H = {}) const override; // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, double deltaT12, - OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 9> H2 = boost::none); + OptionalJacobian<9, 9> H1 = {}, + OptionalJacobian<9, 9> H2 = {}); /// Merge in a different set of measurements and update bias derivatives accordingly /// The derivatives apply to the preintegrated Vector9 diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 81a1a2ceb..f654a8ed9 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -125,7 +125,7 @@ TEST(ImuBias, Correct1) { const Vector3 measurement(1, 2, 3); std::function f = std::bind(&Bias::correctAccelerometer, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); bias1.correctAccelerometer(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); @@ -137,7 +137,7 @@ TEST(ImuBias, Correct2) { const Vector3 measurement(1, 2, 3); std::function f = std::bind(&Bias::correctGyroscope, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); bias1.correctGyroscope(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 585da38b1..923d2b93f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -149,7 +149,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { std::function f = std::bind(&PreintegrationBase::computeError, actual, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - boost::none, boost::none, boost::none); + nullptr, nullptr, nullptr); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); @@ -205,7 +205,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) { pim.biasCorrectedDelta(kZeroBias, actualH); Matrix expectedH = numericalDerivative11( std::bind(&PreintegrationBase::biasCorrectedDelta, pim, - std::placeholders::_1, boost::none), kZeroBias); + std::placeholders::_1, nullptr), kZeroBias); EXPECT(assert_equal(expectedH, actualH)); Matrix9 aH1; @@ -213,11 +213,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) { NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); Matrix eH1 = numericalDerivative11( std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1, - kZeroBias, boost::none, boost::none), state1); + kZeroBias, nullptr, nullptr), state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( std::bind(&PreintegrationBase::predict, pim, state1, - std::placeholders::_1, boost::none, boost::none), kZeroBias); + std::placeholders::_1, nullptr, nullptr), kZeroBias); EXPECT(assert_equal(eH2, aH2)); } @@ -334,7 +334,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { pim.biasCorrectedDelta(bias, actualH); Matrix expectedH = numericalDerivative11( std::bind(&PreintegrationBase::biasCorrectedDelta, pim, - std::placeholders::_1, boost::none), bias); + std::placeholders::_1, nullptr), bias); EXPECT(assert_equal(expectedH, actualH)); // Create factor @@ -520,7 +520,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // Check updatedDeltaXij derivatives Matrix3 D_correctedAcc_measuredOmega = Z_3x3; pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega, - boost::none, D_correctedAcc_measuredOmega, boost::none); + nullptr, D_correctedAcc_measuredOmega, nullptr); Matrix3 expectedD = numericalDerivative11( std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1), measuredOmega, 1e-6); @@ -531,19 +531,19 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // TODO(frank): revive derivative tests // Matrix93 G1, G2; // Vector9 preint = -// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); +// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, {}, G1, G2); // // Matrix93 expectedG1 = numericalDerivative21( // std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, // std::placeholders::_1, std::placeholders::_2, -// dt, boost::none, boost::none, boost::none), measuredAcc, +// dt, {}, {}, {}), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG1, G1, 1e-5)); // // Matrix93 expectedG2 = numericalDerivative22( // std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, // std::placeholders::_1, std::placeholders::_2, -// dt, boost::none, boost::none, boost::none), measuredAcc, +// dt, {}, {}, {}), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG2, G2, 1e-5)); @@ -658,7 +658,7 @@ TEST(ImuFactor, PredictArbitrary) { p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise PreintegratedImuMeasurements pim(p, biasHat); Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); -// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega, +// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, {}, measuredAcc, measuredOmega, // Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); double dt = 0.001; diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 7796ccbda..ba92d8e92 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -101,8 +101,8 @@ TEST(ManifoldPreintegration, computeError) { const imuBias::ConstantBias&)> f = std::bind(&ManifoldPreintegration::computeError, pim, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none); + std::placeholders::_3, nullptr, nullptr, + nullptr); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index e7adb923d..e658639b0 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -41,7 +41,7 @@ static const Vector9 kZeroXi = Vector9::Zero(); TEST(NavState, Constructor) { std::function create = std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none); + std::placeholders::_3, nullptr, nullptr, nullptr); Matrix aH1, aH2, aH3; EXPECT( assert_equal(kState1, @@ -61,7 +61,7 @@ TEST(NavState, Constructor) { TEST(NavState, Constructor2) { std::function construct = std::bind(&NavState::FromPoseVelocity, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); Matrix aH1, aH2; EXPECT( assert_equal(kState1, @@ -76,7 +76,7 @@ TEST( NavState, Attitude) { Rot3 actual = kState1.attitude(aH); EXPECT(assert_equal(actual, kAttitude)); eH = numericalDerivative11( - std::bind(&NavState::attitude, std::placeholders::_1, boost::none), kState1); + std::bind(&NavState::attitude, std::placeholders::_1, nullptr), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -86,7 +86,7 @@ TEST( NavState, Position) { Point3 actual = kState1.position(aH); EXPECT(assert_equal(actual, kPosition)); eH = numericalDerivative11( - std::bind(&NavState::position, std::placeholders::_1, boost::none), + std::bind(&NavState::position, std::placeholders::_1, nullptr), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -97,7 +97,7 @@ TEST( NavState, Velocity) { Velocity3 actual = kState1.velocity(aH); EXPECT(assert_equal(actual, kVelocity)); eH = numericalDerivative11( - std::bind(&NavState::velocity, std::placeholders::_1, boost::none), + std::bind(&NavState::velocity, std::placeholders::_1, nullptr), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -108,7 +108,7 @@ TEST( NavState, BodyVelocity) { Velocity3 actual = kState1.bodyVelocity(aH); EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); eH = numericalDerivative11( - std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none), + std::bind(&NavState::bodyVelocity, std::placeholders::_1, nullptr), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -142,7 +142,7 @@ TEST( NavState, Manifold ) { kState1.retract(xi, aH1, aH2); std::function retract = std::bind(&NavState::retract, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); @@ -155,7 +155,7 @@ TEST( NavState, Manifold ) { // Check localCoordinates derivatives std::function local = std::bind(&NavState::localCoordinates, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); // from state1 to state2 kState1.localCoordinates(state2, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); @@ -174,7 +174,7 @@ TEST( NavState, Manifold ) { static const double dt = 2.0; std::function coriolis = std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis, - std::placeholders::_2, boost::none); + std::placeholders::_2, nullptr); TEST(NavState, Coriolis) { Matrix9 aH; @@ -252,7 +252,7 @@ TEST(NavState, CorrectPIM) { std::function correctPIM = std::bind(&NavState::correctPIM, std::placeholders::_1, std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false, - boost::none, boost::none); + nullptr, nullptr); kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index ada059094..2f6678b97 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -108,8 +108,8 @@ TEST(TangentPreintegration, computeError) { const imuBias::ConstantBias&)> f = std::bind(&TangentPreintegration::computeError, pim, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none); + std::placeholders::_3, nullptr, nullptr, + nullptr); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 682cca29a..b616a0e63 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -47,8 +47,8 @@ class AdaptAutoDiff { public: VectorT operator()(const Vector1& v1, const Vector2& v2, - OptionalJacobian H1 = boost::none, - OptionalJacobian H2 = boost::none) { + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) { using ceres::internal::AutoDiff; bool success; diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 06088c79b..7f7600774 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -241,7 +241,7 @@ struct apply_compose { typedef T result_type; static const int Dim = traits::dimension; T operator()(const T& x, const T& y, OptionalJacobian H1 = - boost::none, OptionalJacobian H2 = boost::none) const { + {}, OptionalJacobian H2 = {}) const { return x.compose(y, H1, H2); } }; @@ -249,8 +249,8 @@ struct apply_compose { template <> struct apply_compose { double operator()(const double& x, const double& y, - OptionalJacobian<1, 1> H1 = boost::none, - OptionalJacobian<1, 1> H2 = boost::none) const { + OptionalJacobian<1, 1> H1 = {}, + OptionalJacobian<1, 1> H2 = {}) const { if (H1) H1->setConstant(y); if (H2) H2->setConstant(x); return x * y; diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 4053891f1..ee927327f 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -42,7 +42,7 @@ namespace gtsam { * public: * MultiplyFunctor(double m) : m_(m) {} * Matrix operator()(const Matrix &X, - * OptionalJacobian<-1, -1> H = boost::none) const { + * OptionalJacobian<-1, -1> H = {}) const { * if (H) * *H = m_ * Matrix::Identity(X.rows()*X.cols(), X.rows()*X.cols()); * return m_ * X; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 3237d7c1e..d20e7dbb8 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -21,6 +21,7 @@ #pragma once +#include #include #include #include @@ -268,11 +269,11 @@ namespace gtsam { /// @{ /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED linearizeToHessianFactor( - const Values& values, boost::none_t, const Dampen& dampen = nullptr) const + const Values& values, std::nullptr_t, const Dampen& dampen = nullptr) const {return linearizeToHessianFactor(values, dampen);} /** @deprecated */ - Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t, + Values GTSAM_DEPRECATED updateCholesky(const Values& values, std::nullptr_t, const Dampen& dampen = nullptr) const {return updateCholesky(values, dampen);} diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index f6afb287e..4c34a1777 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -255,7 +255,7 @@ public: /// Return value T value(const Values& values) const override { - return function_(expression1_->value(values), boost::none); + return function_(expression1_->value(values), {}); } /// Return keys that play in this expression @@ -365,9 +365,9 @@ public: /// Return value T value(const Values& values) const override { - using boost::none; + using std::nullopt; return function_(expression1_->value(values), expression2_->value(values), - none, none); + {}, {}); } /// Return keys that play in this expression @@ -473,9 +473,9 @@ public: /// Return value T value(const Values& values) const override { - using boost::none; + using std::nullopt; return function_(expression1_->value(values), expression2_->value(values), - expression3_->value(values), none, none, none); + expression3_->value(values), {}, {}, {}); } /// Return keys that play in this expression diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index a93f8a0e1..06ad16dfd 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -119,7 +119,7 @@ class Class : public Point3 { using Point3::Point3; const Vector3& vector() const { return *this; } inline static Class Identity() { return Class(0,0,0); } - double norm(OptionalJacobian<1,3> H = boost::none) const { + double norm(OptionalJacobian<1,3> H = {}) const { return norm3(*this, H); } bool equals(const Class &q, double tol) const { diff --git a/gtsam/nonlinear/tests/testFactorTesting.cpp b/gtsam/nonlinear/tests/testFactorTesting.cpp index 53111e1af..48f61a971 100644 --- a/gtsam/nonlinear/tests/testFactorTesting.cpp +++ b/gtsam/nonlinear/tests/testFactorTesting.cpp @@ -28,8 +28,8 @@ using namespace gtsam; /* ************************************************************************* */ Vector3 bodyVelocity(const Pose3& w_t_b, const Vector3& vec_w, - OptionalJacobian<3, 6> Hpose = boost::none, - OptionalJacobian<3, 3> Hvel = boost::none) { + OptionalJacobian<3, 6> Hpose = {}, + OptionalJacobian<3, 3> Hvel = {}) { Matrix36 Hrot__pose; Rot3 w_R_b = w_t_b.rotation(Hrot__pose); Matrix33 Hvel__rot; @@ -51,7 +51,7 @@ class ScaledVelocityFunctor { // and velocity scale factor. Also computes the corresponding jacobian // (w.r.t. the velocity scale). Vector3 operator()(double vscale, - OptionalJacobian<3, 1> H = boost::none) const { + OptionalJacobian<3, 1> H = {}) const { // The velocity scale factor value we are optimizing for is centered around // 0, so we need to add 1 to it before scaling the velocity. const Vector3 scaled_velocity = (vscale + 1.0) * measured_velocity_; diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index dbc22c079..f4ed70885 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -47,7 +47,7 @@ class MultiplyFunctor { MultiplyFunctor(double m) : m_(m) {} Matrix operator()(const Matrix &X, - OptionalJacobian<-1, -1> H = boost::none) const { + OptionalJacobian<-1, -1> H = {}) const { if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols()); return m_ * X; } @@ -57,8 +57,8 @@ class MultiplyFunctor { class ProjectionFunctor { public: Vector operator()(const Matrix &A, const Vector &x, - OptionalJacobian<-1, -1> H1 = boost::none, - OptionalJacobian<-1, -1> H2 = boost::none) const { + OptionalJacobian<-1, -1> H1 = {}, + OptionalJacobian<-1, -1> H2 = {}) const { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; @@ -178,7 +178,7 @@ TEST(FunctorizedFactor, Lambda) { Matrix measurement = multiplier * Matrix::Identity(3, 3); auto lambda = [multiplier](const Matrix &X, - OptionalJacobian<-1, -1> H = boost::none) { + OptionalJacobian<-1, -1> H = {}) { if (H) *H = multiplier * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols()); @@ -251,8 +251,8 @@ TEST(FunctorizedFactor, Lambda2) { Matrix measurement = A * x; auto lambda = [](const Matrix &A, const Vector &x, - OptionalJacobian<-1, -1> H1 = boost::none, - OptionalJacobian<-1, -1> H2 = boost::none) { + OptionalJacobian<-1, -1> H1 = {}, + OptionalJacobian<-1, -1> H2 = {}) { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index ee7fcd189..9d22c0daf 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -60,13 +60,13 @@ public: bool equals(const TestValue& other, double tol = 1e-9) const { return true; } size_t dim() const { return 0; } TestValue retract(const Vector&, - OptionalJacobian H1=boost::none, - OptionalJacobian H2=boost::none) const { + OptionalJacobian H1={}, + OptionalJacobian H2={}) const { return TestValue(); } Vector localCoordinates(const TestValue&, - OptionalJacobian H1=boost::none, - OptionalJacobian H2=boost::none) const { + OptionalJacobian H1={}, + OptionalJacobian H2={}) const { return Vector(); } }; diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 3b60c8a5a..e8824d22b 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -142,16 +142,16 @@ namespace gtsam { if(H1) { gtsam::Matrix H0; PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); - Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, {}) - measured_); *H1 = *H1 * H0; return reprojectionError; } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - return camera.project(point, H1, H2, boost::none) - measured_; + return camera.project(point, H1, H2, {}) - measured_; } } else { PinholeCamera camera(pose, *K_); - return camera.project(point, H1, H2, boost::none) - measured_; + return camera.project(point, H1, H2, {}) - measured_; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 6fac062b9..bd8354435 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -163,7 +163,7 @@ public: // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = traits::Local(camera_.project2(point, boost::none, A), measured_); + b = traits::Local(camera_.project2(point, {}, A), measured_); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 42df4d8fe..bdfade5f2 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -91,7 +91,7 @@ TEST( triangulation, TriangulationFactorStereo ) { // compare same problem against expression factor Expression::UnaryFunction::type f = std::bind(&StereoCamera::project2, camera2, std::placeholders::_1, - boost::none, std::placeholders::_2); + nullptr, std::placeholders::_2); Expression point_(pointKey); Expression project2_(f, point_); diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index c573de2b6..89ae59f9c 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -88,8 +88,8 @@ public: /** range between translations */ double range(const PoseRTV& other, - OptionalJacobian<1,9> H1=boost::none, - OptionalJacobian<1,9> H2=boost::none) const; + OptionalJacobian<1,9> H1={}, + OptionalJacobian<1,9> H2={}) const; /// @} /// @name IMU-specific @@ -138,8 +138,8 @@ public: * Note: the transform jacobian convention is flipped */ PoseRTV transformed_from(const Pose3& trans, - ChartJacobian Dglobal = boost::none, - OptionalJacobian<9, 6> Dtrans = boost::none) const; + ChartJacobian Dglobal = {}, + OptionalJacobian<9, 6> Dtrans = {}) const; /// @} /// @name Utility functions diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 3e4ad0d9f..9a536fba0 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -130,7 +130,7 @@ public: Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1); Matrix D_gravityBody_gk; - Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none); + Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, {}); Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()).finished(); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index ddeda180e..a4055d038 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -55,7 +55,7 @@ class Event { Point3 location() const { return location_; } // TODO(frank) we really have to think of a better way to do linear arguments - double height(OptionalJacobian<1, 4> H = boost::none) const { + double height(OptionalJacobian<1, 4> H = {}) const { static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished(); if (H) *H = JacobianZ; return location_.z(); @@ -101,8 +101,8 @@ class TimeOfArrival { /// Calculate time of arrival, with derivatives double operator()(const Event& event, const Point3& sensor, // - OptionalJacobian<1, 4> H1 = boost::none, // - OptionalJacobian<1, 3> H2 = boost::none) const { + OptionalJacobian<1, 4> H1 = {}, // + OptionalJacobian<1, 3> H2 = {}) const { Matrix13 D1, D2; double distance = gtsam::distance3(event.location(), sensor, D1, D2); if (H1) diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 7e070ca72..643be4e3e 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -101,7 +101,7 @@ public: } else { gtsam::Matrix J2; - gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); + gtsam::Point2 uv= camera.project(landmark,H1, J2, {}); if (H1) { *H1 = (*H1) * I_6x6; } diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 321eba934..1c205c7fb 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -98,12 +98,12 @@ public: static Pose3Upright Identity() { return Pose3Upright(); } /// inverse transformation with derivatives - Pose3Upright inverse(OptionalJacobian<4,4> H1=boost::none) const; + Pose3Upright inverse(OptionalJacobian<4,4> H1={}) const; ///compose this transformation onto another (first *this and then p2) Pose3Upright compose(const Pose3Upright& p2, - OptionalJacobian<4,4> H1=boost::none, - OptionalJacobian<4,4> H2=boost::none) const; + OptionalJacobian<4,4> H1={}, + OptionalJacobian<4,4> H2={}) const; /// compose syntactic sugar inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); } @@ -113,8 +113,8 @@ public: * as well as optionally the derivatives */ Pose3Upright between(const Pose3Upright& p2, - OptionalJacobian<4,4> H1=boost::none, - OptionalJacobian<4,4> H2=boost::none) const; + OptionalJacobian<4,4> H1={}, + OptionalJacobian<4,4> H2={}) const; /// @} /// @name Lie Group diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index f1bbc3970..25a05793d 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -65,11 +65,11 @@ TEST(Event, Derivatives) { Matrix13 actualH2; kToa(exampleEvent, microphoneAt0, actualH1, actualH2); Matrix expectedH1 = numericalDerivative11( - std::bind(kToa, std::placeholders::_1, microphoneAt0, boost::none, boost::none), + std::bind(kToa, std::placeholders::_1, microphoneAt0, nullptr, nullptr), exampleEvent); EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); Matrix expectedH2 = numericalDerivative11( - std::bind(kToa, exampleEvent, std::placeholders::_1, boost::none, boost::none), + std::bind(kToa, exampleEvent, std::placeholders::_1, nullptr, nullptr), microphoneAt0); EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); } diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 6ce7be20a..d338b34aa 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -179,7 +179,7 @@ class SimWall2D { gtsam::Point2 midpoint() const; bool intersects(const gtsam::SimWall2D& wall) const; - // bool intersects(const gtsam::SimWall2D& wall, boost::optional pt=boost::none) const; + // bool intersects(const gtsam::SimWall2D& wall, boost::optional pt={}) const; gtsam::Point2 norm() const; gtsam::Rot2 reflection(const gtsam::Point2& init, const gtsam::Point2& intersection) const; diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index 2bc381f7a..cfe00856a 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -46,7 +46,7 @@ TEST( simulated3D, Values ) TEST( simulated3D, Dprior ) { Point3 x(1,-9, 7); - Matrix numerical = numericalDerivative11(std::bind(simulated3D::prior, std::placeholders::_1, boost::none),x); + Matrix numerical = numericalDerivative11(std::bind(simulated3D::prior, std::placeholders::_1, nullptr),x); Matrix computed; simulated3D::prior(x,computed); EXPECT(assert_equal(numerical,computed,1e-9)); @@ -60,12 +60,12 @@ TEST( simulated3D, DOdo ) simulated3D::odo(x1, x2, H1, H2); Matrix A1 = numericalDerivative21( std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none), + nullptr, nullptr), x1, x2); EXPECT(assert_equal(A1, H1, 1e-9)); Matrix A2 = numericalDerivative22( std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none), + nullptr, nullptr), x1, x2); EXPECT(assert_equal(A2, H2, 1e-9)); }