Merge remote-tracking branch 'origin/feature/BAD_meta' into feature/BAD: Now the Record structures are recursively generated using template meta-programming, basically mpl::fold
Conflicts: gtsam_unstable/nonlinear/Expression-inl.hrelease/4.3a0
dellaert
commit
1bac83381f
|
|
@ -27,6 +27,17 @@
|
|||
#include <new> // for placement new
|
||||
struct TestBinaryExpression;
|
||||
|
||||
// template meta-programming headers
|
||||
#include <boost/mpl/vector.hpp>
|
||||
#include <boost/mpl/plus.hpp>
|
||||
#include <boost/mpl/front.hpp>
|
||||
#include <boost/mpl/pop_front.hpp>
|
||||
#include <boost/mpl/fold.hpp>
|
||||
#include <boost/mpl/empty_base.hpp>
|
||||
#include <boost/mpl/placeholders.hpp>
|
||||
|
||||
namespace MPL = boost::mpl::placeholders;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template<typename T>
|
||||
|
|
@ -44,12 +55,16 @@ typedef std::map<Key, Matrix> JacobianMap;
|
|||
*/
|
||||
template<int COLS>
|
||||
struct CallRecord {
|
||||
virtual void print(const std::string& indent) const = 0;
|
||||
virtual void startReverseAD(JacobianMap& jacobians) const = 0;
|
||||
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
|
||||
virtual void print(const std::string& indent) const {
|
||||
}
|
||||
virtual void startReverseAD(JacobianMap& jacobians) const {
|
||||
}
|
||||
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
|
||||
}
|
||||
typedef Eigen::Matrix<double, 2, COLS> Jacobian2T;
|
||||
virtual void reverseAD2(const Jacobian2T& dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
JacobianMap& jacobians) const {
|
||||
}
|
||||
};
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
|
@ -162,6 +177,84 @@ struct Select<2, A> {
|
|||
}
|
||||
};
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
/**
|
||||
* Record the evaluation of a single argument in a functional expression
|
||||
* Building block for Recursive Record Class
|
||||
*/
|
||||
template<class T, class A, size_t N>
|
||||
struct Argument {
|
||||
typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
|
||||
ExecutionTrace<A> trace;
|
||||
JacobianTA dTdA;
|
||||
};
|
||||
|
||||
/**
|
||||
* Recursive Record Class for Functional Expressions
|
||||
* Abrahams, David; Gurtovoy, Aleksey (2004-12-10).
|
||||
* C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
|
||||
* and Beyond. Pearson Education.
|
||||
*/
|
||||
template<class T, class AN, class More>
|
||||
struct Record: Argument<T, typename AN::type, AN::value>, More {
|
||||
|
||||
typedef typename AN::type A;
|
||||
const static size_t N = AN::value;
|
||||
|
||||
ExecutionTrace<A> const & myTrace() const {
|
||||
return static_cast<const Argument<T, A, AN::value>*>(this)->trace;
|
||||
}
|
||||
|
||||
typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
|
||||
const JacobianTA& myJacobian() const {
|
||||
return static_cast<const Argument<T, A, AN::value>*>(this)->dTdA;
|
||||
}
|
||||
|
||||
/// Print to std::cout
|
||||
virtual void print(const std::string& indent) const {
|
||||
More::print(indent);
|
||||
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
|
||||
std::cout << myJacobian().format(matlab) << std::endl;
|
||||
myTrace().print(indent);
|
||||
}
|
||||
|
||||
/// Start the reverse AD process
|
||||
virtual void startReverseAD(JacobianMap& jacobians) const {
|
||||
More::startReverseAD(jacobians);
|
||||
Select<T::dimension, A>::reverseAD(myTrace(), myJacobian(), jacobians);
|
||||
}
|
||||
|
||||
/// Given df/dT, multiply in dT/dA and continue reverse AD process
|
||||
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
|
||||
More::reverseAD(dFdT, jacobians);
|
||||
myTrace().reverseAD(dFdT * myJacobian(), jacobians);
|
||||
}
|
||||
|
||||
/// Version specialized to 2-dimensional output
|
||||
typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
|
||||
virtual void reverseAD2(const Jacobian2T& dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
More::reverseAD2(dFdT, jacobians);
|
||||
myTrace().reverseAD2(dFdT * myJacobian(), jacobians);
|
||||
}
|
||||
};
|
||||
|
||||
/// Meta-function for generating a numbered type
|
||||
template<class A, size_t N>
|
||||
struct Numbered {
|
||||
typedef A type;
|
||||
typedef size_t value_type;
|
||||
static const size_t value = N;
|
||||
};
|
||||
|
||||
/// Recursive Record class Generator
|
||||
template<class T, class TYPES>
|
||||
struct GenerateRecord {
|
||||
typedef typename boost::mpl::fold<TYPES, CallRecord<T::dimension>,
|
||||
Record<T, MPL::_2, MPL::_1> >::type type;
|
||||
|
||||
};
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
/**
|
||||
* Value and Jacobians
|
||||
|
|
@ -257,7 +350,7 @@ public:
|
|||
}
|
||||
|
||||
/// debugging
|
||||
virtual void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
|
||||
void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
|
||||
BOOST_FOREACH(const Pair& term, jacobians_)
|
||||
std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows()
|
||||
<< "x" << term.second.cols() << ") ";
|
||||
|
|
@ -443,31 +536,9 @@ public:
|
|||
return Augmented<T>(t, dTdA, argument.jacobians());
|
||||
}
|
||||
|
||||
/// Record structure for reverse AD
|
||||
struct Record: public CallRecord<T::dimension> {
|
||||
ExecutionTrace<A1> trace1;
|
||||
JacobianTA dTdA1;
|
||||
/// print to std::cout
|
||||
virtual void print(const std::string& indent) const {
|
||||
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
|
||||
std::cout << dTdA1.format(matlab) << std::endl;
|
||||
trace1.print(indent);
|
||||
}
|
||||
/// Start the reverse AD process
|
||||
virtual void startReverseAD(JacobianMap& jacobians) const {
|
||||
Select<T::dimension, A1>::reverseAD(trace1, dTdA1, 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);
|
||||
}
|
||||
/// 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);
|
||||
}
|
||||
};
|
||||
/// CallRecord structure for reverse AD
|
||||
typedef boost::mpl::vector<Numbered<A1, 1> > Arguments;
|
||||
typedef typename GenerateRecord<T, Arguments>::type Record;
|
||||
|
||||
// Return size needed for memory buffer in traceExecution
|
||||
virtual size_t traceSize() const {
|
||||
|
|
@ -479,9 +550,12 @@ public:
|
|||
char* raw) const {
|
||||
Record* record = new (raw) Record();
|
||||
trace.setFunction(record);
|
||||
|
||||
raw = (char*) (record + 1);
|
||||
A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
|
||||
return function_(a1, record->dTdA1);
|
||||
A1 a1 = this->expressionA1_->traceExecution(values,
|
||||
static_cast<Argument<T, A1, 1>*>(record)->trace, raw);
|
||||
|
||||
return function_(a1, static_cast<Argument<T, A1, 1>*>(record)->dTdA);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -544,38 +618,9 @@ public:
|
|||
return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians());
|
||||
}
|
||||
|
||||
/// Record structure for reverse AD
|
||||
struct Record: public CallRecord<T::dimension> {
|
||||
ExecutionTrace<A1> trace1;
|
||||
ExecutionTrace<A2> trace2;
|
||||
JacobianTA1 dTdA1;
|
||||
JacobianTA2 dTdA2;
|
||||
/// print to std::cout
|
||||
virtual void print(const std::string& indent) const {
|
||||
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
|
||||
std::cout << indent << dTdA1.format(matlab) << std::endl;
|
||||
trace1.print(indent);
|
||||
std::cout << indent << dTdA2.format(matlab) << std::endl;
|
||||
trace2.print(indent);
|
||||
}
|
||||
/// 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);
|
||||
}
|
||||
};
|
||||
/// CallRecord structure for reverse AD
|
||||
typedef boost::mpl::vector<Numbered<A1, 1>, Numbered<A2, 2> > Arguments;
|
||||
typedef typename GenerateRecord<T, Arguments>::type Record;
|
||||
|
||||
// Return size needed for memory buffer in traceExecution
|
||||
virtual size_t traceSize() const {
|
||||
|
|
@ -589,11 +634,17 @@ public:
|
|||
char* raw) const {
|
||||
Record* record = new (raw) Record();
|
||||
trace.setFunction(record);
|
||||
|
||||
raw = (char*) (record + 1);
|
||||
A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
|
||||
A1 a1 = this->expressionA1_->traceExecution(values,
|
||||
static_cast<Argument<T, A1, 1>*>(record)->trace, raw);
|
||||
|
||||
raw = raw + expressionA1_->traceSize();
|
||||
A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw);
|
||||
return function_(a1, a2, record->dTdA1, record->dTdA2);
|
||||
A2 a2 = this->expressionA2_->traceExecution(values,
|
||||
static_cast<Argument<T, A2, 2>*>(record)->trace, raw);
|
||||
|
||||
return function_(a1, a2, static_cast<Argument<T, A1, 1>*>(record)->dTdA,
|
||||
static_cast<Argument<T, A2, 2>*>(record)->dTdA);
|
||||
}
|
||||
|
||||
};
|
||||
|
|
@ -668,45 +719,9 @@ public:
|
|||
a3.jacobians());
|
||||
}
|
||||
|
||||
/// Record structure for reverse AD
|
||||
struct Record: public CallRecord<T::dimension> {
|
||||
ExecutionTrace<A1> trace1;
|
||||
ExecutionTrace<A2> trace2;
|
||||
ExecutionTrace<A3> trace3;
|
||||
JacobianTA1 dTdA1;
|
||||
JacobianTA2 dTdA2;
|
||||
JacobianTA3 dTdA3;
|
||||
/// print to std::cout
|
||||
virtual void print(const std::string& indent) const {
|
||||
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
|
||||
std::cout << dTdA1.format(matlab) << std::endl;
|
||||
trace1.print(indent);
|
||||
std::cout << dTdA2.format(matlab) << std::endl;
|
||||
trace2.print(indent);
|
||||
std::cout << dTdA3.format(matlab) << std::endl;
|
||||
trace3.print(indent);
|
||||
}
|
||||
/// 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);
|
||||
Select<T::dimension, A3>::reverseAD(trace3, dTdA3, 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);
|
||||
trace3.reverseAD(dFdT * dTdA3, 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);
|
||||
trace3.reverseAD2(dFdT * dTdA3, jacobians);
|
||||
}
|
||||
};
|
||||
/// CallRecord structure for reverse AD
|
||||
typedef boost::mpl::vector<Numbered<A1, 1>, Numbered<A2, 2>, Numbered<A3, 3> > Arguments;
|
||||
typedef typename GenerateRecord<T, Arguments>::type Record;
|
||||
|
||||
// Return size needed for memory buffer in traceExecution
|
||||
virtual size_t traceSize() const {
|
||||
|
|
@ -719,13 +734,22 @@ public:
|
|||
char* raw) const {
|
||||
Record* record = new (raw) Record();
|
||||
trace.setFunction(record);
|
||||
|
||||
raw = (char*) (record + 1);
|
||||
A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
|
||||
A1 a1 = this->expressionA1_->traceExecution(values,
|
||||
static_cast<Argument<T, A1, 1>*>(record)->trace, raw);
|
||||
|
||||
raw = raw + expressionA1_->traceSize();
|
||||
A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw);
|
||||
A2 a2 = this->expressionA2_->traceExecution(values,
|
||||
static_cast<Argument<T, A2, 2>*>(record)->trace, raw);
|
||||
|
||||
raw = raw + expressionA2_->traceSize();
|
||||
A3 a3 = this->expressionA3_->traceExecution(values, record->trace3, raw);
|
||||
return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3);
|
||||
A3 a3 = this->expressionA3_->traceExecution(values,
|
||||
static_cast<Argument<T, A3, 3>*>(record)->trace, raw);
|
||||
|
||||
return function_(a1, a2, a3, static_cast<Argument<T, A1, 1>*>(record)->dTdA,
|
||||
static_cast<Argument<T, A2, 2>*>(record)->dTdA,
|
||||
static_cast<Argument<T, A3, 3>*>(record)->dTdA);
|
||||
}
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -158,10 +158,12 @@ TEST(ExpressionFactor, binary) {
|
|||
expected22 << 1, 0, 0, 1;
|
||||
|
||||
// Check matrices
|
||||
boost::optional<Binary::Record*> p = trace.record<Binary::Record>();
|
||||
CHECK(p);
|
||||
EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9));
|
||||
EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9));
|
||||
boost::optional<Binary::Record*> r = trace.record<Binary::Record>();
|
||||
CHECK(r);
|
||||
EXPECT(
|
||||
assert_equal(expected25, (Matrix) static_cast<Argument<Point2, Cal3_S2, 1>*> (*r)->dTdA, 1e-9));
|
||||
EXPECT(
|
||||
assert_equal(expected22, (Matrix) static_cast<Argument<Point2, Point2, 2>*> (*r)->dTdA, 1e-9));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
// Unary(Binary(Leaf,Leaf))
|
||||
|
|
@ -205,9 +207,10 @@ TEST(ExpressionFactor, shallow) {
|
|||
expected23 << 1, 0, 0, 0, 1, 0;
|
||||
|
||||
// Check matrices
|
||||
boost::optional<Unary::Record*> p = trace.record<Unary::Record>();
|
||||
CHECK(p);
|
||||
EXPECT( assert_equal(expected23, (Matrix)(*p)->dTdA1, 1e-9));
|
||||
boost::optional<Unary::Record*> r = trace.record<Unary::Record>();
|
||||
CHECK(r);
|
||||
EXPECT(
|
||||
assert_equal(expected23, (Matrix)static_cast<Argument<Point2, Point3, 1>*>(*r)->dTdA, 1e-9));
|
||||
|
||||
// Linearization
|
||||
ExpressionFactor<Point2> f2(model, measured, expression);
|
||||
|
|
@ -401,6 +404,37 @@ TEST(ExpressionFactor, composeTernary) {
|
|||
EXPECT( assert_equal(expected, *jf,1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
namespace mpl = boost::mpl;
|
||||
|
||||
#include <boost/mpl/assert.hpp>
|
||||
template<class T> struct Incomplete;
|
||||
|
||||
typedef mpl::vector<Numbered<Pose3, 1>, Numbered<Point3, 2>,
|
||||
Numbered<Cal3_S2, 3> > MyTypes;
|
||||
typedef GenerateRecord<Point2, MyTypes>::type Generated;
|
||||
//Incomplete<Generated> incomplete;
|
||||
//BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >));
|
||||
BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Jacobian2T >));
|
||||
|
||||
Generated generated;
|
||||
|
||||
typedef mpl::vector1<Point3> OneType;
|
||||
typedef mpl::pop_front<OneType>::type Empty;
|
||||
typedef mpl::pop_front<Empty>::type Bad;
|
||||
//typedef ProtoTrace<OneType> UnaryTrace;
|
||||
//BOOST_MPL_ASSERT((boost::is_same< UnaryTrace::A, Point3 >));
|
||||
|
||||
#include <boost/static_assert.hpp>
|
||||
#include <boost/mpl/plus.hpp>
|
||||
#include <boost/mpl/int.hpp>
|
||||
//#include <boost/mpl/print.hpp>
|
||||
|
||||
typedef struct {
|
||||
} Expected0;
|
||||
BOOST_MPL_ASSERT((boost::is_same< Expected0, Expected0 >));
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue