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

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.h
release/4.3a0
dellaert 2014-10-11 15:20:12 +02:00
parent 252024cc9b 88f9a423c5
commit 1bac83381f
2 changed files with 175 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -27,6 +27,17 @@
#include <new> // for placement new #include <new> // for placement new
struct TestBinaryExpression; 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 { namespace gtsam {
template<typename T> template<typename T>
@ -44,12 +55,16 @@ typedef std::map<Key, Matrix> JacobianMap;
*/ */
template<int COLS> template<int COLS>
struct CallRecord { struct CallRecord {
virtual void print(const std::string& indent) const = 0; virtual void print(const std::string& indent) const {
virtual void startReverseAD(JacobianMap& jacobians) const = 0; }
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; virtual void startReverseAD(JacobianMap& jacobians) const {
}
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
}
typedef Eigen::Matrix<double, 2, COLS> Jacobian2T; typedef Eigen::Matrix<double, 2, COLS> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT, 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 * Value and Jacobians
@ -257,7 +350,7 @@ public:
} }
/// debugging /// debugging
virtual void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) { void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
BOOST_FOREACH(const Pair& term, jacobians_) BOOST_FOREACH(const Pair& term, jacobians_)
std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows() std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows()
<< "x" << term.second.cols() << ") "; << "x" << term.second.cols() << ") ";
@ -443,31 +536,9 @@ public:
return Augmented<T>(t, dTdA, argument.jacobians()); return Augmented<T>(t, dTdA, argument.jacobians());
} }
/// Record structure for reverse AD /// CallRecord structure for reverse AD
struct Record: public CallRecord<T::dimension> { typedef boost::mpl::vector<Numbered<A1, 1> > Arguments;
ExecutionTrace<A1> trace1; typedef typename GenerateRecord<T, Arguments>::type Record;
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);
}
};
// Return size needed for memory buffer in traceExecution // Return size needed for memory buffer in traceExecution
virtual size_t traceSize() const { virtual size_t traceSize() const {
@ -479,9 +550,12 @@ public:
char* raw) const { char* raw) const {
Record* record = new (raw) Record(); Record* record = new (raw) Record();
trace.setFunction(record); trace.setFunction(record);
raw = (char*) (record + 1); raw = (char*) (record + 1);
A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); A1 a1 = this->expressionA1_->traceExecution(values,
return function_(a1, record->dTdA1); 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()); return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians());
} }
/// Record structure for reverse AD /// CallRecord structure for reverse AD
struct Record: public CallRecord<T::dimension> { typedef boost::mpl::vector<Numbered<A1, 1>, Numbered<A2, 2> > Arguments;
ExecutionTrace<A1> trace1; typedef typename GenerateRecord<T, Arguments>::type Record;
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);
}
};
// Return size needed for memory buffer in traceExecution // Return size needed for memory buffer in traceExecution
virtual size_t traceSize() const { virtual size_t traceSize() const {
@ -589,11 +634,17 @@ public:
char* raw) const { char* raw) const {
Record* record = new (raw) Record(); Record* record = new (raw) Record();
trace.setFunction(record); trace.setFunction(record);
raw = (char*) (record + 1); 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(); raw = raw + expressionA1_->traceSize();
A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw); A2 a2 = this->expressionA2_->traceExecution(values,
return function_(a1, a2, record->dTdA1, record->dTdA2); 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()); a3.jacobians());
} }
/// Record structure for reverse AD /// CallRecord structure for reverse AD
struct Record: public CallRecord<T::dimension> { typedef boost::mpl::vector<Numbered<A1, 1>, Numbered<A2, 2>, Numbered<A3, 3> > Arguments;
ExecutionTrace<A1> trace1; typedef typename GenerateRecord<T, Arguments>::type Record;
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);
}
};
// Return size needed for memory buffer in traceExecution // Return size needed for memory buffer in traceExecution
virtual size_t traceSize() const { virtual size_t traceSize() const {
@ -719,13 +734,22 @@ public:
char* raw) const { char* raw) const {
Record* record = new (raw) Record(); Record* record = new (raw) Record();
trace.setFunction(record); trace.setFunction(record);
raw = (char*) (record + 1); 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(); 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(); raw = raw + expressionA2_->traceSize();
A3 a3 = this->expressionA3_->traceExecution(values, record->trace3, raw); A3 a3 = this->expressionA3_->traceExecution(values,
return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); 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);
} }
}; };

48
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -158,10 +158,12 @@ TEST(ExpressionFactor, binary) {
expected22 << 1, 0, 0, 1; expected22 << 1, 0, 0, 1;
// Check matrices // Check matrices
boost::optional<Binary::Record*> p = trace.record<Binary::Record>(); boost::optional<Binary::Record*> r = trace.record<Binary::Record>();
CHECK(p); CHECK(r);
EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); EXPECT(
EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); 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)) // Unary(Binary(Leaf,Leaf))
@ -205,9 +207,10 @@ TEST(ExpressionFactor, shallow) {
expected23 << 1, 0, 0, 0, 1, 0; expected23 << 1, 0, 0, 0, 1, 0;
// Check matrices // Check matrices
boost::optional<Unary::Record*> p = trace.record<Unary::Record>(); boost::optional<Unary::Record*> r = trace.record<Unary::Record>();
CHECK(p); CHECK(r);
EXPECT( assert_equal(expected23, (Matrix)(*p)->dTdA1, 1e-9)); EXPECT(
assert_equal(expected23, (Matrix)static_cast<Argument<Point2, Point3, 1>*>(*r)->dTdA, 1e-9));
// Linearization // Linearization
ExpressionFactor<Point2> f2(model, measured, expression); ExpressionFactor<Point2> f2(model, measured, expression);
@ -401,6 +404,37 @@ TEST(ExpressionFactor, composeTernary) {
EXPECT( assert_equal(expected, *jf,1e-9)); 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() { int main() {
TestResult tr; TestResult tr;