def find_behavior_end(self, sequence_dir, transition_point):
"""Find the frame at which the behavior ends given the transition point.
Args:
sequence_dir: Full path to the sequence directory.
transition_point: Dict representing transition point with 'begin' and 'end' frames.
Returns:
end_frame_idx: 1-based index of the end behavior frame in the sequence.
behavior_id: Behavior ID (e.g. 'oor').
"""
transition_frame_path = transition_point['end']
begin_frame = load_frame(transition_point['begin'])
end_frame = load_frame(transition_point['end'])
if not self.is_enter_room(begin_frame, end_frame):
transition_yaw = self.get_transition_yaw(begin_frame, end_frame)
transition_frame_path = {
'transition_yaw': transition_yaw,
'transition_frame_path': transition_frame_path
}
cur_frame_idx, behavior_id = self.find_turn(sequence_dir,
transition_frame_path,
forward_time=True)
else: # Special case for when entering a room, since turning after entering does not count
cur_frame_idx = get_frame_idx(transition_frame_path)
behavior_id = None
if behavior_id is not None:
last_frame_idx = len(get_frame_paths(sequence_dir))
cur_frame_idx = min(cur_frame_idx + self.n_after_turn_frames,
last_frame_idx)
return cur_frame_idx, behavior_id
def find_turn(self, sequence_dir, transition_frame_path, forward_time):
if not isinstance(transition_frame_path,
str): # JunctionTurnBehaviorDetector special case
input_yaw = transition_frame_path['transition_yaw']
transition_frame_path = transition_frame_path[
'transition_frame_path']
use_input_yaw = True
else:
use_input_yaw = False
transition_frame = load_frame(transition_frame_path)
transition_quaternion = transition_frame['gt_odom']['orientation']
if use_input_yaw is True:
transition_yaw = input_yaw
else:
transition_yaw = euler_from_quaternion(transition_quaternion)[2]
transition_pos = transition_frame['gt_odom']['position']
cur_pos = transition_pos
transition_frame_idx = get_frame_idx(transition_frame_path)
# Frame at the start of find_turn
if forward_time is True:
begin_frame = transition_frame
else:
begin_frame_path = self.get_frame_path_at_idx(
sequence_dir, transition_frame_idx - 1)
begin_frame = load_frame(begin_frame_path)
cur_frame_idx = transition_frame_idx
frame_delta = 1
in_turn_loop = True
while (
in_turn_loop and
(np.linalg.norm(cur_pos - transition_pos) < self.max_turn_dist)):
prev_frame_idx = cur_frame_idx
if forward_time is True: # Move forward in time
cur_frame_idx = transition_frame_idx + frame_delta
else: # Move backwards in time
cur_frame_idx = transition_frame_idx - frame_delta
cur_frame_path = self.get_frame_path_at_idx(
sequence_dir, cur_frame_idx)
# Break if we have reached beginning/end of sequence
if not os.path.isfile(cur_frame_path):
cur_frame_idx = prev_frame_idx
break
cur_frame = load_frame(cur_frame_path)
cur_quaternion = cur_frame['gt_odom']['orientation']
cur_yaw = euler_from_quaternion(cur_quaternion)[2]
cur_pos = cur_frame['gt_odom']['position']
behavior_id = self.get_behavior(transition_yaw, cur_yaw,
forward_time)
if behavior_id is not None:
return cur_frame_idx, behavior_id
# Update counters
in_turn_loop = self.in_turn_loop(begin_frame, cur_frame)
frame_delta += 1
return cur_frame_idx, None
def process_sequence(self, sequence_dir):
frame_paths = get_frame_paths(sequence_dir)
for cur_frame_path in frame_paths:
cur_frame = load_frame(cur_frame_path)
is_cf = (cur_frame.get('behavior_id') is None) and self.in_room(cur_frame)
if is_cf:
cur_frame_idx = get_frame_idx(cur_frame_path)
self.add_behavior_label(sequence_dir, cur_frame_idx, behavior_id='cf')
def find_behavior_start(self, sequence_dir, transition_point):
"""Find the frame at which the behavior begins.
Args:
sequence_dir: Full path to the sequence directory.
transition point: Dict representing transition point with 'begin' and 'end' frames.
"""
transition_frame_path = transition_point['end']
begin_frame = load_frame(transition_point['begin'])
end_frame = load_frame(transition_point['end'])
transition_yaw = self.get_transition_yaw(begin_frame, end_frame)
transition_frame_path = {
'transition_yaw': transition_yaw,
'transition_frame_path': transition_frame_path
}
cur_frame_idx, behavior_id = self.find_turn(sequence_dir,
transition_frame_path,
forward_time=False)
if behavior_id is not None:
cur_frame_idx = max(cur_frame_idx - self.n_after_turn_frames, 1)
return cur_frame_idx, behavior_id
def get_frame_data(self, frame_fpath):
data = load_frame(frame_fpath)
if 'localized_behavior_id' in data:
localized_node_name, localized_behavior_id = data[
'localized_behavior_id'].split(' ')
else:
localized_node_name = self.NO_NODE_NAME
localized_behavior_id = self.NO_BEHAVIOR_TOKEN
processed = {
'depth':
self.depth_transform(data['depth']),
'rgb':
self.rgb_transform(data['rgb']),
'vel':
torch.from_numpy(data['vel']),
'area_name':
data.get('area_name'),
'room_name':
data['room_name'],
'is_invalid':
1 if data.get('is_invalid', False) is True else False,
'position':
data['gt_odom']['position'], # For debugging only
'orientation':
data['gt_odom']['orientation'], # For junction behavior detector
'sequence_idx':
data['sequence_idx'], # For debugging only
'behavior_id':
data.get('behavior_id', self.NO_BEHAVIOR_TOKEN),
'localized_node_name':
localized_node_name, # This is used as a sanity check and shouldn't actually be used (use node_name) instead
'localized_behavior_id':
localized_behavior_id,
'affordance_vec':
self.affordance_list2vec(
data.get('affordance_list', self.NO_AFFORDANCES_TOKEN)),
'node_name':
data.get('node_name', self.NO_NODE_NAME),
'phase':
data.get('phase', -1.),
'frame_path':
frame_fpath, # Used for building cache
}
if processed['localized_behavior_id'] != self.NO_BEHAVIOR_TOKEN:
assert processed['localized_node_name'] == processed['node_name']
return processed
def find_transition_points(self, sequence_dir):
"""Finds the transition points and assembles them in a list.
"""
frame_paths = get_frame_paths(sequence_dir)
last_frame = None
last_frame_path = None
transition_points = []
for cur_frame_path in frame_paths:
cur_frame = load_frame(cur_frame_path)
if self.is_transition_point(last_frame, cur_frame):
transition_point = {
'begin': last_frame_path,
'end': cur_frame_path
}
transition_points.append(transition_point)
# Update last frame
last_frame = cur_frame
last_frame_path = cur_frame_path
return transition_points
def process_transition_point(self, sequence_dir, transition_point):
start, start_behavior_id = self.find_behavior_start(
sequence_dir, transition_point)
end, end_behavior_id = self.find_behavior_end(sequence_dir,
transition_point)
# Begin frame (frame at transition point)
begin_frame = load_frame(transition_point['begin'])
# Start frame
start_frame_path = self.get_frame_path_at_idx(sequence_dir, start)
start_frame = load_frame(start_frame_path)
if (start_behavior_id is not None) and (end_behavior_id is not None):
# If the behaviors are different, "smartly" choose a behavior by seeing which one
# (start vs. end) has greater yaw delta with transition point
# However, if exiting a room, choose the end behavior instead of start
# Start frame orientation
start_quaternion = start_frame['gt_odom']['orientation']
start_yaw = euler_from_quaternion(start_quaternion)[2]
# End frame orientation
end_frame_path = self.get_frame_path_at_idx(sequence_dir, end)
end_frame = load_frame(end_frame_path)
end_quaternion = end_frame['gt_odom']['orientation']
end_yaw = euler_from_quaternion(end_quaternion)[2]
if start_frame['room_name'] == end_frame['room_name']:
# This is for an edge case where the robot goes from room A to room B to room A,
# after spending like 0.5 seconds in room B. In this case, the start and end frames
# are both in room A and the rooms are not adjacent, failing self.get_transition_yaw
print('Rooms are not adjacent!')
return start, end, None
else:
transition_yaw = self.get_transition_yaw(
start_frame, end_frame)
# Choose a behavior
start_delta = np.abs(compute_angle_delta(start_yaw,
transition_yaw))
end_delta = np.abs(compute_angle_delta(end_yaw, transition_yaw))
if start_delta > end_delta:
behavior_id = start_behavior_id
else:
behavior_id = end_behavior_id
if (start_behavior_id is None) and (end_behavior_id is None):
# Determine whether robot is going straight into a room or not
end_frame_path = self.get_frame_path_at_idx(sequence_dir, end)
end_frame = load_frame(end_frame_path)
if begin_frame['room_name'].startswith('hallway') and self.in_room(
end_frame):
behavior_id = 's'
else:
# Let CorridorFollowBehaviorDetector take care of cf
# We don't want to overwrite previous 'tr' frames by accident
behavior_id = None
# behavior_id = 'cf'
if (start_behavior_id is not None) and (end_behavior_id is None):
behavior_id = start_behavior_id
elif (end_behavior_id is not None) and (start_behavior_id is None):
behavior_id = end_behavior_id
# If start frame is in room, choose end_behavior_id
if self.in_room(begin_frame):
if end_behavior_id is None:
behavior_id = 'cf'
else:
behavior_id = end_behavior_id
return start, end, behavior_id
def add_behavior_label(self, sequence_dir, cur_frame_idx, behavior_id):
cur_frame_path = self.get_frame_path_at_idx(sequence_dir,
cur_frame_idx)
cur_frame = load_frame(cur_frame_path)
cur_frame['behavior_id'] = behavior_id # Overwrite behavior ID
np.savez(cur_frame_path, cur_frame) # Overwrite the .npz file