From e1b3a119574d6aa081097d8c5afddaaff4f95558 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 12 May 2015 01:10:03 -0700 Subject: [PATCH] BinaryExpression now without MPL --- gtsam/nonlinear/internal/ExpressionNode.h | 110 ++++++++++++------ gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 4 +- gtsam/nonlinear/tests/testExpression.cpp | 34 ++++-- .../nonlinear/tests/testExpressionFactor.cpp | 11 +- 4 files changed, 108 insertions(+), 51 deletions(-) diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index d3c736e9d..e72445716 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -454,6 +454,9 @@ struct FunctionalNode { }; //----------------------------------------------------------------------------- +// Eigen format for printing Jacobians +static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); + /// Unary Function Expression template class UnaryExpression: public ExpressionNode { @@ -466,24 +469,24 @@ public: /// Constructor with a unary function f, and input argument e1 UnaryExpression(Function f, const Expression& e1) : - function_(f) { - this->expression1_ = e1.root(); + expression1_(e1.root()), function_(f) { ExpressionNode::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 keys() const { - return this->expression1_->keys(); + return expression1_->keys(); } /// Return dimensions for each argument virtual void dims(std::map& 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 void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { trace1.reverseAD1(dFdT * dTdA1, jacobians); @@ -553,50 +554,93 @@ public: /// Binary Expression template -class BinaryExpression: public FunctionalNode >::type { - typedef typename FunctionalNode >::type Base; - -public: - typedef typename Base::Record Record; - -private: +class BinaryExpression: public ExpressionNode { typedef typename Expression::template BinaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > 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& e1, const Expression& e2) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); + expression1_(e1.root()), expression2_(e2.root()), function_(f) { ExpressionNode::traceSize_ = // upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } - friend class Expression ; friend class ::ExpressionFactorBinaryTest; -public: - /// Return value virtual T value(const Values& values) const { using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->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 keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::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 + 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& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(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(), - record->template value(), record->template jacobian(), - record->template jacobian()); + return function_(record->value1, record->value2, record->dTdA1, + record->dTdA2); } }; diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index f3858c818..ac56b77ca 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -265,9 +265,9 @@ TEST(AdaptAutoDiff, Snavely) { typedef AdaptAutoDiff Adaptor; Expression 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 expected = list_of(1)(2); EXPECT(expected == expression.keys()); diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 5d443e073..3e86bcb8c 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -68,19 +68,19 @@ TEST(Expression, Leaves) { Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); std::vector > points = createUnknowns(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 H) { +Vector f3(const Point3& p, OptionalJacobian H) { return p.vector(); } Expression 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 actual, expected = map_list_of(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 actual, expected = map_list_of(1, 6)(2, 3)(3, 5); tree::uv_hat.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // TraceSize TEST(Expression, TreeTraceSize) { - typedef internal::UnaryExpression Unary; typedef internal::BinaryExpression Binary1; - typedef internal::BinaryExpression 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 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 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); diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp index 3573378ed..3aee9e50a 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -188,7 +188,11 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian::type)); EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian::type)); - size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32; + size_t expectedRecordSize = sizeof(Cal3_S2) + + sizeof(internal::ExecutionTrace) + + +sizeof(internal::Jacobian::type) + sizeof(Point2) + + sizeof(internal::ExecutionTrace) + + sizeof(internal::Jacobian::type); EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); // Check size @@ -212,9 +216,8 @@ TEST(ExpressionFactor, Binary) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT( - assert_equal(expected25, (Matrix) (*r)-> jacobian(), 1e-9)); - EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); + EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf))