Numbered types avoid ambiguity
dellaert
parent
ae93dd9869
commit
88f9a423c5
|
|
@ -156,7 +156,7 @@ struct Select<2, A> {
|
|||
/**
|
||||
* 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 {
|
||||
typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
|
||||
typename JacobianTrace<A>::Pointer trace;
|
||||
|
|
@ -169,16 +169,19 @@ struct SingleTrace {
|
|||
* C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
|
||||
* and Beyond. Pearson Education.
|
||||
*/
|
||||
template<class T, class A, class More>
|
||||
struct Trace: SingleTrace<T, A>, More {
|
||||
template<class T, class AN, class 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 {
|
||||
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;
|
||||
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
|
||||
|
|
@ -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
|
||||
template<class T, class TYPES>
|
||||
struct GenerateTrace {
|
||||
|
|
@ -486,7 +497,7 @@ public:
|
|||
}
|
||||
|
||||
/// 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;
|
||||
|
||||
/// Construct an execution trace for reverse AD
|
||||
|
|
@ -558,7 +569,7 @@ public:
|
|||
}
|
||||
|
||||
/// 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;
|
||||
|
||||
/// Construct an execution trace for reverse AD
|
||||
|
|
@ -567,11 +578,11 @@ public:
|
|||
Trace* trace = new Trace();
|
||||
p.setFunction(trace);
|
||||
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,
|
||||
static_cast<SingleTrace<T, A2>*>(trace)->trace);
|
||||
return function_(a1, a2, static_cast<SingleTrace<T, A1>*>(trace)->dTdA,
|
||||
static_cast<SingleTrace<T, A2>*>(trace)->dTdA);
|
||||
static_cast<SingleTrace<T, A2, 2>*>(trace)->trace);
|
||||
return function_(a1, a2, static_cast<SingleTrace<T, A1, 1>*>(trace)->dTdA,
|
||||
static_cast<SingleTrace<T, A2, 2>*>(trace)->dTdA);
|
||||
}
|
||||
|
||||
};
|
||||
|
|
@ -647,7 +658,7 @@ public:
|
|||
}
|
||||
|
||||
/// 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;
|
||||
|
||||
/// Construct an execution trace for reverse AD
|
||||
|
|
@ -656,14 +667,15 @@ public:
|
|||
Trace* trace = new Trace();
|
||||
p.setFunction(trace);
|
||||
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,
|
||||
static_cast<SingleTrace<T, A2>*>(trace)->trace);
|
||||
static_cast<SingleTrace<T, A2, 2>*>(trace)->trace);
|
||||
A3 a3 = this->expressionA3_->traceExecution(values,
|
||||
static_cast<SingleTrace<T, A3>*>(trace)->trace);
|
||||
return function_(a1, a2, a3, static_cast<SingleTrace<T, A1>*>(trace)->dTdA,
|
||||
static_cast<SingleTrace<T, A2>*>(trace)->dTdA,
|
||||
static_cast<SingleTrace<T, A3>*>(trace)->dTdA);
|
||||
static_cast<SingleTrace<T, A3, 3>*>(trace)->trace);
|
||||
return function_(a1, a2, a3,
|
||||
static_cast<SingleTrace<T, A1, 1>*>(trace)->dTdA,
|
||||
static_cast<SingleTrace<T, A2, 2>*>(trace)->dTdA,
|
||||
static_cast<SingleTrace<T, A3, 3>*>(trace)->dTdA);
|
||||
}
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -184,139 +184,139 @@ TEST(ExpressionFactor, test2) {
|
|||
EXPECT( assert_equal(*expected, *gf3, 1e-9));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//
|
||||
//TEST(ExpressionFactor, compose1) {
|
||||
//
|
||||
// // Create expression
|
||||
// Rot3_ R1(1), R2(2);
|
||||
// Rot3_ R3 = R1 * R2;
|
||||
//
|
||||
// // Create factor
|
||||
// ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
//
|
||||
// // Create some values
|
||||
// Values values;
|
||||
// values.insert(1, Rot3());
|
||||
// values.insert(2, Rot3());
|
||||
//
|
||||
// // Check unwhitenedError
|
||||
// std::vector<Matrix> H(2);
|
||||
// Vector actual = f.unwhitenedError(values, H);
|
||||
// EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
// EXPECT( assert_equal(eye(3), H[1],1e-9));
|
||||
//
|
||||
// // Check linearization
|
||||
// JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
|
||||
// boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
// boost::shared_ptr<JacobianFactor> jf = //
|
||||
// boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
// EXPECT( assert_equal(expected, *jf,1e-9));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//// Test compose with arguments referring to the same rotation
|
||||
//TEST(ExpressionFactor, compose2) {
|
||||
//
|
||||
// // Create expression
|
||||
// Rot3_ R1(1), R2(1);
|
||||
// Rot3_ R3 = R1 * R2;
|
||||
//
|
||||
// // Create factor
|
||||
// ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
//
|
||||
// // Create some values
|
||||
// Values values;
|
||||
// values.insert(1, Rot3());
|
||||
//
|
||||
// // Check unwhitenedError
|
||||
// std::vector<Matrix> H(1);
|
||||
// Vector actual = f.unwhitenedError(values, H);
|
||||
// EXPECT_LONGS_EQUAL(1, H.size());
|
||||
// EXPECT( assert_equal(2*eye(3), H[0],1e-9));
|
||||
//
|
||||
// // Check linearization
|
||||
// JacobianFactor expected(1, 2 * eye(3), zero(3));
|
||||
// boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
// boost::shared_ptr<JacobianFactor> jf = //
|
||||
// boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
// EXPECT( assert_equal(expected, *jf,1e-9));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//// Test compose with one arguments referring to a constant same rotation
|
||||
//TEST(ExpressionFactor, compose3) {
|
||||
//
|
||||
// // Create expression
|
||||
// Rot3_ R1(Rot3::identity()), R2(3);
|
||||
// Rot3_ R3 = R1 * R2;
|
||||
//
|
||||
// // Create factor
|
||||
// ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
//
|
||||
// // Create some values
|
||||
// Values values;
|
||||
// values.insert(3, Rot3());
|
||||
//
|
||||
// // Check unwhitenedError
|
||||
// std::vector<Matrix> H(1);
|
||||
// Vector actual = f.unwhitenedError(values, H);
|
||||
// EXPECT_LONGS_EQUAL(1, H.size());
|
||||
// EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
//
|
||||
// // Check linearization
|
||||
// JacobianFactor expected(3, eye(3), zero(3));
|
||||
// boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
// boost::shared_ptr<JacobianFactor> jf = //
|
||||
// boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
// EXPECT( assert_equal(expected, *jf,1e-9));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//// Test compose with three arguments
|
||||
//Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
|
||||
// boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
|
||||
// boost::optional<Matrix3&> H3) {
|
||||
// // return dummy derivatives (not correct, but that's ok for testing here)
|
||||
// if (H1)
|
||||
// *H1 = eye(3);
|
||||
// if (H2)
|
||||
// *H2 = eye(3);
|
||||
// if (H3)
|
||||
// *H3 = eye(3);
|
||||
// return R1 * (R2 * R3);
|
||||
//}
|
||||
//
|
||||
//TEST(ExpressionFactor, composeTernary) {
|
||||
//
|
||||
// // Create expression
|
||||
// Rot3_ A(1), B(2), C(3);
|
||||
// Rot3_ ABC(composeThree, A, B, C);
|
||||
//
|
||||
// // Create factor
|
||||
// ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
|
||||
//
|
||||
// // Create some values
|
||||
// Values values;
|
||||
// values.insert(1, Rot3());
|
||||
// values.insert(2, Rot3());
|
||||
// values.insert(3, Rot3());
|
||||
//
|
||||
// // Check unwhitenedError
|
||||
// std::vector<Matrix> H(3);
|
||||
// Vector actual = f.unwhitenedError(values, H);
|
||||
// EXPECT_LONGS_EQUAL(3, H.size());
|
||||
// EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
// EXPECT( assert_equal(eye(3), H[1],1e-9));
|
||||
// EXPECT( assert_equal(eye(3), H[2],1e-9));
|
||||
//
|
||||
// // Check linearization
|
||||
// JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
|
||||
// boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
// boost::shared_ptr<JacobianFactor> jf = //
|
||||
// boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
// EXPECT( assert_equal(expected, *jf,1e-9));
|
||||
//}
|
||||
/* ************************************************************************* */
|
||||
|
||||
TEST(ExpressionFactor, compose1) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(1), R2(2);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Create factor
|
||||
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Rot3());
|
||||
values.insert(2, Rot3());
|
||||
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(2);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
EXPECT( assert_equal(eye(3), H[1],1e-9));
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
EXPECT( assert_equal(expected, *jf,1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test compose with arguments referring to the same rotation
|
||||
TEST(ExpressionFactor, compose2) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(1), R2(1);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Create factor
|
||||
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Rot3());
|
||||
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(1);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT_LONGS_EQUAL(1, H.size());
|
||||
EXPECT( assert_equal(2*eye(3), H[0],1e-9));
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, 2 * eye(3), zero(3));
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
EXPECT( assert_equal(expected, *jf,1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test compose with one arguments referring to a constant same rotation
|
||||
TEST(ExpressionFactor, compose3) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(Rot3::identity()), R2(3);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Create factor
|
||||
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(3, Rot3());
|
||||
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(1);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT_LONGS_EQUAL(1, H.size());
|
||||
EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(3, eye(3), zero(3));
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
EXPECT( assert_equal(expected, *jf,1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test compose with three arguments
|
||||
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
|
||||
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
|
||||
boost::optional<Matrix3&> H3) {
|
||||
// return dummy derivatives (not correct, but that's ok for testing here)
|
||||
if (H1)
|
||||
*H1 = eye(3);
|
||||
if (H2)
|
||||
*H2 = eye(3);
|
||||
if (H3)
|
||||
*H3 = eye(3);
|
||||
return R1 * (R2 * R3);
|
||||
}
|
||||
|
||||
TEST(ExpressionFactor, composeTernary) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ A(1), B(2), C(3);
|
||||
Rot3_ ABC(composeThree, A, B, C);
|
||||
|
||||
// Create factor
|
||||
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(1, Rot3());
|
||||
values.insert(2, Rot3());
|
||||
values.insert(3, Rot3());
|
||||
|
||||
// Check unwhitenedError
|
||||
std::vector<Matrix> H(3);
|
||||
Vector actual = f.unwhitenedError(values, H);
|
||||
EXPECT_LONGS_EQUAL(3, H.size());
|
||||
EXPECT( assert_equal(eye(3), H[0],1e-9));
|
||||
EXPECT( assert_equal(eye(3), H[1],1e-9));
|
||||
EXPECT( assert_equal(eye(3), H[2],1e-9));
|
||||
|
||||
// Check linearization
|
||||
JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
EXPECT( assert_equal(expected, *jf,1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -325,7 +325,8 @@ namespace mpl = boost::mpl;
|
|||
#include <boost/mpl/assert.hpp>
|
||||
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;
|
||||
//Incomplete<Generated> incomplete;
|
||||
//BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >));
|
||||
|
|
|
|||
Loading…
Reference in New Issue