diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 7834f235f..7d31e2e2c 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -77,7 +77,7 @@ public: (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); } - return Rotation::Logmap(newR) - Rotation::Logmap(measured_); + return measured_.localCoordinates(newR); } private: