def preprocess_data( convert, data ):
frame_data = {}
for frame in momo.frames( data ):
tmp = []
for o_frame, o_time, o_id, o_x, o_y, o_dx, o_dy in frame:
tmp.append( [o_id, np.array( [o_x, o_y, o_dx, o_dy] )] )
for i in xrange( len( frame ) ):
o_id, o = tmp.pop( 0 )
if not o_id in frame_data:
frame_data[o_id] = {
"states": [],
"frames": []
}
frame_data[o_id]["states"].append( o )
frame_data[o_id]["frames"].append( [f[1] for f in tmp] )
tmp.append( [o_id, o] )
# Rasterize to grid
for o_id, frame in frame_data.items():
old_grid_s = None
states = []
frames = []
for index in xrange( len( frame["states"] ) ):
state = frame["states"][index]
angle = momo.angle.as_angle( np.array( state[2:] ) )
grid_s = convert.from_world( np.array( [state[0], state[1], angle] ) )
if old_grid_s == None or old_grid_s != grid_s:
old_grid_s = grid_s
states.append( frame["states"][index] )
frames.append( frame["frames"][index] )
frame["states"] = states
frame["frames"] = frames
def preprocess_data( self, data ):
frame_data = {}
for frame in momo.frames( data ):
tmp = []
for o_frame, o_time, o_id, o_x, o_y, o_dx, o_dy in frame:
tmp.append( [o_id, o_time, o_frame, np.array( [o_x, o_y, o_dx, o_dy] )])
for i in xrange( len( tmp ) ):
o_id, o_time, o_frame, o = tmp.pop( 0 )
if not o_id in frame_data:
frame_data[o_id] = {
"times": [],
"frame_nums": [],
"states": [],
"frames": []
}
frame_data[o_id]["states"].append( o )
frame_data[o_id]["frames"].append( [f[3] for f in tmp] )
frame_data[o_id]["times"].append( o_time )
frame_data[o_id]["frame_nums"].append( o_frame )
tmp.append( [o_id, o_time, o_frame, o] )
# Rasterize to grid
for o_id, frame in frame_data.items():
old_grid_s = None
states = []
frames = []
frame_nums = []
for index in xrange( len( frame["states"] ) ):
state = frame["states"][index]
angle = momo.angle.as_angle( np.array( state[2:] ) )
grid_s = self.from_world( np.array( [state[0], state[1], angle] ) )
if type( old_grid_s ) == type( None ) or not (old_grid_s == grid_s).all():
old_grid_s = grid_s
states.append( frame["states"][index] )
frames.append( np.array( frame["frames"][index], dtype = np.float32 ) )
frame_nums.append( frame["frame_nums"][index] )
frame["states"] = states
frame["frames"] = frames
frame["frame_nums"] = frame_nums
return frame_data
ts = sorted( theta )
vmin = sum( ts[:2] )
vmax = sum( ts[-2:] )
solver = Solver()
width = 128
height = 32
delta = 0.15
pl.figure( 1, figsize = ( 30, 10 ), dpi = 75 )
pl.ion()
#costs = np.random.random( (8, height, width) )
#costs = ( ( costs > 0.7 ) * 9.9 + 0.1 ).astype( np.float32 )
for f in momo.frames( data ):
frame = []
for o in f:
frame.append( [o[3] - minx, o[4] - miny, o[5], o[6]] )
frame = np.array( frame, dtype=np.float32 )
t = time.time()
costs = solver.compute_weights( width, height, delta, 0.04, 3, theta, frame )
cummulated, parents = solver.compute_dijkstra( width, height, costs, np.array( [100, 24, 0], dtype = np.int32 ) )
t = time.time() - t
print "Compute FPS", 1 / t
t = time.time()
