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

Fix tests

release/4.3a0
Varun Agrawal 2022-02-06 12:27:50 -05:00
parent cc8d80fb7e
commit 37ae7038fa
2 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Similarity2.cpp
View File

@ -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<Rot2>(rotations);
const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
return internal::alignGivenR(abPointPairs, aRb_estimate);
}

22
python/gtsam/tests/test_Sim2.py
View File

@ -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]