Merged in bug/expressionWithDuplicateKeys (pull request #171)

- Unit test from ETH folks which replicates the bug
- Improved printing as part of tracking it down
- Simultaneous bug fixes
- Extra unit test by Frank

gtsam/nonlinear/Expression-inl.h

@@ -29,7 +29,7 @@ namespace gtsam {
 
 template<typename T>
 void Expression<T>::print(const std::string& s) const {
-  std::cout << s << *root_ << std::endl;
+  root_->print(s);
 }
 
 template<typename T>
@@ -155,7 +155,7 @@ size_t Expression<T>::traceSize() const {
 
 template<typename T>
 T Expression<T>::valueAndDerivatives(const Values& values,
-    const FastVector<Key>& keys, const FastVector<int>& dims,
+    const KeyVector& keys, const FastVector<int>& dims,
     std::vector<Matrix>& H) const {
 
   // H should be pre-allocated
@@ -219,7 +219,7 @@ typename Expression<T>::KeysAndDims Expression<T>::keysAndDims() const {
   std::map<Key, int> map;
   dims(map);
   size_t n = map.size();
-  KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
+  KeysAndDims pair = std::make_pair(KeyVector(n), FastVector<int>(n));
   boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
   boost::copy(map | boost::adaptors::map_values, pair.second.begin());
   return pair;

gtsam/nonlinear/Expression.h

@@ -173,11 +173,11 @@ public:
 private:
 
   /// Keys and dimensions in same order
-  typedef std::pair<FastVector<Key>, FastVector<int> > KeysAndDims;
+  typedef std::pair<KeyVector, FastVector<int> > KeysAndDims;
   KeysAndDims keysAndDims() const;
 
   /// private version that takes keys and dimensions, returns derivatives
-  T valueAndDerivatives(const Values& values, const FastVector<Key>& keys,
+  T valueAndDerivatives(const Values& values, const KeyVector& keys,
       const FastVector<int>& dims, std::vector<Matrix>& H) const;
 
   /// trace execution, very unsafe

gtsam/nonlinear/ExpressionFactor.h

@@ -83,13 +83,16 @@ class ExpressionFactor: public NoiseModelFactor {
     if (!active(x))
       return boost::shared_ptr<JacobianFactor>();
 
-    // 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::Constrained>(
+          noiseModel_)->unit();
+    }
+
+    // Create a writeable JacobianFactor in advance
     boost::shared_ptr<JacobianFactor> factor(
-        constrained ? new JacobianFactor(keys_, dims_, Dim,
-            boost::static_pointer_cast<noiseModel::Constrained>(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>(
-        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
 };
 // ExpressionFactor
 

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<JacobianFactor>(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<typename T>
-void testExpressionJacobians(TestResult& result_, const std::string& name_,
+bool testExpressionJacobians(TestResult& result_, const std::string& name_,
     const gtsam::Expression<T>& expression, const gtsam::Values& values,
     double nd_step, double tolerance) {
   // Create factor
   size_t size = traits<T>::dimension;
   ExpressionFactor<T> 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)); }

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 + "  ");
     }
   }

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_;
@@ -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<Key> keys() const {
     std::set<Key> keys;
@@ -200,8 +211,16 @@ struct Jacobian {
   typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
 };
 
-// Eigen format for printing Jacobians
+// Helper function for printing Jacobians with compact Eigen format, and trace
+template <class T, class A>
+static void PrintJacobianAndTrace(const std::string& indent,
+                                  const typename Jacobian<T, A>::type& dTdA,
+                                  const ExecutionTrace<A> 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
@@ -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<T,A1>(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);
   }
 };
@@ -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<T,A1>(indent, dTdA1, trace1);
+      PrintJacobianAndTrace<T,A2>(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);
   }
 };
 
@@ -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<T,A1>(indent, dTdA1, trace1);
+      PrintJacobianAndTrace<T,A2>(indent, dTdA2, trace2);
+      PrintJacobianAndTrace<T,A3>(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);

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<Key> Keys;
-  const FastVector<Key>& 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);
   }

tests/testExpressionFactor.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/expressionTesting.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/expressionTesting.h>
@@ -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<double> v1_(Key(1));
+  Expression<double> v2_(Key(2));
+
+  // BinaryExpression(1,2)
+  //   Leaf, key = 1
+  //   Leaf, key = 2
+  Expression<double> 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<double> 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<double> 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<double> v1_(Key(1));
+  Expression<double> v2_(Key(2));
+  Expression<double> v3_(Key(3));
+
+  Expression<double> sum1_(Combine(4,5), v1_, v2_);
+  EXPECT(sum1_.keys() == list_of(1)(2));
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance);
+
+  Expression<double> sum2_(combine3, v1_, v2_, v3_);
+  EXPECT(sum2_.keys() == list_of(1)(2)(3));
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance);
+
+  Expression<double> sum3_(combine3, v3_, v2_, v1_);
+  EXPECT(sum3_.keys() == list_of(1)(2)(3));
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance);
+
+  Expression<double> 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;