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); rotations.emplace_back(aRb);
abPointPairs.emplace_back(aTi.translation(), bTi.translation()); 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); return internal::alignGivenR(abPointPairs, aRb_estimate);
} }

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

@ -27,10 +27,10 @@ class TestSim2(GtsamTestCase):
Scenario: Scenario:
3 object poses 3 object poses
same scale (no gauge ambiguity) 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 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) # 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 # 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] eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses # Create destination poses
# (same three objects, but instead living in the world/city "w" frame) # (same three objects, but instead living in the world "w" frame)
wTo0 = Pose2(Rx90, np.array([-10, 0])) wTo0 = Pose2(R180, np.array([-10, 0]))
wTo1 = Pose2(Rx90, np.array([-5, 0])) wTo1 = Pose2(R180, np.array([-5, 0]))
wTo2 = Pose2(Rx90, np.array([0, 0])) wTo2 = Pose2(R180, np.array([0, 0]))
wToi_list = [wTo0, wTo1, wTo2] wToi_list = [wTo0, wTo1, wTo2]
@ -62,10 +62,10 @@ class TestSim2(GtsamTestCase):
Scenario: Scenario:
3 object poses 3 object poses
with gauge ambiguity (2x scale) 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 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) # 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 # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
@ -77,9 +77,9 @@ class TestSim2(GtsamTestCase):
# Create destination poses # Create destination poses
# (same three objects, but instead living in the world/city "w" frame) # (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose2(Rz90, np.array([0, 12])) wTo0 = Pose2(R90, np.array([0, 12]))
wTo1 = Pose2(Rz90, np.array([0, 14])) wTo1 = Pose2(R90, np.array([0, 14]))
wTo2 = Pose2(Rz90, np.array([0, 18])) wTo2 = Pose2(R90, np.array([0, 18]))
wToi_list = [wTo0, wTo1, wTo2] wToi_list = [wTo0, wTo1, wTo2]