From 4b29c0370dacab9fc8cafe12e586a0f7d8715b5f Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Sun, 21 Mar 2021 20:46:26 -0400 Subject: [PATCH] Prefer localCoordinates over logmap --- gtsam_unstable/slam/PartialPriorFactor.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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));