Fix tests
parent
cc8d80fb7e
commit
37ae7038fa
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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]
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue