add python unit test on Pose2.align()

python/gtsam/tests/test_Pose2.py

@@ -30,9 +30,41 @@ class TestPose2(GtsamTestCase):
     def test_align(self) -> None:
         """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds.
         
-        """
+        Two point clouds represent horseshoe-shapes of the same size, just rotated and translated:
-        pass
         
+                |  X---X
+                |  |
+                |  X---X
+        ------------------
+                |
+                |
+              O | O
+              | | |
+              O---O
+        """
+        # fmt: off
+        pts_a = [
+            Point2(3, 1),
+            Point2(1, 1),
+            Point2(1, 3),
+            Point2(3, 3),
+        ]
+        pts_b = [
+            Point2(1, -3),
+            Point2(1, -5),
+            Point2(-1, -5),
+            Point2(-1, -3)
+        ]
+
+        # fmt: on
+        ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
+        bTa = Pose2.align(ab_pairs)
+        aTb = bTa.inverse()
+        assert aTb is not None
+
+        for pt_a, pt_b in zip(pts_a, pts_b):
+            pt_a_ = aTb.transformFrom(pt_b)
+            assert np.allclose(pt_a, pt_a_)
         
 
 if __name__ == "__main__":