From bd3f9db7df0eba313b2c2cffed9c1c9efc22cba3 Mon Sep 17 00:00:00 2001 From: Renaud Dube Date: Fri, 7 Nov 2014 11:37:27 +0100 Subject: [PATCH] inlined a fully specialized function template defined in a .hpp --- gtsam_unstable/nonlinear/Expression-inl.h | 111 +++++++++++----------- 1 file changed, 58 insertions(+), 53 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 215525bb9..d7d2a9b62 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -58,9 +58,9 @@ class Expression; class JacobianMap { const FastVector& keys_; VerticalBlockMatrix& Ab_; -public: + public: JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : - keys_(keys), Ab_(Ab) { + keys_(keys), Ab_(Ab) { } /// Access via key VerticalBlockMatrix::Block operator()(Key key) { @@ -89,7 +89,7 @@ struct CallRecord { } typedef Eigen::Matrix Jacobian2T; virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { + JacobianMap& jacobians) const { } }; @@ -97,12 +97,17 @@ struct CallRecord { /// 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 < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference + JacobianMap& jacobians, Key key) { +// if (ROWS == -1 && COLS == -1 ) { +// jacobians(key) += dTdA; +// } else { + jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference +// } + } /// Handle Leaf Case for Dynamic Matrix type (slower) template<> -void handleLeafCase( +inline void handleLeafCase( const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { jacobians(key) += dTdA; @@ -140,10 +145,10 @@ class ExecutionTrace { Key key; CallRecord* ptr; } content; -public: + public: /// Pointer always starts out as a Constant ExecutionTrace() : - kind(Constant) { + kind(Constant) { } /// Change pointer to a Leaf Record void setLeaf(Key key) { @@ -216,7 +221,7 @@ template struct Select { typedef Eigen::Matrix::value> Jacobian; static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, - JacobianMap& jacobians) { + JacobianMap& jacobians) { trace.reverseAD(dTdA, jacobians); } }; @@ -226,7 +231,7 @@ template struct Select<2, A> { typedef Eigen::Matrix::value> Jacobian; static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, - JacobianMap& jacobians) { + JacobianMap& jacobians) { trace.reverseAD2(dTdA, jacobians); } }; @@ -242,16 +247,16 @@ struct Select<2, A> { template class ExpressionNode { -protected: + protected: size_t traceSize_; /// Constructor, traceSize is size of the execution trace of expression rooted here ExpressionNode(size_t traceSize = 0) : - traceSize_(traceSize) { + traceSize_(traceSize) { } -public: + public: /// Destructor virtual ~ExpressionNode() { @@ -277,7 +282,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const = 0; + char* raw) const = 0; }; //----------------------------------------------------------------------------- @@ -290,12 +295,12 @@ class ConstantExpression: public ExpressionNode { /// Constructor with a value, yielding a constant ConstantExpression(const T& value) : - constant_(value) { + constant_(value) { } friend class Expression ; -public: + public: /// Return value virtual T value(const Values& values) const { @@ -304,7 +309,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + char* raw) const { return constant_; } }; @@ -320,13 +325,13 @@ class LeafExpression: public ExpressionNode { /// Constructor with a single key LeafExpression(Key key) : - key_(key) { + key_(key) { } // todo: do we need a virtual destructor here too? friend class Expression ; -public: + public: /// Return keys that play in this expression virtual std::set keys() const { @@ -348,7 +353,7 @@ public: /// Construct an execution trace for reverse AD virtual const value_type& traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + char* raw) const { trace.setLeaf(key_); return dynamic_cast(values.at(key_)); } @@ -366,13 +371,13 @@ class LeafExpression >: public ExpressionNode { /// Constructor with a single key LeafExpression(Key key) : - key_(key) { + key_(key) { } // todo: do we need a virtual destructor here too? friend class Expression ; -public: + public: /// Return keys that play in this expression virtual std::set keys() const { @@ -393,7 +398,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + char* raw) const { trace.setLeaf(key_); return values.at(key_); } @@ -523,7 +528,7 @@ struct GenerateFunctionalNode: Argument, Base { virtual void startReverseAD(JacobianMap& jacobians) const { Base::Record::startReverseAD(jacobians); Select::value, A>::reverseAD(This::trace, This::dTdA, - jacobians); + jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process @@ -535,7 +540,7 @@ struct GenerateFunctionalNode: Argument, Base { /// Version specialized to 2-dimensional output typedef Eigen::Matrix::value> Jacobian2T; virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { + JacobianMap& jacobians) const { Base::Record::reverseAD2(dFdT, jacobians); This::trace.reverseAD2(dFdT * This::dTdA, jacobians); } @@ -549,7 +554,7 @@ struct GenerateFunctionalNode: Argument, Base { // Iff the expression is functional, write all Records in raw buffer // Return value of type T is recorded in record->value record->Record::This::value = This::expression->traceExecution(values, - record->Record::This::trace, raw); + record->Record::This::trace, raw); // raw is never modified by traceExecution, but if traceExecution has // written in the buffer, the next caller expects we advance the pointer raw += This::expression->traceSize(); @@ -623,26 +628,26 @@ struct FunctionalNode { template class UnaryExpression: public FunctionalNode >::type { -public: + public: typedef boost::function::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; -private: + private: Function function_; /// Constructor with a unary function f, and input argument e UnaryExpression(Function f, const Expression& e1) : - function_(f) { + function_(f) { this->template reset(e1.root()); ExpressionNode::traceSize_ = sizeof(Record) + e1.traceSize(); } friend class Expression ; -public: + public: /// Return value virtual T value(const Values& values) const { @@ -651,13 +656,13 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + char* raw) const { Record* record = Base::trace(values, raw); trace.setFunction(record); return function_(record->template value(), - record->template jacobian()); + record->template jacobian()); } }; @@ -667,22 +672,22 @@ public: template class BinaryExpression: public FunctionalNode >::type { -public: + public: typedef boost::function< T(const A1&, const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; + typename OptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; -private: + private: Function function_; /// Constructor with a ternary function f, and three input arguments BinaryExpression(Function f, const Expression& e1, - const Expression& e2) : - function_(f) { + const Expression& e2) : + function_(f) { this->template reset(e1.root()); this->template reset(e2.root()); ExpressionNode::traceSize_ = // @@ -692,26 +697,26 @@ private: friend class Expression ; friend class ::ExpressionFactorBinaryTest; -public: + public: /// Return value virtual T value(const Values& values) const { using boost::none; return function_(this->template expression()->value(values), - this->template expression()->value(values), - none, none); + this->template expression()->value(values), + none, none); } /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + char* raw) const { Record* record = Base::trace(values, raw); trace.setFunction(record); return function_(record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian()); + record->template value(), record->template jacobian(), + record->template jacobian()); } }; @@ -721,22 +726,22 @@ public: template class TernaryExpression: public FunctionalNode >::type { -public: + public: typedef boost::function< T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, - typename OptionalJacobian::type, typename OptionalJacobian::type)> Function; + typename OptionalJacobian::type, typename OptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; -private: + private: Function function_; /// Constructor with a ternary function f, and three input arguments TernaryExpression(Function f, const Expression& e1, - const Expression& e2, const Expression& e3) : - function_(f) { + const Expression& e2, const Expression& e3) : + function_(f) { this->template reset(e1.root()); this->template reset(e2.root()); this->template reset(e3.root()); @@ -746,20 +751,20 @@ private: friend class Expression ; -public: + public: /// Return value virtual T value(const Values& values) const { using boost::none; return function_(this->template expression()->value(values), - this->template expression()->value(values), - this->template expression()->value(values), - none, none, none); + this->template expression()->value(values), + this->template expression()->value(values), + none, none, none); } /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + char* raw) const { Record* record = Base::trace(values, raw); trace.setFunction(record);