def test_align_poses_along_straight_line_gauge(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
Scenario:
3 object poses
with gauge ambiguity (2x scale)
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))
# 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]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose3(Rz90, np.array([0, 12, 0]))
wTo1 = Pose3(Rz90, np.array([0, 14, 0]))
wTo2 = Pose3(Rz90, np.array([0, 18, 0]))
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def align_poses_sim3(aTi_list: List[Pose3],
bTi_list: List[Pose3]) -> Similarity3:
"""Align two pose graphs via similarity transformation. Note: poses cannot be missing/invalid.
We force SIM(3) alignment rather than SE(3) alignment.
We assume the two trajectories are of the exact same length.
Args:
aTi_list: reference poses in frame "a" which are the targets for alignment
bTi_list: input poses which need to be aligned to frame "a"
Returns:
aSb: Similarity(3) object that aligns the two pose graphs.
"""
n_to_align = len(aTi_list)
assert len(aTi_list) == len(bTi_list)
assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames"
ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list)))
aSb = Similarity3.Align(ab_pairs)
if np.isnan(aSb.scale()) or aSb.scale() == 0:
# we have run into a case where points have no translation between them (i.e. panorama).
# We will first align the rotations and then align the translation by using centroids.
# TODO: handle it in GTSAM
# align the rotations first, so that we can find the translation between the two panoramas
aSb = Similarity3(aSb.rotation(), np.zeros((3, )), 1.0)
aTi_list_rot_aligned = [aSb.transformFrom(bTi) for bTi in bTi_list]
# fit a single translation motion to the centroid
aTi_centroid = np.array([aTi.translation()
for aTi in aTi_list]).mean(axis=0)
aTi_rot_aligned_centroid = np.array(
[aTi.translation() for aTi in aTi_list_rot_aligned]).mean(axis=0)
# construct the final SIM3 transform
aSb = Similarity3(aSb.rotation(),
aTi_centroid - aTi_rot_aligned_centroid, 1.0)
return aSb
def test_align_poses_scaled_squares(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
Make sure a big and small square can be aligned.
The u's represent a big square (10x10), and v's represents a small square (4x4).
Scenario:
4 object poses
with gauge ambiguity (2.5x scale)
"""
# 0, 90, 180, and 270 degrees yaw
R0 = Rot3.Rz(np.deg2rad(0))
R90 = Rot3.Rz(np.deg2rad(90))
R180 = Rot3.Rz(np.deg2rad(180))
R270 = Rot3.Rz(np.deg2rad(270))
aTi0 = Pose3(R0, np.array([2, 3, 0]))
aTi1 = Pose3(R90, np.array([12, 3, 0]))
aTi2 = Pose3(R180, np.array([12, 13, 0]))
aTi3 = Pose3(R270, np.array([2, 13, 0]))
aTi_list = [aTi0, aTi1, aTi2, aTi3]
bTi0 = Pose3(R0, np.array([4, 3, 0]))
bTi1 = Pose3(R90, np.array([8, 3, 0]))
bTi2 = Pose3(R180, np.array([8, 7, 0]))
bTi3 = Pose3(R270, np.array([4, 7, 0]))
bTi_list = [bTi0, bTi1, bTi2, bTi3]
ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity3.Align(ab_pairs)
for aTi, bTi in zip(aTi_list, bTi_list):
self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
def align_poses_sim3(aTi_list: List[Pose3],
bTi_list: List[Pose3]) -> Tuple[List[Pose3], Similarity3]:
"""Align two pose graphs via similarity transformation. Note: poses cannot be missing/invalid.
We force SIM(3) alignment rather than SE(3) alignment.
We assume the two trajectories are of the exact same length.
Args:
aTi_list: reference poses in frame "a" which are the targets for alignment
bTi_list: input poses which need to be aligned to frame "a"
Returns:
aTi_list_: transformed input poses previously "bTi_list" but now which
have the same origin and scale as reference (now living in "a" frame)
aSb: Similarity(3) object that aligns the two pose graphs.
"""
assert len(aTi_list) == len(bTi_list)
valid_pose_tuples = [
pose_tuple for pose_tuple in list(zip(aTi_list, bTi_list))
if pose_tuple[0] is not None and pose_tuple[1] is not None
]
n_to_align = len(valid_pose_tuples)
if n_to_align < 2:
logger.error("SIM(3) alignment uses at least 2 frames; Skipping")
return bTi_list, Similarity3(Rot3(), np.zeros((3, )), 1.0)
ab_pairs = Pose3Pairs(valid_pose_tuples)
aSb = Similarity3.Align(ab_pairs)
if np.isnan(aSb.scale()) or aSb.scale() == 0:
# we have run into a case where points have no translation between them (i.e. panorama).
# We will first align the rotations and then align the translation by using centroids.
# TODO: handle it in GTSAM
# align the rotations first, so that we can find the translation between the two panoramas
aSb = Similarity3(aSb.rotation(), np.zeros((3, )), 1.0)
aTi_list_rot_aligned = [
aSb.transformFrom(bTi) for _, bTi in valid_pose_tuples
]
# fit a single translation motion to the centroid
aTi_centroid = np.array(
[aTi.translation() for aTi, _ in valid_pose_tuples]).mean(axis=0)
aTi_rot_aligned_centroid = np.array(
[aTi.translation() for aTi in aTi_list_rot_aligned]).mean(axis=0)
# construct the final SIM3 transform
aSb = Similarity3(aSb.rotation(),
aTi_centroid - aTi_rot_aligned_centroid, 1.0)
# TODO(johnwlambert): fix bug in GTSAM, where scale can flip to a small negative number
# a negative scale destroys cheirality when applied.
# See GTSAM issue here: https://github.com/borglab/gtsam/issues/995
aSb = Similarity3(R=aSb.rotation(),
t=aSb.translation(),
s=np.absolute(aSb.scale()))
# provide a summary of the estimated alignment transform
aRb = aSb.rotation().matrix()
atb = aSb.translation()
rz, ry, rx = Rotation.from_matrix(aRb).as_euler("zyx", degrees=True)
logger.info(
"Sim(3) Rotation `aRb`: rz=%.2f deg., ry=%.2f deg., rx=%.2f deg.", rz,
ry, rx)
logger.info(f"Sim(3) Translation `atb`: [tx,ty,tz]={str(np.round(atb,2))}")
logger.info("Sim(3) Scale `asb`: %.2f", float(aSb.scale()))
aTi_list_ = []
for bTi in bTi_list:
if bTi is None:
aTi_list_.append(None)
else:
aTi_list_.append(aSb.transformFrom(bTi))
logger.info("Pose graph Sim(3) alignment complete.")
return aTi_list_, aSb
def align_poses_sim3(aTi_list: List[Pose3],
bTi_list: List[Pose3]) -> List[Pose3]:
"""Align by similarity transformation.
We force SIM(3) alignment rather than SE(3) alignment.
We assume the two trajectories are of the exact same length.
Args:
aTi_list: reference poses in frame "a" which are the targets for alignment
bTi_list: input poses which need to be aligned to frame "a"
Returns:
aTi_list_: transformed input poses previously "bTi_list" but now which
have the same origin and scale as reference (now living in "a" frame)
"""
n_to_align = len(aTi_list)
assert len(aTi_list) == len(bTi_list)
assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames"
ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list)))
aSb = Similarity3.Align(ab_pairs)
if np.isnan(aSb.scale()) or aSb.scale() == 0:
# we have run into a case where points have no translation between them (i.e. panorama).
# We will first align the rotations and then align the translation by using centroids.
# TODO: handle it in GTSAM
# align the rotations first, so that we can find the translation between the two panoramas
aSb = Similarity3(aSb.rotation(), np.zeros((3, )), 1.0)
aTi_list_rot_aligned = [aSb.transformFrom(bTi) for bTi in bTi_list]
# fit a single translation motion to the centroid
aTi_centroid = np.array([aTi.translation()
for aTi in aTi_list]).mean(axis=0)
aTi_rot_aligned_centroid = np.array(
[aTi.translation() for aTi in aTi_list_rot_aligned]).mean(axis=0)
# construct the final SIM3 transform
aSb = Similarity3(aSb.rotation(),
aTi_centroid - aTi_rot_aligned_centroid, 1.0)
# provide a summary of the estimated alignment transform
aRb = aSb.rotation().matrix()
atb = aSb.translation()
rz, ry, rx = Rotation.from_matrix(aRb).as_euler("zyx", degrees=True)
logger.info(
f"Sim(3) Rotation `aRb`: rz={rz:.2f} deg., ry={ry:.2f} deg., rx={rx:.2f} deg.",
)
logger.info(f"Sim(3) Translation `atb`: [tx,ty,tz]={str(np.round(atb,2))}")
logger.info(f"Sim(3) Scale `asb`: {float(aSb.scale()):.2f}")
aTi_list_ = []
for i in range(n_to_align):
bTi = bTi_list[i]
aTi_list_.append(aSb.transformFrom(bTi))
logger.info("Pose graph Sim(3) alignment complete.")
return aTi_list_