From eef2d49e8d95bd01c75ee4a4cd99e59e09f6202a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 10:27:30 +0200 Subject: [PATCH 1/8] First prototype, segfaults --- gtsam_unstable/nonlinear/Expression-inl.h | 120 ++-- gtsam_unstable/nonlinear/Expression.h | 6 +- .../nonlinear/tests/testExpressionFactor.cpp | 592 +++++++++--------- 3 files changed, 374 insertions(+), 344 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 7e21db45e..08de1ab5b 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -43,10 +44,7 @@ typedef std::map JacobianMap; */ template struct CallRecord { - - /// Make sure destructor is virtual - virtual ~CallRecord() { - } + virtual void print() const = 0; virtual void startReverseAD(JacobianMap& jacobians) const = 0; virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; typedef Eigen::Matrix Jacobian2T; @@ -72,15 +70,11 @@ class ExecutionTrace { CallRecord* ptr; } content; public: + T value; /// Pointer always starts out as a Constant ExecutionTrace() : type(Constant) { } - /// Destructor cleans up pointer if Function - ~ExecutionTrace() { - if (type == Function) - delete content.ptr; - } /// Change pointer to a Leaf Record void setLeaf(Key key) { type = Leaf; @@ -91,6 +85,14 @@ public: type = Function; content.ptr = record; } + /// Print + virtual void print() const { + GTSAM_PRINT(value); + if (type == Leaf) + std::cout << "Leaf, key = " << content.key << std::endl; + else if (type == Function) + content.ptr->print(); + } /// Return record pointer, quite unsafe, used only for testing template boost::optional record() { @@ -158,14 +160,6 @@ struct Select<2, A> { } }; -//template -//struct TypedTrace { -// virtual void startReverseAD(JacobianMap& jacobians) const = 0; -// virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; -//// template -//// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const { -//}; - //----------------------------------------------------------------------------- /** * Value and Jacobians @@ -310,8 +304,8 @@ public: virtual Augmented forward(const Values& values) const = 0; /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, - ExecutionTrace& p) const = 0; + virtual ExecutionTrace traceExecution(const Values& values, + void* raw) const = 0; }; //----------------------------------------------------------------------------- @@ -348,8 +342,11 @@ public: } /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& p) const { - return constant_; + virtual ExecutionTrace traceExecution(const Values& values, + void* raw) const { + ExecutionTrace trace; + trace.value = constant_; + return trace; } }; @@ -388,9 +385,12 @@ public: } /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& p) const { - p.setLeaf(key_); - return values.at(key_); + virtual ExecutionTrace traceExecution(const Values& values, + void* raw) const { + ExecutionTrace trace; + trace.setLeaf(key_); + trace.value = values.at(key_); + return trace; } }; @@ -443,7 +443,11 @@ public: struct Record: public CallRecord { ExecutionTrace trace1; JacobianTA dTdA1; - + /// print to std::cout + virtual void print() const { + std::cout << dTdA1 << std::endl; + trace1.print(); + } /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { Select::reverseAD(trace1, dTdA1, jacobians); @@ -461,11 +465,14 @@ public: }; /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& p) const { - Record* record = new Record(); - p.setFunction(record); - A1 a = this->expressionA1_->traceExecution(values, record->trace1); - return function_(a, record->dTdA1); + virtual ExecutionTrace traceExecution(const Values& values, + void* raw) const { + ExecutionTrace trace; +// Record* record = new Record(); +// p.setFunction(record); +// A1 a = this->expressionA1_->traceExecution(values, record->trace1); +// return function_(a, record->dTdA1); + return trace; } }; @@ -534,7 +541,13 @@ public: ExecutionTrace trace2; JacobianTA1 dTdA1; JacobianTA2 dTdA2; - + /// print to std::cout + virtual void print() const { + std::cout << dTdA1 << std::endl; + trace1.print(); + std::cout << dTdA2 << std::endl; + trace2.print(); + } /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { Select::reverseAD(trace1, dTdA1, jacobians); @@ -555,12 +568,18 @@ public: }; /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& p) const { - Record* record = new Record(); - p.setFunction(record); - A1 a1 = this->expressionA1_->traceExecution(values, record->trace1); - A2 a2 = this->expressionA2_->traceExecution(values, record->trace2); - return function_(a1, a2, record->dTdA1, record->dTdA2); + /// The raw buffer is [Record | A1 raw | A2 raw] + virtual ExecutionTrace traceExecution(const Values& values, + void* raw) const { + ExecutionTrace trace; + Record* record = static_cast(raw); + trace.setFunction(record); + record->trace1 = this->expressionA1_->traceExecution(values, raw); + record->trace2 = this->expressionA2_->traceExecution(values, raw); + trace.value = function_(record->trace1.value, record->trace2.value, + record->dTdA1, record->dTdA2); + trace.print(); + return trace; } }; @@ -643,7 +662,15 @@ public: JacobianTA1 dTdA1; JacobianTA2 dTdA2; JacobianTA3 dTdA3; - + /// print to std::cout + virtual void print() const { + std::cout << dTdA1 << std::endl; + trace1.print(); + std::cout << dTdA2 << std::endl; + trace2.print(); + std::cout << dTdA3 << std::endl; + trace3.print(); + } /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { Select::reverseAD(trace1, dTdA1, jacobians); @@ -667,13 +694,16 @@ public: }; /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& p) const { - Record* record = new Record(); - p.setFunction(record); - A1 a1 = this->expressionA1_->traceExecution(values, record->trace1); - A2 a2 = this->expressionA2_->traceExecution(values, record->trace2); - A3 a3 = this->expressionA3_->traceExecution(values, record->trace3); - return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); + virtual ExecutionTrace traceExecution(const Values& values, + void* raw) const { + ExecutionTrace trace; +// Record* record = new Record(); +// p.setFunction(record); +// A1 a1 = this->expressionA1_->traceExecution(values, record->trace1); +// A2 a2 = this->expressionA2_->traceExecution(values, record->trace2); +// A3 a3 = this->expressionA3_->traceExecution(values, record->trace3); +// return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); + return trace; } }; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index b79fdceef..e784d1c2f 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -117,9 +117,9 @@ public: Augmented augmented(const Values& values) const { #define REVERSE_AD #ifdef REVERSE_AD - ExecutionTrace trace; - T value = root_->traceExecution(values, trace); - Augmented augmented(value); + char raw[10]; + ExecutionTrace trace = root_->traceExecution(values, raw); + Augmented augmented(trace.value); trace.startReverseAD(augmented.jacobians()); return augmented; #else diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index e1694f59a..7a1afcf18 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -33,79 +33,79 @@ using namespace gtsam; Point2 measured(-17, 30); SharedNoiseModel model = noiseModel::Unit::Create(2); -/* ************************************************************************* */ -// Leaf -TEST(ExpressionFactor, leaf) { - - // Create some values - Values values; - values.insert(2, Point2(3, 5)); - - JacobianFactor expected( // - 2, (Matrix(2, 2) << 1, 0, 0, 1), // - (Vector(2) << -3, -5)); - - // Create leaves - Point2_ p(2); - - // Concise version - ExpressionFactor f(model, Point2(0, 0), p); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf, 1e-9)); -} - -/* ************************************************************************* */ -// non-zero noise model -TEST(ExpressionFactor, model) { - - // Create some values - Values values; - values.insert(2, Point2(3, 5)); - - JacobianFactor expected( // - 2, (Matrix(2, 2) << 10, 0, 0, 100), // - (Vector(2) << -30, -500)); - - // Create leaves - Point2_ p(2); - - // Concise version - SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); - - ExpressionFactor f(model, Point2(0, 0), p); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf, 1e-9)); -} - -/* ************************************************************************* */ -// Unary(Leaf)) -TEST(ExpressionFactor, unary) { - - // Create some values - Values values; - values.insert(2, Point3(0, 0, 1)); - - JacobianFactor expected( // - 2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), // - (Vector(2) << -17, 30)); - - // Create leaves - Point3_ p(2); - - // Concise version - ExpressionFactor f(model, measured, project(p)); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf, 1e-9)); -} +///* ************************************************************************* */ +//// Leaf +//TEST(ExpressionFactor, leaf) { +// +// // Create some values +// Values values; +// values.insert(2, Point2(3, 5)); +// +// JacobianFactor expected( // +// 2, (Matrix(2, 2) << 1, 0, 0, 1), // +// (Vector(2) << -3, -5)); +// +// // Create leaves +// Point2_ p(2); +// +// // Concise version +// ExpressionFactor f(model, Point2(0, 0), p); +// EXPECT_LONGS_EQUAL(2, f.dim()); +// boost::shared_ptr gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(gf); +// EXPECT( assert_equal(expected, *jf, 1e-9)); +//} +// +///* ************************************************************************* */ +//// non-zero noise model +//TEST(ExpressionFactor, model) { +// +// // Create some values +// Values values; +// values.insert(2, Point2(3, 5)); +// +// JacobianFactor expected( // +// 2, (Matrix(2, 2) << 10, 0, 0, 100), // +// (Vector(2) << -30, -500)); +// +// // Create leaves +// Point2_ p(2); +// +// // Concise version +// SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); +// +// ExpressionFactor f(model, Point2(0, 0), p); +// EXPECT_LONGS_EQUAL(2, f.dim()); +// boost::shared_ptr gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(gf); +// EXPECT( assert_equal(expected, *jf, 1e-9)); +//} +// +///* ************************************************************************* */ +//// Unary(Leaf)) +//TEST(ExpressionFactor, unary) { +// +// // Create some values +// Values values; +// values.insert(2, Point3(0, 0, 1)); +// +// JacobianFactor expected( // +// 2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), // +// (Vector(2) << -17, 30)); +// +// // Create leaves +// Point3_ p(2); +// +// // Concise version +// ExpressionFactor f(model, measured, project(p)); +// EXPECT_LONGS_EQUAL(2, f.dim()); +// boost::shared_ptr gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(gf); +// EXPECT( assert_equal(expected, *jf, 1e-9)); +//} /* ************************************************************************* */ struct TestBinaryExpression { static Point2 myUncal(const Cal3_S2& K, const Point2& p, @@ -137,236 +137,236 @@ TEST(ExpressionFactor, binary) { Matrix2 expected22; expected22 << 1, 0, 0, 1; - // Do old trace - ExecutionTrace trace; - tester.binary_.traceExecution(values, trace); + // traceRaw will fill raw with [Trace | Binary::Record] + EXPECT_LONGS_EQUAL(8, sizeof(double)); + EXPECT_LONGS_EQUAL(48, sizeof(ExecutionTrace)); + EXPECT_LONGS_EQUAL(72, sizeof(ExecutionTrace)); + EXPECT_LONGS_EQUAL(2*5*8, sizeof(Binary::JacobianTA1)); + EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2)); + size_t expectedRecordSize = 8 + 48 + 72 + 80 + 32; // 240 + EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record)); + size_t size = sizeof(Binary::Record); + // Use Variable Length Array, allocated on stack by gcc + // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla + char raw[size]; + ExecutionTrace trace = tester.binary_.traceExecution(values, raw); // Check matrices - boost::optional p = trace.record(); - CHECK(p); - EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); - EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); - -// // Check raw memory trace -// double raw[10]; -// tester.binary_.traceRaw(values, 0); -// -// // Check matrices // boost::optional p = trace.record(); // CHECK(p); // EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); // EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); } -/* ************************************************************************* */ -// Unary(Binary(Leaf,Leaf)) -TEST(ExpressionFactor, shallow) { - - // Create some values - Values values; - values.insert(1, Pose3()); - values.insert(2, Point3(0, 0, 1)); - - // Create old-style factor to create expected value and derivatives - GenericProjectionFactor old(measured, model, 1, 2, - boost::make_shared()); - double expected_error = old.error(values); - GaussianFactor::shared_ptr expected = old.linearize(values); - - // Create leaves - Pose3_ x(1); - Point3_ p(2); - - // Concise version - ExpressionFactor f2(model, measured, project(transform_to(x, p))); - EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f2.dim()); - boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); -} - -/* ************************************************************************* */ -// Binary(Leaf,Unary(Binary(Leaf,Leaf))) -TEST(ExpressionFactor, tree) { - - // Create some values - Values values; - values.insert(1, Pose3()); - values.insert(2, Point3(0, 0, 1)); - values.insert(3, Cal3_S2()); - - // Create old-style factor to create expected value and derivatives - GeneralSFMFactor2 old(measured, model, 1, 2, 3); - double expected_error = old.error(values); - GaussianFactor::shared_ptr expected = old.linearize(values); - - // Create leaves - Pose3_ x(1); - Point3_ p(2); - Cal3_S2_ K(3); - - // Create expression tree - Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); - Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); - - // Create factor and check value, dimension, linearization - ExpressionFactor f(model, measured, uv_hat); - EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - EXPECT( assert_equal(*expected, *gf, 1e-9)); - - // Concise version - ExpressionFactor f2(model, measured, - uncalibrate(K, project(transform_to(x, p)))); - EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f2.dim()); - boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); - - TernaryExpression::Function fff = project6; - - // Try ternary version - ExpressionFactor f3(model, measured, project3(x, p, K)); - EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f3.dim()); - boost::shared_ptr gf3 = f3.linearize(values); - EXPECT( assert_equal(*expected, *gf3, 1e-9)); -} - -/* ************************************************************************* */ - -TEST(ExpressionFactor, compose1) { - - // Create expression - Rot3_ R1(1), R2(2); - Rot3_ R3 = R1 * R2; - - // Create factor - ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - - // Create some values - Values values; - values.insert(1, Rot3()); - values.insert(2, Rot3()); - - // Check unwhitenedError - std::vector 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 gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(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 f(noiseModel::Unit::Create(3), Rot3(), R3); - - // Create some values - Values values; - values.insert(1, Rot3()); - - // Check unwhitenedError - std::vector 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 gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(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 f(noiseModel::Unit::Create(3), Rot3(), R3); - - // Create some values - Values values; - values.insert(3, Rot3()); - - // Check unwhitenedError - std::vector 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 gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(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 H1, boost::optional H2, - boost::optional 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 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 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 gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); -} +///* ************************************************************************* */ +//// Unary(Binary(Leaf,Leaf)) +//TEST(ExpressionFactor, shallow) { +// +// // Create some values +// Values values; +// values.insert(1, Pose3()); +// values.insert(2, Point3(0, 0, 1)); +// +// // Create old-style factor to create expected value and derivatives +// GenericProjectionFactor old(measured, model, 1, 2, +// boost::make_shared()); +// double expected_error = old.error(values); +// GaussianFactor::shared_ptr expected = old.linearize(values); +// +// // Create leaves +// Pose3_ x(1); +// Point3_ p(2); +// +// // Concise version +// ExpressionFactor f2(model, measured, project(transform_to(x, p))); +// EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); +// EXPECT_LONGS_EQUAL(2, f2.dim()); +// boost::shared_ptr gf2 = f2.linearize(values); +// EXPECT( assert_equal(*expected, *gf2, 1e-9)); +//} +// +///* ************************************************************************* */ +//// Binary(Leaf,Unary(Binary(Leaf,Leaf))) +//TEST(ExpressionFactor, tree) { +// +// // Create some values +// Values values; +// values.insert(1, Pose3()); +// values.insert(2, Point3(0, 0, 1)); +// values.insert(3, Cal3_S2()); +// +// // Create old-style factor to create expected value and derivatives +// GeneralSFMFactor2 old(measured, model, 1, 2, 3); +// double expected_error = old.error(values); +// GaussianFactor::shared_ptr expected = old.linearize(values); +// +// // Create leaves +// Pose3_ x(1); +// Point3_ p(2); +// Cal3_S2_ K(3); +// +// // Create expression tree +// Point3_ p_cam(x, &Pose3::transform_to, p); +// Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); +// Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); +// +// // Create factor and check value, dimension, linearization +// ExpressionFactor f(model, measured, uv_hat); +// EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); +// EXPECT_LONGS_EQUAL(2, f.dim()); +// boost::shared_ptr gf = f.linearize(values); +// EXPECT( assert_equal(*expected, *gf, 1e-9)); +// +// // Concise version +// ExpressionFactor f2(model, measured, +// uncalibrate(K, project(transform_to(x, p)))); +// EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); +// EXPECT_LONGS_EQUAL(2, f2.dim()); +// boost::shared_ptr gf2 = f2.linearize(values); +// EXPECT( assert_equal(*expected, *gf2, 1e-9)); +// +// TernaryExpression::Function fff = project6; +// +// // Try ternary version +// ExpressionFactor f3(model, measured, project3(x, p, K)); +// EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); +// EXPECT_LONGS_EQUAL(2, f3.dim()); +// boost::shared_ptr gf3 = f3.linearize(values); +// EXPECT( assert_equal(*expected, *gf3, 1e-9)); +//} +// +///* ************************************************************************* */ +// +//TEST(ExpressionFactor, compose1) { +// +// // Create expression +// Rot3_ R1(1), R2(2); +// Rot3_ R3 = R1 * R2; +// +// // Create factor +// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +// +// // Create some values +// Values values; +// values.insert(1, Rot3()); +// values.insert(2, Rot3()); +// +// // Check unwhitenedError +// std::vector 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 gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(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 f(noiseModel::Unit::Create(3), Rot3(), R3); +// +// // Create some values +// Values values; +// values.insert(1, Rot3()); +// +// // Check unwhitenedError +// std::vector 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 gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(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 f(noiseModel::Unit::Create(3), Rot3(), R3); +// +// // Create some values +// Values values; +// values.insert(3, Rot3()); +// +// // Check unwhitenedError +// std::vector 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 gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(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 H1, boost::optional H2, +// boost::optional 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 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 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 gf = f.linearize(values); +// boost::shared_ptr jf = // +// boost::dynamic_pointer_cast(gf); +// EXPECT( assert_equal(expected, *jf,1e-9)); +//} /* ************************************************************************* */ int main() { From 69b69a0bc8aedaf9490824676066ffa5f1598f9e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 11:03:35 +0200 Subject: [PATCH 2/8] placement new works! And sophisticated Trace::print --- gtsam_unstable/nonlinear/Expression-inl.h | 91 +++++++++---------- gtsam_unstable/nonlinear/Expression.h | 5 +- .../nonlinear/tests/testExpressionFactor.cpp | 10 +- 3 files changed, 52 insertions(+), 54 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 08de1ab5b..8e62ab708 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -24,7 +24,7 @@ #include #include #include - +#include // for placement new struct TestBinaryExpression; namespace gtsam { @@ -44,7 +44,7 @@ typedef std::map JacobianMap; */ template struct CallRecord { - virtual void print() const = 0; + virtual void print(const std::string& indent) const = 0; virtual void startReverseAD(JacobianMap& jacobians) const = 0; virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; typedef Eigen::Matrix Jacobian2T; @@ -70,7 +70,6 @@ class ExecutionTrace { CallRecord* ptr; } content; public: - T value; /// Pointer always starts out as a Constant ExecutionTrace() : type(Constant) { @@ -86,12 +85,15 @@ public: content.ptr = record; } /// Print - virtual void print() const { - GTSAM_PRINT(value); - if (type == Leaf) - std::cout << "Leaf, key = " << content.key << std::endl; - else if (type == Function) - content.ptr->print(); + void print(const std::string& indent = "") const { + if (type == Constant) + std::cout << indent << "Constant" << std::endl; + else if (type == Leaf) + std::cout << indent << "Leaf, key = " << content.key << std::endl; + else if (type == Function) { + std::cout << indent << "Function" << std::endl; + content.ptr->print(indent + " "); + } } /// Return record pointer, quite unsafe, used only for testing template @@ -304,7 +306,7 @@ public: virtual Augmented forward(const Values& values) const = 0; /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const = 0; }; @@ -342,11 +344,9 @@ public: } /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; - trace.value = constant_; - return trace; + return constant_; } }; @@ -385,12 +385,10 @@ public: } /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; trace.setLeaf(key_); - trace.value = values.at(key_); - return trace; + return values.at(key_); } }; @@ -444,9 +442,10 @@ public: ExecutionTrace trace1; JacobianTA dTdA1; /// print to std::cout - virtual void print() const { - std::cout << dTdA1 << std::endl; - trace1.print(); + virtual void print(const std::string& indent) const { + static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << dTdA1.format(matlab) << std::endl; + trace1.print(indent); } /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { @@ -465,14 +464,13 @@ public: }; /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; // Record* record = new Record(); // p.setFunction(record); // A1 a = this->expressionA1_->traceExecution(values, record->trace1); // return function_(a, record->dTdA1); - return trace; + return T(); } }; @@ -542,11 +540,12 @@ public: JacobianTA1 dTdA1; JacobianTA2 dTdA2; /// print to std::cout - virtual void print() const { - std::cout << dTdA1 << std::endl; - trace1.print(); - std::cout << dTdA2 << std::endl; - trace2.print(); + virtual void print(const std::string& indent) const { + static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << indent << dTdA1.format(matlab) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(matlab) << std::endl; + trace2.print(indent); } /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { @@ -569,17 +568,13 @@ public: /// Construct an execution trace for reverse AD /// The raw buffer is [Record | A1 raw | A2 raw] - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; - Record* record = static_cast(raw); + Record* record = new (raw) Record(); trace.setFunction(record); - record->trace1 = this->expressionA1_->traceExecution(values, raw); - record->trace2 = this->expressionA2_->traceExecution(values, raw); - trace.value = function_(record->trace1.value, record->trace2.value, - record->dTdA1, record->dTdA2); - trace.print(); - return trace; + A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); + A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw); + return function_(a1, a2, record->dTdA1, record->dTdA2); } }; @@ -663,13 +658,14 @@ public: JacobianTA2 dTdA2; JacobianTA3 dTdA3; /// print to std::cout - virtual void print() const { - std::cout << dTdA1 << std::endl; - trace1.print(); - std::cout << dTdA2 << std::endl; - trace2.print(); - std::cout << dTdA3 << std::endl; - trace3.print(); + virtual void print(const std::string& indent) const { + static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << dTdA1.format(matlab) << std::endl; + trace1.print(indent); + std::cout << dTdA2.format(matlab) << std::endl; + trace2.print(indent); + std::cout << dTdA3.format(matlab) << std::endl; + trace3.print(indent); } /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { @@ -694,16 +690,15 @@ public: }; /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; // Record* record = new Record(); // p.setFunction(record); // A1 a1 = this->expressionA1_->traceExecution(values, record->trace1); // A2 a2 = this->expressionA2_->traceExecution(values, record->trace2); // A3 a3 = this->expressionA3_->traceExecution(values, record->trace3); // return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); - return trace; + return T(); } }; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index e784d1c2f..322692c27 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -118,8 +118,9 @@ public: #define REVERSE_AD #ifdef REVERSE_AD char raw[10]; - ExecutionTrace trace = root_->traceExecution(values, raw); - Augmented augmented(trace.value); + ExecutionTrace trace; + T value (root_->traceExecution(values, trace, raw)); + Augmented augmented(value); trace.startReverseAD(augmented.jacobians()); return augmented; #else diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 7a1afcf18..535bdba74 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -139,17 +139,19 @@ TEST(ExpressionFactor, binary) { // traceRaw will fill raw with [Trace | Binary::Record] EXPECT_LONGS_EQUAL(8, sizeof(double)); - EXPECT_LONGS_EQUAL(48, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(72, sizeof(ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); EXPECT_LONGS_EQUAL(2*5*8, sizeof(Binary::JacobianTA1)); EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2)); - size_t expectedRecordSize = 8 + 48 + 72 + 80 + 32; // 240 + size_t expectedRecordSize = 16 + 2*16 + 80 + 32; EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record)); size_t size = sizeof(Binary::Record); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla char raw[size]; - ExecutionTrace trace = tester.binary_.traceExecution(values, raw); + ExecutionTrace trace; + Point2 value = tester.binary_.traceExecution(values, trace, raw); + trace.print(); // Check matrices // boost::optional p = trace.record(); From 1f692638f59c7e80d95af9ab340c39971745ad8e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 11:04:39 +0200 Subject: [PATCH 3/8] Accessing matrices works --- .../nonlinear/tests/testExpressionFactor.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 535bdba74..4d1530d64 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -143,7 +143,7 @@ TEST(ExpressionFactor, binary) { EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); EXPECT_LONGS_EQUAL(2*5*8, sizeof(Binary::JacobianTA1)); EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2)); - size_t expectedRecordSize = 16 + 2*16 + 80 + 32; + size_t expectedRecordSize = 16 + 2 * 16 + 80 + 32; EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record)); size_t size = sizeof(Binary::Record); // Use Variable Length Array, allocated on stack by gcc @@ -151,13 +151,13 @@ TEST(ExpressionFactor, binary) { char raw[size]; ExecutionTrace trace; Point2 value = tester.binary_.traceExecution(values, trace, raw); - trace.print(); + // trace.print(); // Check matrices -// boost::optional p = trace.record(); -// CHECK(p); -// EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); -// EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); + boost::optional p = trace.record(); + CHECK(p); + EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); + EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); } ///* ************************************************************************* */ //// Unary(Binary(Leaf,Leaf)) From 05f78b6dca7941363b8022102fe18e257bad07cd Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 11:29:42 +0200 Subject: [PATCH 4/8] Re-factor, allow traceExecution --- gtsam_unstable/nonlinear/Expression.h | 27 +++++++++++++++++++++------ 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 322692c27..bcda7c331 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -113,18 +113,33 @@ public: return root_->value(values); } - /// Return value and derivatives - Augmented augmented(const Values& values) const { -#define REVERSE_AD -#ifdef REVERSE_AD + /// Return value and derivatives, forward AD version + Augmented forward(const Values& values) const { + return root_->forward(values); + } + + /// trace execution, very unsafe, for testing purposes only + T traceExecution(const Values& values, ExecutionTrace& trace, + void* raw) const { + return root_->traceExecution(values, trace, raw); + } + + /// Return value and derivatives, reverse AD version + Augmented reverse(const Values& values) const { char raw[10]; ExecutionTrace trace; - T value (root_->traceExecution(values, trace, raw)); + T value(root_->traceExecution(values, trace, raw)); Augmented augmented(value); trace.startReverseAD(augmented.jacobians()); return augmented; + } + + /// Return value and derivatives + Augmented augmented(const Values& values) const { +#ifdef EXPRESSION_FORWARD_AD + return forward(values); #else - return root_->forward(values); + return reverse(values); #endif } From deed7b8018101f7815edae616497bc7e953214be Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 11:30:06 +0200 Subject: [PATCH 5/8] Unary prints, but still-faults downstream --- gtsam_unstable/nonlinear/Expression-inl.h | 23 +++-- .../nonlinear/tests/testExpressionFactor.cpp | 90 ++++++++++++------- 2 files changed, 68 insertions(+), 45 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 8e62ab708..ae4224182 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -466,11 +466,11 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { -// Record* record = new Record(); -// p.setFunction(record); -// A1 a = this->expressionA1_->traceExecution(values, record->trace1); -// return function_(a, record->dTdA1); - return T(); + Record* record = new (raw) Record(); + trace.setFunction(record); + A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, + record + 1); + return function_(a1, record->dTdA1); } }; @@ -692,13 +692,12 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { -// Record* record = new Record(); -// p.setFunction(record); -// A1 a1 = this->expressionA1_->traceExecution(values, record->trace1); -// A2 a2 = this->expressionA2_->traceExecution(values, record->trace2); -// A3 a3 = this->expressionA3_->traceExecution(values, record->trace3); -// return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); - return T(); + Record* record = new (raw) Record(); + trace.setFunction(record); + A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); + A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw); + A3 a3 = this->expressionA3_->traceExecution(values, record->trace3, raw); + return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); } }; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 4d1530d64..5a3128d56 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -131,12 +131,6 @@ TEST(ExpressionFactor, binary) { values.insert(1, Cal3_S2()); values.insert(2, Point2(0, 0)); - // Expected Jacobians - Matrix25 expected25; - expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1; - Matrix2 expected22; - expected22 << 1, 0, 0, 1; - // traceRaw will fill raw with [Trace | Binary::Record] EXPECT_LONGS_EQUAL(8, sizeof(double)); EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); @@ -153,39 +147,69 @@ TEST(ExpressionFactor, binary) { Point2 value = tester.binary_.traceExecution(values, trace, raw); // trace.print(); + // Expected Jacobians + Matrix25 expected25; + expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1; + Matrix2 expected22; + expected22 << 1, 0, 0, 1; + // Check matrices boost::optional p = trace.record(); CHECK(p); EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); } -///* ************************************************************************* */ -//// Unary(Binary(Leaf,Leaf)) -//TEST(ExpressionFactor, shallow) { -// -// // Create some values -// Values values; -// values.insert(1, Pose3()); -// values.insert(2, Point3(0, 0, 1)); -// -// // Create old-style factor to create expected value and derivatives -// GenericProjectionFactor old(measured, model, 1, 2, -// boost::make_shared()); -// double expected_error = old.error(values); -// GaussianFactor::shared_ptr expected = old.linearize(values); -// -// // Create leaves -// Pose3_ x(1); -// Point3_ p(2); -// -// // Concise version -// ExpressionFactor f2(model, measured, project(transform_to(x, p))); -// EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); -// EXPECT_LONGS_EQUAL(2, f2.dim()); -// boost::shared_ptr gf2 = f2.linearize(values); -// EXPECT( assert_equal(*expected, *gf2, 1e-9)); -//} -// +/* ************************************************************************* */ +// Unary(Binary(Leaf,Leaf)) +TEST(ExpressionFactor, shallow) { + + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + + // Create old-style factor to create expected value and derivatives + GenericProjectionFactor old(measured, model, 1, 2, + boost::make_shared()); + double expected_error = old.error(values); + GaussianFactor::shared_ptr expected = old.linearize(values); + + // Create leaves + Pose3_ x_(1); + Point3_ p_(2); + + // Construct expression, concise evrsion + Point2_ expression = project(transform_to(x_, p_)); + + // traceExecution of shallow tree + typedef UnaryExpression Unary; + typedef BinaryExpression Binary; + EXPECT_LONGS_EQUAL(80, sizeof(Unary::Record)); + EXPECT_LONGS_EQUAL(272, sizeof(Binary::Record)); + size_t size = sizeof(Unary::Record) + sizeof(Binary::Record); + LONGS_EQUAL(352, size); + char raw[size]; + ExecutionTrace trace; + Point2 value = expression.traceExecution(values, trace, raw); + trace.print(); + + // Expected Jacobians + Matrix23 expected23; + expected23 << 1, 0, 0, 0, 1, 0; + + // Check matrices + boost::optional p = trace.record(); + CHECK(p); + EXPECT( assert_equal(expected23, (Matrix)(*p)->dTdA1, 1e-9)); + + // Linearization + ExpressionFactor f2(model, measured, expression); + EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f2.dim()); + boost::shared_ptr gf2 = f2.linearize(values); + EXPECT( assert_equal(*expected, *gf2, 1e-9)); +} + ///* ************************************************************************* */ //// Binary(Leaf,Unary(Binary(Leaf,Leaf))) //TEST(ExpressionFactor, tree) { From 9585823d5d40a4a09ea925b6a164cc0076d8a68b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 11:32:52 +0200 Subject: [PATCH 6/8] ...but works with correct size ! --- gtsam_unstable/nonlinear/Expression.h | 2 +- gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index bcda7c331..ea2e6280d 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -126,7 +126,7 @@ public: /// Return value and derivatives, reverse AD version Augmented reverse(const Values& values) const { - char raw[10]; + char raw[352]; ExecutionTrace trace; T value(root_->traceExecution(values, trace, raw)); Augmented augmented(value); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 5a3128d56..f64f2a11a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -191,7 +191,7 @@ TEST(ExpressionFactor, shallow) { char raw[size]; ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, raw); - trace.print(); + // trace.print(); // Expected Jacobians Matrix23 expected23; From 599e232d1de82cface68e864e2d804065b37248a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 12:11:22 +0200 Subject: [PATCH 7/8] traceSize, two tests work --- gtsam_unstable/nonlinear/Expression-inl.h | 25 ++++++++++++++++++- gtsam_unstable/nonlinear/Expression.h | 10 ++++++-- .../nonlinear/tests/testExpressionFactor.cpp | 13 +++++++--- 3 files changed, 42 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index ae4224182..1cbc11da5 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -257,7 +257,7 @@ public: } /// debugging - void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) { + virtual void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) { BOOST_FOREACH(const Pair& term, jacobians_) std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows() << "x" << term.second.cols() << ") "; @@ -287,6 +287,7 @@ template class ExpressionNode { protected: + ExpressionNode() { } @@ -305,6 +306,11 @@ public: /// Return value and derivatives virtual Augmented forward(const Values& values) const = 0; + // Return size needed for memory buffer in traceExecution + virtual size_t traceSize() const { + return 0; + } + /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const = 0; @@ -463,6 +469,11 @@ public: } }; + // Return size needed for memory buffer in traceExecution + virtual size_t traceSize() const { + return sizeof(Record) + expressionA1_->traceSize(); + } + /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { @@ -566,6 +577,12 @@ public: } }; + // Return size needed for memory buffer in traceExecution + virtual size_t traceSize() const { + return sizeof(Record) + expressionA1_->traceSize() + + expressionA2_->traceSize(); + } + /// Construct an execution trace for reverse AD /// The raw buffer is [Record | A1 raw | A2 raw] virtual T traceExecution(const Values& values, ExecutionTrace& trace, @@ -689,6 +706,12 @@ public: } }; + // Return size needed for memory buffer in traceExecution + virtual size_t traceSize() const { + return sizeof(Record) + expressionA1_->traceSize() + + expressionA2_->traceSize() + expressionA2_->traceSize(); + } + /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index ea2e6280d..3f31d0517 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -118,6 +118,11 @@ public: return root_->forward(values); } + // Return size needed for memory buffer in traceExecution + size_t traceSize() const { + return root_->traceSize(); + } + /// trace execution, very unsafe, for testing purposes only T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { @@ -126,9 +131,10 @@ public: /// Return value and derivatives, reverse AD version Augmented reverse(const Values& values) const { - char raw[352]; + size_t size = traceSize(); + char raw[size]; ExecutionTrace trace; - T value(root_->traceExecution(values, trace, raw)); + T value(traceExecution(values, trace, raw)); Augmented augmented(value); trace.startReverseAD(augmented.jacobians()); return augmented; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index f64f2a11a..b1ea646a8 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -139,7 +139,11 @@ TEST(ExpressionFactor, binary) { EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2)); size_t expectedRecordSize = 16 + 2 * 16 + 80 + 32; EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record)); - size_t size = sizeof(Binary::Record); + + // Check size + size_t size = tester.binary_.traceSize(); + CHECK(size); + EXPECT_LONGS_EQUAL(expectedRecordSize, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla char raw[size]; @@ -186,8 +190,11 @@ TEST(ExpressionFactor, shallow) { typedef BinaryExpression Binary; EXPECT_LONGS_EQUAL(80, sizeof(Unary::Record)); EXPECT_LONGS_EQUAL(272, sizeof(Binary::Record)); - size_t size = sizeof(Unary::Record) + sizeof(Binary::Record); - LONGS_EQUAL(352, size); + size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); + LONGS_EQUAL(352, expectedTraceSize); + size_t size = expression.traceSize(); + CHECK(size); + EXPECT_LONGS_EQUAL(expectedTraceSize, size); char raw[size]; ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, raw); From ecf6462a258b3bb37ac3c195159a9b78c2ede659 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 13:07:58 +0200 Subject: [PATCH 8/8] Victory!! Unit tests work! --- gtsam_unstable/nonlinear/Expression-inl.h | 21 +- gtsam_unstable/nonlinear/Expression.h | 2 +- .../nonlinear/tests/testExpressionFactor.cpp | 512 +++++++++--------- 3 files changed, 270 insertions(+), 265 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 1cbc11da5..29c6def8f 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -313,7 +313,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - void* raw) const = 0; + char* raw) const = 0; }; //----------------------------------------------------------------------------- @@ -351,7 +351,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - void* raw) const { + char* raw) const { return constant_; } }; @@ -392,7 +392,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - void* raw) const { + char* raw) const { trace.setLeaf(key_); return values.at(key_); } @@ -476,11 +476,11 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - void* raw) const { + char* raw) const { Record* record = new (raw) Record(); trace.setFunction(record); - A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, - record + 1); + raw = (char*) (record + 1); + A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); return function_(a1, record->dTdA1); } }; @@ -586,10 +586,12 @@ public: /// Construct an execution trace for reverse AD /// The raw buffer is [Record | A1 raw | A2 raw] virtual T traceExecution(const Values& values, ExecutionTrace& trace, - void* raw) const { + char* raw) const { Record* record = new (raw) Record(); trace.setFunction(record); + raw = (char*) (record + 1); A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); + raw = raw + expressionA1_->traceSize(); A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw); return function_(a1, a2, record->dTdA1, record->dTdA2); } @@ -714,11 +716,14 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - void* raw) const { + char* raw) const { Record* record = new (raw) Record(); trace.setFunction(record); + raw = (char*) (record + 1); A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); + raw = raw + expressionA1_->traceSize(); A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw); + raw = raw + expressionA2_->traceSize(); A3 a3 = this->expressionA3_->traceExecution(values, record->trace3, raw); return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); } diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 3f31d0517..afd5a60e0 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -125,7 +125,7 @@ public: /// trace execution, very unsafe, for testing purposes only T traceExecution(const Values& values, ExecutionTrace& trace, - void* raw) const { + char* raw) const { return root_->traceExecution(values, trace, raw); } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b1ea646a8..a6dbe842b 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -33,79 +33,79 @@ using namespace gtsam; Point2 measured(-17, 30); SharedNoiseModel model = noiseModel::Unit::Create(2); -///* ************************************************************************* */ -//// Leaf -//TEST(ExpressionFactor, leaf) { -// -// // Create some values -// Values values; -// values.insert(2, Point2(3, 5)); -// -// JacobianFactor expected( // -// 2, (Matrix(2, 2) << 1, 0, 0, 1), // -// (Vector(2) << -3, -5)); -// -// // Create leaves -// Point2_ p(2); -// -// // Concise version -// ExpressionFactor f(model, Point2(0, 0), p); -// EXPECT_LONGS_EQUAL(2, f.dim()); -// boost::shared_ptr gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(gf); -// EXPECT( assert_equal(expected, *jf, 1e-9)); -//} -// -///* ************************************************************************* */ -//// non-zero noise model -//TEST(ExpressionFactor, model) { -// -// // Create some values -// Values values; -// values.insert(2, Point2(3, 5)); -// -// JacobianFactor expected( // -// 2, (Matrix(2, 2) << 10, 0, 0, 100), // -// (Vector(2) << -30, -500)); -// -// // Create leaves -// Point2_ p(2); -// -// // Concise version -// SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); -// -// ExpressionFactor f(model, Point2(0, 0), p); -// EXPECT_LONGS_EQUAL(2, f.dim()); -// boost::shared_ptr gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(gf); -// EXPECT( assert_equal(expected, *jf, 1e-9)); -//} -// -///* ************************************************************************* */ -//// Unary(Leaf)) -//TEST(ExpressionFactor, unary) { -// -// // Create some values -// Values values; -// values.insert(2, Point3(0, 0, 1)); -// -// JacobianFactor expected( // -// 2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), // -// (Vector(2) << -17, 30)); -// -// // Create leaves -// Point3_ p(2); -// -// // Concise version -// ExpressionFactor f(model, measured, project(p)); -// EXPECT_LONGS_EQUAL(2, f.dim()); -// boost::shared_ptr gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(gf); -// EXPECT( assert_equal(expected, *jf, 1e-9)); -//} +/* ************************************************************************* */ +// Leaf +TEST(ExpressionFactor, leaf) { + + // Create some values + Values values; + values.insert(2, Point2(3, 5)); + + JacobianFactor expected( // + 2, (Matrix(2, 2) << 1, 0, 0, 1), // + (Vector(2) << -3, -5)); + + // Create leaves + Point2_ p(2); + + // Concise version + ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf, 1e-9)); +} + +/* ************************************************************************* */ +// non-zero noise model +TEST(ExpressionFactor, model) { + + // Create some values + Values values; + values.insert(2, Point2(3, 5)); + + JacobianFactor expected( // + 2, (Matrix(2, 2) << 10, 0, 0, 100), // + (Vector(2) << -30, -500)); + + // Create leaves + Point2_ p(2); + + // Concise version + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); + + ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf, 1e-9)); +} + +/* ************************************************************************* */ +// Unary(Leaf)) +TEST(ExpressionFactor, unary) { + + // Create some values + Values values; + values.insert(2, Point3(0, 0, 1)); + + JacobianFactor expected( // + 2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), // + (Vector(2) << -17, 30)); + + // Create leaves + Point3_ p(2); + + // Concise version + ExpressionFactor f(model, measured, project(p)); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf, 1e-9)); +} /* ************************************************************************* */ struct TestBinaryExpression { static Point2 myUncal(const Cal3_S2& K, const Point2& p, @@ -217,189 +217,189 @@ TEST(ExpressionFactor, shallow) { EXPECT( assert_equal(*expected, *gf2, 1e-9)); } -///* ************************************************************************* */ -//// Binary(Leaf,Unary(Binary(Leaf,Leaf))) -//TEST(ExpressionFactor, tree) { -// -// // Create some values -// Values values; -// values.insert(1, Pose3()); -// values.insert(2, Point3(0, 0, 1)); -// values.insert(3, Cal3_S2()); -// -// // Create old-style factor to create expected value and derivatives -// GeneralSFMFactor2 old(measured, model, 1, 2, 3); -// double expected_error = old.error(values); -// GaussianFactor::shared_ptr expected = old.linearize(values); -// -// // Create leaves -// Pose3_ x(1); -// Point3_ p(2); -// Cal3_S2_ K(3); -// -// // Create expression tree -// Point3_ p_cam(x, &Pose3::transform_to, p); -// Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); -// Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); -// -// // Create factor and check value, dimension, linearization -// ExpressionFactor f(model, measured, uv_hat); -// EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); -// EXPECT_LONGS_EQUAL(2, f.dim()); -// boost::shared_ptr gf = f.linearize(values); -// EXPECT( assert_equal(*expected, *gf, 1e-9)); -// -// // Concise version -// ExpressionFactor f2(model, measured, -// uncalibrate(K, project(transform_to(x, p)))); -// EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); -// EXPECT_LONGS_EQUAL(2, f2.dim()); -// boost::shared_ptr gf2 = f2.linearize(values); -// EXPECT( assert_equal(*expected, *gf2, 1e-9)); -// -// TernaryExpression::Function fff = project6; -// -// // Try ternary version -// ExpressionFactor f3(model, measured, project3(x, p, K)); -// EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); -// EXPECT_LONGS_EQUAL(2, f3.dim()); -// boost::shared_ptr gf3 = f3.linearize(values); -// EXPECT( assert_equal(*expected, *gf3, 1e-9)); -//} -// -///* ************************************************************************* */ -// -//TEST(ExpressionFactor, compose1) { -// -// // Create expression -// Rot3_ R1(1), R2(2); -// Rot3_ R3 = R1 * R2; -// -// // Create factor -// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); -// -// // Create some values -// Values values; -// values.insert(1, Rot3()); -// values.insert(2, Rot3()); -// -// // Check unwhitenedError -// std::vector 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 gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(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 f(noiseModel::Unit::Create(3), Rot3(), R3); -// -// // Create some values -// Values values; -// values.insert(1, Rot3()); -// -// // Check unwhitenedError -// std::vector 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 gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(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 f(noiseModel::Unit::Create(3), Rot3(), R3); -// -// // Create some values -// Values values; -// values.insert(3, Rot3()); -// -// // Check unwhitenedError -// std::vector 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 gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(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 H1, boost::optional H2, -// boost::optional 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 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 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 gf = f.linearize(values); -// boost::shared_ptr jf = // -// boost::dynamic_pointer_cast(gf); -// EXPECT( assert_equal(expected, *jf,1e-9)); -//} +/* ************************************************************************* */ +// Binary(Leaf,Unary(Binary(Leaf,Leaf))) +TEST(ExpressionFactor, tree) { + + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + values.insert(3, Cal3_S2()); + + // Create old-style factor to create expected value and derivatives + GeneralSFMFactor2 old(measured, model, 1, 2, 3); + double expected_error = old.error(values); + GaussianFactor::shared_ptr expected = old.linearize(values); + + // Create leaves + Pose3_ x(1); + Point3_ p(2); + Cal3_S2_ K(3); + + // Create expression tree + Point3_ p_cam(x, &Pose3::transform_to, p); + Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); + + // Create factor and check value, dimension, linearization + ExpressionFactor f(model, measured, uv_hat); + EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf = f.linearize(values); + EXPECT( assert_equal(*expected, *gf, 1e-9)); + + // Concise version + ExpressionFactor f2(model, measured, + uncalibrate(K, project(transform_to(x, p)))); + EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f2.dim()); + boost::shared_ptr gf2 = f2.linearize(values); + EXPECT( assert_equal(*expected, *gf2, 1e-9)); + + TernaryExpression::Function fff = project6; + + // Try ternary version + ExpressionFactor f3(model, measured, project3(x, p, K)); + EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f3.dim()); + boost::shared_ptr gf3 = f3.linearize(values); + EXPECT( assert_equal(*expected, *gf3, 1e-9)); +} + +/* ************************************************************************* */ + +TEST(ExpressionFactor, compose1) { + + // Create expression + Rot3_ R1(1), R2(2); + Rot3_ R3 = R1 * R2; + + // Create factor + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3()); + + // Check unwhitenedError + std::vector 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 gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(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 f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(1, Rot3()); + + // Check unwhitenedError + std::vector 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 gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(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 f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(3, Rot3()); + + // Check unwhitenedError + std::vector 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 gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(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 H1, boost::optional H2, + boost::optional 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 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 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 gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} /* ************************************************************************* */ int main() {