diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 6c6c155c7..70a872d15 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 @@ -219,7 +219,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/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/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)); } 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 e7aa34db0..928387eb9 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; @@ -200,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 @@ -218,7 +237,7 @@ class UnaryExpression: public ExpressionNode { ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } - friend class Expression ; + friend class Expression; public: @@ -226,6 +245,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); @@ -251,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; } @@ -293,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); } }; @@ -320,7 +344,7 @@ class BinaryExpression: public ExpressionNode { upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } - friend class Expression ; + friend class Expression; friend class ::ExpressionFactorBinaryTest; public: @@ -329,6 +353,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; @@ -364,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; } @@ -392,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); } }; @@ -420,7 +449,7 @@ class TernaryExpression: public ExpressionNode { e1.traceSize() + e2.traceSize() + e3.traceSize(); } - friend class Expression ; + friend class Expression; public: @@ -428,6 +457,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; @@ -470,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; } @@ -502,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); 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); } diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 312ad89eb..48cf03f8c 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -504,6 +505,101 @@ TEST(ExpressionFactor, push_back) { graph.addExpressionFactor(model, Point2(0, 0), leaf::p); } +/* ************************************************************************* */ +// Test with multiple compositions on duplicate keys +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; + const double fd_step = 1e-5; + + Values values; + values.insert(1, 10.0); + values.insert(2, 20.0); + + Expression v1_(Key(1)); + Expression v2_(Key(2)); + + // BinaryExpression(1,2) + // Leaf, key = 1 + // Leaf, key = 2 + Expression sum1_(Combine(1, 2), v1_, v2_); + EXPECT(sum1_.keys() == list_of(1)(2)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); + + // BinaryExpression(3,4) + // BinaryExpression(1,2) + // Leaf, key = 1 + // Leaf, key = 2 + // Leaf, key = 1 + Expression sum2_(Combine(3, 4), sum1_, v1_); + EXPECT(sum2_.keys() == list_of(1)(2)); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); + + // BinaryExpression(5,6) + // BinaryExpression(3,4) + // BinaryExpression(1,2) + // Leaf, key = 1 + // Leaf, key = 2 + // Leaf, key = 1 + // BinaryExpression(1,2) + // Leaf, key = 1 + // Leaf, key = 2 + Expression sum3_(Combine(5, 6), sum1_, sum2_); + EXPECT(sum3_.keys() == list_of(1)(2)); + 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;