diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 29c6def8f..85920735a 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -27,6 +27,17 @@ #include // for placement new struct TestBinaryExpression; +// template meta-programming headers +#include +#include +#include +#include +#include +#include +#include + +namespace MPL = boost::mpl::placeholders; + namespace gtsam { template @@ -44,12 +55,16 @@ typedef std::map JacobianMap; */ template struct CallRecord { - virtual void print(const std::string& indent) const = 0; - virtual void startReverseAD(JacobianMap& jacobians) const = 0; - virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; + virtual void print(const std::string& indent) const { + } + virtual void startReverseAD(JacobianMap& jacobians) const { + } + virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + } typedef Eigen::Matrix Jacobian2T; virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const = 0; + JacobianMap& jacobians) const { + } }; //----------------------------------------------------------------------------- @@ -162,6 +177,84 @@ struct Select<2, A> { } }; +//----------------------------------------------------------------------------- +/** + * Record the evaluation of a single argument in a functional expression + * Building block for Recursive Record Class + */ +template +struct Argument { + typedef Eigen::Matrix JacobianTA; + ExecutionTrace trace; + JacobianTA dTdA; +}; + +/** + * Recursive Record Class for Functional Expressions + * Abrahams, David; Gurtovoy, Aleksey (2004-12-10). + * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost + * and Beyond. Pearson Education. + */ +template +struct Record: Argument, More { + + typedef typename AN::type A; + const static size_t N = AN::value; + + ExecutionTrace const & myTrace() const { + return static_cast*>(this)->trace; + } + + typedef Eigen::Matrix JacobianTA; + const JacobianTA& myJacobian() const { + return static_cast*>(this)->dTdA; + } + + /// Print to std::cout + virtual void print(const std::string& indent) const { + More::print(indent); + static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << myJacobian().format(matlab) << std::endl; + myTrace().print(indent); + } + + /// Start the reverse AD process + virtual void startReverseAD(JacobianMap& jacobians) const { + More::startReverseAD(jacobians); + Select::reverseAD(myTrace(), myJacobian(), jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + More::reverseAD(dFdT, jacobians); + myTrace().reverseAD(dFdT * myJacobian(), jacobians); + } + + /// Version specialized to 2-dimensional output + typedef Eigen::Matrix Jacobian2T; + virtual void reverseAD2(const Jacobian2T& dFdT, + JacobianMap& jacobians) const { + More::reverseAD2(dFdT, jacobians); + myTrace().reverseAD2(dFdT * myJacobian(), jacobians); + } +}; + +/// Meta-function for generating a numbered type +template +struct Numbered { + typedef A type; + typedef size_t value_type; + static const size_t value = N; +}; + +/// Recursive Record class Generator +template +struct GenerateRecord { + typedef typename boost::mpl::fold, + Record >::type type; + +}; + //----------------------------------------------------------------------------- /** * Value and Jacobians @@ -257,7 +350,7 @@ public: } /// debugging - virtual void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) { + void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) { BOOST_FOREACH(const Pair& term, jacobians_) std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows() << "x" << term.second.cols() << ") "; @@ -443,31 +536,9 @@ public: return Augmented(t, dTdA, argument.jacobians()); } - /// Record structure for reverse AD - struct Record: public CallRecord { - ExecutionTrace trace1; - JacobianTA dTdA1; - /// print to std::cout - virtual void print(const std::string& indent) const { - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << dTdA1.format(matlab) << std::endl; - trace1.print(indent); - } - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - Select::reverseAD(trace1, dTdA1, 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 * dTdA1, jacobians); - } - /// Version specialized to 2-dimensional output - typedef Eigen::Matrix Jacobian2T; - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - trace1.reverseAD2(dFdT * dTdA1, jacobians); - } - }; + /// CallRecord structure for reverse AD + typedef boost::mpl::vector > Arguments; + typedef typename GenerateRecord::type Record; // Return size needed for memory buffer in traceExecution virtual size_t traceSize() const { @@ -479,9 +550,12 @@ public: char* raw) const { Record* record = new (raw) Record(); trace.setFunction(record); + raw = (char*) (record + 1); - A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); - return function_(a1, record->dTdA1); + A1 a1 = this->expressionA1_->traceExecution(values, + static_cast*>(record)->trace, raw); + + return function_(a1, static_cast*>(record)->dTdA); } }; @@ -544,38 +618,9 @@ public: return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians()); } - /// Record structure for reverse AD - struct Record: public CallRecord { - ExecutionTrace trace1; - ExecutionTrace trace2; - JacobianTA1 dTdA1; - JacobianTA2 dTdA2; - /// print to std::cout - virtual void print(const std::string& indent) const { - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << indent << dTdA1.format(matlab) << std::endl; - trace1.print(indent); - std::cout << indent << dTdA2.format(matlab) << std::endl; - trace2.print(indent); - } - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - Select::reverseAD(trace1, dTdA1, jacobians); - Select::reverseAD(trace2, dTdA2, 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 * dTdA1, jacobians); - trace2.reverseAD(dFdT * dTdA2, jacobians); - } - /// Version specialized to 2-dimensional output - typedef Eigen::Matrix Jacobian2T; - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - trace1.reverseAD2(dFdT * dTdA1, jacobians); - trace2.reverseAD2(dFdT * dTdA2, jacobians); - } - }; + /// CallRecord structure for reverse AD + typedef boost::mpl::vector, Numbered > Arguments; + typedef typename GenerateRecord::type Record; // Return size needed for memory buffer in traceExecution virtual size_t traceSize() const { @@ -589,11 +634,17 @@ public: char* raw) const { Record* record = new (raw) Record(); trace.setFunction(record); + raw = (char*) (record + 1); - A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); + A1 a1 = this->expressionA1_->traceExecution(values, + static_cast*>(record)->trace, raw); + raw = raw + expressionA1_->traceSize(); - A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw); - return function_(a1, a2, record->dTdA1, record->dTdA2); + A2 a2 = this->expressionA2_->traceExecution(values, + static_cast*>(record)->trace, raw); + + return function_(a1, a2, static_cast*>(record)->dTdA, + static_cast*>(record)->dTdA); } }; @@ -668,45 +719,9 @@ public: a3.jacobians()); } - /// Record structure for reverse AD - struct Record: public CallRecord { - ExecutionTrace trace1; - ExecutionTrace trace2; - ExecutionTrace trace3; - JacobianTA1 dTdA1; - JacobianTA2 dTdA2; - JacobianTA3 dTdA3; - /// print to std::cout - virtual void print(const std::string& indent) const { - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << dTdA1.format(matlab) << std::endl; - trace1.print(indent); - std::cout << dTdA2.format(matlab) << std::endl; - trace2.print(indent); - std::cout << dTdA3.format(matlab) << std::endl; - trace3.print(indent); - } - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - Select::reverseAD(trace1, dTdA1, jacobians); - Select::reverseAD(trace2, dTdA2, jacobians); - Select::reverseAD(trace3, dTdA3, 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 * dTdA1, jacobians); - trace2.reverseAD(dFdT * dTdA2, jacobians); - trace3.reverseAD(dFdT * dTdA3, jacobians); - } - /// Version specialized to 2-dimensional output - typedef Eigen::Matrix Jacobian2T; - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - trace1.reverseAD2(dFdT * dTdA1, jacobians); - trace2.reverseAD2(dFdT * dTdA2, jacobians); - trace3.reverseAD2(dFdT * dTdA3, jacobians); - } - }; + /// CallRecord structure for reverse AD + typedef boost::mpl::vector, Numbered, Numbered > Arguments; + typedef typename GenerateRecord::type Record; // Return size needed for memory buffer in traceExecution virtual size_t traceSize() const { @@ -719,13 +734,22 @@ public: char* raw) const { Record* record = new (raw) Record(); trace.setFunction(record); + raw = (char*) (record + 1); - A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); + A1 a1 = this->expressionA1_->traceExecution(values, + static_cast*>(record)->trace, raw); + raw = raw + expressionA1_->traceSize(); - A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw); + A2 a2 = this->expressionA2_->traceExecution(values, + static_cast*>(record)->trace, raw); + raw = raw + expressionA2_->traceSize(); - A3 a3 = this->expressionA3_->traceExecution(values, record->trace3, raw); - return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); + A3 a3 = this->expressionA3_->traceExecution(values, + static_cast*>(record)->trace, raw); + + return function_(a1, a2, a3, static_cast*>(record)->dTdA, + static_cast*>(record)->dTdA, + static_cast*>(record)->dTdA); } }; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index a6dbe842b..b6bef00b8 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -158,10 +158,12 @@ TEST(ExpressionFactor, binary) { expected22 << 1, 0, 0, 1; // Check matrices - boost::optional p = trace.record(); - CHECK(p); - EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); - EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); + boost::optional r = trace.record(); + CHECK(r); + EXPECT( + assert_equal(expected25, (Matrix) static_cast*> (*r)->dTdA, 1e-9)); + EXPECT( + assert_equal(expected22, (Matrix) static_cast*> (*r)->dTdA, 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) @@ -205,9 +207,10 @@ TEST(ExpressionFactor, shallow) { expected23 << 1, 0, 0, 0, 1, 0; // Check matrices - boost::optional p = trace.record(); - CHECK(p); - EXPECT( assert_equal(expected23, (Matrix)(*p)->dTdA1, 1e-9)); + boost::optional r = trace.record(); + CHECK(r); + EXPECT( + assert_equal(expected23, (Matrix)static_cast*>(*r)->dTdA, 1e-9)); // Linearization ExpressionFactor f2(model, measured, expression); @@ -401,6 +404,37 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } +/* ************************************************************************* */ + +namespace mpl = boost::mpl; + +#include +template struct Incomplete; + +typedef mpl::vector, Numbered, + Numbered > MyTypes; +typedef GenerateRecord::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 >)); + /* ************************************************************************* */ int main() { TestResult tr;