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

BinaryExpression now without MPL

release/4.3a0
dellaert 2015-05-12 01:10:03 -07:00
parent b8024b10b5
commit e1b3a11957
4 changed files with 108 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -454,6 +454,9 @@ struct FunctionalNode {
};
//-----------------------------------------------------------------------------
// 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> {
@ -466,24 +469,24 @@ public:
/// Constructor with a unary function f, and input argument e1
UnaryExpression(Function f, const Expression<A1>& e1) :
function_(f) {
this->expression1_ = e1.root();
expression1_(e1.root()), function_(f) {
ExpressionNode<T>::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize();
}
/// Return value
virtual T value(const Values& values) const {
return function_(this->expression1_->value(values), boost::none);
using boost::none;
return function_(expression1_->value(values), none);
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
return this->expression1_->keys();
return expression1_->keys();
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
this->expression1_->dims(map);
expression1_->dims(map);
}
// Inner Record Class
@ -496,8 +499,7 @@ public:
/// Print to std::cout
void print(const std::string& indent) const {
std::cout << indent << "UnaryExpression::Record {" << std::endl;
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
std::cout << indent << dTdA1.format(matlab) << std::endl;
std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl;
trace1.print(indent);
std::cout << indent << "}" << std::endl;
}
@ -516,7 +518,6 @@ public:
}
/// 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 {
trace1.reverseAD1(dFdT * dTdA1, jacobians);
@ -553,50 +554,93 @@ public:
/// Binary Expression
template<class T, class A1, class A2>
class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
public:
typedef typename Base::Record Record;
private:
class BinaryExpression: public ExpressionNode<T> {
typedef typename Expression<T>::template BinaryFunction<A1, A2>::type Function;
boost::shared_ptr<ExpressionNode<A1> > expression1_;
boost::shared_ptr<ExpressionNode<A2> > expression2_;
Function function_;
/// Constructor with a ternary function f, and three input arguments
public:
/// Constructor with a binary function f, and two input arguments
BinaryExpression(Function f, const Expression<A1>& e1,
const Expression<A2>& e2) :
function_(f) {
this->template reset<A1, 1>(e1.root());
this->template reset<A2, 2>(e2.root());
expression1_(e1.root()), expression2_(e2.root()), function_(f) {
ExpressionNode<T>::traceSize_ = //
upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize();
}
friend class Expression<T> ;
friend class ::ExpressionFactorBinaryTest;
public:
/// Return value
virtual T value(const Values& values) const {
using boost::none;
return function_(this->template expression<A1, 1>()->value(values),
this->template expression<A2, 2>()->value(values),
none, none);
return function_(expression1_->value(values), expression2_->value(values),
none, none);
}
/// Construct an execution trace for reverse AD
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys = expression1_->keys();
std::set<Key> myKeys = expression2_->keys();
keys.insert(myKeys.begin(), myKeys.end());
return keys;
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
expression1_->dims(map);
expression2_->dims(map);
}
// Inner Record Class
struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
A1 value1;
ExecutionTrace<A1> trace1;
typename Jacobian<T, A1>::type dTdA1;
A2 value2;
ExecutionTrace<A2> trace2;
typename Jacobian<T, A2>::type dTdA2;
/// Print to std::cout
void print(const std::string& indent) const {
std::cout << indent << "BinaryExpression::Record {" << std::endl;
std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl;
trace1.print(indent);
std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl;
trace2.print(indent);
std::cout << indent << "}" << std::endl;
}
/// Start the reverse AD process, see comments in Base
void startReverseAD4(JacobianMap& jacobians) const {
trace1.reverseAD1(dTdA1, jacobians);
trace2.reverseAD1(dTdA2, jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
trace1.reverseAD1(dFdT * dTdA1, jacobians);
trace2.reverseAD1(dFdT * dTdA2, jacobians);
}
};
/// Construct an execution trace for reverse AD, see UnaryExpression for explanation
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
Record* record = Base::trace(values, traceStorage);
ExecutionTraceStorage* ptr) const {
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
Record* record = new (ptr) Record();
ptr += upAligned(sizeof(Record));
record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
record->value2 = expression2_->traceExecution(values, record->trace2, ptr);
ptr += expression1_->traceSize() + expression2_->traceSize();
trace.setFunction(record);
return function_(record->template value<A1, 1>(),
record->template value<A2,2>(), record->template jacobian<A1, 1>(),
record->template jacobian<A2, 2>());
return function_(record->value1, record->value2, record->dTdA1,
record->dTdA2);
}
};

