def Run(run_name, cars, the_arena, bair_car_data_location):
D = {}
D['Purpose'] = d2s(inspect.stack()[0][3], ':', 'Run object.')
D['run_name'] = run_name
D['cars'] = cars
D['our_car_name'] = car_name_from_run_name(run_name)
our_car_name = D['our_car_name']
D['our_car'] = cars[our_car_name]
our_car = D['our_car']
D['the_arena'] = the_arena
if len(gg(opjD(bair_car_data_location, 'meta', run_name, '*'))) < 5:
print_stars()
print("len(gg(opjD(bair_car_data_location,'meta',run_name,'*'))) < 5")
print_stars()
return False
traj = our_car['runs'][run_name]['traj']
D['T0'] = traj['ts'][0]
D['Tn'] = traj['ts'][-1]
D['list_of_other_car_trajectories'] = our_car['runs'][run_name][
'list_of_other_car_trajectories']
def _rewind():
for c in C['car_names']:
D['cars'][c]['rewind']()
D['rewind'] = _rewind
car_spatial_dic, marker_spatial_dic = Spatial_Relations.setup_spatial_dics(
D)
D['car_spatial_dic'] = car_spatial_dic
D['marker_spatial_dic'] = marker_spatial_dic
D['rewind']()
return D
def _load_run(N,run_name,bair_car_data_location):
print("load run "+run_name)
D['runs'][run_name] = {}
R = D['runs'][run_name]
ts,dat = data.utils.general.get_metadata(run_name,bair_car_data_location)
traj = lo(opj(bair_car_data_location,'meta',run_name,'traj.pkl'))
R['ts'] = array(ts)
R['data'] = dat
R['traj'] = traj
R['list_of_other_car_trajectories'] = []
for other_run_name in N[car_name][run_name]['other_trajectories']:
other_car_name = car_name_from_run_name(other_run_name)
R['list_of_other_car_trajectories'].append( [other_car_name,other_run_name] )
def time_correct_traj(run_name, ts, N):
"""
There is a time offset between the trajectory data and the older metadata timestamps.
"""
print('time_correct_traj')
car_name = car_name_from_run_name(run_name)
assert ('TIME_CORRECTION_DONE' not in N[car_name][run_name])
traj = N[car_name][run_name]['self_trajectory']
traj['ts'] = traj['ts'][30:-30]
for i in range(len(traj['ts'])):
assert (traj['ts'][i]) == ts[i]
traj['camera_separation'] = traj['camera_separation'][30:-30]
for side in ['left', 'right']:
for e in ['x', 'y', 't_vel', 'timestamp_gap']:
traj[side][e] = traj[side][e][30:-30]
N[car_name][run_name]['TIME_CORRECTION_DONE'] = True
return traj
def Run(run_name, cars, the_arena, bair_car_data_location):
D = {}
D['Purpose'] = d2s(inspect.stack()[0][3], ':', 'Run object.')
D['run_name'] = run_name
D['cars'] = cars
D['our_car_name'] = car_name_from_run_name(run_name)
our_car_name = D['our_car_name']
D['our_car'] = cars[our_car_name]
our_car = D['our_car']
D['the_arena'] = the_arena
if len(
gg(opjD(bair_car_data_location, 'meta', run_name, '*'))
) < 5: #'caffe2_z2_color_direct_local_01Jan13_00h01m07s_Mr_Yellow' in run_name:
print("len(gg(opjD(bair_car_data_location,'meta',run_name,'*'))) < 5")
return False
traj = our_car['runs'][run_name]['trajectory']
D['T0'] = traj['ts'][0]
D['Tn'] = traj['ts'][-1]
D['list_of_other_car_trajectories'] = our_car['runs'][run_name][
'list_of_other_car_trajectories']
def _rewind():
for c in C['car_names']:
D['cars'][c]['rewind']()
D['rewind'] = _rewind
try:
our_car['load_image_and_meta_data'](run_name, bair_car_data_location)
except Exception as e:
print(
"********** Exception *** cars[our_car]['load_image_and_meta_data'](run_name,bair_car_data_location) ********************"
)
print(our_car_name, run_name)
print(e.message, e.args)
return False
car_spatial_dic, marker_spatial_dic = Spatial_Relations.setup_spatial_dics(
D)
D['car_spatial_dic'] = car_spatial_dic
D['marker_spatial_dic'] = marker_spatial_dic
D['rewind']()
return D
from kzpy3.utils2 import *
pythonpaths(['kzpy3', 'kzpy3/teg9'])
from vis2 import *
from arena.planner.Constants import C
from data.utils.general import car_name_from_run_name
bair_car_data_location = C['bair_car_data_location']
if 'N' not in locals():
print("Loading trajectory data . . .")
N = lo(C['trajectory_data_location'])
run_name = 'direct_rewrite_test_28Apr17_18h09m52s_Mr_Black'
car_name = car_name_from_run_name(run_name)
bag_folders_dst_rgb1to4_path = opj(bair_car_data_location, 'rgb_1to4')
bag_folders_dst_meta_path = opj(bair_car_data_location, 'meta')
L = lo(
opj(bair_car_data_location, 'meta', run_name,
'left_image_bound_to_data.pkl'))
ts, data_list = get_sorted_keys_and_data(L)
ts = array(ts)
data_types = data_list[0].keys()
data = {}
for d in data_types:
data[d] = []
for e in data_list:
for d in data_types:
def Car(N, car_name, origin, mult, markers):
D = {}
D['Purpose'] = d2s(inspect.stack()[0][3], ':', 'Car object.')
D['car_name'] = car_name
D['potential_field'] = Arena_Potential_Field(origin, mult, markers)
#D['xy'] = [0,0]
D['runs'] = {}
for run_name in N[car_name].keys():
D['runs'][run_name] = {}
R = D['runs'][run_name]
R['trajectory'] = N[car_name][run_name]['self_trajectory']
R['list_of_other_car_trajectories'] = []
for ot in N[car_name][run_name]['other_trajectories']:
other_run_name = ot['run_name']
other_car_name = car_name_from_run_name(other_run_name)
R['list_of_other_car_trajectories'].append(
[other_car_name, other_run_name])
D['positions'] = {}
D['near_i'] = 0
D['near_t'] = 0
def _rewind():
D['near_i'] = 0
D['near_t'] = 0
D['pts'] = []
D['rewind'] = _rewind
def _check_trajectory_point(traj, side, i, t):
assert (traj['ts'][i] <= t)
if traj['ts'][i] == t:
if traj[side]['t_vel'][i] > 2: # 1.788: # Above 4 mph
return False
elif traj['camera_separation'][
i] > 0.25: # almost larger than length of car
return False
elif traj[side]['timestamp_gap'][i] > 0.1: # missed data points
return False
elif length([traj[side]['x'][i], traj[side]['y'][i]]) > length(
markers['xy'][0]):
return False
return True
assert (False)
def _valid_time_and_index(run_name, t):
traj = D['runs'][run_name]['trajectory']
if t > traj['ts'][0] and t < traj['ts'][-1]:
near_t = -1
for i in range(D['near_i'], len(traj['ts'])):
if traj['ts'][i - 1] < t and traj['ts'][i] > t:
near_t = traj['ts'][i]
near_i = i
break
if near_t > 0:
D['near_i'] = near_i
D['near_t'] = near_t
for side in ['left', 'right']:
if not _check_trajectory_point(traj, side, near_i, near_t):
return False, False
return near_t, near_i
return False, False
def _report_camera_positions(run_name, t):
near_t, near_i = _valid_time_and_index(run_name, t)
if not near_t:
return False
traj = D['runs'][run_name]['trajectory']
positions = []
for side in ['left', 'right']:
positions.append(
[traj[side]['x'][near_i], traj[side]['y'][near_i]])
D['pts'].append(positions[0])
return positions
D['report_camera_positions'] = _report_camera_positions
def _get_left_image(run_name):
traj = D['runs'][run_name]['trajectory']
index = traj['data']['t_to_indx'][D['near_t']]
img = traj['data']['left'][index]
return img
D['get_left_image'] = _get_left_image
def _load_image_and_meta_data(run_name):
import data.utils.general
import data.utils.multi_preprocess_pkl_files_1
D['runs'][run_name]['trajectory'][
'data'] = data.utils.general.get_new_Data_dic()
data.utils.multi_preprocess_pkl_files_1.multi_preprocess_pkl_files(
D['runs'][run_name]['trajectory']['data'],
opj(bag_folders_dst_meta_path, run_name),
opj(bag_folders_dst_rgb1to4_path, run_name))
D['load_image_and_meta_data'] = _load_image_and_meta_data
return D
def Car(N,car_name,origin,mult,markers):
D = {}
D['Purpose'] = d2s(inspect.stack()[0][3],':','Car object.')
D['car_name'] = car_name
D['type'] = 'Car'
D['runs'] = {}
D['n_for_heading'] = C['n_for_heading']
for run_name in N[car_name].keys():
D['runs'][run_name] = {}
R = D['runs'][run_name]
R['trajectory'] = N[car_name][run_name]['self_trajectory']
R['list_of_other_car_trajectories'] = []
for other_run_name in N[car_name][run_name]['other_trajectories']:
other_car_name = car_name_from_run_name(other_run_name)
R['list_of_other_car_trajectories'].append( [other_car_name,other_run_name] )
print("""
Remeber to smooth velocities and look at encoder values.
Also look at raw trajectories.
Also time to collision.
Clockwise?
""")
def _rewind():
D['state_info'] = {}
D['state_info']['near_i'] = 0
D['state_info']['near_t'] = 0
D['state_info']['near_t_prev'] = 0
D['state_info']['pts'] = []
D['state_info']['heading'] = None
D['state_info']['heading_prev'] = [0,1]
D['state_info']['relative_heading'] = 90
D['state_info']['velocity'] = 0
D['rewind'] = _rewind
D['rewind']()
def _report_camera_positions(run_name,t):
near_t,near_i = _valid_time_and_index(run_name,t)
if not near_t:
return []
traj = D['runs'][run_name]['trajectory']
positions = []
for side in ['left','right']:
positions.append([traj[side]['x'][near_i],traj[side]['y'][near_i]])
D['state_info']['pts'].append(array(positions).mean(axis=0))
if len(D['state_info']['pts']) > 3*D['n_for_heading']:
D['state_info']['pts'] = D['state_info']['pts'][-2*D['n_for_heading']:]
if len(D['state_info']['pts']) >= D['n_for_heading']:
n = D['n_for_heading']
D['state_info']['heading'] = normalized_vector_from_pts(D['state_info']['pts'][-n:])
#print(d2s('>',length(D['state_info']['heading'])))
if D['state_info']['pts'][-n][0] > D['state_info']['pts'][-1][0]:
D['state_info']['heading'] *= -1.0
#print(d2s('>.',length(D['state_info']['heading'])))
#print(d2s('<',length(D['state_info']['heading_prev'])))
if D['state_info']['near_t'] - D['state_info']['near_t_prev'] < 0.1:
if np.degrees(angle_between(D['state_info']['heading'],D['state_info']['heading_prev'])) > 45:
#print_stars()
#print('Heading warning!!!')
#print_stars()
#print(d2s('>..',length(D['state_info']['heading'])))
D['state_info']['heading'] = D['state_info']['heading_prev']
#print(d2s('>...',length(D['state_info']['heading'])))
D['state_info']['relative_heading'] = (angle_clockwise(D['state_info']['heading'],D['state_info']['pts'][-1]))
D['state_info']['heading_prev'] = D['state_info']['heading'].copy()
D['state_info']['near_t_prev'] = D['state_info']['near_t']
D['state_info']['velocity'] = (traj['left']['t_vel']+traj['right']['t_vel'])/2.0
else:
D['state_info']['heading'] = None
#if D['state_info']['heading'] != None:
#print(d2s('>....',length(D['state_info']['heading'])))
return (D['state_info']['pts'][-1],D['state_info']['heading']) #positions
D['report_camera_positions'] = _report_camera_positions
def _check_trajectory_point(traj,side,i,t):
assert(traj['ts'][i] <= t)
if traj['ts'][i] == t:
if traj[side]['t_vel'][i] > 3: # 1.788: # Above 4 mph
print "traj[side]['t_vel'][i] > 3"
return False
if traj[side]['t_vel'][i]<0.1: #TEMP
print "raj[side]['t_vel'][i]<0.1"
return False
elif traj['camera_separation'][i] > 0.5: # almost larger than length of car
print "traj['camera_separation'][i] > 0.5"
return False
elif traj[side]['timestamp_gap'][i] > 0.5: # missed data points
print "traj[side]['timestamp_gap'][i] > 0.5"
return False
elif length([traj[side]['x'][i],traj[side]['y'][i]]) > length(markers['xy'][0]):
print "length([traj[side]['x'][i],traj[side]['y'][i]]) > length(markers['xy'][0])"
return False
return True
assert(False)
def __check_trajectory_point(traj,side,i,t):
assert(traj['ts'][i] <= t)
if traj['ts'][i] == t:
if traj[side]['t_vel'][i] > 3: # 1.788: # Above 4 mph
return False
if traj[side]['t_vel'][i]<0.1: #TEMP
return False
elif traj['camera_separation'][i] > 0.5: # almost larger than length of car
return False
elif traj[side]['timestamp_gap'][i] > 0.5: # missed data points
return False
elif length([traj[side]['x'][i],traj[side]['y'][i]]) > length(markers['xy'][0]):
return False
return True
assert(False)
def _valid_time_and_index(run_name,t):
traj = D['runs'][run_name]['trajectory']
if t>traj['ts'][0] and t<traj['ts'][-1]:
near_t = -1
for i in range(D['state_info']['near_i'],len(traj['ts'])):
if traj['ts'][i-1]<t and traj['ts'][i]>=t:
near_t = traj['ts'][i]
near_i = i
break
if near_t > 0:
D['state_info']['near_i'] = near_i
D['state_info']['near_t'] = near_t
for side in ['left','right']:
if not _check_trajectory_point(traj,side,near_i,near_t):
return False,False
return near_t,near_i
return False,False
def _get_image(run_name,side):
traj = D['runs'][run_name]['trajectory']
index = traj['data']['t_to_indx'][D['state_info']['near_t']]
img = traj['data'][side][index]
return img
D['get_image'] = _get_image
def _load_image_and_meta_data(run_name,bair_car_data_location):
import data.utils.general
import data.utils.multi_preprocess_pkl_files_1_1
bag_folders_dst_rgb1to4_path = opj(bair_car_data_location,'rgb_1to4')
bag_folders_dst_meta_path = opj(bair_car_data_location,'meta')
D['runs'][run_name]['trajectory']['data'] = data.utils.general.get_new_Data_dic()
data.utils.multi_preprocess_pkl_files_1_1.multi_preprocess_pkl_files(
D['runs'][run_name]['trajectory']['data'],
opj(bag_folders_dst_meta_path,run_name),
opj(bag_folders_dst_rgb1to4_path,run_name),
print_b=True,
load_right_images=True)
D['load_image_and_meta_data'] = _load_image_and_meta_data
(traj[side]['x_pix'][i], traj[side]['y_pix'][i]), 1, c, -1)
Done = False
while not Done:
if True: #try:
traj_lst = []
CAR_NAME = random.choice(N.keys())
RUN_NAME = random.choice(N[CAR_NAME].keys())
if len(N[CAR_NAME][RUN_NAME]['other_trajectories']) < 2:
continue
for ot in [N[CAR_NAME][RUN_NAME]['self_trajectory']
] + N[CAR_NAME][RUN_NAME]['other_trajectories']:
run_name = ot['run_name']
car_name = car_name_from_run_name(run_name)
traj_lst.append(N[car_name][run_name]['self_trajectory'])
print(car_name, run_name)
for i in range(4):
traj_lst[i]['data'] = data.utils.general.get_new_Data_dic()
if DISPLAY_LEFT:
data.utils.multi_preprocess_pkl_files_1.multi_preprocess_pkl_files(
traj_lst[i]['data'],
opj(bag_folders_dst_meta_path, traj_lst[i]['run_name']),
opj(bag_folders_dst_rgb1to4_path, traj_lst[i]['run_name']))
T0 = traj_lst[0]['ts'][0]
t = T0
Tn = traj_lst[0]['ts'][-1]
DT = 1 / 30.
def display_arena(N,CAR_NAME,RUN_NAME,markers,bair_car_data_location,DISPLAY_LEFT):
if DISPLAY_LEFT:
import data.utils.multi_preprocess_pkl_files_1
Origin = 300
Mult = 50
E = 10
out_img = Image([Origin*2,Origin*2,3],Origin,Mult)
bag_folders_dst_rgb1to4_path = opj(bair_car_data_location,'rgb_1to4')
bag_folders_dst_meta_path = opj(bair_car_data_location,'meta')
Done = False
#markers['cv2_draw'](markers,out_img)
markers['cv2_draw'](out_img)
while not Done:
if True:#try:
traj_lst = []
for ot in [N[CAR_NAME][RUN_NAME]['self_trajectory']]+N[CAR_NAME][RUN_NAME]['other_trajectories']:
run_name = ot['run_name']
car_name = car_name_from_run_name(run_name)
traj_lst.append( N[car_name][run_name]['self_trajectory'] )
print(car_name,run_name)
for i in range(min(len(traj_lst),4)):
traj_lst[i]['data'] = data.utils.general.get_new_Data_dic()
if DISPLAY_LEFT:
data.utils.multi_preprocess_pkl_files_1.multi_preprocess_pkl_files(
traj_lst[i]['data'],
opj(bag_folders_dst_meta_path,traj_lst[i]['run_name']),
opj(bag_folders_dst_rgb1to4_path,traj_lst[i]['run_name']))
T0 = traj_lst[0]['ts'][0]
t = T0
Tn = traj_lst[0]['ts'][-1]
DT = 1/30.
dt = DT
timer = Timer(10)
PAUSE = False
out_img['img'] *= 0
#markers['cv2_draw'](out_img)
markers['cv2_draw'](out_img)
cv2.putText(out_img['img'],RUN_NAME,(50,2*Origin-50),cv2.FONT_HERSHEY_SIMPLEX,0.5,(255,255,255));
while t < traj_lst[0]['ts'][-1]:
if not PAUSE:
if timer.check():
out_img['img'] *= 0
#markers['cv2_draw'](markers,out_img)
markers['cv2_draw'](out_img)
cv2.putText(out_img['img'],RUN_NAME,(50,2*Origin-50),cv2.FONT_HERSHEY_SIMPLEX,0.5,(255,255,255));
timer.reset()
ctr = 0
for traj in traj_lst:
car_name = car_name_from_run_name(traj['run_name'])
if t>traj['ts'][0] and t<traj['ts'][-1]:
near_t = -1
for i in range(1,len(traj['ts'])):
if traj['ts'][i-1]<t and traj['ts'][i]>t:
near_t = traj['ts'][i]
near_i = i
break
if near_t > 0:
for side in ['left','right']:
_plot_trajectory_point(traj,side,near_i,near_t,out_img,colors[car_name])
if ctr < 4 and DISPLAY_LEFT:
quadrant = ctr
index = traj['data']['t_to_indx'][near_t]
img = traj['data']['left'][index]
if quadrant == 0:
out_img['img'][:shape(img)[0]+E,:E+shape(img)[1],:] = colors[car_name]
out_img['img'][:shape(img)[0],:shape(img)[1]] = img
elif quadrant == 1:
out_img['img'][-E-shape(img)[0]:,:E+shape(img)[1],:] = colors[car_name]
out_img['img'][-shape(img)[0]:,:shape(img)[1]] = img
elif quadrant == 2:
out_img['img'][:shape(img)[0]+E,-E-shape(img)[1]:,:] = colors[car_name]
out_img['img'][:shape(img)[0]:,-shape(img)[1]:] = img
elif quadrant == 3:
out_img['img'][-E-shape(img)[0]:,-E-shape(img)[1]:,:] = colors[car_name]
out_img['img'][-shape(img)[0]:,-shape(img)[1]:] = img
ctr += 1
k = mci(out_img['img'],delay=33)
if not PAUSE:
dt = DT
if k == ord('q'):
print('q')
break
if k == ord('d'):
print('done')
DONE = True
cv2.destroyAllWindows()
sys.exit()
elif k == ord('k'):
dt = -2
elif k == ord('l'):
dt = 2
elif k == ord(' '):
if PAUSE:
PAUSE = False
print("<<end pause>>")
else:
PAUSE = True
dt = 0
print("<<pause>>")
if abs(dt) > DT:
timer.trigger()
t += dt
if t < T0:
t = T0
elif t >= Tn:
print('At end')
t = Tn-1
"""
def Car(N, car_name, origin, mult, markers):
D = {}
D['Purpose'] = d2s(inspect.stack()[0][3], ':', 'Car object.')
D['car_name'] = car_name
D['potential_field'] = Potential_Fields.Arena_Potential_Field(
origin, mult, markers)
D['runs'] = {}
D['n_for_heading'] = 15
for run_name in N[car_name].keys():
D['runs'][run_name] = {}
R = D['runs'][run_name]
R['trajectory'] = N[car_name][run_name]['self_trajectory']
R['list_of_other_car_trajectories'] = []
"""
for ot in N[car_name][run_name]['other_trajectories']:
other_run_name = ot['run_name']
other_car_name = car_name_from_run_name(other_run_name)
R['list_of_other_car_trajectories'].append( [other_car_name,other_run_name] )
"""
for other_run_name in N[car_name][run_name]['other_trajectories']:
other_car_name = car_name_from_run_name(other_run_name)
R['list_of_other_car_trajectories'].append(
[other_car_name, other_run_name])
def _rewind():
D['state_info'] = {}
#D['state_info']['positions'] = {}
D['state_info']['near_i'] = 0
D['state_info']['near_t'] = 0
D['state_info']['near_t_prev'] = 0
D['state_info']['pts'] = []
D['state_info']['heading'] = None
D['state_info']['heading_prev'] = 0
D['state_info']['relative_heading'] = 90
D['rewind'] = _rewind
def _check_trajectory_point(traj, side, i, t):
assert (traj['ts'][i] <= t)
if traj['ts'][i] == t:
if traj[side]['t_vel'][i] > 2: # 1.788: # Above 4 mph
return False
if traj[side]['t_vel'][i] < 0.2: #TEMP
return False
elif traj['camera_separation'][
i] > 0.25: # almost larger than length of car
return False
elif traj[side]['timestamp_gap'][i] > 0.1: # missed data points
return False
elif length([traj[side]['x'][i], traj[side]['y'][i]]) > length(
markers['xy'][0]):
return False
return True
assert (False)
def _valid_time_and_index(run_name, t):
traj = D['runs'][run_name]['trajectory']
if t > traj['ts'][0] and t < traj['ts'][-1]:
near_t = -1
for i in range(D['state_info']['near_i'], len(traj['ts'])):
if traj['ts'][i - 1] < t and traj['ts'][i] > t:
near_t = traj['ts'][i]
near_i = i
break
if near_t > 0:
D['state_info']['near_i'] = near_i
D['state_info']['near_t'] = near_t
for side in ['left', 'right']:
if not _check_trajectory_point(traj, side, near_i, near_t):
return False, False
return near_t, near_i
return False, False
def _report_camera_positions(run_name, t):
near_t, near_i = _valid_time_and_index(run_name, t)
if not near_t:
return []
traj = D['runs'][run_name]['trajectory']
positions = []
for side in ['left', 'right']:
positions.append(
[traj[side]['x'][near_i], traj[side]['y'][near_i]])
D['state_info']['pts'].append(array(positions).mean(axis=0))
if len(D['state_info']['pts']) >= D['n_for_heading']:
n = D['n_for_heading']
D['state_info']['heading'] = normalized_vector_from_pts(
D['state_info']['pts'][-n:])
if D['state_info']['pts'][-n][0] > D['state_info']['pts'][-1][0]:
D['state_info']['heading'] *= -1
if D['state_info']['near_t'] - D['state_info']['near_t_prev'] < 0.1:
if np.degrees(
angle_between(D['state_info']['heading'],
D['state_info']['heading_prev'])) > 45:
#print_stars()
#print('Heading warning!!!')
#print_stars()
D['state_info']['heading'] = D['state_info'][
'heading_prev']
D['state_info']['relative_heading'] = (degrees(
angle_between(D['state_info']['heading'],
D['state_info']['pts'][-1])))
D['state_info']['heading_prev'] = D['state_info']['heading']
D['state_info']['near_t_prev'] = D['state_info']['near_t']
else:
D['state_info']['heading'] = None
return D['state_info']['pts'][-1] #positions
D['report_camera_positions'] = _report_camera_positions
"""
def _report_camera_positions(run_name,t):
near_t,near_i = _valid_time_and_index(run_name,t)
if not near_t:
return False
traj = D['runs'][run_name]['trajectory']
positions = []
for side in ['left','right']:
positions.append([traj[side]['x'][near_i],traj[side]['y'][near_i]])
D['state_info']['pts'].append(positions[0])
if len(D['state_info']['pts']) >= D['n_for_heading']:
n = D['n_for_heading']
D['state_info']['heading'] = normalized_vector_from_pts(D['state_info']['pts'][-n:])
if D['state_info']['pts'][-n][0] > D['state_info']['pts'][-1][0]:
D['state_info']['heading'] *= -1
else:
D['state_info']['heading'] = None
return positions
D['report_camera_positions'] = _report_camera_positions
"""
def _get_left_image(run_name):
traj = D['runs'][run_name]['trajectory']
index = traj['data']['t_to_indx'][D['state_info']['near_t']]
img = traj['data']['left'][index]
return img
D['get_left_image'] = _get_left_image
def _get_right_image(run_name):
traj = D['runs'][run_name]['trajectory']
index = traj['data']['t_to_indx'][D['state_info']['near_t']]
img = traj['data']['right'][index]
return img
D['get_right_image'] = _get_right_image
def _load_image_and_meta_data(run_name, bair_car_data_location):
import data.utils.general
import data.utils.multi_preprocess_pkl_files_1_1
bag_folders_dst_rgb1to4_path = opj(bair_car_data_location, 'rgb_1to4')
bag_folders_dst_meta_path = opj(bair_car_data_location, 'meta')
D['runs'][run_name]['trajectory'][
'data'] = data.utils.general.get_new_Data_dic()
data.utils.multi_preprocess_pkl_files_1_1.multi_preprocess_pkl_files(
D['runs'][run_name]['trajectory']['data'],
opj(bag_folders_dst_meta_path, run_name),
opj(bag_folders_dst_rgb1to4_path, run_name),
print_b=True,
load_right_images=True)
D['load_image_and_meta_data'] = _load_image_and_meta_data
return D