diff --git a/gtsam_unstable/nonlinear/CallRecord.h b/gtsam_unstable/nonlinear/CallRecord.h index 3806f1803..f1ac0b044 100644 --- a/gtsam_unstable/nonlinear/CallRecord.h +++ b/gtsam_unstable/nonlinear/CallRecord.h @@ -46,7 +46,7 @@ namespace internal { * it just passes dense Eigen matrices through. */ template -struct ConvertToDynamicRowsIf { +struct ConvertToVirtualFunctionSupportedMatrixType { template static Eigen::Matrix convert( const Eigen::MatrixBase & x) { @@ -55,7 +55,13 @@ struct ConvertToDynamicRowsIf { }; template<> -struct ConvertToDynamicRowsIf { +struct ConvertToVirtualFunctionSupportedMatrixType { + template + static const Eigen::Matrix convert( + const Eigen::MatrixBase & x) { + return x; + } + // special treatment of matrices that don't need conversion template static const Eigen::Matrix & convert( const Eigen::Matrix & x) { @@ -143,11 +149,11 @@ struct CallRecord: virtual private internal::ReverseADInterface< _startReverseAD(jacobians); } - template - inline void reverseAD(const Eigen::Matrix & dFdT, + template + inline void reverseAD(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { _reverseAD( - internal::ConvertToDynamicRowsIf<(Rows > MaxVirtualStaticRows)>::convert( + internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert( dFdT), jacobians); } diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 1a855aeb7..7f59dcb7d 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -64,18 +64,36 @@ public: }; //----------------------------------------------------------------------------- -/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians -template -void handleLeafCase(const Eigen::Matrix& dTdA, - JacobianMap& jacobians, Key key) { - jacobians(key).block(0, 0) += dTdA; // block makes HUGE difference + +namespace internal { + +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key){ + // block makes HUGE difference + jacobians(key).block(0, 0) += dTdA; + }; +}; +/// Handle Leaf Case for Dynamic Matrix type (slower) +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + jacobians(key) += dTdA; + } +}; } -/// Handle Leaf Case for Dynamic ROWS Matrix type (slower) -template -inline void handleLeafCase( - const Eigen::Matrix& dTdA, + +/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians +template +void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { - jacobians(key) += dTdA; + internal::UseBlockIf< + Derived::RowsAtCompileTime != Eigen::Dynamic && + Derived::ColsAtCompileTime != Eigen::Dynamic, + Derived> + ::addToJacobian(dTdA, jacobians, key); } //----------------------------------------------------------------------------- @@ -166,9 +184,9 @@ public: void reverseAD(const Eigen::MatrixBase & dTdA, JacobianMap& jacobians) const { if (kind == Leaf) - handleLeafCase(dTdA.eval(), jacobians, content.key); + handleLeafCase(dTdA, jacobians, content.key); else if (kind == Function) - content.ptr->reverseAD(dTdA.eval(), jacobians); + content.ptr->reverseAD(dTdA, jacobians); } /// Define type so we can apply it as a meta-function @@ -507,7 +525,7 @@ struct GenerateFunctionalNode: Argument, Base { void reverseAD(const Eigen::Matrix & dFdT, JacobianMap& jacobians) const { Base::Record::reverseAD(dFdT, jacobians); - This::trace.reverseAD((dFdT * This::dTdA).eval(), jacobians); + This::trace.reverseAD(dFdT * This::dTdA, jacobians); } };