diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp index 12529f52d..6e5da635b 100644 --- a/gtsam/geometry/Similarity2.cpp +++ b/gtsam/geometry/Similarity2.cpp @@ -193,7 +193,7 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) { rotations.emplace_back(aRb); abPointPairs.emplace_back(aTi.translation(), bTi.translation()); } - const Rot2 aRb_estimate; // = FindKarcherMean(rotations); + const Rot2 aRb_estimate = FindKarcherMean(rotations); return internal::alignGivenR(abPointPairs, aRb_estimate); } diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py index 1d09dcc3b..3e39ac45c 100644 --- a/python/gtsam/tests/test_Sim2.py +++ b/python/gtsam/tests/test_Sim2.py @@ -27,10 +27,10 @@ class TestSim2(GtsamTestCase): Scenario: 3 object poses same scale (no gauge ambiguity) - world frame has poses rotated about x-axis (90 degree roll) + world frame has poses rotated about 180 degrees. world and egovehicle frame translated by 15 meters w.r.t. each other """ - Rx90 = Rot2.fromDegrees(90) + R180 = Rot2.fromDegrees(180) # 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 @@ -41,10 +41,10 @@ class TestSim2(GtsamTestCase): 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])) + # (same three objects, but instead living in the world "w" frame) + wTo0 = Pose2(R180, np.array([-10, 0])) + wTo1 = Pose2(R180, np.array([-5, 0])) + wTo2 = Pose2(R180, np.array([0, 0])) wToi_list = [wTo0, wTo1, wTo2] @@ -62,10 +62,10 @@ class TestSim2(GtsamTestCase): Scenario: 3 object poses with gauge ambiguity (2x scale) - world frame has poses rotated about z-axis (90 degree yaw) + world frame has poses rotated by 90 degrees. world and egovehicle frame translated by 11 meters w.r.t. each other """ - Rz90 = Rot2.Rz(np.deg2rad(90)) + R90 = 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 @@ -77,9 +77,9 @@ class TestSim2(GtsamTestCase): # 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])) + wTo0 = Pose2(R90, np.array([0, 12])) + wTo1 = Pose2(R90, np.array([0, 14])) + wTo2 = Pose2(R90, np.array([0, 18])) wToi_list = [wTo0, wTo1, wTo2]