From 1f12f82e01506b4596c06b97292d405707aa5a66 Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 26 Mar 2021 17:29:47 -0400 Subject: [PATCH] Fix Rot3::LocalCoordinates runtime error when using Cayley map --- gtsam_unstable/slam/PartialPriorFactor.h | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 910ee7bbd..52a57b945 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -107,16 +107,23 @@ namespace gtsam { /** Returns a vector of errors for the measured tangent parameters. */ Vector evaluateError(const T& p, boost::optional H = boost::none) const override { + Eigen::Matrix H_local; + + // If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime error + // when asked to compute the Jacobian matrix (see Rot3M.cpp). + #ifdef GTSAM_ROT3_EXPMAP + const Vector full_tangent = T::LocalCoordinates(p, H ? &H_local : nullptr); + #else + const Vector full_tangent = T::Logmap(p, H ? &H_local : nullptr); + #endif + if (H) { - Matrix H_local; - T::LocalCoordinates(p, H_local); (*H) = Matrix::Zero(indices_.size(), T::dimension); for (size_t i = 0; i < indices_.size(); ++i) { (*H).row(i) = H_local.row(indices_.at(i)); } } - // Compute the tangent vector representation of T and select relevant parameters. - const Vector& full_tangent = T::LocalCoordinates(p); + // Select relevant parameters from the tangent vector. Vector partial_tangent = Vector::Zero(indices_.size()); for (size_t i = 0; i < indices_.size(); ++i) { partial_tangent(i) = full_tangent(indices_.at(i));