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;
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<Rot3>(rotations);

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

@ -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__":