From eaf457e625fcac32a9182d2f1f7dc33897c6e1d1 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 5 Mar 2021 14:26:37 -0500 Subject: [PATCH] update test notation to have just 1 world frame, and fix typo in abPointPairs --- gtsam/geometry/Similarity3.cpp | 2 +- python/gtsam/tests/test_Sim3.py | 79 +++++++++++++++++---------------- 2 files changed, 42 insertions(+), 39 deletions(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index b69cfa991..10cfbfc2b 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -182,7 +182,7 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { std::tie(aTi, bTi) = abPair; Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse()); rotations.emplace_back(aRb); - abPointPairs.emplace_back(wTa.translation(), wTb.translation()); + abPointPairs.emplace_back(aTi.translation(), bTi.translation()); } const Rot3 aRb_estimate = FindKarcherMean(rotations); diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index 81b190e5a..2b9ad4df8 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -26,69 +26,72 @@ class TestSim3(GtsamTestCase): """Test Align Pose3Pairs method. Scenario: - 3 camera frames + 3 object poses same scale (no gauge ambiguity) - world frame 2 (w2) has poses rotated about x-axis (90 degree roll) - world frames translated by 15 meters + 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 = Rot3.Rx(np.deg2rad(90)) - w1Ti0 = Pose3(Rot3(), np.array([5, 0, 0])) - w1Ti1 = Pose3(Rot3(), np.array([10, 0, 0])) - w1Ti2 = Pose3(Rot3(), np.array([15, 0, 0])) + # 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 = Pose3(Rot3(), np.array([5, 0, 0])) + eTo1 = Pose3(Rot3(), np.array([10, 0, 0])) + eTo2 = Pose3(Rot3(), np.array([15, 0, 0])) - w1Ti_list = [w1Ti0, w1Ti1, w1Ti2] + eToi_list = [eTo0, eTo1, eTo2] - w2Ti0 = Pose3(Rx90, np.array([-10, 0, 0])) - w2Ti1 = Pose3(Rx90, np.array([-5, 0, 0])) - w2Ti2 = Pose3(Rx90, np.array([0, 0, 0])) + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose3(Rx90, np.array([-10, 0, 0])) + wTo1 = Pose3(Rx90, np.array([-5, 0, 0])) + wTo2 = Pose3(Rx90, np.array([0, 0, 0])) - w2Ti_list = [w2Ti0, w2Ti1, w2Ti2] + wToi_list = [wTo0, wTo1, wTo2] - pairs = list(zip(w2Ti_list, w1Ti_list)) - pose_pairs = Pose3Pairs(pairs) + wTe_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) - w2Sw1 = Similarity3.Align(pose_pairs) - - for w1Ti, w2Ti in zip(w1Ti_list, w2Ti_list): - self.gtsamAssertEquals(w2Ti, w2Sw1.transformFrom(w1Ti)) + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity3.Align(wTe_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 Align Pose3Pairs method. + """Test if Align Pose3Pairs method can account for gauge ambiguity. Scenario: - 3 camera frames + 3 object poses with gauge ambiguity (2x scale) - world frame 2 (w2) has poses rotated about z-axis (90 degree yaw) - world frames translated by 10 meters + 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)) - w1Ti0 = Pose3(Rot3(), np.array([1, 0, 0])) - w1Ti1 = Pose3(Rot3(), np.array([2, 0, 0])) - w1Ti2 = Pose3(Rot3(), np.array([4, 0, 0])) + # 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 = Pose3(Rot3(), np.array([1, 0, 0])) + eTo1 = Pose3(Rot3(), np.array([2, 0, 0])) + eTo2 = Pose3(Rot3(), np.array([4, 0, 0])) - w1Ti_list = [w1Ti0, w1Ti1, w1Ti2] + eToi_list = [eTo0, eTo1, eTo2] - w2Ti0 = Pose3(Rz90, np.array([12, 0, 0])) - w2Ti1 = Pose3(Rz90, np.array([14, 0, 0])) - w2Ti2 = Pose3(Rz90, np.array([18, 0, 0])) + # 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])) - w2Ti_list = [w2Ti0, w2Ti1, w2Ti2] + wToi_list = [wTo0, wTo1, wTo2] - pairs = list(zip(w2Ti_list, w1Ti_list)) - pose_pairs = Pose3Pairs(pairs) - - w2Sw1 = Similarity3.Align(pose_pairs) - - for w1Ti, w2Ti in zip(w1Ti_list, w2Ti_list): - self.gtsamAssertEquals(w2Ti, w2Sw1.transformFrom(w1Ti)) + wTe_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity3.Align(wTe_pairs) + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) if __name__ == "__main__":