diff --git a/gtsam_unstable/slam/PoseRotationPrior.h b/gtsam_unstable/slam/PoseRotationPrior.h index 60a29b3e4..30d3828fd 100644 --- a/gtsam_unstable/slam/PoseRotationPrior.h +++ b/gtsam_unstable/slam/PoseRotationPrior.h @@ -67,9 +67,9 @@ public: if (H) { *H = gtsam::zeros(rDim, xDim); if (pose_traits::isRotFirst()) - (*H).leftCols(rDim) = eye(rDim); + (*H).leftCols(rDim).setIdentity(rDim, rDim); // = eye(rDim); // FIXME: avoiding use of assign with eye() else - (*H).rightCols(rDim) = eye(rDim); + (*H).rightCols(rDim).setIdentity(rDim, rDim); // = eye(rDim); } return Rotation::Logmap(newR) - Rotation::Logmap(measured_);