Implemented unit test which replicates the bug

gtsam/nonlinear/tests/testExpression.cpp

@@ -24,6 +24,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+
 #include <boost/assign/list_of.hpp>
 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<typename T>
+JacobianFactor computeFiniteDifferenceJacobians(ExpressionFactor<T> expression_factor,
+                                                const Values& values,
+                                                double fd_step) {
+  Eigen::VectorXd e = expression_factor.unwhitenedError(values);
+  const size_t rows = e.size();
+
+  std::map<Key, Eigen::MatrixXd> jacobians;
+  typename ExpressionFactor<T>::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<typename T>
+bool testExpressionJacobians(Expression<T> expression,
+                             const Values& values,
+                             double fd_step,
+                             double tolerance) {
+  // Create factor
+  size_t size = traits<T>::dimension;
+  ExpressionFactor<T> f(noiseModel::Unit::Create(size), expression.value(values), expression);
+
+  // Check linearization
+  JacobianFactor expected = computeFiniteDifferenceJacobians(f, values, fd_step);
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+
+  typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> 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<double> ev1(Key(1));
+  Expression<double> ev2(Key(2));
+
+  Expression<double> sum(doubleSumImplementation, ev1, ev2);
+  Expression<double> sum2(doubleSumImplementation, sum, ev1);
+  Expression<double> 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;