diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 04836a1cb..0bf20f303 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -158,7 +158,13 @@ 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()) + boost::tie(keys_, dims_) = expression_.keysAndDims(); + else { + std::map map; + expression_.dims(map); + for (Key key : keys_) dims_.push_back(map[key]); + } } /// Recreate expression from keys_ and measured_, used in load below. diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 73ff34d2a..2b23e595d 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -38,8 +38,8 @@ typedef RangeFactor RangeFactor3D; typedef RangeFactorWithTransform RangeFactorWithTransform2D; typedef RangeFactorWithTransform RangeFactorWithTransform3D; -Key poseKey(1); -Key pointKey(2); +Key poseKey(2); +Key pointKey(1); double measurement(10.0); /* ************************************************************************* */