diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index e8ec9181c..878bbd44c 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -206,12 +206,11 @@ Values InitializePose3::computeOrientationsGradient( // Return correct rotations const Rot3& Rref = inverseRot.at(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for (const auto key_R : inverseRot) { + for (const auto& key_R : inverseRot) { const Key& key = key_R.first; - const Rot3& Ri = key_R.second; if (key != initialize::kAnchorKey) { const Rot3& R = inverseRot.at(key); - if(setRefFrame) + if (setRefFrame) estimateRot.insert(key, Rref.compose(R.inverse())); else estimateRot.insert(key, R.inverse());