From 5da50a5a6f8e2f70761a2a0a08dbf3ff24067b10 Mon Sep 17 00:00:00 2001
From: John Lambert <johnwl@stanford.edu>
Date: Wed, 10 Mar 2021 08:53:43 -0500
Subject: [PATCH] improve docstring

---
 gtsam/geometry/Similarity3.cpp  |  5 +++-
 python/gtsam/tests/test_Sim3.py | 46 ++++++++++++++++++++++++++++++---
 2 files changed, 46 insertions(+), 5 deletions(-)

diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
index 10cfbfc2b..ded917933 100644
--- a/gtsam/geometry/Similarity3.cpp
+++ b/gtsam/geometry/Similarity3.cpp
@@ -63,10 +63,13 @@ static Matrix3 calculateH(const Point3Pairs &d_abPointPairs) {
   return H;
 }
 
-/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
+/// This method estimates the similarity transform from differences point pairs,
+// given a known or estimated rotation and point centroids.
 static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
                          const Point3Pair &centroids) {
   const double s = calculateScale(d_abPointPairs, aRb);
+  // dividing aTb by s is required because the registration cost function
+  // minimizes ||a - sRb - t||, whereas Sim(3) computes s(Rb + t)
   const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
   return Similarity3(aRb, aTb, s);
 }
diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py
index c74a009ec..ff478306e 100644
--- a/python/gtsam/tests/test_Sim3.py
+++ b/python/gtsam/tests/test_Sim3.py
@@ -9,7 +9,6 @@ Sim3 unit tests.
 Author: John Lambert
 """
 # pylint: disable=no-name-in-module
-import math
 import unittest
 
 import numpy as np
@@ -79,9 +78,9 @@ class TestSim3(GtsamTestCase):
 
         # Create destination poses
         # (same three objects, but instead living in the world/city "w" frame)
-        wTo0 = Pose3(Rz90, np.array([12, 0, 0]))
-        wTo1 = Pose3(Rz90, np.array([14, 0, 0]))
-        wTo2 = Pose3(Rz90, np.array([18, 0, 0]))
+        wTo0 = Pose3(Rz90, np.array([0, 12, 0]))
+        wTo1 = Pose3(Rz90, np.array([0, 14, 0]))
+        wTo2 = Pose3(Rz90, np.array([0, 18, 0]))
 
         wToi_list = [wTo0, wTo1, wTo2]
 
@@ -94,5 +93,44 @@ class TestSim3(GtsamTestCase):
             self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
 
 
+    def test_align_poses_scaled_squares(self):
+        """Test if Align Pose3Pairs method can account for gauge ambiguity.
+
+        Make sure a big and small square can be aligned.
+        The u's represent a big square (10x10), and v's represents a small square (4x4).
+
+        Scenario:
+           4 object poses
+           with gauge ambiguity (2.5x scale)
+        """
+        # 0, 90, 180, and 270 degrees yaw
+        R0 = Rot3.Rz(np.deg2rad(0))
+        R90 = Rot3.Rz(np.deg2rad(90))
+        R180 = Rot3.Rz(np.deg2rad(180))
+        R270 = Rot3.Rz(np.deg2rad(270))
+
+        aTi0 = Pose3(R0, np.array([2, 3, 0]))
+        aTi1 = Pose3(R90, np.array([12, 3, 0]))
+        aTi2 = Pose3(R180, np.array([12, 13, 0]))
+        aTi3 = Pose3(R270, np.array([2, 13, 0]))
+
+        aTi_list = [aTi0, aTi1, aTi2, aTi3]
+
+        bTi0 = Pose3(R0, np.array([4, 3, 0]))
+        bTi1 = Pose3(R90, np.array([8, 3, 0]))
+        bTi2 = Pose3(R180, np.array([8, 7, 0]))
+        bTi3 = Pose3(R270, np.array([4, 7, 0]))
+
+        bTi_list = [bTi0, bTi1, bTi2, bTi3]
+
+        ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        aSb = Similarity3.Align(ab_pairs)
+
+        for aTi, bTi in zip(aTi_list, bTi_list):
+            self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
+
+
 if __name__ == "__main__":
     unittest.main()