diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 08dd18ee3..332c23ca7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -106,7 +106,7 @@ struct UseBlockIf { }; } -/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians +/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians template void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { @@ -186,28 +186,28 @@ public: } } /** - * *** This is the main entry point for reverseAD, called from Expression *** + * *** This is the main entry point for reverse AD, called from Expression *** * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) */ typedef Eigen::Matrix JacobianTT; - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD1(JacobianMap& jacobians) const { if (kind == Leaf) { // This branch will only be called on trivial Leaf expressions, i.e. Priors static const JacobianTT I = JacobianTT::Identity(); handleLeafCase(I, jacobians, content.key); } else if (kind == Function) // This is the more typical entry point, starting the AD pipeline - // Inside the startReverseAD that the correctly dimensioned pipeline is chosen. - content.ptr->startReverseAD(jacobians); + // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. + content.ptr->startReverseAD2(jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) template - void reverseAD(const Eigen::MatrixBase & dTdA, + void reverseAD1(const Eigen::MatrixBase & dTdA, JacobianMap& jacobians) const { if (kind == Leaf) handleLeafCase(dTdA, jacobians, content.key); else if (kind == Function) - content.ptr->reverseAD(dTdA, jacobians); + content.ptr->reverseAD2(dTdA, jacobians); } /// Define type so we can apply it as a meta-function @@ -470,10 +470,10 @@ struct FunctionalBase: ExpressionNode { struct Record { void print(const std::string& indent) const { } - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD4(JacobianMap& jacobians) const { } template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { } }; /// Construct an execution trace for reverse AD @@ -505,9 +505,9 @@ struct JacobianTrace { typename Jacobian::type dTdA; }; -/** - * Recursive Definition of Functional ExpressionNode - */ +// Recursive Definition of Functional ExpressionNode +// The reason we inherit from Argument is because we can then +// case to this unique signature to retrieve the expression at any level template struct GenerateFunctionalNode: Argument, Base { @@ -528,7 +528,9 @@ struct GenerateFunctionalNode: Argument, Base { This::expression->dims(map); } - /// Recursive Record Class for Functional Expressions + // Recursive Record Class for Functional Expressions + // The reason we inherit from JacobianTrace is because we can then + // case to this unique signature to retrieve the value/trace at any level struct Record: JacobianTrace, Base::Record { typedef T return_type; @@ -543,17 +545,26 @@ struct GenerateFunctionalNode: Argument, Base { } /// Start the reverse AD process - void startReverseAD(JacobianMap& jacobians) const { - Base::Record::startReverseAD(jacobians); - This::trace.reverseAD(This::dTdA, jacobians); + void startReverseAD4(JacobianMap& jacobians) const { + Base::Record::startReverseAD4(jacobians); + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 + // which calls the correctly sized CallRecord::reverseAD3, which in turn + // calls reverseAD4 below. + This::trace.reverseAD1(This::dTdA, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process + // Cols is always known at compile time template - void reverseAD(const Eigen::Matrix & dFdT, + void reverseAD4(const Eigen::Matrix & dFdT, JacobianMap& jacobians) const { - Base::Record::reverseAD(dFdT, jacobians); - This::trace.reverseAD(dFdT * This::dTdA, jacobians); + Base::Record::reverseAD4(dFdT, jacobians); + This::trace.reverseAD1(dFdT * This::dTdA, jacobians); } }; @@ -614,8 +625,8 @@ struct FunctionalNode { struct Record: public internal::CallRecordImplementor::value>, public Base::Record { using Base::Record::print; - using Base::Record::startReverseAD; - using Base::Record::reverseAD; + using Base::Record::startReverseAD4; + using Base::Record::reverseAD4; virtual ~Record() { } diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index d44d21cd7..c7f022724 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -209,7 +209,7 @@ private: ExecutionTraceStorage traceStorage[size]; ExecutionTrace trace; T value(traceExecution(values, trace, traceStorage)); - trace.startReverseAD(jacobians); + trace.startReverseAD1(jacobians); return value; } diff --git a/gtsam_unstable/nonlinear/CallRecord.h b/gtsam_unstable/nonlinear/CallRecord.h index f1ac0b044..159a841e5 100644 --- a/gtsam_unstable/nonlinear/CallRecord.h +++ b/gtsam_unstable/nonlinear/CallRecord.h @@ -32,12 +32,6 @@ 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 { /** @@ -57,7 +51,8 @@ struct ConvertToVirtualFunctionSupportedMatrixType { template<> struct ConvertToVirtualFunctionSupportedMatrixType { template - static const Eigen::Matrix convert( + static const Eigen::Matrix convert( const Eigen::MatrixBase & x) { return x; } @@ -69,126 +64,132 @@ struct ConvertToVirtualFunctionSupportedMatrixType { } }; -/** - * 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. + * The CallRecord is an abstract base class for the any class that stores + * the Jacobians of applying a function with respect to each of its arguments, + * as well as an execution trace for each of its arguments. */ template -struct CallRecord: virtual private internal::ReverseADInterface< - MaxVirtualStaticRows, Cols> { +struct CallRecord { + // Print entire record, recursively inline void print(const std::string& indent) const { _print(indent); } - inline void startReverseAD(JacobianMap& jacobians) const { - _startReverseAD(jacobians); + // Main entry point for the reverse AD process of a functional expression. + // Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1 + // This function then calls ExecutionTrace::reverseAD for every argument + // which will in turn call the reverseAD method below. + // This non-virtual function _startReverseAD3, implemented in derived + inline void startReverseAD2(JacobianMap& jacobians) const { + _startReverseAD3(jacobians); } + // Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1 + // Here we convert to dynamic if the template - inline void reverseAD(const Eigen::MatrixBase & dFdT, + inline void reverseAD2(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { - _reverseAD( - internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert( - dFdT), jacobians); + _reverseAD3( + internal::ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), + jacobians); } - inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - _reverseAD(dFdT, jacobians); + // This overload supports matrices with both rows and columns dynamically sized. + // The template version above would be slower by introducing an extra conversion + // to statically sized columns. + inline void reverseAD2(const Matrix & dFdT, JacobianMap& jacobians) const { + _reverseAD3(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; + virtual void _startReverseAD3(JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; }; +/** + * CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific + * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord + * interface. It is used to keep the testCallRecord unit test in sync. + */ +const int CallRecordMaxVirtualStaticRows = 5; + 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 { +struct CallRecordImplementor: public CallRecord { 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); + + virtual void _startReverseAD3(JacobianMap& jacobians) const { + derived().startReverseAD4(jacobians); + } + + virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); } - template friend struct ReverseADImplementor; }; } // namespace internal diff --git a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp index a4561b349..1cc674901 100644 --- a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp +++ b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp @@ -33,7 +33,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > MaxVirtualStaticRows){ + if(i > CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; @@ -43,7 +43,6 @@ struct CallConfig { int compTimeCols; int runTimeRows; int runTimeCols; - CallConfig() {} CallConfig(int rows, int cols): compTimeRows(dynamicIfAboveMax(rows)), compTimeCols(cols), @@ -72,25 +71,26 @@ struct CallConfig { }; struct Record: public internal::CallRecordImplementor { + Record() : cc(0, 0) {} virtual ~Record() { } void print(const std::string& indent) const { } - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD4(JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(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; + template + friend struct internal::CallRecordImplementor; }; JacobianMap & NJM= *static_cast(NULL); @@ -102,56 +102,56 @@ TEST(CallRecord, virtualReverseAdDispatching) { Record record; { const int Rows = 1; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(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); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(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); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(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); + const int Rows = 4; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(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); + const int Rows = 5; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(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); + const int Rows = 6; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index d146ea945..09cad558a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -76,10 +76,13 @@ TEST(ExpressionFactor, Model) { // Concise version ExpressionFactor f(model, Point2(0, 0), p); + + // Check values and derivatives EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf2 = f.linearize(values); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way } /* ************************************************************************* */ @@ -124,6 +127,38 @@ TEST(ExpressionFactor, Unary) { EXPECT( assert_equal(expected, *jf, 1e-9)); } +/* ************************************************************************* */ +// Unary(Leaf)) and Unary(Unary(Leaf))) +// wide version (not handled in fixed-size pipeline) +typedef Eigen::Matrix Matrix93; +Vector9 wide(const Point3& p, boost::optional H) { + Vector9 v; + v << p.vector(), p.vector(), p.vector(); + if (H) *H << eye(3), eye(3), eye(3); + return v; +} +typedef Eigen::Matrix Matrix9; +Vector9 id9(const Vector9& v, boost::optional H) { + if (H) *H = Matrix9::Identity(); + return v; +} +TEST(ExpressionFactor, Wide) { + // Create some values + Values values; + values.insert(2, Point3(0, 0, 1)); + Point3_ point(2); + Vector9 measured; + Expression expression(wide,point); + SharedNoiseModel model = noiseModel::Unit::Create(9); + + ExpressionFactor f1(model, measured, expression); + EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9); + + Expression expression2(id9,expression); + ExpressionFactor f2(model, measured, expression2); + EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9); +} + /* ************************************************************************* */ static Point2 myUncal(const Cal3_S2& K, const Point2& p, boost::optional Dcal, boost::optional Dp) {