From 3121604f6364a9181054c153a919c1eca89da707 Mon Sep 17 00:00:00 2001
From: John Lambert <johnwl@stanford.edu>
Date: Wed, 3 Mar 2021 11:25:55 -0500
Subject: [PATCH] fix bugs in Karcher mean

---
 gtsam/geometry/Similarity3.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

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<Pose3Pair> &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<Rot3>(rotations);
+  const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
 
-  return alignGivenR(abPointPairs, aRb);
+  return alignGivenR(abPointPairs, aRb_estimate);
 }
 
 Matrix4 Similarity3::wedge(const Vector7 &xi) {