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 laptimer(fname, f):
px, py = None, None
prevlap = None
lapcount = 0
for rec in recordreader.RecordIterator(f):
ts = rec['tstamp']
x, y = rec['controldata2'][:2]
if px is not None:
if px < FINISHX and x >= FINISHX and y < FINISHY:
if prevlap is not None:
lapcount += 1
print("%s lap %d: %0.2f" % (fname, lapcount, ts - prevlap))
prevlap = ts
px, py = x, y
def main(fname, skips=0):
f = open(fname, 'rb')
ts0 = None
theta = -0.09
ri = recordreader.RecordIterator(f)
for _ in range(skips):
ri.__next__()
xi, yi = mapsz / 2, mapsz / 2
ptrange = np.max(np.linalg.norm(pts[0], axis=1))
firstiter = False
if firstiter:
poses = []
else:
poses = np.load("poses.npy")
frameno = 0
vidout = None
if False:
vidout = cv2.VideoWriter("ba.mp4",
cv2.VideoWriter_fourcc('X', '2', '6', '4'),
30, (640, 480), True)
lastw = None
for frame in ri:
gray = frame['yuv420'][:480]
bgr = cv2.cvtColor(frame['yuv420'], cv2.COLOR_YUV2BGR_I420)
if ts0 is None:
ts0 = frame['tstamp'] - 1.0 / 30
ts = frame['tstamp']
dt = ts - ts0
ts0 = ts
# throttle, steering, accel, gyro, servo, wheels, periods
gz = frame['carstate'][3][2]
wheels = frame['carstate'][5]
ds = 0
if lastw is not None:
ds = (wheels - lastw)[0]
lastw = wheels
theta -= gz * dt
S, C = np.sin(theta), np.cos(theta)
# now solve for x, y, theta
xi -= ds * WHEELTICK_SCALE * S
yi -= ds * WHEELTICK_SCALE * C
gm = gray[ceilmask]
def L(X):
x, y, theta = X
# m, n = Maplookup(x, y, theta)
# i really thought that this would work better,
# but it totally doesn't
# return (gray[mask].astype(np.float32)*n - m)
# odometry constraint
return np.concatenate([(gm - Maplookup(x, y, theta)) / 170.0,
1e-3 * np.array([xi - x, yi - y])])
if False:
L0 = np.sum(L([xi, yi, theta])**2)
Lx = np.sum(L([xi + 1, yi, theta])**2)
Ly = np.sum(L([xi, yi + 1, theta])**2)
Lt = np.sum(L([xi, yi, theta + 0.01])**2)
print("jac: ", (Lx - L0), (Ly - L0), (Lt - L0) / 0.01)
if firstiter:
lb = ptrange * Z
ub = mapsz - 2 - ptrange * Z
soln = scipy.optimize.least_squares(L,
x0=np.array([xi, yi, theta]),
loss='soft_l1',
bounds=([lb, lb, theta - 0.1],
[ub, ub, theta + 0.1]),
verbose=2)
print(soln)
xi, yi, theta = soln['x']
poses.append([xi, yi, theta, dt, ds, gz])
else:
xi, yi, theta = poses[frameno][:3]
frameno += 1
Mapupdate(xi, yi, theta, gray)
Floorupdate(mapsz - xi, yi, theta, bgr)
S, C = np.sin(theta), np.cos(theta)
disp = ceilmean.astype(np.uint8)
disp = cv2.resize(disp,
dsize=(mapsz, mapsz),
interpolation=cv2.INTER_NEAREST)
for i in range(1, len(poses[:frameno])):
p0 = poses[i - 1]
p1 = poses[i]
cv2.line(disp, (int(32 * p0[0]), int(32 * p0[1])),
(int(32 * p1[0]), int(32 * p1[1])), 255, 1, cv2.LINE_AA,
5)
cv2.circle(disp, (int(16 * xi), int(16 * yi)), 30, 0, -2, cv2.LINE_AA,
4)
cv2.line(disp, (int(32 * xi), int(32 * yi)),
(int(32 * xi - 160 * S), int(32 * yi - 160 * C)), 170, 1,
cv2.LINE_AA, 5)
# mark optical center
cv2.circle(bgr, (int(16 * cx), int(16 * cy)), 50, (170, 255, 170), 1,
cv2.LINE_AA, 4)
if vidout is not None:
bgr[-mapsz:, -mapsz:, :] = disp[:, :, None]
vidout.write(bgr)
if True:
cv2.imshow("ceilmap", disp)
bgr[floormask] = np.clip(bgr[floormask] + 100, 0, 255)
cv2.imshow("bgr", bgr)
cv2.imshow("floor", floormean.astype(np.uint8))
k = cv2.waitKey(1)
if k == ord('q'):
break
cv2.imshow("ceilmap", disp)
cv2.imshow("bgr", bgr)
# pause before quitting
cv2.waitKey(0)
cv2.destroyAllWindows()
f.close()
print("saving ceiling map")
np.save("ceilmapY.npy", ceilmap)
np.save("ceilmapN.npy", ceilN)
np.save("poses.npy", np.array(poses))
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)
if x is not None:
print("ang", params.NOISE_ANGULAR, "lon", params.NOISE_LONG, "lat",
params.NOISE_LAT, "temp", params.PF_TEMP, totalL)
return totalL * -1e-8
if args.gridsearch:
from skopt import gp_minimize
a0 = params.NOISE_ANGULAR
lon0 = params.NOISE_LONG
lat0 = params.NOISE_LAT
print('preloading data...')
for rf in args.recordfile:
datacache = []
f = open(rf, 'rb')
for item in recordreader.RecordIterator(f):
del item['yuv420']
datacache.append(item)
datas.append(datacache)
f.close()
print('minimizing...')
res = gp_minimize(run, [(-4., 4.), (-4., 4.), (-4., 4.)],
random_state=123)
print('final params: ang lon lat',
np.array([a0, lon0, lat0]) * 2**np.array(res.x), 'LL',
-1e8 * res.fun)
else:
for rf in args.recordfile:
f = open(rf, 'rb')
datas = [recordreader.RecordIterator(f)]