def test_relative_so3(self):
a = lie.random_so3()
b = lie.random_so3()
self.assertTrue(lie.is_so3(a) and lie.is_so3(b))
a_to_b = lie.relative_so3(a, b)
b_from_a = a.dot(a_to_b)
self.assertTrue(np.allclose(b_from_a, b))
def filter_pairs_by_angle(poses: typing.Sequence[np.ndarray],
delta: float,
tol: float = 0.0,
degrees: bool = False,
all_pairs: bool = False) -> IdPairs:
"""
filters pairs in a list of SE(3) poses by their relative angle
- by default, the angle accumulated on the path between the two pair poses
is considered
- if <all_pairs> is set to True, the direct angle between the two pair
poses is considered
:param poses: list of SE(3) poses
:param delta: the angle in radians used for filtering
:param tol: absolute angle tolerance to accept or reject pairs
in all_pairs mode
:param degrees: set to True if <delta> is in degrees instead of radians
:param all_pairs: use all pairs instead of consecutive pairs
:return: list of index tuples of the filtered pairs
"""
# Angle-axis angles are within [0, pi] / [0, 180] (Euler theorem).
bounds = [0., 180.] if degrees else [0, np.pi]
if delta < bounds[0] or delta > bounds[1]:
raise FilterException(f"delta angle must be within {bounds}")
delta = np.deg2rad(delta) if degrees else delta
tol = np.deg2rad(tol) if degrees else tol
if all_pairs:
upper_bound = delta + tol
lower_bound = delta - tol
id_pairs = []
ids = list(range(len(poses)))
# All pairs search is O(n^2) here. Use vectorized operations with
# scipy.spatial.transform.Rotation for quicker processing.
logger.info("Searching all pairs with matching rotation delta,"
" this can take a while.")
start_indices = ids[:-1]
for i in start_indices:
if not i % 100:
print(int(i / len(start_indices) * 100), "%", end="\r")
offset = i + 1
end_indices = ids[offset:]
rotations_i = lie.sst_rotation_from_matrix(
np.array([poses[i][:3, :3]] * len(end_indices)))
rotations_j = lie.sst_rotation_from_matrix(
np.array([poses[j][:3, :3] for j in end_indices]))
delta_angles = np.linalg.norm(
(rotations_i.inv() * rotations_j).as_rotvec(), axis=1)
matches = np.argwhere((lower_bound <= delta_angles)
& (delta_angles <= upper_bound)) + offset
id_pairs.extend([(i, j) for j in matches.flatten().tolist()])
else:
delta_angles = [
lie.so3_log_angle(lie.relative_so3(p1[:3, :3], p2[:3, :3]))
for p1, p2 in zip(poses, poses[1:])
]
accumulated_delta = 0.0
current_start_index = 0
id_pairs = []
for i, current_delta in enumerate(delta_angles):
end_index = i + 1
accumulated_delta += current_delta
if accumulated_delta >= delta:
id_pairs.append((current_start_index, end_index))
accumulated_delta = 0.0
current_start_index = end_index
return id_pairs