4
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -265,9 +265,9 @@ TEST(AdaptAutoDiff, Snavely) {
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
Expression<Point2> expression(Adaptor(), P, X);
#ifdef GTSAM_USE_QUATERNIONS
EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero
EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero
#else
EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero
EXPECT_LONGS_EQUAL(416,expression.traceSize()); // Todo, should be zero
#endif
set<Key> expected = list_of(1)(2);
EXPECT(expected == expression.keys());

34
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -68,19 +68,19 @@ TEST(Expression, Leaves) {
Point3 somePoint(1, 2, 3);
values.insert(Symbol('p', 10), somePoint);
std::vector<Expression<Point3> > points = createUnknowns<Point3>(10, 'p', 1);
EXPECT(assert_equal(somePoint,points.back().value(values)));
EXPECT(assert_equal(somePoint, points.back().value(values)));
}
/* ************************************************************************* */
// Unary(Leaf)
namespace unary {
Point2 f1(const Point3& p, OptionalJacobian<2,3> H) {
Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) {
return Point2();
}
double f2(const Point3& p, OptionalJacobian<1, 3> H) {
return 0.0;
}
Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic,3> H) {
Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
return p.vector();
}
Expression<Point3> p(1);
@ -127,7 +127,7 @@ TEST(Expression, NullaryMethod) {
EXPECT(actual == sqrt(50));
Matrix expected(1, 3);
expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
EXPECT(assert_equal(expected,H[0]));
EXPECT(assert_equal(expected, H[0]));
}
/* ************************************************************************* */
// Binary(Leaf,Leaf)
@ -158,7 +158,7 @@ TEST(Expression, BinaryKeys) {
TEST(Expression, BinaryDimensions) {
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
binary::p_cam.dims(actual);
EXPECT(actual==expected);
EXPECT(actual == expected);
}
/* ************************************************************************* */
// dimensions
@ -189,17 +189,26 @@ TEST(Expression, TreeKeys) {
TEST(Expression, TreeDimensions) {
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3)(3, 5);
tree::uv_hat.dims(actual);
EXPECT(actual==expected);
EXPECT(actual == expected);
}
/* ************************************************************************* */
// TraceSize
TEST(Expression, TreeTraceSize) {
typedef internal::UnaryExpression<Point2, Point3> Unary;
typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary1;
typedef internal::BinaryExpression<Point2, Point2, Cal3_S2> Binary2;
size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record)
+ sizeof(Binary2::Record);
EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize());
EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)),
tree::p_cam.traceSize());
typedef internal::UnaryExpression<Point2, Point3> Unary;
EXPECT_LONGS_EQUAL(
internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(),
tree::projection.traceSize());
EXPECT_LONGS_EQUAL(0, tree::K.traceSize());
typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary2;
EXPECT_LONGS_EQUAL(
internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize()
+ tree::projection.traceSize(), tree::uv_hat.traceSize());
}
/* ************************************************************************* */
@ -243,7 +252,8 @@ TEST(Expression, compose3) {
/* ************************************************************************* */
// Test with ternary function
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2,
OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);

11
gtsam/nonlinear/tests/testExpressionFactor.cpp
View File

@ -188,7 +188,11 @@ TEST(ExpressionFactor, Binary) {
EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace<Cal3_S2>));
EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian<Point2,Cal3_S2>::type));
EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian<Point2,Point2>::type));
size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32;
size_t expectedRecordSize = sizeof(Cal3_S2)
+ sizeof(internal::ExecutionTrace<Cal3_S2>)
+ +sizeof(internal::Jacobian<Point2, Cal3_S2>::type) + sizeof(Point2)
+ sizeof(internal::ExecutionTrace<Point2>)
+ sizeof(internal::Jacobian<Point2, Point2>::type);
EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record));
// Check size
@ -212,9 +216,8 @@ TEST(ExpressionFactor, Binary) {
// Check matrices
boost::optional<Binary::Record*> r = trace.record<Binary::Record>();
CHECK(r);
EXPECT(
assert_equal(expected25, (Matrix) (*r)-> jacobian<Cal3_S2, 1>(), 1e-9));
EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian<Point2, 2>(), 1e-9));
EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9));
EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9));
}
/* ************************************************************************* */
// Unary(Binary(Leaf,Leaf))