12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Binary works, but it's ugly and does not work for repeated types

release/4.3a0
dellaert 2014-10-10 13:29:56 +02:00
parent 24714e48c5
commit 406467e341
1 changed files with 37 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -30,8 +30,9 @@
#include <boost/mpl/front.hpp> #include <boost/mpl/front.hpp>
#include <boost/mpl/pop_front.hpp> #include <boost/mpl/pop_front.hpp>
#include <boost/mpl/fold.hpp> #include <boost/mpl/fold.hpp>
#include<boost/mpl/empty_base.hpp> #include <boost/mpl/empty_base.hpp>
#include<boost/mpl/placeholders.hpp> #include <boost/mpl/placeholders.hpp>
namespace MPL = boost::mpl::placeholders; namespace MPL = boost::mpl::placeholders;
namespace gtsam { namespace gtsam {
@ -152,41 +153,52 @@ struct Select<2, A> {
} }
}; };
/**
* Record the evaluation of a single argument in a functional expression
*/
template<class T, class A>
struct SingleTrace {
typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
typename JacobianTrace<A>::Pointer trace;
JacobianTA dTdA;
};
/** /**
* Recursive Trace Class for Functional Expressions * Recursive Trace Class for Functional Expressions
* Abrahams, David; Gurtovoy, Aleksey (2004-12-10). * Abrahams, David; Gurtovoy, Aleksey (2004-12-10).
* C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
* and Beyond (Kindle Location 1244). Pearson Education. * and Beyond. Pearson Education.
*/ */
template<class T, class A, class More> template<class T, class A, class More>
struct Trace: More { struct Trace: SingleTrace<T, A>, More {
// define dimensions
static const size_t m = T::dimension;
static const size_t n = A::dimension;
// define fixed size Jacobian matrix types typename JacobianTrace<A>::Pointer const & myTrace() const {
typedef Eigen::Matrix<double, m, n> JacobianTA; return static_cast<const SingleTrace<T, A>*>(this)->trace;
typedef Eigen::Matrix<double, 2, m> Jacobian2T; }
// declare trace that produces value A, and corresponding Jacobian typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
typename JacobianTrace<A>::Pointer trace; const JacobianTA& myJacobian() const {
JacobianTA dTdA; return static_cast<const SingleTrace<T, A>*>(this)->dTdA;
}
/// Start the reverse AD process /// Start the reverse AD process
virtual void startReverseAD(JacobianMap& jacobians) const { virtual void startReverseAD(JacobianMap& jacobians) const {
More::startReverseAD(jacobians); More::startReverseAD(jacobians);
Select<T::dimension, A>::reverseAD(trace, dTdA, jacobians); Select<T::dimension, A>::reverseAD(myTrace(), myJacobian(), jacobians);
} }
/// Given df/dT, multiply in dT/dA and continue reverse AD process /// Given df/dT, multiply in dT/dA and continue reverse AD process
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
More::reverseAD(dFdT, jacobians); More::reverseAD(dFdT, jacobians);
trace.reverseAD(dFdT * dTdA, jacobians); myTrace().reverseAD(dFdT * myJacobian(), jacobians);
} }
/// Version specialized to 2-dimensional output /// Version specialized to 2-dimensional output
typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT, virtual void reverseAD2(const Jacobian2T& dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const {
More::reverseAD2(dFdT, jacobians); More::reverseAD2(dFdT, jacobians);
trace.reverseAD2(dFdT * dTdA, jacobians); myTrace().reverseAD2(dFdT * myJacobian(), jacobians);
} }
}; };
@ -195,6 +207,7 @@ template<class T, class TYPES>
struct GenerateTrace { struct GenerateTrace {
typedef typename boost::mpl::fold<TYPES, JacobianTrace<T>, typedef typename boost::mpl::fold<TYPES, JacobianTrace<T>,
Trace<T, MPL::_2, MPL::_1> >::type type; Trace<T, MPL::_2, MPL::_1> >::type type;
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -545,39 +558,20 @@ public:
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { typedef boost::mpl::vector<A1, A2> Arguments;
typename JacobianTrace<A1>::Pointer trace1; typedef typename GenerateTrace<T, Arguments>::type Trace;
typename JacobianTrace<A2>::Pointer trace2;
JacobianTA1 dTdA1;
JacobianTA2 dTdA2;
/// Start the reverse AD process
virtual void startReverseAD(JacobianMap& jacobians) const {
Select<T::dimension, A1>::reverseAD(trace1, dTdA1, jacobians);
Select<T::dimension, A2>::reverseAD(trace2, dTdA2, jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
trace1.reverseAD(dFdT * dTdA1, jacobians);
trace2.reverseAD(dFdT * dTdA2, jacobians);
}
/// Version specialized to 2-dimensional output
typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT,
JacobianMap& jacobians) const {
trace1.reverseAD2(dFdT * dTdA1, jacobians);
trace2.reverseAD2(dFdT * dTdA2, jacobians);
}
};
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, virtual T traceExecution(const Values& values,
typename JacobianTrace<T>::Pointer& p) const { typename JacobianTrace<T>::Pointer& p) const {
Trace* trace = new Trace(); Trace* trace = new Trace();
p.setFunction(trace); p.setFunction(trace);
A1 a1 = this->expressionA1_->traceExecution(values, trace->trace1); A1 a1 = this->expressionA1_->traceExecution(values,
A2 a2 = this->expressionA2_->traceExecution(values, trace->trace2); static_cast<SingleTrace<T, A1>*>(trace)->trace);
return function_(a1, a2, trace->dTdA1, trace->dTdA2); A2 a2 = this->expressionA2_->traceExecution(values,
static_cast<SingleTrace<T, A2>*>(trace)->trace);
return function_(a1, a2, static_cast<SingleTrace<T, A1>*>(trace)->dTdA,
static_cast<SingleTrace<T, A2>*>(trace)->dTdA);
} }
}; };