def path_length(self, ids=None):
"""
calculates the path length (arc-length)
:param ids: optional start and end index as tuple (start, end)
:return: path length in meters
"""
if ids is not None:
if len(ids) != 2 or not all(type(i) is int for i in ids):
raise TrajectoryException("ids must be a tuple of positive integers")
return float(geometry.arc_len(self.positions_xyz[ids[0]:ids[1]]))
else:
return float(geometry.arc_len(self.positions_xyz))
def path_length(self, ids=None):
"""
calculates the path length (arc-length)
:param ids: optional start and end index as tuple (start, end)
:return: path length in meters
"""
if ids is not None:
if len(ids) != 2 or not all(type(i) is int for i in ids):
raise TrajectoryException("ids must be a tuple of positive integers")
return float(geometry.arc_len(self.positions_xyz[ids[0]:ids[1]]))
else:
return float(geometry.arc_len(self.positions_xyz))
def filter_pairs_by_path(poses, delta, tol=0.0, all_pairs=False):
"""
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
"""
if all_pairs:
upper_bound = delta + tol
lower_bound = delta - tol
id_pairs = []
ids = range(len(poses))
positions = np.array([pose[:3, 3] for pose in poses])
res_avg = 0.0
n_iter_avg = 0.0
print_progress = logger.isEnabledFor(logging.DEBUG)
num_pairs = 0
for i in ids:
found, j, res, n = bounded_binary_search(
lambda x: geometry.arc_len(positions[i:x + 1]), len(positions),
delta, lower_bound, upper_bound)
n_iter_avg += n
if found:
num_pairs += 1
res_avg += res
id_pairs.append((i, j))
if print_progress:
print("\rSearching",
delta,
"m path sub-sequences - found",
num_pairs,
end="\r")
sys.stdout.flush()
if print_progress:
print("")
if num_pairs != 0:
logger.debug("avg. target residual: " +
"{0:.6f}".format(res_avg / num_pairs) + "m" +
" | avg. num. iterations: " +
"{0:.6f}".format(n_iter_avg / num_pairs))
else:
logger.debug("Found no pairs for delta '" + str(delta) + "m'.")
else:
ids = []
previous_pose = poses[0]
current_path = 0.0
for i, current_pose in enumerate(poses):
current_path += 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 filter_pairs_by_path(poses, delta, tol=0.0, all_pairs=False):
"""
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
"""
if all_pairs:
upper_bound = delta + tol
lower_bound = delta - tol
id_pairs = []
ids = range(len(poses))
positions = np.array([pose[:3, 3] for pose in poses])
res_avg = 0.0
n_iter_avg = 0.0
print_progress = logger.isEnabledFor(logging.DEBUG)
num_pairs = 0
for i in ids:
found, j, res, n = bounded_binary_search(lambda x: geometry.arc_len(positions[i:x+1]),
len(positions), delta,
lower_bound, upper_bound)
n_iter_avg += n
if found:
num_pairs += 1
res_avg += res
id_pairs.append((i, j))
if print_progress:
print("\rSearching", delta, "m path sub-sequences - found", num_pairs, end="\r")
sys.stdout.flush()
if print_progress:
print("")
if num_pairs != 0:
logger.debug("avg. target residual: " + "{0:.6f}".format(res_avg / num_pairs) + "m"
+ " | avg. num. iterations: " + "{0:.6f}".format(n_iter_avg / num_pairs))
else:
logger.debug("Found no pairs for delta '" + str(delta) + "m'.")
else:
ids = []
previous_pose = poses[0]
current_path = 0.0
for i, current_pose in enumerate(poses):
current_path += 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 path_length(self):
"""
calculates the path length (arc-length)
:return: path length in meters
"""
return float(geometry.arc_len(self.positions_xyz))
