diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index df4f385a8..fd3392b67 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -66,7 +66,7 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { /* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, // - FixedRef<2, 3> Dcal, FixedRef<2, 2> Dp) const { + OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { // r = x^2 + y^2; // g = (1 + k(1)*r + k(2)*r^2); // pi(:,i) = g * pn(:,i) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 837888855..ed4f8b391 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -113,8 +113,8 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, FixedRef<2, 3> Dcal = boost::none, - FixedRef<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 3846178fa..1830d56a1 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -91,7 +91,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, /* ************************************************************************* */ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - FixedRef<2,9> H1, FixedRef<2,2> H2) const { + OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index cf80a7741..1b2143278 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -114,8 +114,8 @@ public: * @return point in (distorted) image coordinates */ Point2 uncalibrate(const Point2& p, - FixedRef<2,9> Dcal = boost::none, - FixedRef<2,2> Dp = boost::none) const ; + OptionalJacobian<2,9> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index dbb6c09eb..d0589cc65 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -72,8 +72,8 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, FixedRef<2, 5> Dcal, - FixedRef<2, 2> Dp) const { +Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) const { const double x = p.x(), y = p.y(); if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index a380ff4db..e28b24eae 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -148,8 +148,8 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = boost::none, - FixedRef<2,2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const; /** * convert image coordinates uv to intrinsic coordinates xy diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 96d227912..a70bb2301 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -274,7 +274,7 @@ public: * @param Dpoint is the 2*3 Jacobian w.r.t. P */ static Point2 project_to_camera(const Point3& P, // - FixedRef<2, 3> Dpoint = boost::none) { + OptionalJacobian<2, 3> Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) throw CheiralityException(); @@ -303,9 +303,9 @@ public: */ inline Point2 project( const Point3& pw, - FixedRef<2,6> Dpose = boost::none, - FixedRef<2,3> Dpoint = boost::none, - FixedRef<2,DimK> Dcal = boost::none) const { + OptionalJacobian<2,6> Dpose = boost::none, + OptionalJacobian<2,3> Dpoint = boost::none, + OptionalJacobian<2,DimK> Dcal = boost::none) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); @@ -338,9 +338,9 @@ public: */ inline Point2 projectPointAtInfinity( const Point3& pw, - FixedRef<2,6> Dpose = boost::none, - FixedRef<2,2> Dpoint = boost::none, - FixedRef<2,DimK> Dcal = boost::none) const { + OptionalJacobian<2,6> Dpose = boost::none, + OptionalJacobian<2,2> Dpoint = boost::none, + OptionalJacobian<2,DimK> Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) @@ -378,8 +378,8 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 */ Point2 project2(const Point3& pw, // - FixedRef<2, 6 + DimK> Dcamera = boost::none, - FixedRef<2, 3> Dpoint = boost::none) const { + OptionalJacobian<2, 6 + DimK> Dcamera = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 8b90aed63..6588f051f 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -38,15 +38,9 @@ bool Point2::equals(const Point2& q, double tol) const { } /* ************************************************************************* */ -double Point2::norm() const { - return sqrt(x_ * x_ + y_ * y_); -} - -/* ************************************************************************* */ -double Point2::norm(boost::optional H) const { - double r = norm(); +double Point2::norm(OptionalJacobian<1,2> H) const { + double r = sqrt(x_ * x_ + y_ * y_); if (H) { - H->resize(1,2); if (fabs(r) > 1e-10) *H << x_ / r, y_ / r; else @@ -57,11 +51,11 @@ double Point2::norm(boost::optional H) const { /* ************************************************************************* */ static const Matrix I2 = eye(2); -double Point2::distance(const Point2& point, boost::optional H1, - boost::optional H2) const { +double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, + OptionalJacobian<1,2> H2) const { Point2 d = point - *this; if (H1 || H2) { - Matrix H; + Eigen::Matrix H; double r = d.norm(H); if (H1) *H1 = -H; if (H2) *H2 = H; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index a4e0cc296..a0b185b63 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -125,10 +125,10 @@ public: /// "Compose", just adds the coordinates of two points. With optional derivatives inline Point2 compose(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(2); - if(H2) *H2 = eye(2); + OptionalJacobian<2,2> H1=boost::none, + OptionalJacobian<2,2> H2=boost::none) const { + if(H1) *H1 = Eigen::Matrix2d::Identity(); + if(H2) *H2 = Eigen::Matrix2d::Identity(); return *this + q; } @@ -137,10 +137,10 @@ public: /// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q) inline Point2 between(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(2); - if(H2) *H2 = eye(2); + OptionalJacobian<2,2> H1=boost::none, + OptionalJacobian<2,2> H2=boost::none) const { + if(H1) *H1 = -Eigen::Matrix2d::Identity(); + if(H2) *H2 = Eigen::Matrix2d::Identity(); return q - (*this); } @@ -180,15 +180,12 @@ public: /** creates a unit vector */ Point2 unit() const { return *this/norm(); } - /** norm of point */ - double norm() const; - /** norm of point, with derivative */ - double norm(boost::optional H) const; + double norm(OptionalJacobian<1,2> H = boost::none) const; /** distance between two points */ - double distance(const Point2& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, + OptionalJacobian<1,2> H2 = boost::none) const; /** @deprecated The following function has been deprecated, use distance above */ inline double dist(const Point2& p2) const { diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 43f2239e2..03ed31ab5 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -94,7 +94,7 @@ double Point3::dot(const Point3 &q) const { } /* ************************************************************************* */ -double Point3::norm(FixedRef<1,3> H) const { +double Point3::norm(OptionalJacobian<1,3> H) const { double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); if (H) { if (fabs(r) > 1e-10) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 9ef6696b7..96cf4e0c8 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,9 +21,9 @@ #pragma once -#include #include #include +#include #include @@ -181,7 +181,7 @@ namespace gtsam { } /** Distance of the point from the origin, with Jacobian */ - double norm(FixedRef<1,3> H = boost::none) const; + double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ Point3 normalize(boost::optional H = boost::none) const; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 41929c242..f0daa4054 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -126,7 +126,7 @@ Pose2 Pose2::inverse(boost::optional H1) const { /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, - FixedRef<2, 3> H1, FixedRef<2, 2> H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { Point2 d = point - t_; Point2 q = r_.unrotate(d); if (!H1 && !H2) return q; @@ -150,7 +150,7 @@ Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_from(const Point2& p, - FixedRef<2, 3> H1, FixedRef<2, 2> H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = r_ * p; if (H1 || H2) { const Matrix R = r_.matrix(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index cac79e6fb..f39ca5a81 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -192,13 +192,13 @@ public: /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, - FixedRef<2, 3> H1 = boost::none, - FixedRef<2, 2> H2 = boost::none) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, - FixedRef<2, 3> H1 = boost::none, - FixedRef<2, 2> H2 = boost::none) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** syntactic sugar for transform_from */ inline Point2 operator*(const Point2& point) const { return transform_from(point);} diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 42d207bb1..93a27ed58 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -254,8 +254,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, FixedRef<3,6> Dpose, - FixedRef<3,3> Dpoint) const { +Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index e5460fd82..c31cc48cc 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -250,8 +250,8 @@ public: * @return point in Pose coordinates */ Point3 transform_to(const Point3& p, - FixedRef<3,6> Dpose = boost::none, - FixedRef<3,3> Dpoint = boost::none) const; + OptionalJacobian<3,6> Dpose = boost::none, + OptionalJacobian<3,3> Dpoint = boost::none) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ffa2f81c5..4364678a5 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -218,8 +218,8 @@ namespace gtsam { Rot3 inverse(boost::optional H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, FixedRef<3, 3> H1 = boost::none, - FixedRef<3, 3> H2 = boost::none) const; + Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 623a13664..39648ca1e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -142,7 +142,7 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { } /* ************************************************************************* */ -Rot3 Rot3::compose(const Rot3& R2, FixedRef<3, 3> H1, FixedRef<3, 3> H2) const { +Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { if (H1) *H1 = R2.transpose(); if (H2) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index a87d9896e..ebc1d8f15 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -42,21 +43,21 @@ namespace gtsam { const unsigned TraceAlignment = 16; -template -T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment){ +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { // right now only word sized types are supported. // Easy to extend if needed, // by somehow inferring the unsigned integer of same size BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); size_t & uiValue = reinterpret_cast(value); size_t misAlignment = uiValue % requiredAlignment; - if(misAlignment) { + if (misAlignment) { uiValue += requiredAlignment - misAlignment; } return value; } -template -T upAligned(T value, unsigned requiredAlignment = TraceAlignment){ +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { return upAlign(value, requiredAlignment); } @@ -88,19 +89,21 @@ public: namespace internal { -template +template struct UseBlockIf { static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key){ + JacobianMap& jacobians, Key key) { // block makes HUGE difference - jacobians(key).block(0, 0) += dTdA; - }; + jacobians(key).block( + 0, 0) += dTdA; + } + ; }; /// Handle Leaf Case for Dynamic Matrix type (slower) -template +template struct UseBlockIf { static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { + JacobianMap& jacobians, Key key) { jacobians(key) += dTdA; } }; @@ -111,10 +114,9 @@ template void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { internal::UseBlockIf< - Derived::RowsAtCompileTime != Eigen::Dynamic && - Derived::ColsAtCompileTime != Eigen::Dynamic, - Derived> - ::addToJacobian(dTdA, jacobians, key); + Derived::RowsAtCompileTime != Eigen::Dynamic + && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( + dTdA, jacobians, key); } //----------------------------------------------------------------------------- @@ -265,7 +267,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const = 0; + ExecutionTraceStorage* traceStorage) const = 0; }; //----------------------------------------------------------------------------- @@ -336,7 +338,7 @@ public: /// Construct an execution trace for reverse AD virtual const value_type& traceExecution(const Values& values, - ExecutionTrace& trace, + ExecutionTrace& trace, ExecutionTraceStorage* traceStorage) const { trace.setLeaf(key_); return dynamic_cast(values.at(key_)); @@ -454,8 +456,9 @@ struct Jacobian { /// meta-function to generate JacobianTA optional reference template -struct OptionalJacobian { - typedef FixedRef::value, traits::dimension::value> type; +struct MakeOptionalJacobian { + typedef OptionalJacobian::value, + traits::dimension::value> type; }; /** @@ -556,7 +559,7 @@ struct GenerateFunctionalNode: Argument, Base { }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, + void trace(const Values& values, Record* record, ExecutionTraceStorage*& traceStorage) const { Base::trace(values, record, traceStorage); // recurse // Write an Expression execution trace in record->trace @@ -632,7 +635,7 @@ struct FunctionalNode { }; /// Construct an execution trace for reverse AD - Record* trace(const Values& values, + Record* trace(const Values& values, ExecutionTraceStorage* traceStorage) const { assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); @@ -657,7 +660,8 @@ class UnaryExpression: public FunctionalNode >::type { public: - typedef boost::function::type)> Function; + typedef boost::function< + T(const A1&, typename MakeOptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -702,8 +706,8 @@ class BinaryExpression: public FunctionalNode >::t public: typedef boost::function< - T(const A1&, const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; + T(const A1&, const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -756,9 +760,10 @@ class TernaryExpression: public FunctionalNode public: typedef boost::function< - T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, - typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; + T(const A1&, const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -774,7 +779,8 @@ private: this->template reset(e2.root()); this->template reset(e3.root()); ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + e3.traceSize(); + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + + e3.traceSize(); } friend class Expression ; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 940ad1778..71bd16b1c 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -20,8 +20,8 @@ #pragma once #include -#include #include +#include #include #include @@ -75,7 +75,7 @@ public: /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename OptionalJacobian::type) const) : + T (A::*method)(typename MakeOptionalJacobian::type) const) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -89,8 +89,8 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type) const, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), @@ -224,8 +224,8 @@ template struct apply_compose { typedef T result_type; static const int Dim = traits::dimension::value; - T operator()(const T& x, const T& y, FixedRef H1 = boost::none, - FixedRef H2 = boost::none) const { + T operator()(const T& x, const T& y, OptionalJacobian H1 = + boost::none, OptionalJacobian H2 = boost::none) const { return x.compose(y, H1, H2); } }; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 9fd4f8383..d7713d0d5 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -20,14 +20,14 @@ #pragma once -#include -#include - #include #include #include #include +#include +#include +#include /** * Macro to add a standard clone function to a derived factor diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index b33d0e5cd..1ea6236e8 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -34,8 +34,8 @@ using namespace gtsam; /* ************************************************************************* */ template -Point2 uncalibrate(const CAL& K, const Point2& p, FixedRef<2, 5> Dcal, - FixedRef<2, 2> Dp) { +Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -75,13 +75,13 @@ TEST(Expression, Leaves) { /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f0(const Point3& p, FixedRef<2,3> H) { +Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { return Point2(); } -LieScalar f1(const Point3& p, FixedRef<1, 3> H) { +LieScalar f1(const Point3& p, OptionalJacobian<1, 3> H) { return LieScalar(0.0); } -double f2(const Point3& p, FixedRef<1, 3> H) { +double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } Expression p(1); @@ -134,7 +134,7 @@ TEST(Expression, NullaryMethod) { namespace binary { // Create leaves double doubleF(const Pose3& pose, // - const Point3& point, FixedRef<1, 6> H1, FixedRef<1, 3> H2) { + const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) { return 0.0; } Expression x(1); @@ -243,7 +243,7 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - FixedRef<3, 3> H1, FixedRef<3, 3> H2, FixedRef<3, 3> H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam_unstable/nonlinear/AdaptAutoDiff.h b/gtsam_unstable/nonlinear/AdaptAutoDiff.h index 70ba285c1..5a4b5a09a 100644 --- a/gtsam_unstable/nonlinear/AdaptAutoDiff.h +++ b/gtsam_unstable/nonlinear/AdaptAutoDiff.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { @@ -50,8 +51,8 @@ class AdaptAutoDiff { public: - T operator()(const A1& a1, const A2& a2, FixedRef H1 = boost::none, - FixedRef H2 = boost::none) { + T operator()(const A1& a1, const A2& a2, OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) { using ceres::internal::AutoDiff; diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp index 6afeac15b..eda90e203 100644 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp @@ -60,7 +60,7 @@ public: } /// Given coefficients c, predict value for x - double operator()(const Coefficients& c, FixedRef<1, N> H) { + double operator()(const Coefficients& c, OptionalJacobian<1, N> H) { if (H) (*H) = H_; return H_ * c; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index d37f1f80a..a412b47eb 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -126,7 +126,7 @@ TEST(ExpressionFactor, Unary) { /* ************************************************************************* */ static Point2 myUncal(const Cal3_S2& K, const Point2& p, - FixedRef<2,5> Dcal, FixedRef<2,2> Dp) { + OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -392,7 +392,7 @@ TEST(ExpressionFactor, compose3) { /* ************************************************************************* */ // Test compose with three arguments Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - FixedRef<3, 3> H1, FixedRef<3, 3> H2, FixedRef<3, 3> H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index 178b1803c..45e294c3d 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -201,7 +201,7 @@ TEST(ExpressionFactor, InvokeDerivatives) { // Now, let's create the optional Jacobian arguments typedef Point3 T; typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; + typedef boost::mpl::transform >::type Optionals; // Unfortunately this is moot: we need a pointer to a function with the // optional derivatives; I don't see a way of calling a function that we @@ -215,8 +215,8 @@ struct proxy { return pose.transform_to(point); } Point3 operator()(const Pose3& pose, const Point3& point, - FixedRef<3,6> Dpose, - FixedRef<3,3> Dpoint) const { + OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { return pose.transform_to(point, Dpose, Dpoint); } }; diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h index c72a9d3f7..031baea3d 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -20,8 +20,8 @@ typedef Expression Rot2_; typedef Expression Pose2_; Point2_ transform_to(const Pose2_& x, const Point2_& p) { - Point2 (Pose2::*transform)(const Point2& p, FixedRef<2, 3> H1, - FixedRef<2, 2> H2) const = &Pose2::transform_to; + Point2 (Pose2::*transform)(const Point2& p, OptionalJacobian<2, 3> H1, + OptionalJacobian<2, 2> H2) const = &Pose2::transform_to; return Point2_(x, transform, p); } @@ -34,8 +34,8 @@ typedef Expression Pose3_; Point3_ transform_to(const Pose3_& x, const Point3_& p) { - Point3 (Pose3::*transform)(const Point3& p, FixedRef<3, 6> Dpose, - FixedRef<3, 3> Dpoint) const = &Pose3::transform_to; + Point3 (Pose3::*transform)(const Point3& p, OptionalJacobian<3, 6> Dpose, + OptionalJacobian<3, 3> Dpoint) const = &Pose3::transform_to; return Point3_(x, transform, p); } @@ -49,7 +49,7 @@ Point2_ project(const Point3_& p_cam) { } Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, - FixedRef<2, 6> Dpose, FixedRef<2, 3> Dpoint, FixedRef<2, 5> Dcal) { + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); }