Merged in feature/fixExpressionFactorKeys (pull request #240)

Fix a bug in ExpressionFactor::initialize that changes the key ordering of factors derived from ExpressionFactor2. This impacts serialization and user expectation.

gtsam/nonlinear/Expression-inl.h

@@ -134,8 +134,10 @@ T Expression<T>::value(const Values& values,
 
   if (H) {
     // Call private version that returns derivatives in H
-    KeysAndDims pair = keysAndDims();
+    KeyVector keys;
-    return valueAndDerivatives(values, pair.first, pair.second, *H);
+    FastVector<int> dims;
+    boost::tie(keys, dims) = keysAndDims();
+    return valueAndDerivatives(values, keys, dims, *H);
   } else
     // no derivatives needed, just return value
     return root_->value(values);

gtsam/nonlinear/ExpressionFactor.h

@@ -47,7 +47,13 @@ protected:
  public:
   typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
 
-  /// Constructor
+  /**
+   * Constructor: creates a factor from a measurement and measurement function
+   *   @param noiseModel the noise model associated with a measurement
+   *   @param measurement actual value of the measurement, of type T
+   *   @param expression predicts the measurement from Values
+   * The keys associated with the factor, returned by keys(), are sorted.
+   */
   ExpressionFactor(const SharedNoiseModel& noiseModel,  //
                    const T& measurement, const Expression<T>& expression)
       : NoiseModelFactor(noiseModel), measured_(measurement) {
@@ -158,7 +164,18 @@ protected:
 
    // Get keys and dimensions for Jacobian matrices
    // An Expression is assumed unmutable, so we do this now
+   if (keys_.empty()) {
+     // This is the case when called in ExpressionFactor Constructor.
+     // We then take the keys from the expression in sorted order.
      boost::tie(keys_, dims_) = expression_.keysAndDims();
+   } else {
+     // This happens with classes derived from BinaryExpressionFactor etc.
+     // In that case, the keys_ are already defined and we just need to grab
+     // the dimensions in the correct order.
+     std::map<Key, int> keyedDims;
+     expression_.dims(keyedDims);
+     for (Key key : keys_) dims_.push_back(keyedDims[key]);
+   }
  }
 
  /// Recreate expression from keys_ and measured_, used in load below.
@@ -196,9 +213,9 @@ template <typename T>
 struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
 
 /**
- * Binary specialization of ExpressionFactor meant as a base class for binary factors
+ * Binary specialization of ExpressionFactor meant as a base class for binary
- * Enforces expression method with two keys, and provides evaluateError
+ * factors. Enforces an 'expression' method with two keys, and provides 'evaluateError'.
- * Derived needs to call initialize.
+ * Derived class (a binary factor!) needs to call 'initialize'.
  */
 template <typename T, typename A1, typename A2>
 class ExpressionFactor2 : public ExpressionFactor<T> {
@@ -248,4 +265,3 @@ class ExpressionFactor2 : public ExpressionFactor<T> {
 // ExpressionFactor2
 
 }// \ namespace gtsam
-

gtsam/sam/tests/testRangeFactor.cpp

@@ -38,8 +38,9 @@ typedef RangeFactor<Pose3, Point3> RangeFactor3D;
 typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
 typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
 
-Key poseKey(1);
+// Keys are deliberately *not* in sorted order to test that case.
-Key pointKey(2);
+Key poseKey(2);
+Key pointKey(1);
 double measurement(10.0);
 
 /* ************************************************************************* */
@@ -101,8 +102,13 @@ TEST( RangeFactor, ConstructorWithTransform) {
 
   RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
       body_P_sensor_2D);
+  KeyVector expected;
+  expected.push_back(2);
+  expected.push_back(1);
+  CHECK(factor2D.keys() == expected);
   RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
       body_P_sensor_3D);
+  CHECK(factor3D.keys() == expected);
 }
 
 /* ************************************************************************* */
@@ -395,4 +401,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-