commented, verified fix with Duy

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();
-    return valueAndDerivatives(values, pair.first, pair.second, *H);
+    KeyVector keys;
+    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,12 +164,17 @@ protected:
 
    // Get keys and dimensions for Jacobian matrices
    // An Expression is assumed unmutable, so we do this now
-   if (keys_.empty())
+   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 {
-      std::map<Key, int> map;
-      expression_.dims(map);
-      for (Key key : keys_) dims_.push_back(map[key]);   
+   } 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]);
    }
  }
 
@@ -202,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
- * Enforces expression method with two keys, and provides evaluateError
- * Derived needs to call initialize.
+ * Binary specialization of ExpressionFactor meant as a base class for binary
+ * factors. Enforces an 'expression' method with two keys, and provides 'evaluateError'.
+ * Derived class (a binary factor!) needs to call 'initialize'.
  */
 template <typename T, typename A1, typename A2>
 class ExpressionFactor2 : public ExpressionFactor<T> {
@@ -254,4 +265,3 @@ class ExpressionFactor2 : public ExpressionFactor<T> {
 // ExpressionFactor2
 
 }// \ namespace gtsam
-

gtsam/sam/tests/testRangeFactor.cpp

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