def __init__(self, fname): self.f = open(fname, "rb") print("scanning ", fname, "...") self.scanner = recordreader.RecordScanner(self.f) self.frametexid = None self.fronttexid = None self.f.seek(0, 0) self.controlstate = [] self.controls = [] self.carstate = [] self.ts = [] self.learn_controls = False self.lap_timer = False self.show_frontview = False self.startlinexy = np.array([7.4, -3]) for frdata in recordreader.RecordIterator(self.f): self.ts.append(frdata['tstamp']) (throttle, steering, accel, gyro, servo, wheels, periods) = frdata['carstate'] self.carstate.append(frdata['carstate']) self.controls.append([throttle, steering]) self.controlstate.append(frdata['controldata2']) self.controlstate = np.float32(self.controlstate) self.controls = np.float32(self.controls) self.ts = np.array(self.ts) self.loadframe(0) self.playing = False print("done") self.lm = None self.track = None self.unloadlist = [] try: f = open("lm.txt", "r") n = int(f.readline()) self.lm = np.zeros((n, 2)) for i in range(n): self.lm[i] = [float(x) for x in f.readline().strip().split()] f.close() except IOError: print("no lm.txt found; skipping") try: #f = open("track.txt", "r") #n = int(f.readline()) #self.track = np.zeros((n, 5)) #for i in range(n): # self.track[i] = [ # float(x) for x in f.readline().strip().split()] #f.close() self.track = np.load("../trackplan/trackdata.npy") self.track[:, 2:4] = np.stack([-self.track[:, 3], self.track[:, 2]]).T except IOError: print("no track.txt found; skipping")
def __init__(self, fname): self.unloadlist = [] self.f = open(fname, "rb") print("scanning ", fname, "...") self.scanner = recordreader.RecordScanner(self.f) self.frametexid = None self.playing = False self.ts = [] self.camdata = ceiltrack.ceillut() self.f.seek(0, 0) self.ceilheight = ceiltrack.CEIL_HEIGHT # do a full tracking here on load B = np.float32([HOME[0], HOME[1], 0]) self.track = [] match_time = 0 opt_time = 0 first = True floordata = [] floormask = None for frdata in recordreader.RecordIterator(self.f): if 'yuv420' not in frdata: continue self.ts.append(frdata['tstamp']) yuv420 = frdata['yuv420'] gray = yuv420[:480] bgr = cv2.cvtColor(yuv420, cv2.COLOR_YUV2BGR_I420) t0 = time.time() xy = ceiltrack.match(gray, *self.camdata) tm = time.time() if first: first = False for i in range(6): cost, dB = ceiltrack.cost(xy, *B) B += dB #B_straight, cost_straight = B, cost #B = np.float32([HOME[0], HOME[1], np.pi/2]) #for i in range(6): # cost, dB = ceiltrack.cost(xy, *B) # B += dB #if cost_straight < cost: # B = B_straight # we need an example frame to initialize the floor lookup table # to filter out the visible body posts self.floorlut = ceiltrack.floorlut(gray) floormask = self.floorlut[0] else: for i in range(2): c, dB = ceiltrack.cost(xy, *B) B += dB topt = time.time() match_time += tm - t0 opt_time += topt - tm self.track.append(B.copy()) floordata.append(bgr[floormask]) self.ts = np.array(self.ts) self.track = np.array(self.track) self.origtrack = self.track.copy() self.track[:, 0] = -self.track[:, 0] self.track[:, 2] = -self.track[:, 2] # mirror the floor-pixel lookup table x coordinates also self.floorlut[1][0] = -self.floorlut[1][0] self.floordata = np.array(floordata) self.loadframe(0) print("done,", match_time, "secs match_time", opt_time, "sec opt_time") floorimg = ceiltrack.render_floor(self.track, self.floordata, self.floorlut[1]) if True: xgm = ceiltrack.X_GRID * ceiltrack.CEIL_HEIGHT ygm = ceiltrack.Y_GRID * ceiltrack.CEIL_HEIGHT Z = 50 # pixels per meter for x in range(0, 1 + int(1000 / (xgm * Z))): for y in range(0, 1 + int(500 / (ygm * Z))): cv2.circle(floorimg, (int(x * xgm * Z), int(y * ygm * Z)), int(0.25 * Z), (255, 255, 0)) cv2.imwrite("map.png", floorimg) self.floortex = load_texture(floorimg) print("home location:", HOME)