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

Experimenting w Fusion

release/4.3a0
dellaert 2014-10-13 22:50:47 +02:00
parent dda91df6e1
commit 70f0caf0e3
1 changed files with 62 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -425,7 +425,6 @@ TEST(ExpressionFactor, composeTernary) {
}
/* ************************************************************************* */
namespace mpl = boost::mpl;
#include <boost/mpl/assert.hpp>
@ -441,43 +440,96 @@ BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Record::Jacobian2T >));
// Try generating vectors of ExecutionTrace
typedef mpl::vector<ExecutionTrace<Pose3>, ExecutionTrace<Point3> > ExpectedTraces;
typedef mpl::transform<MyTypes,ExecutionTrace<MPL::_1> >::type MyTraces;
typedef mpl::transform<MyTypes, ExecutionTrace<MPL::_1> >::type MyTraces;
BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >));
template <class T>
template<class T>
struct MakeTrace {
typedef ExecutionTrace<T> type;
};
typedef mpl::transform<MyTypes,MakeTrace<MPL::_1> >::type MyTraces1;
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;
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;
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 !
typedef mpl::at_c<Jacobians, 1>::type Jacobian23; // base zero !
BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>));
// Create a value and access it
TEST(ExpressionFactor, JacobiansValue) {
using boost::fusion::at_c;
Matrix23 expected;
Jacobians jacobians;
using boost::fusion::at_c;
at_c<1>(jacobians) << 1,2,3,4,5,6;
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());
}
/* ************************************************************************* */
#include <boost/fusion/include/make_vector.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(double)> {
typedef double type; // result for double argument
};
// actual function
template<typename T>
typename result<triple(T)>::type operator()(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>
// Test out polymorphic transform
TEST(ExpressionFactor, Invoke) {
std::plus<int> add;
assert(invoke(add,boost::fusion::make_vector(1,1)) == 2);
// Creating a Pose3 (is there another way?)
boost::fusion::vector<Rot3, Point3> pair;
Pose3 pose = boost::fusion::invoke(boost::value_factory<Pose3>(), pair);
}
/* ************************************************************************* */
int main() {
TestResult tr;