diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index da6e0d535..bc28a6830 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -111,15 +111,14 @@ namespace gtsam { Vector evaluateError(const T& p, boost::optional H = boost::none) const override { if (H) { Matrix H_logmap; - T::Logmap(p, H_logmap); + p.localCoordinates(T::identity(), H_logmap); (*H) = Matrix::Zero(indices_.size(), T::dimension); for (size_t i = 0; i < indices_.size(); ++i) { (*H).row(i) = H_logmap.row(indices_.at(i)); } } - // FIXME: this was originally the generic retraction - may not produce same results. - // Compute the tangent vector representation of T. - const Vector& full_logmap = T::Logmap(p); + // Compute the tangent vector representation of T and select relevant parameters. + const Vector& full_logmap = p.localCoordinates(T::identity()); Vector partial_logmap = Vector::Zero(T::dimension); for (size_t i = 0; i < indices_.size(); ++i) { partial_logmap(i) = full_logmap(indices_.at(i));