diff --git a/.cproject b/.cproject index 412359938..52c627f5c 100644 --- a/.cproject +++ b/.cproject @@ -600,6 +600,7 @@ make + tests/testBayesTree.run true false @@ -607,6 +608,7 @@ make + testBinaryBayesNet.run true false @@ -654,6 +656,7 @@ make + testSymbolicBayesNet.run true false @@ -661,6 +664,7 @@ make + tests/testSymbolicFactor.run true false @@ -668,6 +672,7 @@ make + testSymbolicFactorGraph.run true false @@ -683,6 +688,7 @@ make + tests/testBayesTree true false @@ -1114,6 +1120,7 @@ make + testErrors.run true false @@ -1343,6 +1350,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1425,7 +1472,6 @@ make - testSimulated2DOriented.run true false @@ -1465,7 +1511,6 @@ make - testSimulated2D.run true false @@ -1473,7 +1518,6 @@ make - testSimulated3D.run true false @@ -1487,46 +1531,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1784,6 +1788,7 @@ cpack + -G DEB true false @@ -1791,6 +1796,7 @@ cpack + -G RPM true false @@ -1798,6 +1804,7 @@ cpack + -G TGZ true false @@ -1805,6 +1812,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2619,6 +2627,7 @@ make + testGraph.run true false @@ -2626,6 +2635,7 @@ make + testJunctionTree.run true false @@ -2633,6 +2643,7 @@ make + testSymbolicBayesNetB.run true false @@ -2742,6 +2753,14 @@ true true + + make + -j5 + testCallRecord.run + true + true + true + make -j2 @@ -3152,7 +3171,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp index 936c9957b..ac43fa428 100644 --- a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp +++ b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose2SLAMExample.cpp + * @file Pose2SLAMExampleExpressions.cpp * @brief Expressions version of Pose2SLAMExample.cpp * @date Oct 2, 2014 * @author Frank Dellaert diff --git a/gtsam_unstable/nonlinear/CallRecord.h b/gtsam_unstable/nonlinear/CallRecord.h new file mode 100644 index 000000000..3806f1803 --- /dev/null +++ b/gtsam_unstable/nonlinear/CallRecord.h @@ -0,0 +1,190 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CallRecord.h + * @date November 21, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @author Hannes Sommer + * @brief Internals for Expression.h, not for general consumption + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +class JacobianMap; +// forward declaration + +//----------------------------------------------------------------------------- +/** + * MaxVirtualStaticRows defines how many separate virtual reverseAD with specific + * static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface. + */ +const int MaxVirtualStaticRows = 4; + +namespace internal { + +/** + * ConvertToDynamicIf converts to a dense matrix with dynamic rows iff + * ConvertToDynamicRows (colums stay as they are) otherwise + * it just passes dense Eigen matrices through. + */ +template +struct ConvertToDynamicRowsIf { + template + static Eigen::Matrix convert( + const Eigen::MatrixBase & x) { + return x; + } +}; + +template<> +struct ConvertToDynamicRowsIf { + template + static const Eigen::Matrix & convert( + const Eigen::Matrix & x) { + return x; + } +}; + +/** + * Recursive definition of an interface having several purely + * virtual _reverseAD(const Eigen::Matrix &, JacobianMap&) + * with Rows in 1..MaxSupportedStaticRows + */ +template +struct ReverseADInterface: ReverseADInterface { + using ReverseADInterface::_reverseAD; + virtual void _reverseAD( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; +}; + +template +struct ReverseADInterface<0, Cols> { + virtual void _reverseAD( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD(const Matrix & dFdT, + JacobianMap& jacobians) const = 0; +}; + +/** + * ReverseADImplementor is a utility class used by CallRecordImplementor to + * implementing the recursive ReverseADInterface interface. + */ +template +struct ReverseADImplementor: ReverseADImplementor { +private: + using ReverseADImplementor::_reverseAD; + virtual void _reverseAD( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + static_cast(this)->reverseAD(dFdT, jacobians); + } + friend struct internal::ReverseADImplementor; +}; + +template +struct ReverseADImplementor : virtual internal::ReverseADInterface< + MaxVirtualStaticRows, Cols> { +private: + using internal::ReverseADInterface::_reverseAD; + const Derived & derived() const { + return static_cast(*this); + } + virtual void _reverseAD( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD(dFdT, jacobians); + } + virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { + derived().reverseAD(dFdT, jacobians); + } + friend struct internal::ReverseADImplementor; +}; + +} // namespace internal + +/** + * The CallRecord class stores the Jacobians of applying a function + * with respect to each of its arguments. It also stores an execution trace + * (defined below) for each of its arguments. + * + * It is implemented in the function-style ExpressionNode's nested Record class below. + */ +template +struct CallRecord: virtual private internal::ReverseADInterface< + MaxVirtualStaticRows, Cols> { + + inline void print(const std::string& indent) const { + _print(indent); + } + + inline void startReverseAD(JacobianMap& jacobians) const { + _startReverseAD(jacobians); + } + + template + inline void reverseAD(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + _reverseAD( + internal::ConvertToDynamicRowsIf<(Rows > MaxVirtualStaticRows)>::convert( + dFdT), jacobians); + } + + inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { + _reverseAD(dFdT, jacobians); + } + + virtual ~CallRecord() { + } + +private: + virtual void _print(const std::string& indent) const = 0; + virtual void _startReverseAD(JacobianMap& jacobians) const = 0; + using internal::ReverseADInterface::_reverseAD; +}; + +namespace internal { +/** + * The CallRecordImplementor implements the CallRecord interface for a Derived class by + * delegating to its corresponding (templated) non-virtual methods. + */ +template +struct CallRecordImplementor: public CallRecord, + private ReverseADImplementor { +private: + const Derived & derived() const { + return static_cast(*this); + } + virtual void _print(const std::string& indent) const { + derived().print(indent); + } + virtual void _startReverseAD(JacobianMap& jacobians) const { + derived().startReverseAD(jacobians); + } + template friend struct ReverseADImplementor; +}; + +} // namespace internal + +} // gtsam diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 40fc54751..a98ab349f 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -19,29 +19,21 @@ #pragma once +#include #include -#include #include #include -#include #include #include #include // template meta-programming headers -#include -#include -#include -#include #include -#include -#include -#include -#include namespace MPL = boost::mpl::placeholders; -#include // for placement new +#include + class ExpressionFactorBinaryTest; // Forward declare for testing @@ -71,29 +63,6 @@ public: } }; -//----------------------------------------------------------------------------- -/** - * The CallRecord class stores the Jacobians of applying a function - * with respect to each of its arguments. It also stores an executation trace - * (defined below) for each of its arguments. - * - * It is sub-classed in the function-style ExpressionNode sub-classes below. - */ -template -struct CallRecord { - static size_t const N = 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 { - } -}; - //----------------------------------------------------------------------------- /// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians template @@ -101,10 +70,10 @@ void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { jacobians(key).block(0, 0) += dTdA; // block makes HUGE difference } -/// Handle Leaf Case for Dynamic Matrix type (slower) -template<> +/// Handle Leaf Case for Dynamic ROWS Matrix type (slower) +template inline void handleLeafCase( - const Eigen::Matrix& dTdA, + const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { jacobians(key) += dTdA; } @@ -193,45 +162,19 @@ public: content.ptr->startReverseAD(jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) - void reverseAD(const Matrix& dTdA, JacobianMap& jacobians) const { + template + void reverseAD(const Eigen::MatrixBase & dTdA, + JacobianMap& jacobians) const { if (kind == Leaf) - handleLeafCase(dTdA, jacobians, content.key); + handleLeafCase(dTdA.eval(), jacobians, content.key); else if (kind == Function) - content.ptr->reverseAD(dTdA, jacobians); - } - // Either add to Jacobians (Leaf) or propagate (Function) - typedef Eigen::Matrix Jacobian2T; - void reverseAD2(const Jacobian2T& dTdA, JacobianMap& jacobians) const { - if (kind == Leaf) - handleLeafCase(dTdA, jacobians, content.key); - else if (kind == Function) - content.ptr->reverseAD2(dTdA, jacobians); + content.ptr->reverseAD(dTdA.eval(), jacobians); } /// Define type so we can apply it as a meta-function typedef ExecutionTrace type; }; -/// Primary template calls the generic Matrix reverseAD pipeline -template -struct Select { - typedef Eigen::Matrix::value> Jacobian; - static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, - JacobianMap& jacobians) { - trace.reverseAD(dTdA, jacobians); - } -}; - -/// Partially specialized template calls the 2-dimensional output version -template -struct Select<2, A> { - typedef Eigen::Matrix::value> Jacobian; - static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, - JacobianMap& jacobians) { - trace.reverseAD2(dTdA, jacobians); - } -}; - //----------------------------------------------------------------------------- /** * Expression node. The superclass for objects that do the heavy lifting @@ -479,8 +422,15 @@ template struct FunctionalBase: ExpressionNode { static size_t const N = 0; // number of arguments - typedef CallRecord::value> Record; - + struct Record { + void print(const std::string& indent) const { + } + void startReverseAD(JacobianMap& jacobians) const { + } + template + void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + } + }; /// Construct an execution trace for reverse AD void trace(const Values& values, Record* record, char*& raw) const { // base case: does not do anything @@ -539,7 +489,7 @@ struct GenerateFunctionalNode: Argument, Base { typedef JacobianTrace This; /// Print to std::cout - virtual void print(const std::string& indent) const { + void print(const std::string& indent) const { Base::Record::print(indent); static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); std::cout << This::dTdA.format(matlab) << std::endl; @@ -547,25 +497,18 @@ struct GenerateFunctionalNode: Argument, Base { } /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD(JacobianMap& jacobians) const { Base::Record::startReverseAD(jacobians); - Select::value, A>::reverseAD(This::trace, This::dTdA, - jacobians); + This::trace.reverseAD(This::dTdA, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process - virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + template + void reverseAD(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { Base::Record::reverseAD(dFdT, jacobians); This::trace.reverseAD(dFdT * This::dTdA, jacobians); } - - /// Version specialized to 2-dimensional output - typedef Eigen::Matrix::value> Jacobian2T; - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - Base::Record::reverseAD2(dFdT, jacobians); - This::trace.reverseAD2(dFdT * This::dTdA, jacobians); - } }; /// Construct an execution trace for reverse AD @@ -619,8 +562,16 @@ struct FunctionalNode { return static_cast const &>(*this).expression; } - /// Provide convenience access to Record storage - struct Record: public Base::Record { + /// Provide convenience access to Record storage and implement + /// the virtual function based interface of CallRecord using the CallRecordImplementor + struct Record: public internal::CallRecordImplementor::value>, public Base::Record { + using Base::Record::print; + using Base::Record::startReverseAD; + using Base::Record::reverseAD; + + virtual ~Record() { + } /// Access Value template @@ -633,7 +584,6 @@ struct FunctionalNode { typename Jacobian::type& jacobian() { return static_cast&>(*this).dTdA; } - }; /// Construct an execution trace for reverse AD diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index fc1efeb87..e8bd8bbe7 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -116,7 +116,7 @@ public: return root_->traceSize(); } - /// trace execution, very unsafe, for testing purposes only + /// trace execution, very unsafe, for testing purposes only //TODO this is not only used for testing, but in value() below! T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { return root_->traceExecution(values, trace, raw); diff --git a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp new file mode 100644 index 000000000..a4561b349 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp @@ -0,0 +1,165 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------1------------------------------------------- */ + +/** + * @file testCallRecord.cpp + * @date November 21, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @author Hannes Sommer + * @brief unit tests for CallRecord class + */ + +#include + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +static const int Cols = 3; + + +int dynamicIfAboveMax(int i){ + if(i > MaxVirtualStaticRows){ + return Eigen::Dynamic; + } + else return i; +} +struct CallConfig { + int compTimeRows; + int compTimeCols; + int runTimeRows; + int runTimeCols; + CallConfig() {} + CallConfig(int rows, int cols): + compTimeRows(dynamicIfAboveMax(rows)), + compTimeCols(cols), + runTimeRows(rows), + runTimeCols(cols) + { + } + CallConfig(int compTimeRows, int compTimeCols, int runTimeRows, int runTimeCols): + compTimeRows(compTimeRows), + compTimeCols(compTimeCols), + runTimeRows(runTimeRows), + runTimeCols(runTimeCols) + { + } + + bool equals(const CallConfig & c, double /*tol*/) const { + return + this->compTimeRows == c.compTimeRows && + this->compTimeCols == c.compTimeCols && + this->runTimeRows == c.runTimeRows && + this->runTimeCols == c.runTimeCols; + } + void print(const std::string & prefix) const { + std::cout << prefix << "{" << compTimeRows << ", " << compTimeCols << ", " << runTimeRows << ", " << runTimeCols << "}\n" ; + } +}; + +struct Record: public internal::CallRecordImplementor { + virtual ~Record() { + } + void print(const std::string& indent) const { + } + void startReverseAD(JacobianMap& jacobians) const { + } + + mutable CallConfig cc; + private: + template + void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + cc.compTimeRows = SomeMatrix::RowsAtCompileTime; + cc.compTimeCols = SomeMatrix::ColsAtCompileTime; + cc.runTimeRows = dFdT.rows(); + cc.runTimeCols = dFdT.cols(); + } + + template + friend struct internal::ReverseADImplementor; +}; + +JacobianMap & NJM= *static_cast(NULL); + +/* ************************************************************************* */ +typedef Eigen::Matrix DynRowMat; + +TEST(CallRecord, virtualReverseAdDispatching) { + Record record; + { + const int Rows = 1; + record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); + record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); + record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); + } + { + const int Rows = 2; + record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); + record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); + record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); + } + { + const int Rows = 3; + record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); + record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); + record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); + } + { + const int Rows = MaxVirtualStaticRows; + record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); + record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); + record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); + } + { + const int Rows = MaxVirtualStaticRows + 1; + record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); + record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); + record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); + } + { + const int Rows = MaxVirtualStaticRows + 2; + record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); + record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); + record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); + } +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index b2cdcdf34..d10e31002 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -38,7 +38,6 @@ template struct Incomplete; typedef mpl::vector MyTypes; typedef FunctionalNode::type Generated; //Incomplete incomplete; -BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Record::Jacobian2T >)); // Try generating vectors of ExecutionTrace typedef mpl::vector, ExecutionTrace > ExpectedTraces;