diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index f49b985ba..40fc54751 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -64,7 +64,8 @@ public: } /// Access via key VerticalBlockMatrix::Block operator()(Key key) { - FastVector::const_iterator it = std::find(keys_.begin(),keys_.end(),key); + FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), + key); DenseIndex block = it - keys_.begin(); return Ab_(block); } @@ -98,7 +99,7 @@ struct CallRecord { template void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { - jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference + jacobians(key).block(0, 0) += dTdA; // block makes HUGE difference } /// Handle Leaf Case for Dynamic Matrix type (slower) template<> @@ -311,9 +312,9 @@ public: //----------------------------------------------------------------------------- /// Leaf Expression -template > +template > class LeafExpression: public ExpressionNode { - typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? + typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? /// The key into values Key key_; @@ -347,8 +348,8 @@ public: } /// Construct an execution trace for reverse AD - virtual const value_type& traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + virtual const value_type& traceExecution(const Values& values, + ExecutionTrace& trace, char* raw) const { trace.setLeaf(key_); return dynamic_cast(values.at(key_)); } @@ -358,7 +359,7 @@ public: //----------------------------------------------------------------------------- /// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value template -class LeafExpression >: public ExpressionNode { +class LeafExpression > : public ExpressionNode { typedef T value_type; /// The key into values @@ -405,6 +406,7 @@ public: // C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost // and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. // to recursively generate a class, that will be the base for function nodes. +// // The class generated, for three arguments A1, A2, and A3 will be // // struct Base1 : Argument, FunctionalBase { @@ -429,6 +431,30 @@ public: // // All this magic happens when we generate the Base3 base class of FunctionalNode // by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode +// +// Similarly, the inner Record struct will be +// +// struct Record1 : JacobianTrace, CallRecord::value> { +// ... storage related to A1 ... +// ... methods that work on A1 ... +// }; +// +// struct Record2 : JacobianTrace, Record1 { +// ... storage related to A2 ... +// ... methods that work on A2 and (recursively) on A1 ... +// }; +// +// struct Record3 : JacobianTrace, Record2 { +// ... storage related to A3 ... +// ... methods that work on A3 and (recursively) on A2 and A1 ... +// }; +// +// struct Record : Record3 { +// Provides convenience access to storage in hierarchy by using +// static_cast &>(*this) +// } +// + //----------------------------------------------------------------------------- /// meta-function to generate fixed-size JacobianTA type @@ -457,6 +483,7 @@ struct FunctionalBase: ExpressionNode { /// Construct an execution trace for reverse AD void trace(const Values& values, Record* record, char*& raw) const { + // base case: does not do anything } }; @@ -562,15 +589,23 @@ struct GenerateFunctionalNode: Argument, Base { template struct FunctionalNode { + /// The following typedef generates the recursively defined Base class typedef typename boost::mpl::fold, GenerateFunctionalNode >::type Base; + /** + * The type generated by this meta-function derives from Base + * and adds access functions as well as the crucial [trace] function + */ struct type: public Base { // Argument types and derived, note these are base 0 ! + // These are currently not used - useful for Phoenix in future +#ifdef EXPRESSIONS_PHOENIX typedef TYPES Arguments; typedef typename boost::mpl::transform >::type Jacobians; typedef typename boost::mpl::transform >::type Optionals; +#endif /// Reset expression shared pointer template @@ -725,7 +760,8 @@ 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;