diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 7f7aef8cb..8fbbd4d88 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -81,9 +81,9 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = eye(traits::dimension()); - // manifold equivalent of h(x)-z -> log(z,h(x)) DefaultChart chart; + if (H) (*H) = eye(chart.getDimension(p)); + // manifold equivalent of h(x)-z -> log(z,h(x)) return chart.local(prior_,p); }