From eef2d49e8d95bd01c75ee4a4cd99e59e09f6202a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 11 Oct 2014 10:27:30 +0200 Subject: [PATCH] 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() {