From bbb997f89506cb465634b204adc15122bafa5468 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Tue, 10 Jan 2023 15:43:35 -0800 Subject: [PATCH] removed some more boost optional matrix references --- gtsam/base/numericalDerivative.h | 2 +- gtsam/navigation/MagFactor.h | 4 +-- gtsam/navigation/tests/testMagFactor.cpp | 29 ++++++---------- gtsam/nonlinear/NonlinearFactor.h | 10 +++--- gtsam/slam/BoundingConstraint.h | 12 +++---- gtsam_unstable/geometry/Pose3Upright.cpp | 43 +++++++++++++++--------- gtsam_unstable/geometry/Pose3Upright.h | 10 +++--- tests/simulated2DConstraints.h | 13 +++---- timing/timeRot2.cpp | 9 ++--- 9 files changed, 70 insertions(+), 62 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 05b60033f..e4255bc63 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -35,7 +35,7 @@ namespace gtsam { * * Usage of the boost bind version to rearrange arguments: * for a function with one relevant param and an optional derivative: - * Foo bar(const Obj& a, boost::optional H1) + * 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 diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index d57a5a72d..d95409bda 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -62,7 +62,7 @@ public: } static Point3 unrotate(const Rot2& R, const Point3& p, - boost::optional HR = boost::none) { + OptionalMatrixType HR = OptionalNone) { Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none); if (HR) { // assign to temporary first to avoid error in Win-Debug mode @@ -115,7 +115,7 @@ public: */ Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override { // measured bM = nRb� * nM + b - Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; + Point3 hx = nRb.unrotate(nM_, H, OptionalNone) + bias_; return (hx - measured_); } }; diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 971803dbf..f15e00456 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -56,15 +56,13 @@ Unit3 dir(nM); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); } // namespace -using boost::none; - // ************************************************************************* TEST( MagFactor, unrotate ) { Matrix H; Point3 expected(22735.5, 314.502, 44202.5); - EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); + EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,&H),1e-1)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6)); + (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, OptionalNone), theta), H, 1e-6)); } // ************************************************************************* @@ -75,37 +73,32 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); - EXPECT(assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7)); + EXPECT(assert_equal((Matrix)numericalDerivative11 + ([&f] (const Rot2& theta) { return f.evaluateError(theta); }, theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); - EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7)); + EXPECT(assert_equal(numericalDerivative11 + ([&f1] (const Rot3& nRb) { return f1.evaluateError(nRb); }, nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),// - H1, 1e-7)); + ([&f2] (const Point3& scaled) { return f2.evaluateError(scaled,bias); }, scaled), H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),// - H2, 1e-7)); + ([&f2] (const Point3& bias) { return f2.evaluateError(scaled,bias); }, bias), H2, 1e-7)); // MagFactor3 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),// - H1, 1e-7)); + ([&f3] (double s) { return f3.evaluateError(s,dir,bias); }, s), H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),// - H2, 1e-7)); + ([&f3] (const Unit3& dir) { return f3.evaluateError(s,dir,bias); }, dir), H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),// - H3, 1e-7)); + ([&f3] (const Point3& bias) { return f3.evaluateError(s,dir,bias); }, bias), H3, 1e-7)); } // ************************************************************************* diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 3c03b717b..89bb85e8d 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -381,8 +381,8 @@ struct NoiseModelFactorAliases { * * Vector evaluateError( * const Pose3& T, const Point3& p, - * boost::optional H_T = boost::none, - * boost::optional H_p = boost::none) const override { + * OptionalMatrixType H_T = OptionalNone, + * OptionalMatrixType H_p = OptionalNone) const override { * Matrix36 t_H_T; // partial derivative of translation w.r.t. pose T * * // Only compute t_H_T if needed: @@ -449,7 +449,7 @@ protected: /// @} - /* Like std::void_t, except produces `boost::optional` instead of + /* Like std::void_t, except produces `OptionalMatrixType` instead of * `void`. Used to expand fixed-type parameter-packs with same length as * ValueTypes. */ @@ -582,8 +582,8 @@ protected: * ``` * Vector evaluateError( * const Pose3& x1, const Point3& x2, - * boost::optional H1 = boost::none, - * boost::optional H2 = boost::none) const override { ... } + * OptionalMatrixType H1 = OptionalNone, + * OptionalMatrixType H2 = OptionalNone) const override { ... } * ``` * * If any of the optional Matrix reference arguments are specified, it should diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index b48a3e762..d4770b4cc 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -57,8 +57,8 @@ struct BoundingConstraint1: public NoiseModelFactorN { * Must have optional argument for derivative with 1xN matrix, where * N = X::dim() */ - virtual double value(const X& x, boost::optional H = - boost::none) const = 0; + virtual double value(const X& x, OptionalMatrixType H = + OptionalNone) const = 0; /** active when constraint *NOT* met */ bool active(const Values& c) const override { @@ -69,7 +69,7 @@ struct BoundingConstraint1: public NoiseModelFactorN { Vector evaluateError(const X& x, OptionalMatrixType H) const override { Matrix D; - double error = value(x, D) - threshold_; + double error = value(x, &D) - threshold_; if (H) { if (isGreaterThan_) *H = D; else *H = -1.0 * D; @@ -128,8 +128,8 @@ struct BoundingConstraint2: public NoiseModelFactorN { * Must have optional argument for derivatives) */ virtual double value(const X1& x1, const X2& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const = 0; + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const = 0; /** active when constraint *NOT* met */ bool active(const Values& c) const override { @@ -141,7 +141,7 @@ struct BoundingConstraint2: public NoiseModelFactorN { Vector evaluateError(const X1& x1, const X2& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override { Matrix D1, D2; - double error = value(x1, x2, D1, D2) - threshold_; + double error = value(x1, x2, &D1, &D2) - threshold_; if (H1) { if (isGreaterThan_) *H1 = D1; else *H1 = -1.0 * D1; diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 2a2afa476..b740ba6f2 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -5,7 +5,10 @@ * @author Alex Cunningham */ +#include #include +#include "gtsam/base/OptionalJacobian.h" +#include "gtsam/base/Vector.h" #include @@ -78,27 +81,34 @@ Pose3 Pose3Upright::pose() const { } /* ************************************************************************* */ -Pose3Upright Pose3Upright::inverse(boost::optional H1) const { - Pose3Upright result(T_.inverse(H1), -z_); - if (H1) { - Matrix H1_ = -I_4x4; - H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); - H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); - *H1 = H1_; +Pose3Upright Pose3Upright::inverse(OptionalJacobian<4, 4> H1) const { + if (!H1) { + return Pose3Upright(T_.inverse(), -z_); } + OptionalJacobian<3, 3>::Jacobian H3x3; + // TODO: Could not use reference to a view into H1 and reuse memory + // Eigen::Ref> H3x3 = H1->topLeftCorner(3,3); + Pose3Upright result(T_.inverse(H3x3), -z_); + Matrix H1_ = -I_4x4; + H1_.topLeftCorner(2, 2) = H3x3.topLeftCorner(2, 2); + H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1); + *H1 = H1_; return result; } /* ************************************************************************* */ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const { if (!H1 && !H2) return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_); - Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_); + + // TODO: Could not use reference to a view into H1 and reuse memory + OptionalJacobian<3, 3>::Jacobian H3x3; + Pose3Upright result(T_.compose(p2.T_, H3x3), z_ + p2.z_); if (H1) { Matrix H1_ = I_4x4; - H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); - H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); + H1_.topLeftCorner(2,2) = H3x3.topLeftCorner(2,2); + H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1); *H1 = H1_; } if (H2) *H2 = I_4x4; @@ -107,14 +117,17 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2, /* ************************************************************************* */ Pose3Upright Pose3Upright::between(const Pose3Upright& p2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const { if (!H1 && !H2) return Pose3Upright(T_.between(p2.T_), p2.z_ - z_); - Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_); + + // TODO: Could not use reference to a view into H1 and H2 to reuse memory + OptionalJacobian<3, 3>::Jacobian H3x3_1, H3x3_2; + Pose3Upright result(T_.between(p2.T_, H3x3_1, H3x3_2), p2.z_ - z_); if (H1) { Matrix H1_ = -I_4x4; - H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); - H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); + H1_.topLeftCorner(2,2) = H3x3_1.topLeftCorner(2,2); + H1_.topRightCorner(2, 1) = H3x3_1.topRightCorner(2, 1); *H1 = H1_; } if (H2) *H2 = I_4x4; diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 037a6cc9d..321eba934 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(boost::optional H1=boost::none) const; + Pose3Upright inverse(OptionalJacobian<4,4> H1=boost::none) const; ///compose this transformation onto another (first *this and then p2) Pose3Upright compose(const Pose3Upright& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<4,4> H1=boost::none, + OptionalJacobian<4,4> H2=boost::none) 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, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<4,4> H1=boost::none, + OptionalJacobian<4,4> H2=boost::none) const; /// @} /// @name Lie Group diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index 1609876b6..fad7e0c39 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -25,6 +25,7 @@ #include #include #include +#include "gtsam/nonlinear/NonlinearFactor.h" // \namespace @@ -87,8 +88,8 @@ namespace simulated2D { * @param x is the estimate of the constrained variable being evaluated * @param H is an optional Jacobian, linearized at x */ - double value(const Point& x, boost::optional H = - boost::none) const override { + double value(const Point& x, OptionalMatrixType H = + OptionalNone) const override { if (H) { Matrix D = Matrix::Zero(1, traits::GetDimension(x)); D(0, IDX) = 1.0; @@ -151,8 +152,8 @@ namespace simulated2D { * @return the distance between the variables */ double value(const Point& x1, const Point& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); if (H1) *H2 = numericalDerivative22(range_trait, x1, x2, 1e-5); return range_trait(x1, x2); @@ -201,8 +202,8 @@ namespace simulated2D { * @return the distance between the variables */ double value(const Pose& x1, const Point& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); if (H1) *H2 = numericalDerivative22(range_trait, x1, x2, 1e-5); return range_trait(x1, x2); diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 387d458ea..f33ba5a1e 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -19,6 +19,7 @@ #include #include #include +#include "gtsam/base/OptionalJacobian.h" using namespace std; using namespace gtsam; @@ -43,7 +44,7 @@ Rot2 Rot2betweenOptimized(const Rot2& r1, const Rot2& r2) { /* ************************************************************************* */ Vector Rot2BetweenFactorEvaluateErrorDefault(const Rot2& measured, const Rot2& p1, const Rot2& p2, - boost::optional H1, boost::optional H2) + OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2) { Rot2 hx = p1.between(p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) @@ -52,7 +53,7 @@ Vector Rot2BetweenFactorEvaluateErrorDefault(const Rot2& measured, const Rot2& p /* ************************************************************************* */ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetween(const Rot2& measured, const Rot2& p1, const Rot2& p2, - boost::optional H1, boost::optional H2) + OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2) { Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) if (H1) *H1 = -I_1x1; @@ -63,7 +64,7 @@ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetween(const Rot2& measured, cons /* ************************************************************************* */ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured, const Rot2& p1, const Rot2& p2, - boost::optional H1, boost::optional H2) + OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2) { // TODO: Implement Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) @@ -76,7 +77,7 @@ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured, /* ************************************************************************* */ typedef Eigen::Matrix Matrix1; Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Rot2& measured, const Rot2& p1, const Rot2& p2, - boost::optional H1, boost::optional H2) + OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2) { // TODO: Implement Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x)