From 8e264f4289f70440c7382e90453a4c0c1e89c831 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 9 Oct 2014 14:38:16 +0200 Subject: [PATCH 01/10] Attempt at defining Trace recursively --- gtsam_unstable/nonlinear/Expression-inl.h | 50 ++++++++++++++--------- 1 file changed, 31 insertions(+), 19 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index c97a903eb..b7038fc4c 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -378,6 +378,36 @@ public: }; +//----------------------------------------------------------------------------- + +#include + +/// Recursive Trace Class +template +struct MakeTrace: public JacobianTrace { + typedef boost::mpl::front A1; + static const size_t dimA = A1::dimension; + typedef Eigen::Matrix JacobianTA; + typedef Eigen::Matrix Jacobian2T; + + typename JacobianTrace::Pointer trace1; + JacobianTA dTdA1; + + /// 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 + virtual void reverseAD2(const Jacobian2T& dFdT, + JacobianMap& jacobians) const { + trace1.reverseAD2(dFdT * dTdA1, jacobians); + } +}; + //----------------------------------------------------------------------------- /// Unary Function Expression template @@ -423,25 +453,7 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { - typename JacobianTrace::Pointer trace1; - JacobianTA dTdA1; - - /// 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); - } - }; + typedef MakeTrace > Trace; /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, From 732ff54b83c72549fc92e4f43e006af9352a2f78 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 10 Oct 2014 11:41:01 +0200 Subject: [PATCH 02/10] More experiments --- gtsam_unstable/nonlinear/Expression-inl.h | 40 +++++++--- .../nonlinear/tests/testExpressionFactor.cpp | 74 +++++++++++++++++++ 2 files changed, 102 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index b7038fc4c..53f531149 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -24,6 +24,13 @@ #include #include +// template meta-programming headers +#include +#include +#include +#include +#include + namespace gtsam { template @@ -380,31 +387,40 @@ public: //----------------------------------------------------------------------------- -#include +// Abrahams, David; Gurtovoy, Aleksey (2004-12-10). +// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost +// and Beyond (Kindle Location 1244). Pearson Education. /// Recursive Trace Class template struct MakeTrace: public JacobianTrace { - typedef boost::mpl::front A1; - static const size_t dimA = A1::dimension; - typedef Eigen::Matrix JacobianTA; - typedef Eigen::Matrix Jacobian2T; - typename JacobianTrace::Pointer trace1; - JacobianTA dTdA1; + typedef typename boost::mpl::front::type A; + + // define dimensions + static const size_t m = T::dimension; + static const size_t n = A::dimension; + + // define fixed size Jacobian matrix types + typedef Eigen::Matrix JacobianTA; + typedef Eigen::Matrix Jacobian2T; + + // declare trace that produces value A, and corresponding Jacobian + typename JacobianTrace::Pointer trace; + JacobianTA dTdA; /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { - Select::reverseAD(trace1, dTdA1, jacobians); + Select::reverseAD(trace, 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 * dTdA1, jacobians); + trace.reverseAD(dFdT * dTdA, jacobians); } /// Version specialized to 2-dimensional output virtual void reverseAD2(const Jacobian2T& dFdT, JacobianMap& jacobians) const { - trace1.reverseAD2(dFdT * dTdA1, jacobians); + trace.reverseAD2(dFdT * dTdA, jacobians); } }; @@ -460,8 +476,8 @@ public: typename JacobianTrace::Pointer& p) const { Trace* trace = new Trace(); p.setFunction(trace); - A1 a = this->expressionA1_->traceExecution(values, trace->trace1); - return function_(a, trace->dTdA1); + A1 a = this->expressionA1_->traceExecution(values, trace->trace); + return function_(a, trace->dTdA); } }; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 87da6fde9..163139543 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -318,6 +318,80 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } +/* ************************************************************************* */ + +namespace mpl = boost::mpl; + +template +struct ProtoTrace: public ProtoTrace::type> { + + typedef typename mpl::front::type A; + +}; + +/// Recursive Trace Class, Base case +template<> +struct ProtoTrace > { +}; + +template<> +struct ProtoTrace { +}; + +/// Recursive Trace Class, Primary Template +template +struct store: More { + // define dimensions + static const size_t m = 3; + static const size_t n = A::dimension; + + // define fixed size Jacobian matrix types + typedef Eigen::Matrix JacobianTA; + typedef Eigen::Matrix Jacobian2T; + + // declare trace that produces value A, and corresponding Jacobian + typename JacobianTrace::Pointer trace; + JacobianTA dTdA; + +}; +typedef mpl::vector MyTypes; + +template struct Incomplete; + +#include +#include +#include +namespace MPL = mpl::placeholders; +typedef mpl::reverse_fold >::type Generated; +Generated generated; +//Incomplete incomplete; +#include + +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 +//#include + +BOOST_STATIC_ASSERT((mpl::plus,mpl::int_<3> >::type::value==5)); + +typedef mpl::vector0<> List0; +typedef ProtoTrace Proto0; +//typedef ProtoTrace > Proto0; +//typedef mpl::print::type Dbg; +//incomplete > proto0; + +typedef struct { +} Expected0; +BOOST_MPL_ASSERT((boost::is_same< Expected0, Expected0 >)); +//BOOST_MPL_ASSERT((boost::is_same< ProtoTrace, ProtoTrace >)); + /* ************************************************************************* */ int main() { TestResult tr; From dd1b931802a7f74925566e5e261a88fcf9285a53 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 10 Oct 2014 12:03:13 +0200 Subject: [PATCH 03/10] Successfully defined Jacobian --- .../nonlinear/tests/testExpressionFactor.cpp | 54 +++++++------------ 1 file changed, 18 insertions(+), 36 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 163139543..6441b78a6 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -322,27 +322,11 @@ TEST(ExpressionFactor, composeTernary) { namespace mpl = boost::mpl; -template -struct ProtoTrace: public ProtoTrace::type> { - - typedef typename mpl::front::type A; - -}; - -/// Recursive Trace Class, Base case -template<> -struct ProtoTrace > { -}; - -template<> -struct ProtoTrace { -}; - -/// Recursive Trace Class, Primary Template +/// Recursive Trace Class template -struct store: More { +struct Trace: More { // define dimensions - static const size_t m = 3; + static const size_t m = 2; static const size_t n = A::dimension; // define fixed size Jacobian matrix types @@ -354,18 +338,26 @@ struct store: More { JacobianTA dTdA; }; -typedef mpl::vector MyTypes; - -template struct Incomplete; #include #include -#include namespace MPL = mpl::placeholders; -typedef mpl::reverse_fold >::type Generated; -Generated generated; -//Incomplete incomplete; + +/// Recursive Trace Class Generator +template +struct GenerateTrace { + typedef typename mpl::fold >::type type; +}; + #include +template struct Incomplete; + +typedef mpl::vector MyTypes; +typedef GenerateTrace::type Generated; +Incomplete incomplete; +BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); + +Generated generated; typedef mpl::vector1 OneType; typedef mpl::pop_front::type Empty; @@ -376,21 +368,11 @@ typedef mpl::pop_front::type Bad; #include #include #include -#include //#include -BOOST_STATIC_ASSERT((mpl::plus,mpl::int_<3> >::type::value==5)); - -typedef mpl::vector0<> List0; -typedef ProtoTrace Proto0; -//typedef ProtoTrace > Proto0; -//typedef mpl::print::type Dbg; -//incomplete > proto0; - typedef struct { } Expected0; BOOST_MPL_ASSERT((boost::is_same< Expected0, Expected0 >)); -//BOOST_MPL_ASSERT((boost::is_same< ProtoTrace, ProtoTrace >)); /* ************************************************************************* */ int main() { From 40fc6f5c036eca356d9589362bcbf99f8477929f Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 10 Oct 2014 12:29:01 +0200 Subject: [PATCH 04/10] Working prototype --- .../nonlinear/tests/testExpressionFactor.cpp | 26 ++++++++++++++----- 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 6441b78a6..d3f80d2a6 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -323,10 +323,10 @@ TEST(ExpressionFactor, composeTernary) { namespace mpl = boost::mpl; /// Recursive Trace Class -template +template struct Trace: More { // define dimensions - static const size_t m = 2; + static const size_t m = T::dimension; static const size_t n = A::dimension; // define fixed size Jacobian matrix types @@ -337,6 +337,19 @@ struct Trace: More { typename JacobianTrace::Pointer trace; JacobianTA dTdA; + /// Start the reverse AD process + virtual void startReverseAD(JacobianMap& jacobians) const { + Select::reverseAD(trace, dTdA, jacobians); + } + /// Given df/dT, multiply in dT/dA and continue reverse AD process + virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + trace.reverseAD(dFdT * dTdA, jacobians); + } + /// Version specialized to 2-dimensional output + virtual void reverseAD2(const Jacobian2T& dFdT, + JacobianMap& jacobians) const { + trace.reverseAD2(dFdT * dTdA, jacobians); + } }; #include @@ -344,18 +357,19 @@ struct Trace: More { namespace MPL = mpl::placeholders; /// Recursive Trace Class Generator -template +template struct GenerateTrace { - typedef typename mpl::fold >::type type; + typedef typename mpl::fold, Trace >::type type; }; #include template struct Incomplete; typedef mpl::vector MyTypes; -typedef GenerateTrace::type Generated; -Incomplete incomplete; +typedef GenerateTrace::type Generated; +//Incomplete incomplete; BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); +BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Jacobian2T >)); Generated generated; From f8468bd59624906e4982c11c253b4f365f1ae395 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 10 Oct 2014 12:31:40 +0200 Subject: [PATCH 05/10] Recursion done --- gtsam_unstable/nonlinear/Expression-inl.h | 9 ++++++--- gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp | 3 +++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 53f531149..8b98b3b94 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -120,10 +120,13 @@ struct JacobianTrace { /// Make sure destructor is virtual virtual ~JacobianTrace() { } - virtual void startReverseAD(JacobianMap& jacobians) const = 0; - virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; + virtual void startReverseAD(JacobianMap& jacobians) const { + } + virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + } virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const = 0; + JacobianMap& jacobians) const { + } }; /// Primary template calls the generic Matrix reverseAD pipeline diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index d3f80d2a6..e5e5e41e6 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -339,15 +339,18 @@ struct Trace: More { /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { + More::startReverseAD(jacobians); Select::reverseAD(trace, dTdA, 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); trace.reverseAD(dFdT * dTdA, jacobians); } /// Version specialized to 2-dimensional output virtual void reverseAD2(const Jacobian2T& dFdT, JacobianMap& jacobians) const { + More::reverseAD2(dFdT, jacobians); trace.reverseAD2(dFdT * dTdA, jacobians); } }; From 24714e48c5bdc74385d4d5280c301c48e7017f78 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 10 Oct 2014 12:38:26 +0200 Subject: [PATCH 06/10] Works for Unary ! --- gtsam_unstable/nonlinear/Expression-inl.h | 96 ++++++++++--------- .../nonlinear/tests/testExpressionFactor.cpp | 43 --------- 2 files changed, 49 insertions(+), 90 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 8b98b3b94..4ffb9afcd 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -30,6 +30,9 @@ #include #include #include +#include +#include +namespace MPL = boost::mpl::placeholders; namespace gtsam { @@ -149,13 +152,50 @@ struct Select<2, A> { } }; -//template -//struct TypedTrace { -// virtual void startReverseAD(JacobianMap& jacobians) const = 0; -// virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; -//// template -//// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const { -//}; +/** + * Recursive Trace Class for Functional Expressions + * Abrahams, David; Gurtovoy, Aleksey (2004-12-10). + * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost + * and Beyond (Kindle Location 1244). Pearson Education. + */ +template +struct Trace: More { + // define dimensions + static const size_t m = T::dimension; + static const size_t n = A::dimension; + + // define fixed size Jacobian matrix types + typedef Eigen::Matrix JacobianTA; + typedef Eigen::Matrix Jacobian2T; + + // declare trace that produces value A, and corresponding Jacobian + typename JacobianTrace::Pointer trace; + JacobianTA dTdA; + + /// Start the reverse AD process + virtual void startReverseAD(JacobianMap& jacobians) const { + More::startReverseAD(jacobians); + Select::reverseAD(trace, dTdA, 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); + trace.reverseAD(dFdT * dTdA, jacobians); + } + /// Version specialized to 2-dimensional output + virtual void reverseAD2(const Jacobian2T& dFdT, + JacobianMap& jacobians) const { + More::reverseAD2(dFdT, jacobians); + trace.reverseAD2(dFdT * dTdA, jacobians); + } +}; + +/// Recursive Trace Class Generator +template +struct GenerateTrace { + typedef typename boost::mpl::fold, + Trace >::type type; +}; //----------------------------------------------------------------------------- /** @@ -388,45 +428,6 @@ public: }; -//----------------------------------------------------------------------------- - -// Abrahams, David; Gurtovoy, Aleksey (2004-12-10). -// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost -// and Beyond (Kindle Location 1244). Pearson Education. - -/// Recursive Trace Class -template -struct MakeTrace: public JacobianTrace { - - typedef typename boost::mpl::front::type A; - - // define dimensions - static const size_t m = T::dimension; - static const size_t n = A::dimension; - - // define fixed size Jacobian matrix types - typedef Eigen::Matrix JacobianTA; - typedef Eigen::Matrix Jacobian2T; - - // declare trace that produces value A, and corresponding Jacobian - typename JacobianTrace::Pointer trace; - JacobianTA dTdA; - - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - Select::reverseAD(trace, dTdA, jacobians); - } - /// Given df/dT, multiply in dT/dA and continue reverse AD process - virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { - trace.reverseAD(dFdT * dTdA, jacobians); - } - /// Version specialized to 2-dimensional output - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - trace.reverseAD2(dFdT * dTdA, jacobians); - } -}; - //----------------------------------------------------------------------------- /// Unary Function Expression template @@ -472,7 +473,8 @@ public: } /// Trace structure for reverse AD - typedef MakeTrace > Trace; + typedef boost::mpl::vector Arguments; + typedef typename GenerateTrace::type Trace; /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index e5e5e41e6..ccbc303f2 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -322,49 +322,6 @@ TEST(ExpressionFactor, composeTernary) { namespace mpl = boost::mpl; -/// Recursive Trace Class -template -struct Trace: More { - // define dimensions - static const size_t m = T::dimension; - static const size_t n = A::dimension; - - // define fixed size Jacobian matrix types - typedef Eigen::Matrix JacobianTA; - typedef Eigen::Matrix Jacobian2T; - - // declare trace that produces value A, and corresponding Jacobian - typename JacobianTrace::Pointer trace; - JacobianTA dTdA; - - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - More::startReverseAD(jacobians); - Select::reverseAD(trace, dTdA, 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); - trace.reverseAD(dFdT * dTdA, jacobians); - } - /// Version specialized to 2-dimensional output - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - More::reverseAD2(dFdT, jacobians); - trace.reverseAD2(dFdT * dTdA, jacobians); - } -}; - -#include -#include -namespace MPL = mpl::placeholders; - -/// Recursive Trace Class Generator -template -struct GenerateTrace { - typedef typename mpl::fold, Trace >::type type; -}; - #include template struct Incomplete; From 406467e341c938eb160a2607edc12592ad17501b Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 10 Oct 2014 13:29:56 +0200 Subject: [PATCH 07/10] Binary works, but it's ugly and does not work for repeated types --- gtsam_unstable/nonlinear/Expression-inl.h | 80 +++++++++++------------ 1 file changed, 37 insertions(+), 43 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 4ffb9afcd..45a932351 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -30,8 +30,9 @@ #include #include #include -#include -#include +#include +#include + namespace MPL = boost::mpl::placeholders; namespace gtsam { @@ -152,41 +153,52 @@ struct Select<2, A> { } }; +/** + * Record the evaluation of a single argument in a functional expression + */ +template +struct SingleTrace { + typedef Eigen::Matrix JacobianTA; + typename JacobianTrace::Pointer trace; + JacobianTA dTdA; +}; + /** * Recursive Trace Class for Functional Expressions * Abrahams, David; Gurtovoy, Aleksey (2004-12-10). * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost - * and Beyond (Kindle Location 1244). Pearson Education. + * and Beyond. Pearson Education. */ template -struct Trace: More { - // define dimensions - static const size_t m = T::dimension; - static const size_t n = A::dimension; +struct Trace: SingleTrace, More { - // define fixed size Jacobian matrix types - typedef Eigen::Matrix JacobianTA; - typedef Eigen::Matrix Jacobian2T; + typename JacobianTrace::Pointer const & myTrace() const { + return static_cast*>(this)->trace; + } - // declare trace that produces value A, and corresponding Jacobian - typename JacobianTrace::Pointer trace; - JacobianTA dTdA; + typedef Eigen::Matrix JacobianTA; + const JacobianTA& myJacobian() const { + return static_cast*>(this)->dTdA; + } /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { More::startReverseAD(jacobians); - Select::reverseAD(trace, dTdA, 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); - trace.reverseAD(dFdT * dTdA, 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); - trace.reverseAD2(dFdT * dTdA, jacobians); + myTrace().reverseAD2(dFdT * myJacobian(), jacobians); } }; @@ -195,6 +207,7 @@ template struct GenerateTrace { typedef typename boost::mpl::fold, Trace >::type type; + }; //----------------------------------------------------------------------------- @@ -545,39 +558,20 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { - typename JacobianTrace::Pointer trace1; - typename JacobianTrace::Pointer trace2; - JacobianTA1 dTdA1; - JacobianTA2 dTdA2; - - /// 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); - } - }; + typedef boost::mpl::vector Arguments; + typedef typename GenerateTrace::type Trace; /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, typename JacobianTrace::Pointer& p) const { Trace* trace = new Trace(); p.setFunction(trace); - A1 a1 = this->expressionA1_->traceExecution(values, trace->trace1); - A2 a2 = this->expressionA2_->traceExecution(values, trace->trace2); - return function_(a1, a2, trace->dTdA1, trace->dTdA2); + A1 a1 = this->expressionA1_->traceExecution(values, + static_cast*>(trace)->trace); + A2 a2 = this->expressionA2_->traceExecution(values, + static_cast*>(trace)->trace); + return function_(a1, a2, static_cast*>(trace)->dTdA, + static_cast*>(trace)->dTdA); } }; From 58bbce482d89b5fba05ade7e386ddaecc75f33b0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 10 Oct 2014 13:33:13 +0200 Subject: [PATCH 08/10] Ternary works, same caveat --- gtsam_unstable/nonlinear/Expression-inl.h | 44 ++++++----------------- 1 file changed, 11 insertions(+), 33 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 45a932351..87c07f976 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -647,45 +647,23 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { - typename JacobianTrace::Pointer trace1; - typename JacobianTrace::Pointer trace2; - typename JacobianTrace::Pointer trace3; - JacobianTA1 dTdA1; - JacobianTA2 dTdA2; - JacobianTA3 dTdA3; - - /// 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); - } - }; + typedef boost::mpl::vector Arguments; + typedef typename GenerateTrace::type Trace; /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, typename JacobianTrace::Pointer& p) const { Trace* trace = new Trace(); p.setFunction(trace); - A1 a1 = this->expressionA1_->traceExecution(values, trace->trace1); - A2 a2 = this->expressionA2_->traceExecution(values, trace->trace2); - A3 a3 = this->expressionA3_->traceExecution(values, trace->trace3); - return function_(a1, a2, a3, trace->dTdA1, trace->dTdA2, trace->dTdA3); + A1 a1 = this->expressionA1_->traceExecution(values, + static_cast*>(trace)->trace); + A2 a2 = this->expressionA2_->traceExecution(values, + static_cast*>(trace)->trace); + A3 a3 = this->expressionA3_->traceExecution(values, + static_cast*>(trace)->trace); + return function_(a1, a2, a3, static_cast*>(trace)->dTdA, + static_cast*>(trace)->dTdA, + static_cast*>(trace)->dTdA); } }; From ae93dd9869063403110625cfa321f5822c1bbe01 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 10 Oct 2014 13:57:37 +0200 Subject: [PATCH 09/10] Commented out repeated arguments --- .../nonlinear/tests/testExpressionFactor.cpp | 268 +++++++++--------- 1 file changed, 134 insertions(+), 134 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index ccbc303f2..8364a498a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -184,139 +184,139 @@ TEST(ExpressionFactor, test2) { EXPECT( assert_equal(*expected, *gf3, 1e-9)); } -/* ************************************************************************* */ - -TEST(ExpressionFactor, compose1) { - - // Create expression - Rot3_ R1(1), R2(2); - Rot3_ R3 = R1 * R2; - - // Create factor - ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - - // Create some values - Values values; - values.insert(1, Rot3()); - values.insert(2, Rot3()); - - // Check unwhitenedError - std::vector H(2); - Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); - - // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); -} - -/* ************************************************************************* */ -// Test compose with arguments referring to the same rotation -TEST(ExpressionFactor, compose2) { - - // Create expression - Rot3_ R1(1), R2(1); - Rot3_ R3 = R1 * R2; - - // Create factor - ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - - // Create some values - Values values; - values.insert(1, Rot3()); - - // Check unwhitenedError - std::vector H(1); - Vector actual = f.unwhitenedError(values, H); - EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(2*eye(3), H[0],1e-9)); - - // Check linearization - JacobianFactor expected(1, 2 * eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); -} - -/* ************************************************************************* */ -// Test compose with one arguments referring to a constant same rotation -TEST(ExpressionFactor, compose3) { - - // Create expression - Rot3_ R1(Rot3::identity()), R2(3); - Rot3_ R3 = R1 * R2; - - // Create factor - ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - - // Create some values - Values values; - values.insert(3, Rot3()); - - // Check unwhitenedError - std::vector H(1); - Vector actual = f.unwhitenedError(values, H); - EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - - // Check linearization - JacobianFactor expected(3, eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); -} - -/* ************************************************************************* */ -// Test compose with three arguments -Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - 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); - if (H2) - *H2 = eye(3); - if (H3) - *H3 = eye(3); - return R1 * (R2 * R3); -} - -TEST(ExpressionFactor, composeTernary) { - - // Create expression - Rot3_ A(1), B(2), C(3); - Rot3_ ABC(composeThree, A, B, C); - - // Create factor - ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), ABC); - - // Create some values - Values values; - values.insert(1, Rot3()); - values.insert(2, Rot3()); - values.insert(3, Rot3()); - - // Check unwhitenedError - std::vector H(3); - Vector actual = f.unwhitenedError(values, H); - EXPECT_LONGS_EQUAL(3, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); - EXPECT( assert_equal(eye(3), H[2],1e-9)); - - // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); -} +///* ************************************************************************* */ +// +//TEST(ExpressionFactor, compose1) { +// +// // Create expression +// Rot3_ R1(1), R2(2); +// Rot3_ R3 = R1 * R2; +// +// // Create factor +// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +// +// // Create some values +// Values values; +// values.insert(1, Rot3()); +// values.insert(2, Rot3()); +// +// // Check unwhitenedError +// std::vector H(2); +// Vector actual = f.unwhitenedError(values, H); +// EXPECT( assert_equal(eye(3), H[0],1e-9)); +// EXPECT( assert_equal(eye(3), H[1],1e-9)); +// +// // Check linearization +// JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); +// boost::shared_ptr gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(gf); +// EXPECT( assert_equal(expected, *jf,1e-9)); +//} +// +///* ************************************************************************* */ +//// Test compose with arguments referring to the same rotation +//TEST(ExpressionFactor, compose2) { +// +// // Create expression +// Rot3_ R1(1), R2(1); +// Rot3_ R3 = R1 * R2; +// +// // Create factor +// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +// +// // Create some values +// Values values; +// values.insert(1, Rot3()); +// +// // Check unwhitenedError +// std::vector H(1); +// Vector actual = f.unwhitenedError(values, H); +// EXPECT_LONGS_EQUAL(1, H.size()); +// EXPECT( assert_equal(2*eye(3), H[0],1e-9)); +// +// // Check linearization +// JacobianFactor expected(1, 2 * eye(3), zero(3)); +// boost::shared_ptr gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(gf); +// EXPECT( assert_equal(expected, *jf,1e-9)); +//} +// +///* ************************************************************************* */ +//// Test compose with one arguments referring to a constant same rotation +//TEST(ExpressionFactor, compose3) { +// +// // Create expression +// Rot3_ R1(Rot3::identity()), R2(3); +// Rot3_ R3 = R1 * R2; +// +// // Create factor +// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +// +// // Create some values +// Values values; +// values.insert(3, Rot3()); +// +// // Check unwhitenedError +// std::vector H(1); +// Vector actual = f.unwhitenedError(values, H); +// EXPECT_LONGS_EQUAL(1, H.size()); +// EXPECT( assert_equal(eye(3), H[0],1e-9)); +// +// // Check linearization +// JacobianFactor expected(3, eye(3), zero(3)); +// boost::shared_ptr gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(gf); +// EXPECT( assert_equal(expected, *jf,1e-9)); +//} +// +///* ************************************************************************* */ +//// Test compose with three arguments +//Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, +// 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); +// if (H2) +// *H2 = eye(3); +// if (H3) +// *H3 = eye(3); +// return R1 * (R2 * R3); +//} +// +//TEST(ExpressionFactor, composeTernary) { +// +// // Create expression +// Rot3_ A(1), B(2), C(3); +// Rot3_ ABC(composeThree, A, B, C); +// +// // Create factor +// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), ABC); +// +// // Create some values +// Values values; +// values.insert(1, Rot3()); +// values.insert(2, Rot3()); +// values.insert(3, Rot3()); +// +// // Check unwhitenedError +// std::vector H(3); +// Vector actual = f.unwhitenedError(values, H); +// EXPECT_LONGS_EQUAL(3, H.size()); +// EXPECT( assert_equal(eye(3), H[0],1e-9)); +// EXPECT( assert_equal(eye(3), H[1],1e-9)); +// EXPECT( assert_equal(eye(3), H[2],1e-9)); +// +// // Check linearization +// JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); +// boost::shared_ptr gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(gf); +// EXPECT( assert_equal(expected, *jf,1e-9)); +//} /* ************************************************************************* */ @@ -328,7 +328,7 @@ template struct Incomplete; typedef mpl::vector MyTypes; typedef GenerateTrace::type Generated; //Incomplete incomplete; -BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); +//BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Jacobian2T >)); Generated generated; From 88f9a423c504cf7fc5f047e5ea0dc51495dfc009 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 14:19:39 +0200 Subject: [PATCH 10/10] Numbered types avoid ambiguity --- gtsam_unstable/nonlinear/Expression-inl.h | 48 ++-- .../nonlinear/tests/testExpressionFactor.cpp | 269 +++++++++--------- 2 files changed, 165 insertions(+), 152 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 87c07f976..4185a6bb2 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -156,7 +156,7 @@ struct Select<2, A> { /** * Record the evaluation of a single argument in a functional expression */ -template +template struct SingleTrace { typedef Eigen::Matrix JacobianTA; typename JacobianTrace::Pointer trace; @@ -169,16 +169,19 @@ struct SingleTrace { * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost * and Beyond. Pearson Education. */ -template -struct Trace: SingleTrace, More { +template +struct Trace: SingleTrace, More { + + typedef typename AN::type A; + const static size_t N = AN::value; typename JacobianTrace::Pointer const & myTrace() const { - return static_cast*>(this)->trace; + return static_cast*>(this)->trace; } typedef Eigen::Matrix JacobianTA; const JacobianTA& myJacobian() const { - return static_cast*>(this)->dTdA; + return static_cast*>(this)->dTdA; } /// Start the reverse AD process @@ -202,6 +205,14 @@ struct Trace: SingleTrace, More { } }; +/// 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 Trace Class Generator template struct GenerateTrace { @@ -486,7 +497,7 @@ public: } /// Trace structure for reverse AD - typedef boost::mpl::vector Arguments; + typedef boost::mpl::vector > Arguments; typedef typename GenerateTrace::type Trace; /// Construct an execution trace for reverse AD @@ -558,7 +569,7 @@ public: } /// Trace structure for reverse AD - typedef boost::mpl::vector Arguments; + typedef boost::mpl::vector, Numbered > Arguments; typedef typename GenerateTrace::type Trace; /// Construct an execution trace for reverse AD @@ -567,11 +578,11 @@ public: Trace* trace = new Trace(); p.setFunction(trace); A1 a1 = this->expressionA1_->traceExecution(values, - static_cast*>(trace)->trace); + static_cast*>(trace)->trace); A2 a2 = this->expressionA2_->traceExecution(values, - static_cast*>(trace)->trace); - return function_(a1, a2, static_cast*>(trace)->dTdA, - static_cast*>(trace)->dTdA); + static_cast*>(trace)->trace); + return function_(a1, a2, static_cast*>(trace)->dTdA, + static_cast*>(trace)->dTdA); } }; @@ -647,7 +658,7 @@ public: } /// Trace structure for reverse AD - typedef boost::mpl::vector Arguments; + typedef boost::mpl::vector, Numbered, Numbered > Arguments; typedef typename GenerateTrace::type Trace; /// Construct an execution trace for reverse AD @@ -656,14 +667,15 @@ public: Trace* trace = new Trace(); p.setFunction(trace); A1 a1 = this->expressionA1_->traceExecution(values, - static_cast*>(trace)->trace); + static_cast*>(trace)->trace); A2 a2 = this->expressionA2_->traceExecution(values, - static_cast*>(trace)->trace); + static_cast*>(trace)->trace); A3 a3 = this->expressionA3_->traceExecution(values, - static_cast*>(trace)->trace); - return function_(a1, a2, a3, static_cast*>(trace)->dTdA, - static_cast*>(trace)->dTdA, - static_cast*>(trace)->dTdA); + static_cast*>(trace)->trace); + return function_(a1, a2, a3, + static_cast*>(trace)->dTdA, + static_cast*>(trace)->dTdA, + static_cast*>(trace)->dTdA); } }; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 8364a498a..cc26c722b 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -184,139 +184,139 @@ TEST(ExpressionFactor, test2) { EXPECT( assert_equal(*expected, *gf3, 1e-9)); } -///* ************************************************************************* */ -// -//TEST(ExpressionFactor, compose1) { -// -// // Create expression -// Rot3_ R1(1), R2(2); -// Rot3_ R3 = R1 * R2; -// -// // Create factor -// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); -// -// // Create some values -// Values values; -// values.insert(1, Rot3()); -// values.insert(2, Rot3()); -// -// // Check unwhitenedError -// std::vector H(2); -// Vector actual = f.unwhitenedError(values, H); -// EXPECT( assert_equal(eye(3), H[0],1e-9)); -// EXPECT( assert_equal(eye(3), H[1],1e-9)); -// -// // Check linearization -// JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); -// boost::shared_ptr gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(gf); -// EXPECT( assert_equal(expected, *jf,1e-9)); -//} -// -///* ************************************************************************* */ -//// Test compose with arguments referring to the same rotation -//TEST(ExpressionFactor, compose2) { -// -// // Create expression -// Rot3_ R1(1), R2(1); -// Rot3_ R3 = R1 * R2; -// -// // Create factor -// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); -// -// // Create some values -// Values values; -// values.insert(1, Rot3()); -// -// // Check unwhitenedError -// std::vector H(1); -// Vector actual = f.unwhitenedError(values, H); -// EXPECT_LONGS_EQUAL(1, H.size()); -// EXPECT( assert_equal(2*eye(3), H[0],1e-9)); -// -// // Check linearization -// JacobianFactor expected(1, 2 * eye(3), zero(3)); -// boost::shared_ptr gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(gf); -// EXPECT( assert_equal(expected, *jf,1e-9)); -//} -// -///* ************************************************************************* */ -//// Test compose with one arguments referring to a constant same rotation -//TEST(ExpressionFactor, compose3) { -// -// // Create expression -// Rot3_ R1(Rot3::identity()), R2(3); -// Rot3_ R3 = R1 * R2; -// -// // Create factor -// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); -// -// // Create some values -// Values values; -// values.insert(3, Rot3()); -// -// // Check unwhitenedError -// std::vector H(1); -// Vector actual = f.unwhitenedError(values, H); -// EXPECT_LONGS_EQUAL(1, H.size()); -// EXPECT( assert_equal(eye(3), H[0],1e-9)); -// -// // Check linearization -// JacobianFactor expected(3, eye(3), zero(3)); -// boost::shared_ptr gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(gf); -// EXPECT( assert_equal(expected, *jf,1e-9)); -//} -// -///* ************************************************************************* */ -//// Test compose with three arguments -//Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, -// 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); -// if (H2) -// *H2 = eye(3); -// if (H3) -// *H3 = eye(3); -// return R1 * (R2 * R3); -//} -// -//TEST(ExpressionFactor, composeTernary) { -// -// // Create expression -// Rot3_ A(1), B(2), C(3); -// Rot3_ ABC(composeThree, A, B, C); -// -// // Create factor -// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), ABC); -// -// // Create some values -// Values values; -// values.insert(1, Rot3()); -// values.insert(2, Rot3()); -// values.insert(3, Rot3()); -// -// // Check unwhitenedError -// std::vector H(3); -// Vector actual = f.unwhitenedError(values, H); -// EXPECT_LONGS_EQUAL(3, H.size()); -// EXPECT( assert_equal(eye(3), H[0],1e-9)); -// EXPECT( assert_equal(eye(3), H[1],1e-9)); -// EXPECT( assert_equal(eye(3), H[2],1e-9)); -// -// // Check linearization -// JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); -// boost::shared_ptr gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(gf); -// EXPECT( assert_equal(expected, *jf,1e-9)); -//} +/* ************************************************************************* */ + +TEST(ExpressionFactor, compose1) { + + // Create expression + Rot3_ R1(1), R2(2); + Rot3_ R3 = R1 * R2; + + // Create factor + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3()); + + // Check unwhitenedError + std::vector H(2); + Vector actual = f.unwhitenedError(values, H); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(eye(3), H[1],1e-9)); + + // Check linearization + JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ +// Test compose with arguments referring to the same rotation +TEST(ExpressionFactor, compose2) { + + // Create expression + Rot3_ R1(1), R2(1); + Rot3_ R3 = R1 * R2; + + // Create factor + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(1, Rot3()); + + // Check unwhitenedError + std::vector H(1); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(2*eye(3), H[0],1e-9)); + + // Check linearization + JacobianFactor expected(1, 2 * eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ +// Test compose with one arguments referring to a constant same rotation +TEST(ExpressionFactor, compose3) { + + // Create expression + Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R3 = R1 * R2; + + // Create factor + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(3, Rot3()); + + // Check unwhitenedError + std::vector H(1); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + + // Check linearization + JacobianFactor expected(3, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ +// Test compose with three arguments +Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, + 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); + if (H2) + *H2 = eye(3); + if (H3) + *H3 = eye(3); + return R1 * (R2 * R3); +} + +TEST(ExpressionFactor, composeTernary) { + + // Create expression + Rot3_ A(1), B(2), C(3); + Rot3_ ABC(composeThree, A, B, C); + + // Create factor + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), ABC); + + // Create some values + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3()); + values.insert(3, Rot3()); + + // Check unwhitenedError + std::vector H(3); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(3, H.size()); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(eye(3), H[1],1e-9)); + EXPECT( assert_equal(eye(3), H[2],1e-9)); + + // Check linearization + JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} /* ************************************************************************* */ @@ -325,7 +325,8 @@ namespace mpl = boost::mpl; #include template struct Incomplete; -typedef mpl::vector MyTypes; +typedef mpl::vector, Numbered, + Numbered > MyTypes; typedef GenerateTrace::type Generated; //Incomplete incomplete; //BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >));