diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index f84df016e..d7ea4cb9c 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -399,12 +399,12 @@ public: typedef boost::shared_ptr > shared_ptr; NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0) - : Base(key1, X::dimension, mu), value_(value) {} + : Base(key1, X::Dim(), mu), value_(value) {} virtual ~NonlinearEquality1() {} /** g(x) with optional derivative */ Vector evaluateError(const X& x1, boost::optional H1 = boost::none) const { - const size_t p = X::dimension; + const size_t p = X::Dim(); if (H1) *H1 = eye(p); return logmap(value_, x1); } @@ -425,14 +425,14 @@ public: typedef boost::shared_ptr > shared_ptr; NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0) - : Base(key1, key2, X::dimension, mu) {} + : Base(key1, key2, X::Dim(), mu) {} virtual ~NonlinearEquality2() {} /** g(x) with optional derivative2 */ Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - const size_t p = X::dimension; + const size_t p = X::Dim(); if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); return logmap(x1, x2); diff --git a/slam/BetweenConstraint.h b/slam/BetweenConstraint.h index 9fd9dcee1..4b02bf99a 100644 --- a/slam/BetweenConstraint.h +++ b/slam/BetweenConstraint.h @@ -26,7 +26,7 @@ namespace gtsam { typedef boost::shared_ptr > shared_ptr; BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0) - : Base(key1, key2, X::dimension, mu), measured_(measured) {} + : Base(key1, key2, X::Dim(), mu), measured_(measured) {} /** g(x) with optional derivative2 */ Vector evaluateError(const X& x1, const X& x2,