diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index e9addeec7..e4606c243 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -38,6 +38,9 @@ struct TestBinaryExpression; namespace MPL = boost::mpl::placeholders; +class ExpressionFactorBinaryTest; +// Forward declare for testing + namespace gtsam { template @@ -421,84 +424,6 @@ public: }; -//----------------------------------------------------------------------------- -/// meta-function to generate fixed-size JacobianTA type -template -struct Jacobian { - typedef Eigen::Matrix type; - typedef boost::optional optional; -}; - -/** - * Building block for Recursive Record Class - * Records the evaluation of a single argument in a functional expression - * The integer argument N is to guarantee a unique type signature, - * so we are guaranteed to be able to extract their values by static cast. - */ -template -struct JacobianTrace { - ExecutionTrace trace; - typename Jacobian::type dTdA; -}; - -/** - * Recursive Record Class for Functional Expressions - */ -template -struct GenerateRecord: JacobianTrace, Base { - - typedef T return_type; - static size_t const N = Base::N + 1; - typedef JacobianTrace This; - - /// Print to std::cout - virtual void print(const std::string& indent) const { - Base::print(indent); - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); - } - - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - Base::startReverseAD(jacobians); - Select::reverseAD(This::trace, This::dTdA, jacobians); - } - - /// Given df/dT, multiply in dT/dA and continue reverse AD process - virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { - Base::reverseAD(dFdT, jacobians); - This::trace.reverseAD(dFdT * This::dTdA, jacobians); - } - - /// Version specialized to 2-dimensional output - typedef Eigen::Matrix Jacobian2T; - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - Base::reverseAD2(dFdT, jacobians); - This::trace.reverseAD2(dFdT * This::dTdA, jacobians); - } -}; - -/// Recursive Record class Generator -template -struct Record: public boost::mpl::fold, - GenerateRecord >::type { - - /// Access Trace - template - ExecutionTrace& trace() { - return static_cast&>(*this).trace; - } - - /// Access Jacobian - template - typename Jacobian::type& jacobian() { - return static_cast&>(*this).dTdA; - } - -}; - //----------------------------------------------------------------------------- // Below we use the "Class Composition" technique described in the book // C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost @@ -530,6 +455,13 @@ struct Record: public boost::mpl::fold, // by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode //----------------------------------------------------------------------------- +/// meta-function to generate fixed-size JacobianTA type +template +struct Jacobian { + typedef Eigen::Matrix type; + typedef boost::optional optional; +}; + /** * Base case for recursive FunctionalNode class */ @@ -537,10 +469,10 @@ template struct FunctionalBase: ExpressionNode { static size_t const N = 0; // number of arguments - typedef CallRecord Record2; + typedef CallRecord Record; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record2* record, char*& raw) const { + void trace(const Values& values, Record* record, char*& raw) const { } }; @@ -555,6 +487,16 @@ struct Argument { boost::shared_ptr > expression; }; +/** + * Building block for Recursive Record Class + * Records the evaluation of a single argument in a functional expression + */ +template +struct JacobianTrace { + ExecutionTrace trace; + typename Jacobian::type dTdA; +}; + /** * Recursive Definition of Functional ExpressionNode */ @@ -572,17 +514,15 @@ struct GenerateFunctionalNode: Argument, Base { return keys; } - /** - * Recursive Record Class for Functional Expressions - */ - struct Record2: JacobianTrace, Base::Record2 { + /// Recursive Record Class for Functional Expressions + struct Record: JacobianTrace, Base::Record { typedef T return_type; typedef JacobianTrace This; /// Print to std::cout virtual void print(const std::string& indent) const { - Base::Record2::print(indent); + Base::Record::print(indent); static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); std::cout << This::dTdA.format(matlab) << std::endl; This::trace.print(indent); @@ -590,13 +530,13 @@ struct GenerateFunctionalNode: Argument, Base { /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { - Base::Record2::startReverseAD(jacobians); + Base::Record::startReverseAD(jacobians); Select::reverseAD(This::trace, This::dTdA, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { - Base::Record2::reverseAD(dFdT, jacobians); + Base::Record::reverseAD(dFdT, jacobians); This::trace.reverseAD(dFdT * This::dTdA, jacobians); } @@ -604,15 +544,16 @@ struct GenerateFunctionalNode: Argument, Base { typedef Eigen::Matrix Jacobian2T; virtual void reverseAD2(const Jacobian2T& dFdT, JacobianMap& jacobians) const { - Base::Record2::reverseAD2(dFdT, jacobians); + Base::Record::reverseAD2(dFdT, jacobians); This::trace.reverseAD2(dFdT * This::dTdA, jacobians); } }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record2* record, char*& raw) const { + void trace(const Values& values, Record* record, char*& raw) const { Base::trace(values, record, raw); - A a = This::expression->traceExecution(values, record->Record2::This::trace, raw); + A a = This::expression->traceExecution(values, record->Record::This::trace, + raw); raw = raw + This::expression->traceSize(); } }; @@ -639,10 +580,27 @@ struct FunctionalNode { return static_cast const &>(*this).expression; } + /// Provide convenience access to Record storage + struct Record: public Base::Record { + + /// Access Trace + template + ExecutionTrace& trace() { + return static_cast&>(*this).trace; + } + + /// Access Jacobian + template + typename Jacobian::type& jacobian() { + return static_cast&>(*this).dTdA; + } + + }; + /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { - typename Base::Record2* record = new (raw) typename Base::Record2(); + Record* record = new (raw) Record(); trace.setFunction(record); raw = (char*) (record + 1); @@ -658,12 +616,11 @@ struct FunctionalNode { template class UnaryExpression: public FunctionalNode >::type { - /// The automatically generated Base class - typedef FunctionalNode > Base; - public: typedef boost::function::optional)> Function; + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; private: @@ -694,10 +651,6 @@ public: return Augmented(t, dTdA1, a1.jacobians()); } - /// CallRecord structure for reverse AD - typedef boost::mpl::vector Arguments; - typedef Record Record; - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { @@ -724,6 +677,8 @@ public: typedef boost::function< T(const A1&, const A2&, typename Jacobian::optional, typename Jacobian::optional)> Function; + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; private: @@ -740,7 +695,7 @@ private: } friend class Expression ; - friend struct ::TestBinaryExpression; + friend class ::ExpressionFactorBinaryTest; public: @@ -765,10 +720,6 @@ public: return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians()); } - /// CallRecord structure for reverse AD - typedef boost::mpl::vector Arguments; - typedef Record Record; - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { @@ -801,6 +752,8 @@ public: T(const A1&, const A2&, const A3&, typename Jacobian::optional, typename Jacobian::optional, typename Jacobian::optional)> Function; + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; private: @@ -847,10 +800,6 @@ public: a3.jacobians()); } - /// CallRecord structure for reverse AD - typedef boost::mpl::vector Arguments; - typedef Record Record; - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index c92853d33..25dd35218 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -48,7 +48,7 @@ Point2_ p(2); /* ************************************************************************* */ // Leaf -TEST(ExpressionFactor, leaf) { +TEST(ExpressionFactor, Leaf) { using namespace leaf; // Create old-style factor to create expected value and derivatives @@ -64,7 +64,7 @@ TEST(ExpressionFactor, leaf) { /* ************************************************************************* */ // non-zero noise model -TEST(ExpressionFactor, model) { +TEST(ExpressionFactor, Model) { using namespace leaf; SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); @@ -82,7 +82,7 @@ TEST(ExpressionFactor, model) { /* ************************************************************************* */ // Constrained noise model -TEST(ExpressionFactor, constrained) { +TEST(ExpressionFactor, Constrained) { using namespace leaf; SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); @@ -100,7 +100,7 @@ TEST(ExpressionFactor, constrained) { /* ************************************************************************* */ // Unary(Leaf)) -TEST(ExpressionFactor, unary) { +TEST(ExpressionFactor, Unary) { // Create some values Values values; @@ -121,25 +121,21 @@ TEST(ExpressionFactor, unary) { boost::dynamic_pointer_cast(gf); EXPECT( assert_equal(expected, *jf, 1e-9)); } + /* ************************************************************************* */ -struct TestBinaryExpression { - static Point2 myUncal(const Cal3_S2& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { - return K.uncalibrate(p, Dcal, Dp); - } - Cal3_S2_ K_; - Point2_ p_; - BinaryExpression binary_; - TestBinaryExpression() : - K_(1), p_(2), binary_(myUncal, K_, p_) { - } -}; -/* ************************************************************************* */ +static Point2 myUncal(const Cal3_S2& K, const Point2& p, + boost::optional Dcal, boost::optional Dp) { + return K.uncalibrate(p, Dcal, Dp); +} + // Binary(Leaf,Leaf) -TEST(ExpressionFactor, binary) { +TEST(ExpressionFactor, Binary) { typedef BinaryExpression Binary; - TestBinaryExpression tester; + + Cal3_S2_ K_(1); + Point2_ p_(2); + Binary binary(myUncal, K_, p_); // Create some values Values values; @@ -156,14 +152,14 @@ TEST(ExpressionFactor, binary) { EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record)); // Check size - size_t size = tester.binary_.traceSize(); + size_t size = binary.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedRecordSize, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla char raw[size]; ExecutionTrace trace; - Point2 value = tester.binary_.traceExecution(values, trace, raw); + Point2 value = binary.traceExecution(values, trace, raw); // trace.print(); // Expected Jacobians @@ -181,7 +177,7 @@ TEST(ExpressionFactor, binary) { } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) -TEST(ExpressionFactor, shallow) { +TEST(ExpressionFactor, Shallow) { // Create some values Values values; @@ -434,27 +430,9 @@ namespace mpl = boost::mpl; template struct Incomplete; typedef mpl::vector MyTypes; -typedef Record Generated; +typedef FunctionalNode::type Generated; //Incomplete incomplete; -//BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); -BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Jacobian2T >)); - -Generated generated; - -typedef mpl::vector1 OneType; -typedef mpl::pop_front::type Empty; -typedef mpl::pop_front::type Bad; -//typedef ProtoTrace UnaryTrace; -//BOOST_MPL_ASSERT((boost::is_same< UnaryTrace::A, Point3 >)); - -#include -#include -#include -//#include - -typedef struct { -} Expected0; -BOOST_MPL_ASSERT((boost::is_same< Expected0, Expected0 >)); +BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Record::Jacobian2T >)); /* ************************************************************************* */ int main() {