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

No more boost::mpl needed for Expressions

release/4.3a0
dellaert 2015-05-12 01:33:33 -07:00
parent 4a8dbd689a
commit 4f846ff75f
4 changed files with 1 additions and 520 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -2038,14 +2038,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpressionMeta.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testExpressionMeta.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

1
gtsam/nonlinear/internal/CallRecord.h
View File

@ -21,7 +21,6 @@
#pragma once
#include <gtsam/nonlinear/internal/JacobianMap.h>
#include <boost/mpl/transform.hpp>
namespace gtsam {
namespace internal {

264
gtsam/nonlinear/internal/ExpressionNode.h
View File

@ -23,10 +23,6 @@
#include <gtsam/nonlinear/internal/CallRecord.h>
#include <gtsam/nonlinear/Values.h>
// template meta-programming headers
#include <boost/mpl/fold.hpp>
namespace MPL = boost::mpl::placeholders;
#include <typeinfo> // operator typeid
#include <ostream>
#include <map>
@ -191,272 +187,16 @@ public:
};
//-----------------------------------------------------------------------------
// Below we use the "Class Composition" technique described in the book
// 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<T,A1,1>, FunctionalBase<T> {
// ... storage related to A1 ...
// ... methods that work on A1 ...
// };
//
// struct Base2 : Argument<T,A2,2>, Base1 {
// ... storage related to A2 ...
// ... methods that work on A2 and (recursively) on A1 ...
// };
//
// struct Base3 : Argument<T,A3,3>, Base2 {
// ... storage related to A3 ...
// ... methods that work on A3 and (recursively) on A2 and A1 ...
// };
//
// struct FunctionalNode : Base3 {
// Provides convenience access to storage in hierarchy by using
// static_cast<Argument<T, A, N> &>(*this)
// }
//
// 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<T,A1,1>, CallRecord<traits::dimension<T>::value> {
// ... storage related to A1 ...
// ... methods that work on A1 ...
// };
//
// struct Record2 : JacobianTrace<T,A2,2>, Record1 {
// ... storage related to A2 ...
// ... methods that work on A2 and (recursively) on A1 ...
// };
//
// struct Record3 : JacobianTrace<T,A3,3>, 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<JacobianTrace<T, A, N> &>(*this)
// }
//
//-----------------------------------------------------------------------------
/// meta-function to generate fixed-size JacobianTA type
template<class T, class A>
struct Jacobian {
typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
};
/**
* Base case for recursive FunctionalNode class
*/
template<class T>
struct FunctionalBase: ExpressionNode<T> {
static size_t const N = 0; // number of arguments
struct Record {
void print(const std::string& indent) const {
}
void startReverseAD4(JacobianMap& jacobians) const {
}
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
}
};
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record,
ExecutionTraceStorage*& traceStorage) const {
// base case: does not do anything
}
};
/**
* Building block for recursive FunctionalNode class
* The integer argument N is to guarantee a unique type signature,
* so we are guaranteed to be able to extract their values by static cast.
*/
template<class T, class A, size_t N>
struct Argument {
/// Expression that will generate value/derivatives for argument
boost::shared_ptr<ExpressionNode<A> > expression;
};
/**
* Building block for Recursive Record Class
* Records the evaluation of a single argument in a functional expression
*/
template<class T, class A, size_t N>
struct JacobianTrace {
A value;
ExecutionTrace<A> trace;
typename Jacobian<T, A>::type dTdA;
};
// Recursive Definition of Functional ExpressionNode
// The reason we inherit from Argument<T, A, N> is because we can then
// case to this unique signature to retrieve the expression at any level
template<class T, class A, class Base>
struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy
typedef Argument<T, A, N> This; ///< The storage we have direct access to
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys = Base::keys();
std::set<Key> myKeys = This::expression->keys();
keys.insert(myKeys.begin(), myKeys.end());
return keys;
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
Base::dims(map);
This::expression->dims(map);
}
// Recursive Record Class for Functional Expressions
// The reason we inherit from JacobianTrace<T, A, N> is because we can then
// case to this unique signature to retrieve the value/trace at any level
struct Record: JacobianTrace<T, A, N>, Base::Record {
typedef JacobianTrace<T, A, N> This;
/// Print to std::cout
void print(const std::string& indent) const {
Base::Record::print(indent);
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
std::cout << This::dTdA.format(matlab) << std::endl;
This::trace.print(indent);
}
/// Start the reverse AD process
void startReverseAD4(JacobianMap& jacobians) const {
Base::Record::startReverseAD4(jacobians);
// This is the crucial point where the size of the AD pipeline is selected.
// One pipeline is started for each argument, but the number of rows in each
// pipeline is the same, namely the dimension of the output argument T.
// For example, if the entire expression is rooted by a binary function
// yielding a 2D result, then the matrix dTdA will have 2 rows.
// ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2
// which calls the correctly sized CallRecord::reverseAD3, which in turn
// calls reverseAD4 below.
This::trace.reverseAD1(This::dTdA, jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
// Cols is always known at compile time
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
Base::Record::reverseAD4(dFdT, jacobians);
This::trace.reverseAD1(dFdT * This::dTdA, jacobians);
}
};
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record,
ExecutionTraceStorage*& traceStorage) const {
Base::trace(values, record, traceStorage); // recurse
// Write an Expression<A> execution trace in record->trace
// Iff Constant or Leaf, this will not write to traceStorage, only to trace.
// Iff the expression is functional, write all Records in traceStorage buffer
// Return value of type T is recorded in record->value
record->Record::This::value = This::expression->traceExecution(values,
record->Record::This::trace, traceStorage);
// traceStorage is never modified by traceExecution, but if traceExecution has
// written in the buffer, the next caller expects we advance the pointer
traceStorage += This::expression->traceSize();
}
};
/**
* Recursive GenerateFunctionalNode class Generator
*/
template<class T, class TYPES>
struct FunctionalNode {
/// The following typedef generates the recursively defined Base class
typedef typename boost::mpl::fold<TYPES, FunctionalBase<T>,
GenerateFunctionalNode<T, MPL::_2, MPL::_1> >::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<TYPES, Jacobian<T, MPL::_1> >::type Jacobians;
typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
#endif
/// Reset expression shared pointer
template<class A, size_t N>
void reset(const boost::shared_ptr<ExpressionNode<A> >& ptr) {
static_cast<Argument<T, A, N> &>(*this).expression = ptr;
}
/// Access Expression shared pointer
template<class A, size_t N>
boost::shared_ptr<ExpressionNode<A> > expression() const {
return static_cast<Argument<T, A, N> const &>(*this).expression;
}
/// Provide convenience access to Record storage and implement
/// the virtual function based interface of CallRecord using the CallRecordImplementor
struct Record: public CallRecordImplementor<Record, traits<T>::dimension>,
public Base::Record {
using Base::Record::print;
using Base::Record::startReverseAD4;
using Base::Record::reverseAD4;
virtual ~Record() {
}
/// Access Value
template<class A, size_t N>
const A& value() const {
return static_cast<JacobianTrace<T, A, N> const &>(*this).value;
}
/// Access Jacobian
template<class A, size_t N>
typename Jacobian<T, A>::type& jacobian() {
return static_cast<JacobianTrace<T, A, N>&>(*this).dTdA;
}
};
/// Construct an execution trace for reverse AD
Record* trace(const Values& values,
ExecutionTraceStorage* traceStorage) const {
assert(reinterpret_cast<size_t>(traceStorage) % TraceAlignment == 0);
// Create the record and advance the pointer
Record* record = new (traceStorage) Record();
traceStorage += upAligned(sizeof(Record));
// Record the traces for all arguments
// After this, the traceStorage pointer is set to after what was written
Base::trace(values, record, traceStorage);
// Return the record for this function evaluation
return record;
}
};
};
//-----------------------------------------------------------------------------
// Eigen format for printing Jacobians
static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]");
//-----------------------------------------------------------------------------
/// Unary Function Expression
template<class T, class A1>
class UnaryExpression: public ExpressionNode<T> {
@ -552,7 +292,6 @@ public:
//-----------------------------------------------------------------------------
/// Binary Expression
template<class T, class A1, class A2>
class BinaryExpression: public ExpressionNode<T> {
@ -646,7 +385,6 @@ public:
//-----------------------------------------------------------------------------
/// Ternary Expression
template<class T, class A1, class A2, class A3>
class TernaryExpression: public ExpressionNode<T> {

248
gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
View File

@ -1,248 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testExpressionMeta.cpp
* @date October 14, 2014
* @author Frank Dellaert
* @brief Test meta-programming constructs for Expressions
*/
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <algorithm>
using namespace std;
using namespace gtsam;
using namespace gtsam::internal;
/* ************************************************************************* */
namespace mpl = boost::mpl;
#include <boost/mpl/assert.hpp>
#include <boost/mpl/equal.hpp>
template<class T> struct Incomplete;
// Check generation of FunctionalNode
typedef mpl::vector<Pose3, Point3> MyTypes;
typedef FunctionalNode<Point2, MyTypes>::type Generated;
//Incomplete<Generated> incomplete;
// Try generating vectors of ExecutionTrace
typedef mpl::vector<ExecutionTrace<Pose3>, ExecutionTrace<Point3> > ExpectedTraces;
typedef mpl::transform<MyTypes, ExecutionTrace<MPL::_1> >::type MyTraces;
BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >));
template<class T>
struct MakeTrace {
typedef ExecutionTrace<T> type;
};
typedef mpl::transform<MyTypes, MakeTrace<MPL::_1> >::type MyTraces1;
BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >));
// Try generating vectors of Expression types
typedef mpl::vector<Expression<Pose3>, Expression<Point3> > ExpectedExpressions;
typedef mpl::transform<MyTypes, Expression<MPL::_1> >::type Expressions;
BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >));
// Try generating vectors of Jacobian types
typedef mpl::vector<Matrix26, Matrix23> ExpectedJacobians;
typedef mpl::transform<MyTypes, Jacobian<Point2, MPL::_1> >::type Jacobians;
BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >));
// Try accessing a Jacobian
typedef mpl::at_c<Jacobians, 1>::type Jacobian23; // base zero !
BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>));
/* ************************************************************************* */
// Boost Fusion includes allow us to create/access values from MPL vectors
#include <boost/fusion/include/mpl.hpp>
#include <boost/fusion/include/at_c.hpp>
// Create a value and access it
TEST(ExpressionFactor, JacobiansValue) {
using boost::fusion::at_c;
Matrix23 expected;
Jacobians jacobians;
at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6;
Matrix23 actual = at_c<1>(jacobians);
CHECK(actual.cols() == expected.cols());
CHECK(actual.rows() == expected.rows());
}
/* ************************************************************************* */
// Test out polymorphic transform
#include <boost/fusion/include/make_vector.hpp>
#include <boost/fusion/include/transform.hpp>
#include <boost/utility/result_of.hpp>
struct triple {
template<class > struct result; // says we will provide result
template<class F>
struct result<F(int)> {
typedef double type; // result for int argument
};
template<class F>
struct result<F(const int&)> {
typedef double type; // result for int argument
};
template<class F>
struct result<F(const double &)> {
typedef double type; // result for double argument
};
template<class F>
struct result<F(double)> {
typedef double type; // result for double argument
};
// actual function
template<typename T>
typename result<triple(T)>::type operator()(const T& x) const {
return (double) x;
}
};
// Test out polymorphic transform
TEST(ExpressionFactor, Triple) {
typedef boost::fusion::vector<int, double> IntDouble;
IntDouble H = boost::fusion::make_vector(1, 2.0);
// Only works if I use Double2
typedef boost::fusion::result_of::transform<IntDouble, triple>::type Result;
typedef boost::fusion::vector<double, double> Double2;
Double2 D = boost::fusion::transform(H, triple());
using boost::fusion::at_c;
DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9);
DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9);
}
/* ************************************************************************* */
#include <boost/fusion/include/invoke.hpp>
#include <boost/functional/value_factory.hpp>
#include <boost/fusion/functional/adapter/fused.hpp>
#include <boost/fusion/adapted/mpl.hpp>
// Test out invoke
TEST(ExpressionFactor, Invoke) {
EXPECT_LONGS_EQUAL(2, invoke(plus<int>(),boost::fusion::make_vector(1,1)));
// Creating a Pose3 (is there another way?)
boost::fusion::vector<Rot3, Point3> pair;
Pose3 pose = boost::fusion::invoke(boost::value_factory<Pose3>(), pair);
}
/* ************************************************************************* */
// debug const issue (how to make read/write arguments for invoke)
struct test {
typedef void result_type;
void operator()(int& a, int& b) const {
a = 6;
b = 7;
}
};
TEST(ExpressionFactor, ConstIssue) {
int a, b;
boost::fusion::invoke(test(),
boost::fusion::make_vector(boost::ref(a), boost::ref(b)));
LONGS_EQUAL(6, a);
LONGS_EQUAL(7, b);
}
/* ************************************************************************* */
// Test out invoke on a given GTSAM function
// then construct prototype for it's derivatives
TEST(ExpressionFactor, InvokeDerivatives) {
// This is the method in Pose3:
// Point3 transform_to(const Point3& p) const;
// Point3 transform_to(const Point3& p,
// boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
// Let's assign it it to a boost function object
// cast is needed because Pose3::transform_to is overloaded
// typedef boost::function<Point3(const Pose3&, const Point3&)> F;
// F f = static_cast<Point3 (Pose3::*)(
// const Point3&) const >(&Pose3::transform_to);
//
// // Create arguments
// Pose3 pose;
// Point3 point;
// typedef boost::fusion::vector<Pose3, Point3> Arguments;
// Arguments args = boost::fusion::make_vector(pose, point);
//
// // Create fused function (takes fusion vector) and call it
// boost::fusion::fused<F> g(f);
// Point3 actual = g(args);
// CHECK(assert_equal(point,actual));
//
// // We can *immediately* do this using invoke
// Point3 actual2 = boost::fusion::invoke(f, args);
// CHECK(assert_equal(point,actual2));
// Now, let's create the optional Jacobian arguments
typedef Point3 T;
typedef boost::mpl::vector<Pose3, Point3> TYPES;
typedef boost::mpl::transform<TYPES, MakeOptionalJacobian<T, MPL::_1> >::type Optionals;
// Unfortunately this is moot: we need a pointer to a function with the
// optional derivatives; I don't see a way of calling a function that we
// did not get access to by the caller passing us a pointer.
// Let's test below whether we can have a proxy object
}
struct proxy {
typedef Point3 result_type;
Point3 operator()(const Pose3& pose, const Point3& point) const {
return pose.transform_to(point);
}
Point3 operator()(const Pose3& pose, const Point3& point,
OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
return pose.transform_to(point, Dpose, Dpoint);
}
};
TEST(ExpressionFactor, InvokeDerivatives2) {
// without derivatives
Pose3 pose;
Point3 point;
Point3 actual = boost::fusion::invoke(proxy(),
boost::fusion::make_vector(pose, point));
CHECK(assert_equal(point,actual));
// with derivatives, does not work, const issue again
Matrix36 Dpose;
Matrix3 Dpoint;
Point3 actual2 = boost::fusion::invoke(proxy(),
boost::fusion::make_vector(pose, point, boost::ref(Dpose),
boost::ref(Dpoint)));
CHECK(assert_equal(point,actual2));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */