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

Numbered types avoid ambiguity

release/4.3a0
dellaert 2014-10-11 14:19:39 +02:00
parent ae93dd9869
commit 88f9a423c5
2 changed files with 165 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -156,7 +156,7 @@ struct Select<2, A> {
/** /**
* Record the evaluation of a single argument in a functional expression * Record the evaluation of a single argument in a functional expression
*/ */
template<class T, class A> template<class T, class A, size_t N>
struct SingleTrace { struct SingleTrace {
typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA; typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
typename JacobianTrace<A>::Pointer trace; typename JacobianTrace<A>::Pointer trace;
@ -169,16 +169,19 @@ struct SingleTrace {
* C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
* and Beyond. Pearson Education. * and Beyond. Pearson Education.
*/ */
template<class T, class A, class More> template<class T, class AN, class More>
struct Trace: SingleTrace<T, A>, More { struct Trace: SingleTrace<T, typename AN::type, AN::value>, More {
typedef typename AN::type A;
const static size_t N = AN::value;
typename JacobianTrace<A>::Pointer const & myTrace() const { typename JacobianTrace<A>::Pointer const & myTrace() const {
return static_cast<const SingleTrace<T, A>*>(this)->trace; return static_cast<const SingleTrace<T, A, AN::value>*>(this)->trace;
} }
typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA; typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
const JacobianTA& myJacobian() const { const JacobianTA& myJacobian() const {
return static_cast<const SingleTrace<T, A>*>(this)->dTdA; return static_cast<const SingleTrace<T, A, AN::value>*>(this)->dTdA;
} }
/// Start the reverse AD process /// Start the reverse AD process
@ -202,6 +205,14 @@ struct Trace: SingleTrace<T, A>, More {
} }
}; };
/// 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 Trace Class Generator /// Recursive Trace Class Generator
template<class T, class TYPES> template<class T, class TYPES>
struct GenerateTrace { struct GenerateTrace {
@ -486,7 +497,7 @@ public:
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
typedef boost::mpl::vector<A1> Arguments; typedef boost::mpl::vector<Numbered<A1, 1> > Arguments;
typedef typename GenerateTrace<T, Arguments>::type Trace; typedef typename GenerateTrace<T, Arguments>::type Trace;
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
@ -558,7 +569,7 @@ public:
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
typedef boost::mpl::vector<A1, A2> Arguments; typedef boost::mpl::vector<Numbered<A1, 1>, Numbered<A2, 2> > Arguments;
typedef typename GenerateTrace<T, Arguments>::type Trace; typedef typename GenerateTrace<T, Arguments>::type Trace;
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
@ -567,11 +578,11 @@ public:
Trace* trace = new Trace(); Trace* trace = new Trace();
p.setFunction(trace); p.setFunction(trace);
A1 a1 = this->expressionA1_->traceExecution(values, A1 a1 = this->expressionA1_->traceExecution(values,
static_cast<SingleTrace<T, A1>*>(trace)->trace); static_cast<SingleTrace<T, A1, 1>*>(trace)->trace);
A2 a2 = this->expressionA2_->traceExecution(values, A2 a2 = this->expressionA2_->traceExecution(values,
static_cast<SingleTrace<T, A2>*>(trace)->trace); static_cast<SingleTrace<T, A2, 2>*>(trace)->trace);
return function_(a1, a2, static_cast<SingleTrace<T, A1>*>(trace)->dTdA, return function_(a1, a2, static_cast<SingleTrace<T, A1, 1>*>(trace)->dTdA,
static_cast<SingleTrace<T, A2>*>(trace)->dTdA); static_cast<SingleTrace<T, A2, 2>*>(trace)->dTdA);
} }
}; };
@ -647,7 +658,7 @@ public:
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
typedef boost::mpl::vector<A1, A2, A3> Arguments; typedef boost::mpl::vector<Numbered<A1, 1>, Numbered<A2, 2>, Numbered<A3, 3> > Arguments;
typedef typename GenerateTrace<T, Arguments>::type Trace; typedef typename GenerateTrace<T, Arguments>::type Trace;
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
@ -656,14 +667,15 @@ public:
Trace* trace = new Trace(); Trace* trace = new Trace();
p.setFunction(trace); p.setFunction(trace);
A1 a1 = this->expressionA1_->traceExecution(values, A1 a1 = this->expressionA1_->traceExecution(values,
static_cast<SingleTrace<T, A1>*>(trace)->trace); static_cast<SingleTrace<T, A1, 1>*>(trace)->trace);
A2 a2 = this->expressionA2_->traceExecution(values, A2 a2 = this->expressionA2_->traceExecution(values,
static_cast<SingleTrace<T, A2>*>(trace)->trace); static_cast<SingleTrace<T, A2, 2>*>(trace)->trace);
A3 a3 = this->expressionA3_->traceExecution(values, A3 a3 = this->expressionA3_->traceExecution(values,
static_cast<SingleTrace<T, A3>*>(trace)->trace); static_cast<SingleTrace<T, A3, 3>*>(trace)->trace);
return function_(a1, a2, a3, static_cast<SingleTrace<T, A1>*>(trace)->dTdA, return function_(a1, a2, a3,
static_cast<SingleTrace<T, A2>*>(trace)->dTdA, static_cast<SingleTrace<T, A1, 1>*>(trace)->dTdA,
static_cast<SingleTrace<T, A3>*>(trace)->dTdA); static_cast<SingleTrace<T, A2, 2>*>(trace)->dTdA,
static_cast<SingleTrace<T, A3, 3>*>(trace)->dTdA);
} }
}; };

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

@ -184,139 +184,139 @@ TEST(ExpressionFactor, test2) {
EXPECT( assert_equal(*expected, *gf3, 1e-9)); EXPECT( assert_equal(*expected, *gf3, 1e-9));
} }
///* ************************************************************************* */ /* ************************************************************************* */
//
//TEST(ExpressionFactor, compose1) { TEST(ExpressionFactor, compose1) {
//
// // Create expression // Create expression
// Rot3_ R1(1), R2(2); Rot3_ R1(1), R2(2);
// Rot3_ R3 = R1 * R2; Rot3_ R3 = R1 * R2;
//
// // Create factor // Create factor
// ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
//
// // Create some values // Create some values
// Values values; Values values;
// values.insert(1, Rot3()); values.insert(1, Rot3());
// values.insert(2, Rot3()); values.insert(2, Rot3());
//
// // Check unwhitenedError // Check unwhitenedError
// std::vector<Matrix> H(2); std::vector<Matrix> H(2);
// Vector actual = f.unwhitenedError(values, H); Vector actual = f.unwhitenedError(values, H);
// EXPECT( assert_equal(eye(3), H[0],1e-9)); EXPECT( assert_equal(eye(3), H[0],1e-9));
// EXPECT( assert_equal(eye(3), H[1],1e-9)); EXPECT( assert_equal(eye(3), H[1],1e-9));
//
// // Check linearization // Check linearization
// JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
// boost::shared_ptr<GaussianFactor> gf = f.linearize(values); boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
// boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
// boost::dynamic_pointer_cast<JacobianFactor>(gf); boost::dynamic_pointer_cast<JacobianFactor>(gf);
// EXPECT( assert_equal(expected, *jf,1e-9)); EXPECT( assert_equal(expected, *jf,1e-9));
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//// Test compose with arguments referring to the same rotation // Test compose with arguments referring to the same rotation
//TEST(ExpressionFactor, compose2) { TEST(ExpressionFactor, compose2) {
//
// // Create expression // Create expression
// Rot3_ R1(1), R2(1); Rot3_ R1(1), R2(1);
// Rot3_ R3 = R1 * R2; Rot3_ R3 = R1 * R2;
//
// // Create factor // Create factor
// ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
//
// // Create some values // Create some values
// Values values; Values values;
// values.insert(1, Rot3()); values.insert(1, Rot3());
//
// // Check unwhitenedError // Check unwhitenedError
// std::vector<Matrix> H(1); std::vector<Matrix> H(1);
// Vector actual = f.unwhitenedError(values, H); Vector actual = f.unwhitenedError(values, H);
// EXPECT_LONGS_EQUAL(1, H.size()); EXPECT_LONGS_EQUAL(1, H.size());
// EXPECT( assert_equal(2*eye(3), H[0],1e-9)); EXPECT( assert_equal(2*eye(3), H[0],1e-9));
//
// // Check linearization // Check linearization
// JacobianFactor expected(1, 2 * eye(3), zero(3)); JacobianFactor expected(1, 2 * eye(3), zero(3));
// boost::shared_ptr<GaussianFactor> gf = f.linearize(values); boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
// boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
// boost::dynamic_pointer_cast<JacobianFactor>(gf); boost::dynamic_pointer_cast<JacobianFactor>(gf);
// EXPECT( assert_equal(expected, *jf,1e-9)); EXPECT( assert_equal(expected, *jf,1e-9));
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//// Test compose with one arguments referring to a constant same rotation // Test compose with one arguments referring to a constant same rotation
//TEST(ExpressionFactor, compose3) { TEST(ExpressionFactor, compose3) {
//
// // Create expression // Create expression
// Rot3_ R1(Rot3::identity()), R2(3); Rot3_ R1(Rot3::identity()), R2(3);
// Rot3_ R3 = R1 * R2; Rot3_ R3 = R1 * R2;
//
// // Create factor // Create factor
// ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
//
// // Create some values // Create some values
// Values values; Values values;
// values.insert(3, Rot3()); values.insert(3, Rot3());
//
// // Check unwhitenedError // Check unwhitenedError
// std::vector<Matrix> H(1); std::vector<Matrix> H(1);
// Vector actual = f.unwhitenedError(values, H); Vector actual = f.unwhitenedError(values, H);
// EXPECT_LONGS_EQUAL(1, H.size()); EXPECT_LONGS_EQUAL(1, H.size());
// EXPECT( assert_equal(eye(3), H[0],1e-9)); EXPECT( assert_equal(eye(3), H[0],1e-9));
//
// // Check linearization // Check linearization
// JacobianFactor expected(3, eye(3), zero(3)); JacobianFactor expected(3, eye(3), zero(3));
// boost::shared_ptr<GaussianFactor> gf = f.linearize(values); boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
// boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
// boost::dynamic_pointer_cast<JacobianFactor>(gf); boost::dynamic_pointer_cast<JacobianFactor>(gf);
// EXPECT( assert_equal(expected, *jf,1e-9)); EXPECT( assert_equal(expected, *jf,1e-9));
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//// Test compose with three arguments // Test compose with three arguments
//Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
// boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2, boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
// boost::optional<Matrix3&> H3) { boost::optional<Matrix3&> H3) {
// // return dummy derivatives (not correct, but that's ok for testing here) // return dummy derivatives (not correct, but that's ok for testing here)
// if (H1) if (H1)
// *H1 = eye(3); *H1 = eye(3);
// if (H2) if (H2)
// *H2 = eye(3); *H2 = eye(3);
// if (H3) if (H3)
// *H3 = eye(3); *H3 = eye(3);
// return R1 * (R2 * R3); return R1 * (R2 * R3);
//} }
//
//TEST(ExpressionFactor, composeTernary) { TEST(ExpressionFactor, composeTernary) {
//
// // Create expression // Create expression
// Rot3_ A(1), B(2), C(3); Rot3_ A(1), B(2), C(3);
// Rot3_ ABC(composeThree, A, B, C); Rot3_ ABC(composeThree, A, B, C);
//
// // Create factor // Create factor
// ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC); ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
//
// // Create some values // Create some values
// Values values; Values values;
// values.insert(1, Rot3()); values.insert(1, Rot3());
// values.insert(2, Rot3()); values.insert(2, Rot3());
// values.insert(3, Rot3()); values.insert(3, Rot3());
//
// // Check unwhitenedError // Check unwhitenedError
// std::vector<Matrix> H(3); std::vector<Matrix> H(3);
// Vector actual = f.unwhitenedError(values, H); Vector actual = f.unwhitenedError(values, H);
// EXPECT_LONGS_EQUAL(3, H.size()); EXPECT_LONGS_EQUAL(3, H.size());
// EXPECT( assert_equal(eye(3), H[0],1e-9)); EXPECT( assert_equal(eye(3), H[0],1e-9));
// EXPECT( assert_equal(eye(3), H[1],1e-9)); EXPECT( assert_equal(eye(3), H[1],1e-9));
// EXPECT( assert_equal(eye(3), H[2],1e-9)); EXPECT( assert_equal(eye(3), H[2],1e-9));
//
// // Check linearization // Check linearization
// JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
// boost::shared_ptr<GaussianFactor> gf = f.linearize(values); boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
// boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
// boost::dynamic_pointer_cast<JacobianFactor>(gf); boost::dynamic_pointer_cast<JacobianFactor>(gf);
// EXPECT( assert_equal(expected, *jf,1e-9)); EXPECT( assert_equal(expected, *jf,1e-9));
//} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -325,7 +325,8 @@ namespace mpl = boost::mpl;
#include <boost/mpl/assert.hpp> #include <boost/mpl/assert.hpp>
template<class T> struct Incomplete; template<class T> struct Incomplete;
typedef mpl::vector<Pose3, Point3, Cal3_S2> MyTypes; typedef mpl::vector<Numbered<Pose3, 1>, Numbered<Point3, 2>,
Numbered<Cal3_S2, 3> > MyTypes;
typedef GenerateTrace<Point2, MyTypes>::type Generated; typedef GenerateTrace<Point2, MyTypes>::type Generated;
//Incomplete<Generated> incomplete; //Incomplete<Generated> incomplete;
//BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); //BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >));