diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 869bff2ea..2dcf5b43f 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -134,16 +135,23 @@ public: }; //----------------------------------------------------------------------------- -template struct JacobianTrace { - T t; - T value() const { - return t; - } virtual void reverseAD(JacobianMap& jacobians) const = 0; virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; +// template +// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const { }; +typedef boost::shared_ptr TracePtr; + +//template +//struct TypedTrace { +// virtual void reverseAD(JacobianMap& jacobians) const = 0; +// virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; +//// template +//// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const { +//}; + //----------------------------------------------------------------------------- /** * Expression node. The superclass for objects that do the heavy lifting @@ -175,8 +183,7 @@ public: virtual Augmented forward(const Values& values) const = 0; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const = 0; + virtual std::pair traceExecution(const Values& values) const = 0; }; //----------------------------------------------------------------------------- @@ -217,7 +224,7 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { + struct Trace: public JacobianTrace { /// If the expression is just a constant, we do nothing virtual void reverseAD(JacobianMap& jacobians) const { } @@ -227,11 +234,9 @@ public: }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { boost::shared_ptr trace = boost::make_shared(); - trace->t = constant_; - return trace; + return std::make_pair(constant_, trace); } }; @@ -270,12 +275,11 @@ public: /// Return value and derivatives virtual Augmented forward(const Values& values) const { - T t = value(values); - return Augmented(t, key_); + return Augmented(values.at(key_), key_); } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { + struct Trace: public JacobianTrace { Key key; /// If the expression is just a leaf, we just insert an identity matrix virtual void reverseAD(JacobianMap& jacobians) const { @@ -293,12 +297,10 @@ public: }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { boost::shared_ptr trace = boost::make_shared(); - trace->t = value(values); trace->key = key_; - return trace; + return std::make_pair(values.at(key_), trace); } }; @@ -352,26 +354,25 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { - boost::shared_ptr > trace1; + struct Trace: public JacobianTrace { + TracePtr trace; JacobianTA dTdA; /// Start the reverse AD process virtual void reverseAD(JacobianMap& jacobians) const { - trace1->reverseAD(dTdA, jacobians); + trace->reverseAD(dTdA, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { - trace1->reverseAD(dFdT * dTdA, jacobians); + trace->reverseAD(dFdT * dTdA, jacobians); } }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { + A a; boost::shared_ptr trace = boost::make_shared(); - trace->trace1 = this->expressionA_->traceExecution(values); - trace->t = function_(trace->trace1->value(), trace->dTdA); - return trace; + boost::tie(a, trace->trace) = this->expressionA_->traceExecution(values); + return std::make_pair(function_(a, trace->dTdA),trace); } }; @@ -438,9 +439,8 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { - boost::shared_ptr > trace1; - boost::shared_ptr > trace2; + struct Trace: public JacobianTrace { + TracePtr trace1, trace2; JacobianTA1 dTdA1; JacobianTA2 dTdA2; /// Start the reverse AD process @@ -456,14 +456,13 @@ public: }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { + A1 a1; + A2 a2; boost::shared_ptr trace = boost::make_shared(); - trace->trace1 = this->expressionA1_->traceExecution(values); - trace->trace2 = this->expressionA2_->traceExecution(values); - trace->t = function_(trace->trace1->value(), trace->trace2->value(), - trace->dTdA1, trace->dTdA2); - return trace; + boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values); + boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values); + return std::make_pair(function_(a1, a2, trace->dTdA1, trace->dTdA2), trace); } }; @@ -543,10 +542,10 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { - boost::shared_ptr > trace1; - boost::shared_ptr > trace2; - boost::shared_ptr > trace3; + struct Trace: public JacobianTrace { + TracePtr trace1; + TracePtr trace2; + TracePtr trace3; JacobianTA1 dTdA1; JacobianTA2 dTdA2; JacobianTA3 dTdA3; @@ -565,15 +564,16 @@ public: }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { + A1 a1; + A2 a2; + A3 a3; boost::shared_ptr trace = boost::make_shared(); - trace->trace1 = this->expressionA1_->traceExecution(values); - trace->trace2 = this->expressionA2_->traceExecution(values); - trace->trace3 = this->expressionA3_->traceExecution(values); - trace->t = function_(trace->trace1->value(), trace->trace2->value(), - trace->trace3->value(), trace->dTdA1, trace->dTdA2, trace->dTdA3); - return trace; + boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values); + boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values); + boost::tie(a3, trace->trace3) = this->expressionA3_->traceExecution(values); + return std::make_pair( + function_(a1, a2, a3, trace->dTdA1, trace->dTdA2, trace->dTdA3), trace); } }; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 1c16fab25..968a0f814 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -117,8 +117,10 @@ public: Augmented augmented(const Values& values) const { #define REVERSE_AD #ifdef REVERSE_AD - boost::shared_ptr > trace(root_->traceExecution(values)); - Augmented augmented(trace->value()); + T value; + TracePtr trace; + boost::tie(value,trace) = root_->traceExecution(values); + Augmented augmented(value); trace->reverseAD(augmented.jacobians()); return augmented; #else diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp index 0eb806c98..44a7eab3f 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -124,7 +124,7 @@ TEST(BADFactor, test2) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - TernaryExpression::Function fff = project6; + TernaryExpression::Function fff = project6; // Try ternary version BADFactor f3(model, measured, project3(x, p, K)); diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 5532b0da3..188394e0a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -32,12 +32,12 @@ using namespace gtsam; /* ************************************************************************* */ template -Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, - boost::optional Dp) { +Point2 uncalibrate(const CAL& K, const Point2& p, + boost::optional Dcal, boost::optional Dp) { return K.uncalibrate(p, Dcal, Dp); } -static const Rot3 someR = Rot3::RzRyRx(1,2,3); +static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ @@ -55,7 +55,7 @@ TEST(Expression, constant) { TEST(Expression, leaf) { Expression R(100); Values values; - values.insert(100,someR); + values.insert(100, someR); Augmented a = R.augmented(values); EXPECT(assert_equal(someR, a.value())); JacobianMap expected; @@ -76,7 +76,6 @@ TEST(Expression, leaf) { // expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50)); // EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); //} - /* ************************************************************************* */ TEST(Expression, test) { @@ -149,8 +148,8 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + boost::optional H1, boost::optional H2, + boost::optional H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3);