def filter_pairs_by_path(poses: typing.Sequence[np.ndarray],
delta: float,
tol: float = 0.0,
all_pairs: bool = False) -> IdPairs:
"""
filters pairs in a list of SE(3) poses by their path distance in meters
- the accumulated, traveled path distance between the two pair points
is considered
:param poses: list of SE(3) poses
:param delta: the path distance in meters used for filtering
:param tol: absolute path tolerance to accept or reject pairs
in all_pairs mode
:param all_pairs: use all pairs instead of consecutive pairs
:return: list of index tuples of the filtered pairs
"""
id_pairs = []
if all_pairs:
positions = np.array([pose[:3, 3] for pose in poses])
distances = geometry.accumulated_distances(positions)
for i in range(distances.size - 1):
offset = i + 1
distances_from_here = distances[offset:] - distances[i]
candidate_index = int(
np.argmin(np.abs(distances_from_here - delta)))
if (np.abs(distances_from_here[candidate_index] - delta) > tol):
continue
id_pairs.append((i, candidate_index + offset))
else:
ids = []
previous_pose = poses[0]
current_path = 0.0
for i, current_pose in enumerate(poses):
current_path += float(
np.linalg.norm(current_pose[:3, 3] - previous_pose[:3, 3]))
previous_pose = current_pose
if current_path >= delta:
ids.append(i)
current_path = 0.0
id_pairs = [(i, j) for i, j in zip(ids, ids[1:])]
return id_pairs
def distances(self):
return geometry.accumulated_distances(self.positions_xyz)
def distances(self) -> np.ndarray:
return geometry.accumulated_distances(self.positions_xyz)
