12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update test notation to have just 1 world frame, and fix typo in abPointPairs

release/4.3a0
John Lambert 2021-03-05 14:26:37 -05:00
parent 063a41a3f8
commit eaf457e625
2 changed files with 42 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Similarity3.cpp
View File

@ -182,7 +182,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
std::tie(aTi, bTi) = abPair; std::tie(aTi, bTi) = abPair;
Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse()); Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse());
rotations.emplace_back(aRb); rotations.emplace_back(aRb);
abPointPairs.emplace_back(wTa.translation(), wTb.translation()); abPointPairs.emplace_back(aTi.translation(), bTi.translation());
} }
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations); const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);

79
python/gtsam/tests/test_Sim3.py
View File

@ -26,69 +26,72 @@ class TestSim3(GtsamTestCase):
"""Test Align Pose3Pairs method. """Test Align Pose3Pairs method.
Scenario: Scenario:
3 camera frames 3 object poses
same scale (no gauge ambiguity) same scale (no gauge ambiguity)
world frame 2 (w2) has poses rotated about x-axis (90 degree roll) world frame has poses rotated about x-axis (90 degree roll)
world frames translated by 15 meters world and egovehicle frame translated by 15 meters w.r.t. each other
""" """
Rx90 = Rot3.Rx(np.deg2rad(90)) Rx90 = Rot3.Rx(np.deg2rad(90))
w1Ti0 = Pose3(Rot3(), np.array([5, 0, 0])) # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
w1Ti1 = Pose3(Rot3(), np.array([10, 0, 0])) # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
w1Ti2 = Pose3(Rot3(), np.array([15, 0, 0])) 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])) # Create destination poses
w2Ti1 = Pose3(Rx90, np.array([-5, 0, 0])) # (same three objects, but instead living in the world/city "w" frame)
w2Ti2 = Pose3(Rx90, np.array([0, 0, 0])) 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)) wTe_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list)))
pose_pairs = Pose3Pairs(pairs)
w2Sw1 = Similarity3.Align(pose_pairs) # Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(wTe_pairs)
for w1Ti, w2Ti in zip(w1Ti_list, w2Ti_list):
self.gtsamAssertEquals(w2Ti, w2Sw1.transformFrom(w1Ti))
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_along_straight_line_gauge(self): def test_align_poses_along_straight_line_gauge(self):
"""Test Align Pose3Pairs method. """Test if Align Pose3Pairs method can account for gauge ambiguity.
Scenario: Scenario:
3 camera frames 3 object poses
with gauge ambiguity (2x scale) with gauge ambiguity (2x scale)
world frame 2 (w2) has poses rotated about z-axis (90 degree yaw) world frame has poses rotated about z-axis (90 degree yaw)
world frames translated by 10 meters world and egovehicle frame translated by 11 meters w.r.t. each other
""" """
Rz90 = Rot3.Rz(np.deg2rad(90)) Rz90 = Rot3.Rz(np.deg2rad(90))
w1Ti0 = Pose3(Rot3(), np.array([1, 0, 0])) # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
w1Ti1 = Pose3(Rot3(), np.array([2, 0, 0])) # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
w1Ti2 = Pose3(Rot3(), np.array([4, 0, 0])) 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])) # Create destination poses
w2Ti1 = Pose3(Rz90, np.array([14, 0, 0])) # (same three objects, but instead living in the world/city "w" frame)
w2Ti2 = Pose3(Rz90, np.array([18, 0, 0])) 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)) wTe_pairs = Pose3Pairs(list(zip(wToi_list, eToi_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))
# 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__": if __name__ == "__main__":