def set_cursor_pos(self, pos):
if self._ox and self._pos:
x, y = self._pos
a = utils.compute_angle(x - self._ox, y - self._oy)
self.dr = a - utils.compute_angle(pos[0] - self._ox,
pos[1] - self._oy)
self._pos = pos
def get_puller_idx(self, anchor_pose, anchor_class):
indices = []
for anchor_c, anchor_p in zip(anchor_class, anchor_pose):
start, end = self.get_slice(anchor_c)
poses = np.array(self.poses)[start:end, :]
dist = []
for p in poses:
dist.append(utils.compute_angle(anchor_p, p))
indices.append(np.argmin(dist))
return indices
def compute_histogram(model, dg, count_only=False, test_descriptors=None):
if test_descriptors is None:
test_descriptors = net.compute_descriptor(model, dg.test_loader)
db_descriptors = net.compute_descriptor(model, dg.db_loader)
angular_diffs = []
for match in utils.knn_matching_search(test_descriptors, db_descriptors):
m = dg.test_dataset.__getitem__(match.queryIdx)
n = dg.db_dataset.__getitem__(match.trainIdx)
if m['target'] == n['target']:
angular_diffs.append(utils.compute_angle(m['pose'], n['pose']))
bins = get_histogram_bins(angular_diffs)
if count_only:
return bins
fig = get_histogram_plot(bins)
return bins, fig
def move_handler(self, hl, *pos):
delta = hl.get_rel(*pos)
if hl == self._move:
# Global move
for hl in self._handlers:
hl.move_rel(*delta)
elif hl == self._movex:
# take care of current rotation
x = hl.x - self._move.x
y = hl.y - self._move.y
g = (delta[0] * x + delta[1] * y) / hypot(x, y)**2
x *= g
y *= g
hl.move_rel(x, y)
self._rot.move_rel(-x, -y)
self._compute_data()
elif hl == self._movey:
# take care of current rotation
x = hl.x - self._move.x
y = hl.y - self._move.y
g = (delta[0] * x + delta[1] * y) / hypot(x, y)**2
x *= g
y *= g
hl.move_rel(x, y)
self._compute_data()
elif hl == self._rot:
mhx = self._move.x
mhy = self._move.y
a = self._angle
self._angle = utils.compute_angle(hl.x + delta[0] - mhx,
hl.y + delta[1] - mhy)
a = self._angle - a
cs = cos(a)
sn = sin(a)
self._rot.rotate(mhx, mhy, cs, sn)
self._movex.rotate(mhx, mhy, cs, sn)
self._movey.rotate(mhx, mhy, cs, sn)
self._compute_data()
def RPE(traj_gt,
traj_est,
param_max_pairs=10000,
param_fixed_delta=False,
param_delta=1.00,
param_delta_unit="m",
param_offset=0.00):
"""
This method computes the Relative Pose Error (RPE) and Drift Per Distance Travelled (DDT)
Ref: Sturm et al. (2012), Scona et al. (2017)
Input:
traj_gt -- the first trajectory (ground truth)
traj_est -- the second trajectory (estimated trajectory)
param_max_pairs -- number of relative poses to be evaluated
param_fixed_delta -- false: evaluate over all possible pairs
true: only evaluate over pairs with a given distance (delta)
param_delta -- distance between the evaluated pairs
param_delta_unit -- unit for comparison:
"s": seconds
"m": meters
"rad": radians
"deg": degrees
"f": frames
param_offset -- time offset between two trajectories (to traj_xyz_gt the delay)
param_scale -- scale to be applied to the second trajectory
Output:
list of compared poses and the resulting translation and rotation error
"""
stamps_gt = list(traj_gt.keys())
stamps_est = list(traj_est.keys())
stamps_gt.sort()
stamps_est.sort()
stamps_est_return = []
for t_est in stamps_est:
t_gt = stamps_gt[utils.find_closest_index(stamps_gt,
t_est + param_offset)]
t_est_return = stamps_est[utils.find_closest_index(
stamps_est, t_gt - param_offset)]
t_gt_return = stamps_gt[utils.find_closest_index(
stamps_gt, t_est_return + param_offset)]
if not t_est_return in stamps_est_return:
stamps_est_return.append(t_est_return)
if (len(stamps_est_return) < 2):
raise Exception(
"Number of overlap in the timestamps is too small. Did you run the evaluation on the right files?"
)
if param_delta_unit == "s":
index_est = list(traj_est.keys())
index_est.sort()
elif param_delta_unit == "m":
index_est = utils.distances_along_trajectory(traj_est)
elif param_delta_unit == "rad":
index_est = utils.rotations_along_trajectory(traj_est, 1)
elif param_delta_unit == "deg":
index_est = utils.rotations_along_trajectory(traj_est, 180 / np.pi)
elif param_delta_unit == "f":
index_est = range(len(traj_est))
else:
raise Exception("Unknown unit for delta: '%s'" % param_delta_unit)
if not param_fixed_delta:
if (param_max_pairs == 0 or len(traj_est) < np.sqrt(param_max_pairs)):
pairs = [(i, j) for i in range(len(traj_est))
for j in range(len(traj_est))]
else:
pairs = [(random.randint(0,
len(traj_est) - 1),
random.randint(0,
len(traj_est) - 1))
for i in range(param_max_pairs)]
else:
pairs = []
for i in range(len(traj_est)):
j = utils.find_closest_index(index_est, index_est[i] + param_delta)
if j != len(traj_est) - 1:
pairs.append((i, j))
if (param_max_pairs != 0 and len(pairs) > param_max_pairs):
pairs = random.sample(pairs, param_max_pairs)
gt_interval = np.median(
[s - t for s, t in zip(stamps_gt[1:], stamps_gt[:-1])])
gt_max_time_difference = 2 * gt_interval
result = []
diff_pose = []
for i, j in pairs:
stamp_est_0 = stamps_est[i]
stamp_est_1 = stamps_est[j]
stamp_gt_0 = stamps_gt[utils.find_closest_index(
stamps_gt, stamp_est_0 + param_offset)]
stamp_gt_1 = stamps_gt[utils.find_closest_index(
stamps_gt, stamp_est_1 + param_offset)]
if (abs(stamp_gt_0 -
(stamp_est_0 + param_offset)) > gt_max_time_difference
or abs(stamp_gt_1 -
(stamp_est_1 + param_offset)) > gt_max_time_difference):
continue
gt_delta = utils.transform_diff(traj_gt[stamp_gt_1],
traj_gt[stamp_gt_0])
est_delta = utils.transform_diff(traj_est[stamp_est_1],
traj_est[stamp_est_0])
error44 = utils.transform_diff(est_delta, gt_delta)
gt_distance_travelled = utils.compute_distance(gt_delta)
# check if the distance is not nan or inf
gt_distance_travelled = gt_distance_travelled if (
not 0) else utils._EPS
diff_pose.append(error44)
trans = utils.compute_distance(error44)
rot = utils.compute_angle(error44)
result.append(
[stamp_est_0, stamp_est_1, stamp_gt_0, stamp_gt_1, trans, rot])
if len(result) < 2:
raise Exception(
"Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!"
)
stamps = np.array(result)[:, 0]
trans_error = np.array(result)[:, 4]
rot_error = np.array(result)[:, 5]
errors = np.matrix([SE3Lib.TranToVec(dT) for dT in diff_pose]).transpose()
return errors, trans_error, rot_error, gt_distance_travelled
def pareto_coverage_set(self,
frame_skip=1,
discount=0.98,
incremental_frame_skip=True,
symmetric=True):
"""
Computes an approximate pareto coverage set
Keyword Arguments:
frame_skip {int} -- How many times each action is repeated (default: {1})
discount {float} -- Discount factor to apply to rewards (default: {1})
incremental_frame_skip {bool} -- Wether actions are repeated incrementally (default: {1})
symmetric {bool} -- If true, we assume the pattern of accelerations from the base to the mine is the same as from the mine to the base (default: {True})
Returns:
The pareto coverage set
"""
all_rewards = []
base_perimeter = BASE_RADIUS * BASE_SCALE
# Empty mine just outside the base
virtual_mine = Mine(self.ore_cnt, (base_perimeter**2 / 2)**(1 / 2),
(base_perimeter**2 / 2)**(1 / 2))
virtual_mine.distributions = [
scipy.stats.norm(0, 0) for _ in range(self.ore_cnt)
]
for mine in (self.mines + [virtual_mine]):
mine_distance = mag(mine.pos - HOME_POS) - \
MINE_RADIUS * MINE_SCALE - BASE_RADIUS * BASE_SCALE / 2
# Number of rotations required to face the mine
angle = compute_angle(mine.pos, HOME_POS, [1, 1])
rotations = int(ceil(abs(angle) / (ROTATION * frame_skip)))
# Build pattern of accelerations/nops to reach the mine
# initialize with single acceleration
queue = [{
"speed":
ACCELERATION * frame_skip,
"dist":
mine_distance - frame_skip *
(frame_skip + 1) / 2 * ACCELERATION if incremental_frame_skip
else mine_distance - ACCELERATION * frame_skip * frame_skip,
"seq": [ACT_ACCEL]
}]
trimmed_sequences = []
while len(queue) > 0:
seq = queue.pop()
# accelerate
new_speed = seq["speed"] + ACCELERATION * frame_skip
accelerations = new_speed / ACCELERATION
movement = accelerations * (
accelerations +
1) / 2 * ACCELERATION - (accelerations - frame_skip) * (
(accelerations - frame_skip) + 1) / 2 * ACCELERATION
dist = seq["dist"] - movement
speed = new_speed
if dist <= 0:
trimmed_sequences.append(seq["seq"] + [ACT_ACCEL])
else:
queue.append({
"speed": speed,
"dist": dist,
"seq": seq["seq"] + [ACT_ACCEL]
})
# idle
dist = seq["dist"] - seq["speed"] * frame_skip
if dist <= 0:
trimmed_sequences.append(seq["seq"] + [ACT_NONE])
else:
queue.append({
"speed": seq["speed"],
"dist": dist,
"seq": seq["seq"] + [ACT_NONE]
})
# Build rational mining sequences
mine_means = mine.distribution_means() * frame_skip
mn_sum = np.sum(mine_means)
# on average it takes up to this many actions to fill cart
max_mine_actions = 0 if mn_sum == 0 else int(
ceil(self.capacity / mn_sum))
# all possible mining sequences (i.e. how many times we mine)
mine_sequences = [[ACT_MINE] * i
for i in range(1, max_mine_actions + 1)]
# All possible combinations of actions before, during and after mining
if len(mine_sequences) > 0:
if not symmetric:
all_sequences = map(
lambda sequences: list(sequences[0]) + list(sequences[
1]) + list(sequences[2]) + list(sequences[3]) +
list(sequences[4]),
itertools.product([[ACT_LEFT] * rotations],
trimmed_sequences,
[[ACT_BRAKE] + [ACT_LEFT] *
(180 // (ROTATION * frame_skip))],
mine_sequences, trimmed_sequences))
else:
all_sequences = map(
lambda sequences: list(sequences[0]) + list(sequences[
1]) + list(sequences[2]) + list(sequences[3]) +
list(sequences[1]),
itertools.product([[ACT_LEFT] * rotations],
trimmed_sequences,
[[ACT_BRAKE] + [ACT_LEFT] *
(180 // (ROTATION * frame_skip))],
mine_sequences))
else:
if not symmetric:
print([ACT_NONE] + trimmed_sequences[1:],
trimmed_sequences[1:], trimmed_sequences)
all_sequences = map(
lambda sequences: list(sequences[0]) + list(sequences[
1]) + list(sequences[2]) + [ACT_NONE] + list(
sequences[3])[1:],
itertools.product(
[[ACT_LEFT] * rotations], trimmed_sequences,
[[ACT_LEFT] * (180 // (ROTATION * frame_skip))],
trimmed_sequences))
else:
all_sequences = map(
lambda sequences: list(sequences[0]) + list(sequences[
1]) + list(sequences[2]) + [ACT_NONE] + list(
sequences[1][1:]),
itertools.product(
[[ACT_LEFT] * rotations], trimmed_sequences,
[[ACT_LEFT] * (180 // (ROTATION * frame_skip))]))
# Compute rewards for each sequence
fuel_costs = np.array([f * frame_skip for f in FUEL_LIST])
def maxlen(l):
if len(l) == 0:
return 0
return max([len(s) for s in l])
longest_pattern = maxlen(trimmed_sequences)
max_len = rotations + longest_pattern + 1 + \
(180 // (ROTATION * frame_skip)) + \
maxlen(mine_sequences) + longest_pattern
discount_map = discount**np.arange(max_len)
for s in all_sequences:
reward = np.zeros((len(s), self.obj_cnt()))
reward[:, -1] = fuel_costs[s]
mine_actions = s.count(ACT_MINE)
reward[-1, :-1] = mine_means * mine_actions / \
max(1, (mn_sum * mine_actions) / self.capacity)
reward = np.dot(discount_map[:len(s)], reward)
all_rewards.append(reward)
all_rewards = pareto_filter(all_rewards, minimize=False)
return all_rewards
