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)