def ekf_test():
x, P = ekf.initial_state()
for u in np.linspace(0, 0.2, 10):
x, P = ekf.predict(x, P, 0.0625, u, 0)
print " negligible accel", u, x
x, P = ekf.predict(x, P, 0.0625, 1, 0)
x, P = ekf.predict(x, P, 0.0625, 1, 0)
x, P = ekf.predict(x, P, 0.0625, 1, 0)
print 'after 3 accel', x
x, P = ekf.predict(x, P, 0.0625, -1, 0)
x, P = ekf.predict(x, P, 0.0625, -1, 0)
print 'after 2 brake', x
for i in range(100):
x, P = ekf.predict(x, P, 0.0625, -1, 0)
print x[0]
print 'after 100 coast', x
x, P = ekf.update_centerline(x, P, 0, 0.01, 0.1, np.eye(3) * 0.1)
print x
def replay(fname, f):
vidout = None
x, P = ekf.initial_state()
t0 = None
dt = 1.0 / 30
# gyrozs = []
wheels_last = None
frameno = 0
statelist = []
encoderlist = []
tlist = []
last_throttle = 0
last_steering = 0
LL_center, LL_imu, LL_encoders = 0, 0, 0
while True:
ok, record = recordreader.read_frame(f)
(tstamp, throttle, steering, accel, gyro, servo, wheels, periods,
frame) = record
if not ok:
break
bgr = cv2.cvtColor(frame, cv2.COLOR_YUV2BGR)
bgrbig = cv2.resize(bgr[::-1],
None,
fx=8,
fy=8,
interpolation=cv2.INTER_NEAREST)
frame = np.int32(frame)
cv2.imshow("frame", bgrbig)
# cv2.waitKey()
frameno += 1
print(fname, 'frame', frameno)
if t0 is not None:
dt = tstamp - t0
x0, P0 = np.copy(x), np.copy(P)
t = (0.7 * last_throttle + 0.3 * throttle)
s = last_steering
x, P = ekf.predict(x, P, dt, t / 127.0, s / 127.0)
last_throttle, last_steering = throttle, steering
#print 'x_predict\n', x
xpred, Ppred = np.copy(x), np.copy(P)
hv, th, B, yc, Rk = imgproc.detect_centerline(frame[:, :, 1])
if B is not None:
x, P, LL_center = ekf.update_centerline(x, P, B[0], B[1], B[2], yc,
Rk)
#print 'x_centerline\n', x
#print 'accel', accel
#print 'gyro', gyro[2]
x, P, LL_imu = ekf.update_IMU(x, P, gyro[2])
#print 'x_gyro\n', x
#print 'wheels', wheels, 'periods', periods
if wheels_last is not None:
ds = np.sum(wheels - wheels_last) / 4.0
if ds != 0:
x, P, LL_encoders = ekf.update_encoders(
x, P, ds / dt, float(servo))
#print 'x_encoders\n', x
else:
x, P, LL_encoders = ekf.update_encoders(x, P, 0, float(servo))
#print 'x_encoders\n', x
# gyrozs.append(gyro[2])
# print 'gyro', gyro[2], 'mean', np.mean(gyrozs), 'std', np.std(gyrozs)
#print 'LL', LL_center, LL_imu, LL_encoders
#print 'k', x[4], 1.0 / P[4, 4]
timg = cv2.resize(th[::-1],
(320, int(320 * th.shape[0] / th.shape[1])),
interpolation=cv2.INTER_NEAREST)
timg = np.uint8(timg * (255.0 / np.max(timg)))
mimg = cv2.resize(bgr[::-1],
(320, int(320 * bgr.shape[0] / bgr.shape[1])),
interpolation=cv2.INTER_NEAREST)
hvimg = cv2.resize(hv[::-1],
(320, int(320 * hv.shape[0] / hv.shape[1])),
interpolation=cv2.INTER_NEAREST)
vidframe = np.zeros((timg.shape[0] + frame.shape[0], 640, 3), np.uint8)
if vidout is None:
vidout = cv2.VideoWriter(
"replay.h264", cv2.VideoWriter_fourcc('X', '2', '6', '4'), 30,
(640, vidframe.shape[0]), True)
# vidframe[:frame.shape[0], :320, 0] = 255 - frame
# vidframe[:frame.shape[0], :320, 1] = 255 - frame
# vidframe[:frame.shape[0], :320, 2] = 255 - frame
vidframe[:mimg.shape[0]:, 320:, :] = mimg
vidframe[-hvimg.shape[0]:, :320, 0] = 128 - hvimg * 0.25
vidframe[-hvimg.shape[0]:, :320, 1] = 128 - hvimg * 0.25
vidframe[-hvimg.shape[0]:, :320, 2] = 128 - hvimg * 0.25
vidframe[frame.shape[0]:, 320:, 0] = timg
vidframe[frame.shape[0]:, 320:, 1] = timg
vidframe[frame.shape[0]:, 320:, 2] = timg
vidscale = timg.shape[1] / th.shape[1]
cv2.line(vidframe, (160, 130), (160 + throttle, 130), (0, 255, 0), 3)
cv2.line(vidframe, (160, 135), (160 + steering, 135), (255, 180, 180),
3)
cv2.putText(vidframe, "YUV -U channel", (10, 12),
cv2.FONT_HERSHEY_PLAIN, 0.9, (200, 255, 255))
cv2.putText(vidframe, "birdseye, 25mm/pixel", (330, 12),
cv2.FONT_HERSHEY_PLAIN, 0.9, (200, 255, 255))
cv2.putText(vidframe, "horizontal convolution w/ kernel",
(10, 140 + 12), cv2.FONT_HERSHEY_PLAIN, 0.9,
(200, 255, 255))
cv2.putText(vidframe, "[-1, -1, 2, 2, -1, -1]", (10, 140 + 12 + 12),
cv2.FONT_HERSHEY_PLAIN, 0.9, (200, 255, 255))
cv2.putText(vidframe, "ReLU after conv, quadratic fit",
(330, 140 + 12), cv2.FONT_HERSHEY_PLAIN, 0.9,
(200, 255, 255))
if B is not None:
# ye = -B[2]
# psi_e = np.arctan(B[1])
# k = 2*B[0] / (np.sqrt(B[1]**2 + 1)**3)
draw_measurement(vidframe[frame.shape[0]:, 320:], B, vidscale,
(0, 255, 0), 1)
# draw_state(vidframe[frame.shape[0]:, 320:], x[:5], vidscale, 2)
draw_state(vidframe[frame.shape[0]:, 320:], xpred[:5], vidscale, 2)
try:
# U = np.linalg.cholesky(P[:5, :5]).T
U = np.linalg.cholesky(Ppred[:5, :5]).T
# now we render x, and each x + U[i] / x - U[i]
for i in range(5):
draw_state(vidframe[frame.shape[0]:, 320:], x[:5] + U[i],
vidscale, 1)
draw_state(vidframe[frame.shape[0]:, 320:], x[:5] - U[i],
vidscale, 1)
except np.linalg.linalg.LinAlgError:
pass
#print 'x5 ', x[:5]
#print 'Prec5', 1.0 / np.diag(P[:5, :5])
vidout.write(vidframe)
cv2.imshow('f', vidframe)
k = cv2.waitKey()
if k == ord('q'):
break
if k == ord(',') and len(statelist) > 0: # previous frame
x, P = statelist[-1]
wheels_last = encoderlist[-1]
#print 'wheels_last', wheels_last
t0 = tlist[-1]
statelist.pop()
encoderlist.pop()
tlist.pop()
f.seek(-recordreader.framesiz * 2, 1)
frameno -= 2
else:
statelist.append((x0, P0))
encoderlist.append(wheels_last)
tlist.append(t0)
wheels_last = wheels
t0 = tstamp
def replay_LL(fname, f):
x, P = ekf.initial_state()
t0 = None
dt = 1.0 / 30
# gyrozs = []
wheels_last = None
frameno = 0
last_throttle = 0
last_steering = 0
LLsum = np.zeros(3)
steertrim = open("steertrim.txt", "w")
likelihood = open("likelihood.txt", "w")
trackcurv = open("trackcurv.txt", "w")
s_coord = 0
while True:
imgsiz = imgproc.bucketcount.shape[0] * imgproc.bucketcount.shape[1] * 3
framesiz = 55 + imgsiz
buf = f.read(framesiz)
if len(buf) < framesiz:
break
header = struct.unpack("=IIIbbffffffBHHHHHHHH", buf[:55])
if header[0] != framesiz:
print "recording frame size doesn't match this parser"
raise Exception("image should be %d bytes (%dx%d), is %d bytes" %
((framesiz, ) + imgproc.bucketcount.shape +
(header[0], )))
tstamp = header[1] + header[2] / 1000000.
throttle, steering = header[3:5]
accel = np.float32(header[5:8])
gyro = np.float32(header[8:11])
servo = header[11]
wheels = np.uint16(header[12:16])
periods = np.uint16(header[16:20])
frame = np.frombuffer(buf[55:], np.uint8).reshape(
(imgproc.bucketcount.shape[0], imgproc.bucketcount.shape[1], 3))
frame = np.int32(frame)
frameno += 1
if t0 is not None:
dt = tstamp - t0
print 'dt', dt
t0 = tstamp
t = (last_throttle + 2 * throttle) / 3.0
s = last_steering
#x, P = ekf.predict(x, P, dt, throttle / 127.0, steering / 127.0)
x, P = ekf.predict(x, P, dt, t / 127.0, s / 127.0)
last_throttle, last_steering = throttle, steering
# print 'x_predict\n', x
hv, th, B, yc, Rk = imgproc.detect_centerline(frame[:, :, 1])
LL_center, LL_imu, LL_encoders = 0, 0, 0
if B is not None:
x, P, LL_center = ekf.update_centerline(x, P, B[0], B[1], B[2], yc,
Rk)
# print 'x_centerline\n', x
# print 'accel', accel
# print 'gyro', gyro[2]
x, P, LL_imu = ekf.update_IMU(x, P, gyro[2])
# print 'x_gyro\n', x
# print 'wheels', wheels, 'periods', periods
if wheels_last is not None:
# wait WHAT? this isn't what i'm doing in the car..
ds = np.sum(wheels - wheels_last) / 4.0
if ds != 0:
x, P, LL_encoders = ekf.update_encoders(
x, P, ds / dt, float(servo))
# print 'x_encoders\n', x
else:
x, P, LL_encoders = ekf.update_encoders(x, P, 0, float(servo))
# print 'x_encoders\n', x
wheels_last = wheels
print >> steertrim, x[0], x[1], last_steering, gyro
print >> likelihood, LL_center, LL_imu, LL_encoders
v, delta, ye, psie, kappa = x[:5]
s_coord += dt * v / (1 - kappa * ye)
# we should get our error estimate on s also..
if B is not None:
# measurement was taken ds meters in front of the car
ds = yc * np.cos(x[2])
print >> trackcurv, s_coord + ds, kappa, np.sqrt(P[4, 4])
# gyrozs.append(gyro[2])
# print 'gyro', gyro[2], 'mean', np.mean(gyrozs), 'std', np.std(gyrozs)
print 'LL', LL_center, LL_imu, LL_encoders
LLsum += [LL_center, LL_imu, LL_encoders]
print 'final x\n', x
print 'final P\n', np.diag(P)
return LLsum