diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2652fc073..3779d401a 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -439,14 +439,14 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself, Hpose->setZero(); return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); } - return bearing(pose.translation(), Hself, boost::none); + return bearing(pose.translation(), Hself, {}); } /* ************************************************************************* */ -boost::optional Pose3::Align(const Point3Pairs &abPointPairs) { +std::optional Pose3::Align(const Point3Pairs &abPointPairs) { const size_t n = abPointPairs.size(); if (n < 3) { - return boost::none; // we need at least three pairs + return {}; // we need at least three pairs } // calculate centroids @@ -466,7 +466,7 @@ boost::optional Pose3::Align(const Point3Pairs &abPointPairs) { return Pose3(aRb, aTb); } -boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { +std::optional Pose3::Align(const Matrix& a, const Matrix& b) { if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) { throw std::invalid_argument( "Pose3:Align expects 3*N matrices of equal shape."); @@ -479,7 +479,7 @@ boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -boost::optional align(const Point3Pairs &baPointPairs) { +std::optional align(const Point3Pairs &baPointPairs) { Point3Pairs abPointPairs; for (const Point3Pair &baPair : baPointPairs) { abPointPairs.emplace_back(baPair.second, baPair.first); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 80741e1c3..3dd9e6b9f 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -75,18 +75,18 @@ public: /// Named constructor with derivatives static Pose3 Create(const Rot3& R, const Point3& t, - OptionalJacobian<6, 3> HR = boost::none, - OptionalJacobian<6, 3> Ht = boost::none); + OptionalJacobian<6, 3> HR = {}, + OptionalJacobian<6, 3> Ht = {}); /** * Create Pose3 by aligning two point pairs * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point * Note this allows for noise on the points but in that case the mapping will not be exact. */ - static boost::optional Align(const Point3Pairs& abPointPairs); + static std::optional Align(const Point3Pairs& abPointPairs); // Version of Pose3::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 @@ -139,10 +139,10 @@ public: /// @{ /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ - static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none); + static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation - static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none); + static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {}); /** * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame @@ -157,13 +157,13 @@ public: * Note that H_xib = AdjointMap() */ Vector6 Adjoint(const Vector6& xi_b, - OptionalJacobian<6, 6> H_this = boost::none, - OptionalJacobian<6, 6> H_xib = boost::none) const; + OptionalJacobian<6, 6> H_this = {}, + OptionalJacobian<6, 6> H_xib = {}) const; /// The dual version of Adjoint Vector6 AdjointTranspose(const Vector6& x, - OptionalJacobian<6, 6> H_this = boost::none, - OptionalJacobian<6, 6> H_x = boost::none) const; + OptionalJacobian<6, 6> H_this = {}, + OptionalJacobian<6, 6> H_x = {}) const; /** * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 @@ -186,8 +186,8 @@ public: * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ static Vector6 adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = boost::none, - OptionalJacobian<6, 6> H_y = boost::none); + OptionalJacobian<6, 6> Hxi = {}, + OptionalJacobian<6, 6> H_y = {}); // temporary fix for wrappers until case issue is resolved static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} @@ -197,8 +197,8 @@ public: * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = boost::none, - OptionalJacobian<6, 6> H_y = boost::none); + OptionalJacobian<6, 6> Hxi = {}, + OptionalJacobian<6, 6> H_y = {}); /// Derivative of Expmap static Matrix6 ExpmapDerivative(const Vector6& xi); @@ -208,8 +208,8 @@ public: // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct ChartAtOrigin { - static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none); - static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); + static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); + static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); }; /** @@ -250,7 +250,7 @@ public: * @return point in world coordinates */ Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself = - boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; + {}, OptionalJacobian<3, 3> Hpoint = {}) const; /** * @brief transform many points in Pose coordinates and transform to world. @@ -272,7 +272,7 @@ public: * @return point in Pose coordinates */ Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself = - boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; + {}, OptionalJacobian<3, 3> Hpoint = {}) const; /** * @brief transform many points in world coordinates and transform to Pose. @@ -286,10 +286,10 @@ public: /// @{ /// get rotation - const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const; + const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const; /// get translation - const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const; + const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const; /// get x double x() const { @@ -314,39 +314,39 @@ public: * and transforms it to world coordinates wTb = wTa * aTb. * This is identical to compose. */ - Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none, - OptionalJacobian<6, 6> HaTb = boost::none) const; + Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = {}, + OptionalJacobian<6, 6> HaTb = {}) const; /** * Assuming self == wTa, takes a pose wTb in world coordinates * and transforms it to local coordinates aTb = inv(wTa) * wTb */ - Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none, - OptionalJacobian<6, 6> HwTb = boost::none) const; + Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = {}, + OptionalJacobian<6, 6> HwTb = {}) const; /** * Calculate range to a landmark * @param point 3D location of landmark * @return range (double) */ - double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none, - OptionalJacobian<1, 3> Hpoint = boost::none) const; + double range(const Point3& point, OptionalJacobian<1, 6> Hself = {}, + OptionalJacobian<1, 3> Hpoint = {}) const; /** * Calculate range to another pose * @param pose Other SO(3) pose * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none, - OptionalJacobian<1, 6> Hpose = boost::none) const; + double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = {}, + OptionalJacobian<1, 6> Hpose = {}) const; /** * Calculate bearing to a landmark * @param point 3D location of landmark * @return bearing (Unit3) */ - Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none, - OptionalJacobian<2, 3> Hpoint = boost::none) const; + Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = {}, + OptionalJacobian<2, 3> Hpoint = {}) const; /** * Calculate bearing to another pose @@ -354,8 +354,8 @@ public: * information is ignored. * @return bearing (Unit3) */ - Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none, - OptionalJacobian<2, 6> Hpose = boost::none) const; + Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = {}, + OptionalJacobian<2, 6> Hpose = {}) const; /// @} /// @name Advanced Interface @@ -384,8 +384,8 @@ public: * @param s a value between 0 and 1.5 * @param other final point of interpolation geodesic on manifold */ - Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, - OptionalJacobian<6, 6> Hy = boost::none) const; + Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {}, + OptionalJacobian<6, 6> Hy = {}) const; /// Output stream operator GTSAM_EXPORT diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index dd7da172e..93cf99972 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -23,6 +23,7 @@ #include #include +#include using namespace std; using namespace gtsam; @@ -286,8 +287,8 @@ TEST(Pose3, translation) { Matrix actualH; EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8)); - Matrix numericalH = numericalDerivative11( - std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T); + std::function f = [](const Pose3& T) { return T.translation(); }; + Matrix numericalH = numericalDerivative11(f, T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -297,8 +298,8 @@ TEST(Pose3, rotation) { Matrix actualH; EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); - Matrix numericalH = numericalDerivative11( - std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T); + std::function f = [](const Pose3& T) { return T.rotation(); }; + Matrix numericalH = numericalDerivative11(f, T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -396,7 +397,7 @@ Point3 transformFrom_(const Pose3& pose, const Point3& point) { } TEST(Pose3, Dtransform_from1_a) { Matrix actualDtransform_from1; - T.transformFrom(P, actualDtransform_from1, boost::none); + T.transformFrom(P, actualDtransform_from1, {}); Matrix numerical = numericalDerivative21(transformFrom_, T, P); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } @@ -404,7 +405,7 @@ TEST(Pose3, Dtransform_from1_a) { TEST(Pose3, Dtransform_from1_b) { Pose3 origin; Matrix actualDtransform_from1; - origin.transformFrom(P, actualDtransform_from1, boost::none); + origin.transformFrom(P, actualDtransform_from1, {}); Matrix numerical = numericalDerivative21(transformFrom_, origin, P); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } @@ -413,7 +414,7 @@ TEST(Pose3, Dtransform_from1_c) { Point3 origin(0, 0, 0); Pose3 T0(R, origin); Matrix actualDtransform_from1; - T0.transformFrom(P, actualDtransform_from1, boost::none); + T0.transformFrom(P, actualDtransform_from1, {}); Matrix numerical = numericalDerivative21(transformFrom_, T0, P); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } @@ -423,7 +424,7 @@ TEST(Pose3, Dtransform_from1_d) { Point3 t0(100, 0, 0); Pose3 T0(I, t0); Matrix actualDtransform_from1; - T0.transformFrom(P, actualDtransform_from1, boost::none); + T0.transformFrom(P, actualDtransform_from1, {}); // print(computed, "Dtransform_from1_d computed:"); Matrix numerical = numericalDerivative21(transformFrom_, T0, P); // print(numerical, "Dtransform_from1_d numerical:"); @@ -433,7 +434,7 @@ TEST(Pose3, Dtransform_from1_d) { /* ************************************************************************* */ TEST(Pose3, Dtransform_from2) { Matrix actualDtransform_from2; - T.transformFrom(P, boost::none, actualDtransform_from2); + T.transformFrom(P, {}, actualDtransform_from2); Matrix numerical = numericalDerivative22(transformFrom_, T, P); EXPECT(assert_equal(numerical, actualDtransform_from2, 1e-8)); } @@ -444,7 +445,7 @@ Point3 transform_to_(const Pose3& pose, const Point3& point) { } TEST(Pose3, Dtransform_to1) { Matrix computed; - T.transformTo(P, computed, boost::none); + T.transformTo(P, computed, {}); Matrix numerical = numericalDerivative21(transform_to_, T, P); EXPECT(assert_equal(numerical, computed, 1e-8)); } @@ -452,7 +453,7 @@ TEST(Pose3, Dtransform_to1) { /* ************************************************************************* */ TEST(Pose3, Dtransform_to2) { Matrix computed; - T.transformTo(P, boost::none, computed); + T.transformTo(P, {}, computed); Matrix numerical = numericalDerivative22(transform_to_, T, P); EXPECT(assert_equal(numerical, computed, 1e-8)); } @@ -812,7 +813,7 @@ TEST(Pose3, Align1) { Point3Pair ab3(Point3(20,30,0), Point3(10,20,0)); const vector correspondences{ab1, ab2, ab3}; - boost::optional actual = Pose3::Align(correspondences); + std::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual)); } @@ -829,7 +830,7 @@ TEST(Pose3, Align2) { const Point3Pair ab1{q1, p1}, ab2{q2, p2}, ab3{q3, p3}; const vector correspondences{ab1, ab2, ab3}; - boost::optional actual = Pose3::Align(correspondences); + std::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual, 1e-5)); } @@ -839,7 +840,7 @@ TEST( Pose3, ExpmapDerivative1) { Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; Pose3::Expmap(w,actualH); Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none); + OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {}); EXPECT(assert_equal(expectedH, actualH)); } @@ -884,7 +885,7 @@ TEST( Pose3, ExpmapDerivativeQr) { w.head<3>() = w.head<3>() * 0.9e-2; Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none); + OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {}); Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); } @@ -896,7 +897,7 @@ TEST( Pose3, LogmapDerivative) { Pose3 p = Pose3::Expmap(w); EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); Matrix expectedH = numericalDerivative21 >(&Pose3::Logmap, p, boost::none); + OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {}); EXPECT(assert_equal(expectedH, actualH)); } @@ -1189,9 +1190,9 @@ TEST(Pose3, Create) { Matrix63 actualH1, actualH2; Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2); EXPECT(assert_equal(T, actual)); - std::function create = - std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none); + std::function create = [](Rot3 R, Point3 t) { + return Pose3::Create(R, t); + }; EXPECT(assert_equal(numericalDerivative21(create, R, P2), actualH1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(create, R, P2), actualH2, 1e-9)); }