From fdf9c10b422e61044e0973fda3c9d5fad9256587 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 5 Oct 2014 15:00:10 +0200 Subject: [PATCH] Implemented value and now testBADFactor also runs --- gtsam_unstable/nonlinear/Expression-inl.h | 48 +++- gtsam_unstable/nonlinear/Expression.h | 6 +- .../nonlinear/tests/testBADFactor.cpp | 260 +++++++++++------- 3 files changed, 194 insertions(+), 120 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index cfd0bf7f2..f9187ec65 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -21,6 +21,7 @@ #include #include +#include #include namespace gtsam { @@ -44,6 +45,17 @@ private: typedef std::pair Pair; + /// Insert terms into jacobians_, premultiplying by H, adding if already exists + void add(const JacobianMap& terms) { + BOOST_FOREACH(const Pair& term, terms) { + JacobianMap::iterator it = jacobians_.find(term.first); + if (it != jacobians_.end()) + it->second += term.second; + else + jacobians_[term.first] = term.second; + } + } + /// Insert terms into jacobians_, premultiplying by H, adding if already exists void add(const Matrix& H, const JacobianMap& terms) { BOOST_FOREACH(const Pair& term, terms) { @@ -90,7 +102,7 @@ public: Augmented(const T& t, const JacobianMap& jacobians1, const JacobianMap& jacobians2) : value_(t), jacobians_(jacobians1) { - jacobians_.insert(jacobians2.begin(), jacobians2.end()); + add(jacobians2); } /// Construct value, pre-multiply jacobians by H @@ -143,8 +155,9 @@ protected: public: struct Trace { + T t; T value() const { - return T(); + return t; } virtual Augmented augmented(const Matrix& H) const = 0; }; @@ -208,11 +221,10 @@ public: /// Trace structure for reverse AD typedef typename ExpressionNode::Trace BaseTrace; struct Trace: public BaseTrace { - T t; /// Return value and derivatives virtual Augmented augmented(const Matrix& H) const { // Base case: just return value and empty map - return Augmented(t); + return Augmented(this->t); } }; @@ -220,6 +232,8 @@ public: virtual boost::shared_ptr reverse(const Values& values) const { boost::shared_ptr trace = boost::make_shared(); trace->t = constant_; + std::cout << "constant\n:"; + GTSAM_PRINT(trace->t); return trace; } }; @@ -266,14 +280,12 @@ public: /// Trace structure for reverse AD typedef typename ExpressionNode::Trace BaseTrace; struct Trace: public BaseTrace { - T t; Key key; /// Return value and derivatives virtual Augmented augmented(const Matrix& H) const { - // This is a top-down calculation - // The end-result needs Jacobians to all leaf nodes. - // Since this is a leaf node, we are done and just insert H in the JacobianMap - return Augmented(t, key, H); + // Base case: just insert H in the JacobianMap with correct key + std::cout << "Inserting Jacobian " << DefaultKeyFormatter(key) << "\n"; + return Augmented(this->t, key, H); } }; @@ -282,6 +294,8 @@ public: boost::shared_ptr trace = boost::make_shared(); trace->t = value(values); trace->key = key_; + std::cout << "Leaf(" << DefaultKeyFormatter(key_) << "):\n"; + GTSAM_PRINT(trace->t); return trace; } @@ -339,14 +353,13 @@ public: struct Trace: public BaseTrace { boost::shared_ptr::Trace> trace1; Matrix H1; - T t; /// Return value and derivatives virtual Augmented augmented(const Matrix& H) const { // This is a top-down calculation // The end-result needs Jacobians to all leaf nodes. // Since this is not a leaf node, we compute what is needed for leaf nodes here Augmented augmented1 = trace1->augmented(H * H1); - return Augmented(t, augmented1.jacobians()); + return Augmented(this->t, augmented1.jacobians()); } }; @@ -354,7 +367,10 @@ public: virtual boost::shared_ptr reverse(const Values& values) const { boost::shared_ptr trace = boost::make_shared(); trace->trace1 = this->expressionA_->reverse(values); + std::cout << "Unary:\n"; + GTSAM_PRINT(trace->trace1->value()); trace->t = function_(trace->trace1->value(), trace->H1); + GTSAM_PRINT(trace->t); return trace; } }; @@ -424,7 +440,6 @@ public: boost::shared_ptr::Trace> trace1; boost::shared_ptr::Trace> trace2; Matrix H1, H2; - T t; /// Return value and derivatives virtual Augmented augmented(const Matrix& H) const { // This is a top-down calculation @@ -432,8 +447,9 @@ public: // Since this is not a leaf node, we compute what is needed for leaf nodes here // The binary node represents a fork in the tree, and hence we will get two Augmented maps Augmented augmented1 = trace1->augmented(H * H1); - Augmented augmented2 = trace1->augmented(H * H2); - return Augmented(t, augmented1.jacobians(), augmented2.jacobians()); + Augmented augmented2 = trace2->augmented(H * H2); + return Augmented(this->t, augmented1.jacobians(), + augmented2.jacobians()); } }; @@ -442,8 +458,12 @@ public: boost::shared_ptr trace = boost::make_shared(); trace->trace1 = this->expressionA1_->reverse(values); trace->trace2 = this->expressionA2_->reverse(values); + std::cout << "Binary:\n"; + GTSAM_PRINT(trace->trace1->value()); + GTSAM_PRINT(trace->trace2->value()); trace->t = function_(trace->trace1->value(), trace->trace2->value(), trace->H1, trace->H2); + GTSAM_PRINT(trace->t); return trace; } diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 18adea27e..64b1013fe 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -103,10 +103,14 @@ public: /// Return value and derivatives Augmented augmented(const Values& values) const { +#define REVERSE_AD +#ifdef REVERSE_AD boost::shared_ptr::Trace> trace = root_->reverse(values); size_t n = T::Dim(); return trace->augmented(Eigen::MatrixXd::Identity(n, n)); -// return root_->forward(values); +#else + return root_->forward(values); +#endif } const boost::shared_ptr >& root() const { diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp index 7b9dcd765..4a5b1a76f 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -29,141 +30,190 @@ using namespace std; using namespace gtsam; -/* ************************************************************************* */ +Point2 measured(-17, 30); +SharedNoiseModel model = noiseModel::Unit::Create(2); +/* ************************************************************************* */ +// Unary(Leaf)) TEST(BADFactor, test) { // 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 - Point2 measured(-17, 30); - SharedNoiseModel model = noiseModel::Unit::Create(2); - GeneralSFMFactor2 old(measured, model, 1, 2, 3); - double expected_error = old.error(values); - GaussianFactor::shared_ptr expected = old.linearize(values); - - // Test Constant expression - Expression c(0); + JacobianFactor expected( // + 2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), // + (Vector(2) << -17, 30)); // 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 - BADFactor 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)); // Try concise version - BADFactor 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)); -} - -/* ************************************************************************* */ - -TEST(BADFactor, compose1) { - - // Create expression - Rot3_ R1(1), R2(2); - Rot3_ R3 = R1 * R2; - - // Create factor - BADFactor 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)); + BADFactor 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)); + EXPECT( assert_equal(expected, *jf, 1e-9)); } /* ************************************************************************* */ -// Test compose with arguments referring to the same rotation -TEST(BADFactor, compose2) { + // Unary(Binary(Leaf,Leaf)) + TEST(BADFactor, test1) { - // Create expression - Rot3_ R1(1), R2(1); - Rot3_ R3 = R1 * R2; + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); - // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + // 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 some values - Values values; - values.insert(1, Rot3()); + // Create leaves + Pose3_ x(1); + Point3_ p(2); - // 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)); + // Try concise version + BADFactor 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)); + } - // 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)); -} + /* ************************************************************************* */ + // Binary(Leaf,Unary(Binary(Leaf,Leaf))) + TEST(BADFactor, test2) { -/* ************************************************************************* */ -// Test compose with one arguments referring to a constant same rotation -TEST(BADFactor, compose3) { + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + values.insert(3, Cal3_S2()); - // Create expression - Rot3_ R1(Rot3::identity()), R2(3); - Rot3_ R3 = R1 * R2; + // 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 factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + // Create leaves + Pose3_ x(1); + Point3_ p(2); + Cal3_S2_ K(3); - // Create some values - Values values; - values.insert(3, Rot3()); + // 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); - // 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)); + // Create factor and check value, dimension, linearization + BADFactor 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)); - // 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)); -} + // Try concise version + BADFactor 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)); + } -/* ************************************************************************* */ + /* ************************************************************************* */ + + TEST(BADFactor, compose1) { + + // Create expression + Rot3_ R1(1), R2(2); + Rot3_ R3 = R1 * R2; + + // Create factor + BADFactor 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(BADFactor, compose2) { + + // Create expression + Rot3_ R1(1), R2(1); + Rot3_ R3 = R1 * R2; + + // Create factor + BADFactor 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(BADFactor, compose3) { + + // Create expression + Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R3 = R1 * R2; + + // Create factor + BADFactor 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)); + } + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);