diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index b37674b92..d7996f74f 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -327,10 +327,10 @@ double Pose2::range(const Pose2& pose, * as they also satisfy ca = t + R*cb, hence t = ca - R*cb */ -boost::optional Pose2::Align(const Point2Pairs &ab_pairs) { +std::optional Pose2::Align(const Point2Pairs &ab_pairs) { const size_t n = ab_pairs.size(); if (n < 2) { - return boost::none; // we need at least 2 pairs + return {}; // we need at least 2 pairs } // calculate centroids @@ -359,7 +359,7 @@ boost::optional Pose2::Align(const Point2Pairs &ab_pairs) { return Pose2(R, t); } -boost::optional Pose2::Align(const Matrix& a, const Matrix& b) { +std::optional Pose2::Align(const Matrix& a, const Matrix& b) { if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) { throw std::invalid_argument( "Pose2:Align expects 2*N matrices of equal shape."); @@ -372,7 +372,7 @@ boost::optional Pose2::Align(const Matrix& a, const Matrix& b) { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -boost::optional align(const Point2Pairs& ba_pairs) { +std::optional align(const Point2Pairs& ba_pairs) { Point2Pairs ab_pairs; for (const Point2Pair &baPair : ba_pairs) { ab_pairs.emplace_back(baPair.second, baPair.first); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 0ecd95e1c..7a6a846be 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -25,6 +25,9 @@ #include #include #include +#include + +#include namespace gtsam { @@ -99,10 +102,10 @@ public: * Note this allows for noise on the points but in that case the mapping * will not be exact. */ - static boost::optional Align(const Point2Pairs& abPointPairs); + static std::optional Align(const Point2Pairs& abPointPairs); // Version of Pose2::Align that takes 2 matrices. - static boost::optional Align(const Matrix& a, const Matrix& b); + static std::optional Align(const Matrix& a, const Matrix& b); /// @} /// @name Testable @@ -134,10 +137,10 @@ public: /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none); + GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {}); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); + GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {}); /** * Calculate Adjoint map @@ -196,8 +199,8 @@ public: // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP struct ChartAtOrigin { - GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); - GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); + GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); + GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {}); }; using LieGroup::inverse; // version with derivative @@ -208,8 +211,8 @@ public: /** Return point coordinates in pose coordinate frame */ GTSAM_EXPORT Point2 transformTo(const Point2& point, - OptionalJacobian<2, 3> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const; + OptionalJacobian<2, 3> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const; /** * @brief transform many points in world coordinates and transform to Pose. @@ -220,8 +223,8 @@ public: /** Return point coordinates in global frame */ GTSAM_EXPORT Point2 transformFrom(const Point2& point, - OptionalJacobian<2, 3> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const; + OptionalJacobian<2, 3> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const; /** * @brief transform many points in Pose coordinates and transform to world. @@ -269,7 +272,7 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ GTSAM_EXPORT Rot2 bearing(const Point2& point, - OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const; + OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const; /** * Calculate bearing to another pose @@ -277,7 +280,7 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ GTSAM_EXPORT Rot2 bearing(const Pose2& pose, - OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const; + OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const; /** * Calculate range to a landmark @@ -285,8 +288,8 @@ public: * @return range (double) */ GTSAM_EXPORT double range(const Point2& point, - OptionalJacobian<1, 3> H1=boost::none, - OptionalJacobian<1, 2> H2=boost::none) const; + OptionalJacobian<1, 3> H1={}, + OptionalJacobian<1, 2> H2={}) const; /** * Calculate range to another pose @@ -294,8 +297,8 @@ public: * @return range (double) */ GTSAM_EXPORT double range(const Pose2& point, - OptionalJacobian<1, 3> H1=boost::none, - OptionalJacobian<1, 3> H2=boost::none) const; + OptionalJacobian<1, 3> H1={}, + OptionalJacobian<1, 3> H2={}) const; /// @} /// @name Advanced Interface @@ -349,7 +352,7 @@ inline Matrix wedge(const Vector& xi) { * Calculate pose between a vector of 2D point correspondences (p,q) * where q = Pose2::transformFrom(p) = t + R*p */ -GTSAM_EXPORT boost::optional +GTSAM_EXPORT std::optional GTSAM_DEPRECATED align(const Point2Pairs& pairs); #endif diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 44dc55a81..34ce3932d 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -718,7 +718,7 @@ TEST(Pose2, align_1) { Pose2 expected(Rot2::fromAngle(0), Point2(10, 10)); Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)}, {Point2(30, 20), Point2(20, 10)}}; - boost::optional aTb = Pose2::Align(ab_pairs); + std::optional aTb = Pose2::Align(ab_pairs); EXPECT(assert_equal(expected, *aTb)); } @@ -731,7 +731,7 @@ TEST(Pose2, align_2) { Point2Pairs ab_pairs {{expected.transformFrom(b1), b1}, {expected.transformFrom(b2), b2}}; - boost::optional aTb = Pose2::Align(ab_pairs); + std::optional aTb = Pose2::Align(ab_pairs); EXPECT(assert_equal(expected, *aTb)); } @@ -752,7 +752,7 @@ TEST(Pose2, align_3) { Point2Pair ab3(make_pair(a3, b3)); const Point2Pairs ab_pairs{ab1, ab2, ab3}; - boost::optional aTb = Pose2::Align(ab_pairs); + std::optional aTb = Pose2::Align(ab_pairs); EXPECT(assert_equal(expected, *aTb)); } @@ -762,7 +762,7 @@ namespace { /* ************************************************************************* */ struct Triangle { size_t i_, j_, k_;}; - boost::optional align2(const Point2Vector& as, const Point2Vector& bs, + std::optional align2(const Point2Vector& as, const Point2Vector& bs, const pair& trianglePair) { const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]}, @@ -780,7 +780,7 @@ TEST(Pose2, align_4) { Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2; Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0; - boost::optional actual = align2(as, bs, {t1, t2}); + std::optional actual = align2(as, bs, {t1, t2}); EXPECT(assert_equal(expected, *actual)); }