diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index ded917933..c76b8eb32 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -180,10 +180,11 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { Point3Pairs abPointPairs; rotations.reserve(n); abPointPairs.reserve(n); + // note that frame "i" is the i'th object/camera/etc body frame Pose3 aTi, bTi; for (const Pose3Pair &abPair : abPosePairs) { std::tie(aTi, bTi) = abPair; - Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse()); + const Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse()); rotations.emplace_back(aRb); abPointPairs.emplace_back(aTi.translation(), bTi.translation()); }