From a282ef54ff949f066a3285183c04cf56ce873260 Mon Sep 17 00:00:00 2001 From: Renaud Dube Date: Wed, 8 Jul 2015 14:01:57 +0200 Subject: [PATCH 01/13] Implemented unit test which replicates the bug --- gtsam/nonlinear/tests/testExpression.cpp | 122 +++++++++++++++++++++++ 1 file changed, 122 insertions(+) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 75af5f634..1be3dabed 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -24,6 +24,9 @@ #include +#include +#include + #include using boost::assign::list_of; using boost::assign::map_list_of; @@ -276,6 +279,125 @@ TEST(Expression, ternary) { EXPECT(expected == ABC.keys()); } +/* ************************************************************************* */ +// Test with multiple compositions on duplicate keys + +bool checkMatricesNear(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, + double tolerance) { + bool near = true; + for (int i = 0; i < lhs.rows(); ++i) { + for (int j = 0; j < lhs.cols(); ++j) { + const double& lij = lhs(i, j); + const double& rij = rhs(i, j); + const double& diff = std::abs(lij - rij); + if (!std::isfinite(lij) || + !std::isfinite(rij) || + diff > tolerance) { + near = false; + + std::cout << "\nposition " << i << "," << j << " evaluates to " + << lij << " and " << rij << std::endl; + } + } + } + return near; +} + + +// Compute finite difference Jacobians for an expression factor. +template +JacobianFactor computeFiniteDifferenceJacobians(ExpressionFactor expression_factor, + const Values& values, + double fd_step) { + Eigen::VectorXd e = expression_factor.unwhitenedError(values); + const size_t rows = e.size(); + + std::map jacobians; + typename ExpressionFactor::const_iterator key_it = expression_factor.begin(); + VectorValues dX = values.zeroVectors(); + for(; key_it != expression_factor.end(); ++key_it) { + size_t key = *key_it; + // Compute central differences using the values struct. + const size_t cols = dX.dim(key); + Eigen::MatrixXd J = Eigen::MatrixXd::Zero(rows, cols); + for(size_t col = 0; col < cols; ++col) { + Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + dx[col] = fd_step; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = expression_factor.unwhitenedError(eval_values); + dx[col] = -fd_step; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = expression_factor.unwhitenedError(eval_values); + J.col(col) = (left - right) * (1.0/(2.0 * fd_step)); + } + jacobians[key] = J; + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); +} + +template +bool testExpressionJacobians(Expression expression, + const Values& values, + double fd_step, + double tolerance) { + // Create factor + size_t size = traits::dimension; + ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); + + // Check linearization + JacobianFactor expected = computeFiniteDifferenceJacobians(f, values, fd_step); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + + typedef std::pair Jacobian; + Jacobian evalJ = jf->jacobianUnweighted(); + Jacobian estJ = expected.jacobianUnweighted(); + + return checkMatricesNear(evalJ.first, estJ.first, tolerance); +} + +double doubleSumImplementation(const double& v1, const double& v2, + OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { + if (H1) { + H1->setIdentity(); + } + if (H2) { + H2->setIdentity(); + } + return v1+v2; +} + +TEST(Expression, testMultipleCompositions) { + const double tolerance = 1e-5; + const double fd_step = 1e-9; + + double v1 = 0; + double v2 = 1; + + Values values; + values.insert(1, v1); + values.insert(2, v2); + + Expression ev1(Key(1)); + Expression ev2(Key(2)); + + Expression sum(doubleSumImplementation, ev1, ev2); + Expression sum2(doubleSumImplementation, sum, ev1); + Expression sum3(doubleSumImplementation, sum2, sum); + + std::cout << "Testing sum " << std::endl; + EXPECT(testExpressionJacobians(sum, values, fd_step, tolerance)); + std::cout << "Testing sum2 " << std::endl; + EXPECT(testExpressionJacobians(sum2, values, fd_step, tolerance)); + std::cout << "Testing sum3 " << std::endl; + EXPECT(testExpressionJacobians(sum3, values, fd_step, tolerance)); +} + /* ************************************************************************* */ int main() { TestResult tr; From b9b761e06b94750678b079a32492120a611782a7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 8 Jul 2015 18:45:27 -0700 Subject: [PATCH 02/13] Made tests work better with CppUnitLite --- gtsam/nonlinear/expressionTesting.h | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h index 47f61b8b1..f2f1c9578 100644 --- a/gtsam/nonlinear/expressionTesting.h +++ b/gtsam/nonlinear/expressionTesting.h @@ -34,7 +34,7 @@ namespace gtsam { namespace internal { // CPPUnitLite-style test for linearization of a factor -void testFactorJacobians(TestResult& result_, const std::string& name_, +bool testFactorJacobians(TestResult& result_, const std::string& name_, const NoiseModelFactor& factor, const gtsam::Values& values, double delta, double tolerance) { @@ -46,8 +46,7 @@ void testFactorJacobians(TestResult& result_, const std::string& name_, boost::dynamic_pointer_cast(factor.linearize(values)); // Check cast result and then equality - CHECK(actual); - EXPECT(assert_equal(expected, *actual, tolerance)); + return actual && assert_equal(expected, *actual, tolerance); } } @@ -57,19 +56,19 @@ void testFactorJacobians(TestResult& result_, const std::string& name_, /// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ - { gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } + { EXPECT(gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance)); } namespace internal { // CPPUnitLite-style test for linearization of an ExpressionFactor template -void testExpressionJacobians(TestResult& result_, const std::string& name_, +bool testExpressionJacobians(TestResult& result_, const std::string& name_, const gtsam::Expression& expression, const gtsam::Values& values, double nd_step, double tolerance) { // Create factor size_t size = traits::dimension; ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); - testFactorJacobians(result_, name_, f, values, nd_step, tolerance); + return testFactorJacobians(result_, name_, f, values, nd_step, tolerance); } } // namespace internal } // namespace gtsam @@ -80,4 +79,4 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_, /// \param numerical_derivative_step The step to use when computing the finite difference Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \ - { gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); } + { EXPECT(gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance)); } From 61d6ba8a575758585cbe3884130b5108005bfd64 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 8 Jul 2015 18:46:06 -0700 Subject: [PATCH 03/13] Refactored tests a bit to use existing test framework (also originated from ETH so almost identical) --- gtsam/nonlinear/tests/testExpression.cpp | 124 ++++++----------------- 1 file changed, 30 insertions(+), 94 deletions(-) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 1be3dabed..ccb49942b 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -18,6 +18,7 @@ */ #include +#include #include #include #include @@ -281,95 +282,15 @@ TEST(Expression, ternary) { /* ************************************************************************* */ // Test with multiple compositions on duplicate keys - -bool checkMatricesNear(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, - double tolerance) { - bool near = true; - for (int i = 0; i < lhs.rows(); ++i) { - for (int j = 0; j < lhs.cols(); ++j) { - const double& lij = lhs(i, j); - const double& rij = rhs(i, j); - const double& diff = std::abs(lij - rij); - if (!std::isfinite(lij) || - !std::isfinite(rij) || - diff > tolerance) { - near = false; - - std::cout << "\nposition " << i << "," << j << " evaluates to " - << lij << " and " << rij << std::endl; - } - } - } - return near; -} - - -// Compute finite difference Jacobians for an expression factor. -template -JacobianFactor computeFiniteDifferenceJacobians(ExpressionFactor expression_factor, - const Values& values, - double fd_step) { - Eigen::VectorXd e = expression_factor.unwhitenedError(values); - const size_t rows = e.size(); - - std::map jacobians; - typename ExpressionFactor::const_iterator key_it = expression_factor.begin(); - VectorValues dX = values.zeroVectors(); - for(; key_it != expression_factor.end(); ++key_it) { - size_t key = *key_it; - // Compute central differences using the values struct. - const size_t cols = dX.dim(key); - Eigen::MatrixXd J = Eigen::MatrixXd::Zero(rows, cols); - for(size_t col = 0; col < cols; ++col) { - Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = fd_step; - dX[key] = dx; - Values eval_values = values.retract(dX); - Eigen::VectorXd left = expression_factor.unwhitenedError(eval_values); - dx[col] = -fd_step; - dX[key] = dx; - eval_values = values.retract(dX); - Eigen::VectorXd right = expression_factor.unwhitenedError(eval_values); - J.col(col) = (left - right) * (1.0/(2.0 * fd_step)); - } - jacobians[key] = J; - } - - // Next step...return JacobianFactor - return JacobianFactor(jacobians, -e); -} - -template -bool testExpressionJacobians(Expression expression, - const Values& values, - double fd_step, - double tolerance) { - // Create factor - size_t size = traits::dimension; - ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); - - // Check linearization - JacobianFactor expected = computeFiniteDifferenceJacobians(f, values, fd_step); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - - typedef std::pair Jacobian; - Jacobian evalJ = jf->jacobianUnweighted(); - Jacobian estJ = expected.jacobianUnweighted(); - - return checkMatricesNear(evalJ.first, estJ.first, tolerance); -} - -double doubleSumImplementation(const double& v1, const double& v2, - OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { +static double doubleSum(const double& v1, const double& v2, + OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { if (H1) { H1->setIdentity(); } if (H2) { H2->setIdentity(); } - return v1+v2; + return v1 + v2; } TEST(Expression, testMultipleCompositions) { @@ -383,19 +304,34 @@ TEST(Expression, testMultipleCompositions) { values.insert(1, v1); values.insert(2, v2); - Expression ev1(Key(1)); - Expression ev2(Key(2)); + Expression v1_(Key(1)); + Expression v2_(Key(2)); - Expression sum(doubleSumImplementation, ev1, ev2); - Expression sum2(doubleSumImplementation, sum, ev1); - Expression sum3(doubleSumImplementation, sum2, sum); + // binary(doubleSum) + // - leaf 1 + // - leaf 2 + Expression sum_(doubleSum, v1_, v2_); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum_, values, fd_step, tolerance); - std::cout << "Testing sum " << std::endl; - EXPECT(testExpressionJacobians(sum, values, fd_step, tolerance)); - std::cout << "Testing sum2 " << std::endl; - EXPECT(testExpressionJacobians(sum2, values, fd_step, tolerance)); - std::cout << "Testing sum3 " << std::endl; - EXPECT(testExpressionJacobians(sum3, values, fd_step, tolerance)); + // binary(doubleSum) + // - sum_ + // - leaf 1 + // - leaf 2 + // - leaf 1 + Expression sum2_(doubleSum, sum_, v1_); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); + + // binary(doubleSum) + // sum2_ + // - sum_ + // - leaf 1 + // - leaf 2 + // - leaf 1 + // - sum_ + // - leaf 1 + // - leaf 2 + Expression sum3_(doubleSum, sum2_, sum_); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); } /* ************************************************************************* */ From 0c292150180abe014b3051facce7187132d92b9b Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 8 Jul 2015 19:14:23 -0700 Subject: [PATCH 04/13] Slight refactor/reformatting --- gtsam/nonlinear/ExpressionFactor.h | 20 ++++++++++++-------- gtsam/nonlinear/internal/JacobianMap.h | 7 +++---- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b32b84df3..cac55563f 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -32,7 +32,7 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { - protected: +protected: typedef ExpressionFactor This; @@ -42,7 +42,7 @@ class ExpressionFactor: public NoiseModelFactor { static const int Dim = traits::dimension; - public: +public: typedef boost::shared_ptr > shared_ptr; @@ -83,13 +83,16 @@ class ExpressionFactor: public NoiseModelFactor { if (!active(x)) return boost::shared_ptr(); - // Create a writeable JacobianFactor in advance // In case noise model is constrained, we need to provide a noise model - bool constrained = noiseModel_->isConstrained(); + SharedDiagonal noiseModel; + if (noiseModel_->isConstrained()) { + noiseModel = boost::static_pointer_cast( + noiseModel_)->unit(); + } + + // Create a writeable JacobianFactor in advance boost::shared_ptr factor( - constrained ? new JacobianFactor(keys_, dims_, Dim, - boost::static_pointer_cast(noiseModel_)->unit()) : - new JacobianFactor(keys_, dims_, Dim)); + new JacobianFactor(keys_, dims_, Dim, noiseModel)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); @@ -114,7 +117,8 @@ class ExpressionFactor: public NoiseModelFactor { /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } }; // ExpressionFactor diff --git a/gtsam/nonlinear/internal/JacobianMap.h b/gtsam/nonlinear/internal/JacobianMap.h index f4d2e9565..c99f05b7d 100644 --- a/gtsam/nonlinear/internal/JacobianMap.h +++ b/gtsam/nonlinear/internal/JacobianMap.h @@ -31,19 +31,18 @@ namespace internal { // The JacobianMap provides a mapping from keys to the underlying blocks. class JacobianMap { private: - typedef FastVector Keys; - const FastVector& keys_; + const KeyVector& keys_; VerticalBlockMatrix& Ab_; public: /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab - JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) : + JacobianMap(const KeyVector& keys, VerticalBlockMatrix& Ab) : keys_(keys), Ab_(Ab) { } /// Access blocks of via key VerticalBlockMatrix::Block operator()(Key key) { - Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key); + KeyVector::const_iterator it = std::find(keys_.begin(), keys_.end(), key); DenseIndex block = it - keys_.begin(); return Ab_(block); } From f7c5f0cb79bd444c1be96701430f781adf1ab3c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 8 Jul 2015 22:50:24 -0700 Subject: [PATCH 05/13] Moved test to tests/ExpressionFactor --- gtsam/nonlinear/tests/testExpression.cpp | 58 ------------------------ tests/testExpressionFactor.cpp | 55 ++++++++++++++++++++++ 2 files changed, 55 insertions(+), 58 deletions(-) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index ccb49942b..75af5f634 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -18,16 +18,12 @@ */ #include -#include #include #include #include #include -#include -#include - #include using boost::assign::list_of; using boost::assign::map_list_of; @@ -280,60 +276,6 @@ TEST(Expression, ternary) { EXPECT(expected == ABC.keys()); } -/* ************************************************************************* */ -// Test with multiple compositions on duplicate keys -static double doubleSum(const double& v1, const double& v2, - OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { - if (H1) { - H1->setIdentity(); - } - if (H2) { - H2->setIdentity(); - } - return v1 + v2; -} - -TEST(Expression, testMultipleCompositions) { - const double tolerance = 1e-5; - const double fd_step = 1e-9; - - double v1 = 0; - double v2 = 1; - - Values values; - values.insert(1, v1); - values.insert(2, v2); - - Expression v1_(Key(1)); - Expression v2_(Key(2)); - - // binary(doubleSum) - // - leaf 1 - // - leaf 2 - Expression sum_(doubleSum, v1_, v2_); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum_, values, fd_step, tolerance); - - // binary(doubleSum) - // - sum_ - // - leaf 1 - // - leaf 2 - // - leaf 1 - Expression sum2_(doubleSum, sum_, v1_); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); - - // binary(doubleSum) - // sum2_ - // - sum_ - // - leaf 1 - // - leaf 2 - // - leaf 1 - // - sum_ - // - leaf 1 - // - leaf 2 - Expression sum3_(doubleSum, sum2_, sum_); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 312ad89eb..aa990805e 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -504,6 +505,60 @@ TEST(ExpressionFactor, push_back) { graph.addExpressionFactor(model, Point2(0, 0), leaf::p); } +/* ************************************************************************* */ +// Test with multiple compositions on duplicate keys +static double doubleSum(const double& v1, const double& v2, + OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { + if (H1) { + H1->setIdentity(); + } + if (H2) { + H2->setIdentity(); + } + return v1 + v2; +} + +TEST(Expression, testMultipleCompositions) { + const double tolerance = 1e-5; + const double fd_step = 1e-9; + + double v1 = 0; + double v2 = 1; + + Values values; + values.insert(1, v1); + values.insert(2, v2); + + Expression v1_(Key(1)); + Expression v2_(Key(2)); + + // binary(doubleSum) + // - leaf 1 + // - leaf 2 + Expression sum_(doubleSum, v1_, v2_); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum_, values, fd_step, tolerance); + + // binary(doubleSum) + // - sum_ + // - leaf 1 + // - leaf 2 + // - leaf 1 + Expression sum2_(doubleSum, sum_, v1_); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); + + // binary(doubleSum) + // sum2_ + // - sum_ + // - leaf 1 + // - leaf 2 + // - leaf 1 + // - sum_ + // - leaf 1 + // - leaf 2 + Expression sum3_(doubleSum, sum2_, sum_); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); +} + /* ************************************************************************* */ int main() { TestResult tr; From 6df0d49769a584f8ef421dccd6c5b45cf1ba5605 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 8 Jul 2015 23:21:35 -0700 Subject: [PATCH 06/13] Recursive print --- gtsam/nonlinear/Expression-inl.h | 7 ++-- gtsam/nonlinear/Expression.h | 4 +- gtsam/nonlinear/internal/ExpressionNode.h | 48 +++++++++++++++++++---- 3 files changed, 46 insertions(+), 13 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 6c6c155c7..815f9c3da 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -29,7 +29,7 @@ namespace gtsam { template void Expression::print(const std::string& s) const { - std::cout << s << *root_ << std::endl; + root_->print(s); } template @@ -155,7 +155,7 @@ size_t Expression::traceSize() const { template T Expression::valueAndDerivatives(const Values& values, - const FastVector& keys, const FastVector& dims, + const KeyVector& keys, const FastVector& dims, std::vector& H) const { // H should be pre-allocated @@ -205,6 +205,7 @@ T Expression::valueAndJacobianMap(const Values& values, internal::ExecutionTrace trace; T value(this->traceExecution(values, trace, traceStorage)); + GTSAM_PRINT(trace); trace.startReverseAD1(jacobians); #ifdef _MSC_VER @@ -219,7 +220,7 @@ typename Expression::KeysAndDims Expression::keysAndDims() const { std::map map; dims(map); size_t n = map.size(); - KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); + KeysAndDims pair = std::make_pair(KeyVector(n), FastVector(n)); boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); boost::copy(map | boost::adaptors::map_values, pair.second.begin()); return pair; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index d24a568f5..4fdbe8610 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -173,11 +173,11 @@ public: private: /// Keys and dimensions in same order - typedef std::pair, FastVector > KeysAndDims; + typedef std::pair > KeysAndDims; KeysAndDims keysAndDims() const; /// private version that takes keys and dimensions, returns derivatives - T valueAndDerivatives(const Values& values, const FastVector& keys, + T valueAndDerivatives(const Values& values, const KeyVector& keys, const FastVector& dims, std::vector& H) const; /// trace execution, very unsafe diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index e7aa34db0..d8f9cf3ff 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -78,13 +78,14 @@ public: virtual ~ExpressionNode() { } + /// Print + virtual void print(const std::string& indent = "") const = 0; + /// Streaming GTSAM_EXPORT - friend std::ostream &operator<<(std::ostream &os, - const ExpressionNode& node) { + friend std::ostream& operator<<(std::ostream& os, const ExpressionNode& node) { os << "Expression of type " << typeid(T).name(); - if (node.traceSize_ > 0) - os << ", trace size = " << node.traceSize_; + if (node.traceSize_ > 0) os << ", trace size = " << node.traceSize_; os << "\n"; return os; } @@ -133,6 +134,11 @@ public: virtual ~ConstantExpression() { } + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "Constant" << std::endl; + } + /// Return value virtual T value(const Values& values) const { return constant_; @@ -159,7 +165,7 @@ class LeafExpression: public ExpressionNode { key_(key) { } - friend class Expression ; + friend class Expression; public: @@ -167,6 +173,11 @@ public: virtual ~LeafExpression() { } + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "Leaf, key = " << key_ << std::endl; + } + /// Return keys that play in this expression virtual std::set keys() const { std::set keys; @@ -218,7 +229,7 @@ class UnaryExpression: public ExpressionNode { ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } - friend class Expression ; + friend class Expression; public: @@ -226,6 +237,12 @@ public: virtual ~UnaryExpression() { } + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "UnaryExpression" << std::endl; + expression1_->print(indent + " "); + } + /// Return value virtual T value(const Values& values) const { return function_(expression1_->value(values), boost::none); @@ -320,7 +337,7 @@ class BinaryExpression: public ExpressionNode { upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } - friend class Expression ; + friend class Expression; friend class ::ExpressionFactorBinaryTest; public: @@ -329,6 +346,13 @@ public: virtual ~BinaryExpression() { } + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "BinaryExpression" << std::endl; + expression1_->print(indent + " "); + expression2_->print(indent + " "); + } + /// Return value virtual T value(const Values& values) const { using boost::none; @@ -420,7 +444,7 @@ class TernaryExpression: public ExpressionNode { e1.traceSize() + e2.traceSize() + e3.traceSize(); } - friend class Expression ; + friend class Expression; public: @@ -428,6 +452,14 @@ public: virtual ~TernaryExpression() { } + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "TernaryExpression" << std::endl; + expression1_->print(indent + " "); + expression2_->print(indent + " "); + expression3_->print(indent + " "); + } + /// Return value virtual T value(const Values& values) const { using boost::none; From 2bc0d580e70d9d9d12100d1a4a1fa3654fa2cef0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 8 Jul 2015 23:29:21 -0700 Subject: [PATCH 07/13] Slightly changed example, debugging output --- tests/testExpressionFactor.cpp | 61 +++++++++++++++++----------------- 1 file changed, 30 insertions(+), 31 deletions(-) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index aa990805e..5e348afd3 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -507,23 +507,19 @@ TEST(ExpressionFactor, push_back) { /* ************************************************************************* */ // Test with multiple compositions on duplicate keys -static double doubleSum(const double& v1, const double& v2, +static double specialSum(const double& v1, const double& v2, OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { - if (H1) { - H1->setIdentity(); - } - if (H2) { - H2->setIdentity(); - } - return v1 + v2; + if (H1) (*H1) << 1.0; + if (H2) (*H2) << 2.0; + return v1 + 2.0 * v2; } TEST(Expression, testMultipleCompositions) { const double tolerance = 1e-5; const double fd_step = 1e-9; - double v1 = 0; - double v2 = 1; + double v1 = 2; + double v2 = 3; Values values; values.insert(1, v1); @@ -532,30 +528,33 @@ TEST(Expression, testMultipleCompositions) { Expression v1_(Key(1)); Expression v2_(Key(2)); - // binary(doubleSum) - // - leaf 1 - // - leaf 2 - Expression sum_(doubleSum, v1_, v2_); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum_, values, fd_step, tolerance); + // BinaryExpression + // Leaf, key = 1 + // Leaf, key = 2 + Expression sum1_(specialSum, v1_, v2_); + GTSAM_PRINT(sum1_); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); - // binary(doubleSum) - // - sum_ - // - leaf 1 - // - leaf 2 - // - leaf 1 - Expression sum2_(doubleSum, sum_, v1_); + // BinaryExpression + // BinaryExpression + // Leaf, key = 1 + // Leaf, key = 2 + // Leaf, key = 1 + Expression sum2_(specialSum, sum1_, v1_); + GTSAM_PRINT(sum2_); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); - // binary(doubleSum) - // sum2_ - // - sum_ - // - leaf 1 - // - leaf 2 - // - leaf 1 - // - sum_ - // - leaf 1 - // - leaf 2 - Expression sum3_(doubleSum, sum2_, sum_); + // BinaryExpression + // BinaryExpression + // BinaryExpression + // Leaf, key = 1 + // Leaf, key = 2 + // Leaf, key = 1 + // BinaryExpression + // Leaf, key = 1 + // Leaf, key = 2 + Expression sum3_(specialSum, sum2_, sum1_); + GTSAM_PRINT(sum3_); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); } From 4516c6738919d8d2b1e5767341c1ebfc366bff98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 8 Jul 2015 23:52:25 -0700 Subject: [PATCH 08/13] More compact and informative trace/record printing --- gtsam/nonlinear/internal/ExecutionTrace.h | 1 - gtsam/nonlinear/internal/ExpressionNode.h | 30 ++++++++++++----------- 2 files changed, 16 insertions(+), 15 deletions(-) diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index 45817a3f9..315261293 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -119,7 +119,6 @@ public: else if (kind == Leaf) std::cout << indent << "Leaf, key = " << content.key << std::endl; else if (kind == Function) { - std::cout << indent << "Function" << std::endl; content.ptr->print(indent + " "); } } diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index d8f9cf3ff..73852cee8 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -211,8 +211,16 @@ struct Jacobian { typedef Eigen::Matrix::dimension, traits::dimension> type; }; -// Eigen format for printing Jacobians -static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); +// Helper function for printing Jacobians with compact Eigen format, and trace +template +static void PrintJacobianAndTrace(const std::string& indent, + const typename Jacobian::type& dTdA, + const ExecutionTrace trace) { + static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << indent << "d(" << typeid(T).name() << ")/d(" << typeid(A).name() + << ") = " << dTdA.format(kMatlabFormat) << std::endl; + trace.print(indent); +} //----------------------------------------------------------------------------- /// Unary Function Expression @@ -268,8 +276,7 @@ public: /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "UnaryExpression::Record {" << std::endl; - std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; - trace1.print(indent); + PrintJacobianAndTrace(indent, dTdA1, trace1); std::cout << indent << "}" << std::endl; } @@ -388,10 +395,8 @@ public: /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "BinaryExpression::Record {" << std::endl; - std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; - trace1.print(indent); - std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; - trace2.print(indent); + PrintJacobianAndTrace(indent, dTdA1, trace1); + PrintJacobianAndTrace(indent, dTdA2, trace2); std::cout << indent << "}" << std::endl; } @@ -502,12 +507,9 @@ public: /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "TernaryExpression::Record {" << std::endl; - std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; - trace1.print(indent); - std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; - trace2.print(indent); - std::cout << indent << dTdA3.format(kMatlabFormat) << std::endl; - trace3.print(indent); + PrintJacobianAndTrace(indent, dTdA1, trace1); + PrintJacobianAndTrace(indent, dTdA2, trace2); + PrintJacobianAndTrace(indent, dTdA3, trace3); std::cout << indent << "}" << std::endl; } From e9ddee4322463db425c40d950a35d8d80a71d93a Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Thu, 9 Jul 2015 09:39:13 +0200 Subject: [PATCH 09/13] fixed bug in expression traceExecution --- gtsam/nonlinear/internal/ExpressionNode.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index d8f9cf3ff..f3c19e1aa 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -416,8 +416,9 @@ public: Record* record = new (ptr) Record(); ptr += upAligned(sizeof(Record)); record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + ptr += expression1_->traceSize(); record->value2 = expression2_->traceExecution(values, record->trace2, ptr); - ptr += expression1_->traceSize() + expression2_->traceSize(); + ptr += expression2_->traceSize(); trace.setFunction(record); return function_(record->value1, record->value2, record->dTdA1, record->dTdA2); @@ -534,10 +535,11 @@ public: Record* record = new (ptr) Record(); ptr += upAligned(sizeof(Record)); record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + ptr += expression1_->traceSize(); record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + ptr += expression2_->traceSize(); record->value3 = expression3_->traceExecution(values, record->trace3, ptr); - ptr += expression1_->traceSize() + expression2_->traceSize() - + expression3_->traceSize(); + ptr += expression3_->traceSize(); trace.setFunction(record); return function_(record->value1, record->value2, record->value3, record->dTdA1, record->dTdA2, record->dTdA3); From 20b669bed62b7d13df698e4de09d8a07688145d1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 9 Jul 2015 00:40:21 -0700 Subject: [PATCH 10/13] Refined testcase even more for debugging --- tests/testExpressionFactor.cpp | 39 ++++++++++++++++++++-------------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 5e348afd3..301da21e5 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -507,12 +507,16 @@ TEST(ExpressionFactor, push_back) { /* ************************************************************************* */ // Test with multiple compositions on duplicate keys -static double specialSum(const double& v1, const double& v2, - OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { - if (H1) (*H1) << 1.0; - if (H2) (*H2) << 2.0; - return v1 + 2.0 * v2; -} +struct Combine { + double a, b; + Combine(double a, double b) : a(a), b(b) {} + double operator()(const double& x, const double& y, OptionalJacobian<1, 1> H1, + OptionalJacobian<1, 1> H2) { + if (H1) (*H1) << a; + if (H2) (*H2) << b; + return a * x + b * y; + } +}; TEST(Expression, testMultipleCompositions) { const double tolerance = 1e-5; @@ -528,33 +532,36 @@ TEST(Expression, testMultipleCompositions) { Expression v1_(Key(1)); Expression v2_(Key(2)); - // BinaryExpression + // BinaryExpression(1,2) // Leaf, key = 1 // Leaf, key = 2 - Expression sum1_(specialSum, v1_, v2_); + Expression sum1_(Combine(1, 2), v1_, v2_); GTSAM_PRINT(sum1_); + EXPECT(sum1_.keys() == list_of(1)(2)); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); - // BinaryExpression - // BinaryExpression + // BinaryExpression(3,4) + // BinaryExpression(1,2) // Leaf, key = 1 // Leaf, key = 2 // Leaf, key = 1 - Expression sum2_(specialSum, sum1_, v1_); + Expression sum2_(Combine(3, 4), sum1_, v1_); GTSAM_PRINT(sum2_); + EXPECT(sum2_.keys() == list_of(1)(2)); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); - // BinaryExpression - // BinaryExpression - // BinaryExpression + // BinaryExpression(5,6) + // BinaryExpression(3,4) + // BinaryExpression(1,2) // Leaf, key = 1 // Leaf, key = 2 // Leaf, key = 1 - // BinaryExpression + // BinaryExpression(1,2) // Leaf, key = 1 // Leaf, key = 2 - Expression sum3_(specialSum, sum2_, sum1_); + Expression sum3_(Combine(5, 6), sum1_, sum2_); GTSAM_PRINT(sum3_); + EXPECT(sum3_.keys() == list_of(1)(2)); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); } From 0922624b9e982772424c70603c1310d58983017e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 9 Jul 2015 00:41:03 -0700 Subject: [PATCH 11/13] FIXED PRETTY TERRIBLE BUG --- gtsam/nonlinear/internal/ExpressionNode.h | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 73852cee8..928387eb9 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -217,7 +217,7 @@ static void PrintJacobianAndTrace(const std::string& indent, const typename Jacobian::type& dTdA, const ExecutionTrace trace) { static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << indent << "d(" << typeid(T).name() << ")/d(" << typeid(A).name() + std::cout << indent << "D(" << typeid(T).name() << ")/D(" << typeid(A).name() << ") = " << dTdA.format(kMatlabFormat) << std::endl; trace.print(indent); } @@ -317,11 +317,11 @@ public: // Return value of type T is recorded in record->value record->value1 = expression1_->traceExecution(values, record->trace1, ptr); - // ptr is never modified by traceExecution, but if traceExecution has - // written in the buffer, the next caller expects we advance the pointer + // We have written in the buffer, the next caller expects we advance the pointer ptr += expression1_->traceSize(); trace.setFunction(record); + // Finally, the function call fills in the Jacobian dTdA1 return function_(record->value1, record->dTdA1); } }; @@ -421,11 +421,11 @@ public: Record* record = new (ptr) Record(); ptr += upAligned(sizeof(Record)); record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + ptr += expression1_->traceSize(); record->value2 = expression2_->traceExecution(values, record->trace2, ptr); - ptr += expression1_->traceSize() + expression2_->traceSize(); + ptr += expression2_->traceSize(); trace.setFunction(record); - return function_(record->value1, record->value2, record->dTdA1, - record->dTdA2); + return function_(record->value1, record->value2, record->dTdA1, record->dTdA2); } }; @@ -536,10 +536,11 @@ public: Record* record = new (ptr) Record(); ptr += upAligned(sizeof(Record)); record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + ptr += expression1_->traceSize(); record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + ptr += expression2_->traceSize(); record->value3 = expression3_->traceExecution(values, record->trace3, ptr); - ptr += expression1_->traceSize() + expression2_->traceSize() - + expression3_->traceSize(); + ptr += expression3_->traceSize(); trace.setFunction(record); return function_(record->value1, record->value2, record->value3, record->dTdA1, record->dTdA2, record->dTdA3); From a923086a000f1cd41fe7c58bbd62fdc7efcf8381 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 9 Jul 2015 00:42:48 -0700 Subject: [PATCH 12/13] Removed print statements --- gtsam/nonlinear/Expression-inl.h | 1 - tests/testExpressionFactor.cpp | 3 --- 2 files changed, 4 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 815f9c3da..70a872d15 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -205,7 +205,6 @@ T Expression::valueAndJacobianMap(const Values& values, internal::ExecutionTrace trace; T value(this->traceExecution(values, trace, traceStorage)); - GTSAM_PRINT(trace); trace.startReverseAD1(jacobians); #ifdef _MSC_VER diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 301da21e5..440e67017 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -536,7 +536,6 @@ TEST(Expression, testMultipleCompositions) { // Leaf, key = 1 // Leaf, key = 2 Expression sum1_(Combine(1, 2), v1_, v2_); - GTSAM_PRINT(sum1_); EXPECT(sum1_.keys() == list_of(1)(2)); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); @@ -546,7 +545,6 @@ TEST(Expression, testMultipleCompositions) { // Leaf, key = 2 // Leaf, key = 1 Expression sum2_(Combine(3, 4), sum1_, v1_); - GTSAM_PRINT(sum2_); EXPECT(sum2_.keys() == list_of(1)(2)); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); @@ -560,7 +558,6 @@ TEST(Expression, testMultipleCompositions) { // Leaf, key = 1 // Leaf, key = 2 Expression sum3_(Combine(5, 6), sum1_, sum2_); - GTSAM_PRINT(sum3_); EXPECT(sum3_.keys() == list_of(1)(2)); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); } From aa1fae41a9c36628223e7354dad90e91c4c7a0e7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 9 Jul 2015 00:53:40 -0700 Subject: [PATCH 13/13] Added testcase mixing binary and ternary cases --- tests/testExpressionFactor.cpp | 50 ++++++++++++++++++++++++++++++---- 1 file changed, 44 insertions(+), 6 deletions(-) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 440e67017..48cf03f8c 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -520,14 +520,11 @@ struct Combine { TEST(Expression, testMultipleCompositions) { const double tolerance = 1e-5; - const double fd_step = 1e-9; - - double v1 = 2; - double v2 = 3; + const double fd_step = 1e-5; Values values; - values.insert(1, v1); - values.insert(2, v2); + values.insert(1, 10.0); + values.insert(2, 20.0); Expression v1_(Key(1)); Expression v2_(Key(2)); @@ -562,6 +559,47 @@ TEST(Expression, testMultipleCompositions) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); } +/* ************************************************************************* */ +// Another test, with Ternary Expressions +static double combine3(const double& x, const double& y, const double& z, + OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2, + OptionalJacobian<1, 1> H3) { + if (H1) (*H1) << 1.0; + if (H2) (*H2) << 2.0; + if (H3) (*H3) << 3.0; + return x + 2.0 * y + 3.0 * z; +} + +TEST(Expression, testMultipleCompositions2) { + const double tolerance = 1e-5; + const double fd_step = 1e-5; + + Values values; + values.insert(1, 10.0); + values.insert(2, 20.0); + values.insert(3, 30.0); + + Expression v1_(Key(1)); + Expression v2_(Key(2)); + Expression v3_(Key(3)); + + Expression sum1_(Combine(4,5), v1_, v2_); + EXPECT(sum1_.keys() == list_of(1)(2)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); + + Expression sum2_(combine3, v1_, v2_, v3_); + EXPECT(sum2_.keys() == list_of(1)(2)(3)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); + + Expression sum3_(combine3, v3_, v2_, v1_); + EXPECT(sum3_.keys() == list_of(1)(2)(3)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); + + Expression sum4_(combine3, sum1_, sum2_, sum3_); + EXPECT(sum4_.keys() == list_of(1)(2)(3)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance); +} + /* ************************************************************************* */ int main() { TestResult tr;