diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index b44914a94..fc9c7acf9 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/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. - """ - pass + Two point clouds represent horseshoe-shapes of the same size, just rotated and translated: + | 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__":