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 0bf20f303..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,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 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 keyedDims; + expression_.dims(keyedDims); + for (Key key : keys_) dims_.push_back(keyedDims[key]); } } @@ -202,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 { @@ -254,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 2b23e595d..74a278f4b 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,6 +38,7 @@ typedef RangeFactor RangeFactor3D; typedef RangeFactorWithTransform RangeFactorWithTransform2D; typedef RangeFactorWithTransform 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); } /* ************************************************************************* */ -