diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 259bb1efe..22172e44f 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -134,8 +134,10 @@ T Expression::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 dims; + boost::tie(keys, dims) = keysAndDims(); + return valueAndDerivatives(values, keys, dims, *H); } else // no derivatives needed, just return value return root_->value(values); diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 04836a1cb..64a8a6bb6 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -47,7 +47,13 @@ protected: public: typedef boost::shared_ptr > 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& 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 - boost::tie(keys_, dims_) = expression_.keysAndDims(); + 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 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 struct traits > : public Testable > {}; /** - * 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 class ExpressionFactor2 : public ExpressionFactor { @@ -248,4 +265,3 @@ class ExpressionFactor2 : public ExpressionFactor { // ExpressionFactor2 }// \ namespace gtsam - diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 73ff34d2a..c7309786d 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -38,8 +38,9 @@ typedef RangeFactor RangeFactor3D; typedef RangeFactorWithTransform RangeFactorWithTransform2D; typedef RangeFactorWithTransform RangeFactorWithTransform3D; -Key poseKey(1); -Key pointKey(2); +// Keys are deliberately *not* in sorted order to test that case. +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); } /* ************************************************************************* */ -