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