diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py new file mode 100644 index 000000000..67bc770d2 --- /dev/null +++ b/python/gtsam/tests/test_Sim2.py @@ -0,0 +1,196 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Sim3 unit tests. +Author: John Lambert +""" +# pylint: disable=no-name-in-module +import unittest + +import numpy as np + +import gtsam +from gtsam import Point2, Pose2, Pose2Pairs, Rot2, Similarity2 +from gtsam.utils.test_case import GtsamTestCase + + +class TestSim2(GtsamTestCase): + """Test selected Sim2 methods.""" + + def test_align_poses_along_straight_line(self) -> None: + """Test Align Pose2Pairs method. + + Scenario: + 3 object poses + same scale (no gauge ambiguity) + world frame has poses rotated about x-axis (90 degree roll) + world and egovehicle frame translated by 15 meters w.r.t. each other + """ + Rx90 = Rot2.fromDegrees(90) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose2(Rot2(), np.array([5, 0])) + eTo1 = Pose2(Rot2(), np.array([10, 0])) + eTo2 = Pose2(Rot2(), np.array([15, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose2(Rx90, np.array([-10, 0])) + wTo1 = Pose2(Rx90, np.array([-5, 0])) + wTo2 = Pose2(Rx90, np.array([0, 0])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity2.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses_along_straight_line_gauge(self): + """Test if Align Pose3Pairs method can account for gauge ambiguity. + + Scenario: + 3 object poses + with gauge ambiguity (2x scale) + world frame has poses rotated about z-axis (90 degree yaw) + world and egovehicle frame translated by 11 meters w.r.t. each other + """ + Rz90 = Rot3.Rz(np.deg2rad(90)) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose2(Rot2(), np.array([1, 0])) + eTo1 = Pose2(Rot2(), np.array([2, 0])) + eTo2 = Pose2(Rot2(), np.array([4, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose2(Rz90, np.array([0, 12])) + wTo1 = Pose2(Rz90, np.array([0, 14])) + wTo2 = Pose2(Rz90, np.array([0, 18])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity2.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses_scaled_squares(self): + """Test if Align Pose2Pairs 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 = Rot2.fromDegrees(0) + R90 = Rot2.fromDegrees(90) + R180 = Rot2.fromDegrees(180) + R270 = Rot2.fromDegrees(270) + + aTi0 = Pose3(R0, np.array([2, 3])) + aTi1 = Pose3(R90, np.array([12, 3])) + aTi2 = Pose3(R180, np.array([12, 13])) + aTi3 = Pose3(R270, np.array([2, 13])) + + aTi_list = [aTi0, aTi1, aTi2, aTi3] + + bTi0 = Pose2(R0, np.array([4, 3])) + bTi1 = Pose2(R90, np.array([8, 3])) + bTi2 = Pose2(R180, np.array([8, 7])) + bTi3 = Pose2(R270, np.array([4, 7])) + + bTi_list = [bTi0, bTi1, bTi2, bTi3] + + ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + aSb = Similarity2.Align(ab_pairs) + + for aTi, bTi in zip(aTi_list, bTi_list): + self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi)) + + def test_constructor(self) -> None: + """Sim(2) to perform p_b = bSa * p_a""" + bRa = Rot2() + bta = np.array([1, 2]) + bsa = 3.0 + bSa = Similarity2(R=bRa, t=bta, s=bsa) + assert isinstance(bSa, Similarity2) + assert np.allclose(bSa.rotation().matrix(), bRa.matrix()) + assert np.allclose(bSa.translation(), bta) + assert np.allclose(bSa.scale(), bsa) + + def test_is_eq(self) -> None: + """Ensure object equality works properly (are equal).""" + bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0) + bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3) + assert bSa == bSa_ + + def test_not_eq_translation(self) -> None: + """Ensure object equality works properly (not equal translation).""" + bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0) + bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3) + assert bSa != bSa_ + + def test_not_eq_rotation(self) -> None: + """Ensure object equality works properly (not equal rotation).""" + bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0) + bSa_ = Similarity2(R=Rot2.fromDegrees(180), t=np.array([2.0, 1.0]), s=3) + assert bSa != bSa_ + + def test_not_eq_scale(self) -> None: + """Ensure object equality works properly (not equal scale).""" + bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0) + bSa_ = Similarity2(R=Rot2(), t=np.array([2.0, 1.0]), s=1.0) + assert bSa != bSa_ + + def test_rotation(self) -> None: + """Ensure rotation component is returned properly.""" + R = Rot2.fromDegrees(90) + t = np.array([1, 2]) + bSa = Similarity2(R=R, t=t, s=3.0) + + # evaluates to [[0, -1], [1, 0]] + expected_R = Rot2.fromDegrees(90) + assert np.allclose(expected_R.matrix(), bSa.rotation().matrix()) + + def test_translation(self) -> None: + """Ensure translation component is returned properly.""" + R = Rot2.fromDegrees(90) + t = np.array([1, 2]) + bSa = Similarity2(R=R, t=t, s=3.0) + + expected_t = np.array([1, 2]) + assert np.allclose(expected_t, bSa.translation()) + + def test_scale(self) -> None: + """Ensure the scale factor is returned properly.""" + bRa = Rot2() + bta = np.array([1, 2]) + bsa = 3.0 + bSa = Similarity2(R=bRa, t=bta, s=bsa) + assert bSa.scale() == 3.0 + + +if __name__ == "__main__": + unittest.main()