diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index ea6ed2563..c426fef09 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -179,12 +179,13 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { Pose3 wTa, wTb; for (const Pose3Pair &abPair : abPosePairs) { std::tie(wTa, wTb) = abPair; - rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); + Rot3 aRb = wTa.rotation().between(wTb.rotation()); + rotations.emplace_back(aRb); abPointPairs.emplace_back(wTa.translation(), wTb.translation()); } - const Rot3 aRb = FindKarcherMean(rotations); + const Rot3 aRb_estimate = FindKarcherMean(rotations); - return alignGivenR(abPointPairs, aRb); + return alignGivenR(abPointPairs, aRb_estimate); } Matrix4 Similarity3::wedge(const Vector7 &xi) {