pull/1407/head
George Hotz
5 years ago
parent
ba0d92de52
commit
abad49110e
9 changed files with 0 additions and 993 deletions
@ -1,284 +0,0 @@ |
|||||||
import os |
|
||||||
import struct |
|
||||||
import bisect |
|
||||||
import numpy as np |
|
||||||
import _io |
|
||||||
|
|
||||||
import capnp |
|
||||||
from cereal import log as capnp_log |
|
||||||
|
|
||||||
class RawData(): |
|
||||||
def __init__(self, f): |
|
||||||
self.f = _io.FileIO(f, 'rb') |
|
||||||
self.lenn = struct.unpack("I", self.f.read(4))[0] |
|
||||||
self.count = os.path.getsize(f) / (self.lenn+4) |
|
||||||
|
|
||||||
def read(self, i): |
|
||||||
self.f.seek((self.lenn+4)*i + 4) |
|
||||||
return self.f.read(self.lenn) |
|
||||||
|
|
||||||
def yuv420_to_rgb(raw, image_dim=None, swizzled=False): |
|
||||||
def expand(x): |
|
||||||
x = np.repeat(x, 2, axis=0) |
|
||||||
return np.repeat(x, 2, axis=1) |
|
||||||
|
|
||||||
if image_dim is None: |
|
||||||
image_dim = (raw.shape[1]*2, raw.shape[2]*2) |
|
||||||
swizzled = True |
|
||||||
|
|
||||||
if not swizzled: |
|
||||||
img_data = np.array(raw, copy=False, dtype=np.uint8) |
|
||||||
uv_len = (image_dim[0]/2)*(image_dim[1]/2) |
|
||||||
img_data_u = expand(img_data[image_dim[0]*image_dim[1]: \ |
|
||||||
image_dim[0]*image_dim[1]+uv_len]. \ |
|
||||||
reshape(image_dim[0]/2, image_dim[1]/2)) |
|
||||||
|
|
||||||
img_data_v = expand(img_data[image_dim[0]*image_dim[1]+uv_len: \ |
|
||||||
image_dim[0]*image_dim[1]+2*uv_len]. \ |
|
||||||
reshape(image_dim[0]/2, image_dim[1]/2)) |
|
||||||
img_data_y = img_data[0:image_dim[0]*image_dim[1]].reshape(image_dim) |
|
||||||
else: |
|
||||||
img_data_y = np.zeros(image_dim, dtype=np.uint8) |
|
||||||
img_data_y[0::2, 0::2] = raw[0] |
|
||||||
img_data_y[1::2, 0::2] = raw[1] |
|
||||||
img_data_y[0::2, 1::2] = raw[2] |
|
||||||
img_data_y[1::2, 1::2] = raw[3] |
|
||||||
img_data_u = expand(raw[4]) |
|
||||||
img_data_v = expand(raw[5]) |
|
||||||
|
|
||||||
yuv = np.stack((img_data_y, img_data_u, img_data_v)).swapaxes(0,2).swapaxes(0,1) |
|
||||||
yuv = yuv.astype(np.int16) |
|
||||||
|
|
||||||
# http://maxsharabayko.blogspot.com/2016/01/fast-yuv-to-rgb-conversion-in-python-3.html |
|
||||||
# according to ITU-R BT.709 |
|
||||||
yuv[:,:, 0] = yuv[:,:, 0].clip(16, 235).astype(yuv.dtype) - 16 |
|
||||||
yuv[:,:,1:] = yuv[:,:,1:].clip(16, 240).astype(yuv.dtype) - 128 |
|
||||||
|
|
||||||
A = np.array([[1.164, 0.000, 1.793], |
|
||||||
[1.164, -0.213, -0.533], |
|
||||||
[1.164, 2.112, 0.000]]) |
|
||||||
|
|
||||||
# our result |
|
||||||
img = np.dot(yuv, A.T).clip(0, 255).astype('uint8') |
|
||||||
return img |
|
||||||
|
|
||||||
|
|
||||||
class YuvData(): |
|
||||||
def __init__(self, f, dim=(160,320)): |
|
||||||
self.f = _io.FileIO(f, 'rb') |
|
||||||
self.image_dim = dim |
|
||||||
self.image_size = self.image_dim[0]/2 * self.image_dim[1]/2 * 6 |
|
||||||
self.count = os.path.getsize(f) / self.image_size |
|
||||||
|
|
||||||
def read_frame(self, frame): |
|
||||||
self.f.seek(self.image_size*frame) |
|
||||||
raw = self.f.read(self.image_size) |
|
||||||
return raw |
|
||||||
|
|
||||||
def read_frames(self, range_start, range_len): |
|
||||||
self.f.seek(self.image_size*range_start) |
|
||||||
raw = self.f.read(self.image_size*range_len) |
|
||||||
return raw |
|
||||||
|
|
||||||
def read_frames_into(self, range_start, buf): |
|
||||||
self.f.seek(self.image_size*range_start) |
|
||||||
return self.f.readinto(buf) |
|
||||||
|
|
||||||
def read(self, frame): |
|
||||||
return yuv420_to_rgb(self.read_frame(frame), self.image_dim) |
|
||||||
|
|
||||||
def close(self): |
|
||||||
self.f.close() |
|
||||||
|
|
||||||
def __enter__(self): |
|
||||||
return self |
|
||||||
|
|
||||||
def __exit__(self, type, value, traceback): |
|
||||||
self.close() |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
class OneReader(): |
|
||||||
def __init__(self, base_path, goofy=False, segment_range=None): |
|
||||||
self.base_path = base_path |
|
||||||
|
|
||||||
route_name = os.path.basename(base_path) |
|
||||||
|
|
||||||
self.rcamera_size = (304, 560) |
|
||||||
|
|
||||||
if segment_range is None: |
|
||||||
parent_path = os.path.dirname(base_path) |
|
||||||
self.segment_nums = [] |
|
||||||
for p in os.listdir(parent_path): |
|
||||||
if not p.startswith(route_name+"--"): |
|
||||||
continue |
|
||||||
self.segment_nums.append(int(p.rsplit("--", 1)[-1])) |
|
||||||
if not self.segment_nums: |
|
||||||
raise Exception("no route segments found") |
|
||||||
self.segment_nums.sort() |
|
||||||
self.segment_range = (self.segment_nums[0], self.segment_nums[-1]) |
|
||||||
else: |
|
||||||
self.segment_range = segment_range |
|
||||||
self.segment_nums = range(segment_range[0], segment_range[1]+1) |
|
||||||
for i in self.segment_nums: |
|
||||||
if not os.path.exists(base_path+"--"+str(i)): |
|
||||||
raise Exception("missing segment in provided range") |
|
||||||
|
|
||||||
# goofy data is broken with discontinuous logs |
|
||||||
if goofy and (self.segment_range[0] != 0 |
|
||||||
or self.segment_nums != range(self.segment_range[0], self.segment_range[1]+1)): |
|
||||||
raise Exception("goofy data needs all the segments for a route") |
|
||||||
|
|
||||||
self.cur_seg = None |
|
||||||
self.cur_seg_f = None |
|
||||||
|
|
||||||
# index the frames |
|
||||||
print("indexing frames {}...".format(self.segment_nums)) |
|
||||||
|
|
||||||
self.rcamera_encode_map = {} # frame_id -> (segment num, segment id, frame_time) |
|
||||||
last_frame_id = -1 |
|
||||||
|
|
||||||
if goofy: |
|
||||||
# goofy is goofy |
|
||||||
|
|
||||||
frame_size = self.rcamera_size[0]*self.rcamera_size[1]*3/2 |
|
||||||
|
|
||||||
# find the encode id ranges for each segment by using the rcamera file size |
|
||||||
segment_encode_ids = [] |
|
||||||
cur_encode_id = 0 |
|
||||||
for n in self.segment_nums: |
|
||||||
camera_path = os.path.join(self.seg_path(n), "rcamera") |
|
||||||
if not os.path.exists(camera_path): |
|
||||||
# for goofy, missing camera files means a bad route |
|
||||||
raise Exception("Missing camera file {}".format(camera_path)) |
|
||||||
camera_size = os.path.getsize(camera_path) |
|
||||||
assert (camera_size % frame_size) == 0 |
|
||||||
|
|
||||||
num_frames = camera_size / frame_size |
|
||||||
segment_encode_ids.append(cur_encode_id) |
|
||||||
cur_encode_id += num_frames |
|
||||||
|
|
||||||
last_encode_id = -1 |
|
||||||
# use the segment encode id map and frame events to build the frame index |
|
||||||
for n in self.segment_nums: |
|
||||||
log_path = os.path.join(self.seg_path(n), "rlog") |
|
||||||
if os.path.exists(log_path): |
|
||||||
with open(log_path, "rb") as f: |
|
||||||
for evt in capnp_log.Event.read_multiple(f): |
|
||||||
if evt.which() == 'frame': |
|
||||||
|
|
||||||
if evt.frame.frameId < last_frame_id: |
|
||||||
# a non-increasing frame id is bad route (eg visiond was restarted) |
|
||||||
raise Exception("non-increasing frame id") |
|
||||||
last_frame_id = evt.frame.frameId |
|
||||||
|
|
||||||
seg_i = bisect.bisect_right(segment_encode_ids, evt.frame.encodeId)-1 |
|
||||||
assert seg_i >= 0 |
|
||||||
seg_num = self.segment_nums[seg_i] |
|
||||||
seg_id = evt.frame.encodeId-segment_encode_ids[seg_i] |
|
||||||
frame_time = evt.logMonoTime / 1.0e9 |
|
||||||
|
|
||||||
self.rcamera_encode_map[evt.frame.frameId] = (seg_num, seg_id, |
|
||||||
frame_time) |
|
||||||
|
|
||||||
last_encode_id = evt.frame.encodeId |
|
||||||
|
|
||||||
if last_encode_id-cur_encode_id > 10: |
|
||||||
# too many missing frames is a bad route (eg route from before encoder rotating worked) |
|
||||||
raise Exception("goofy route is missing frames: {}, {}".format( |
|
||||||
last_encode_id, cur_encode_id)) |
|
||||||
|
|
||||||
else: |
|
||||||
# for harry data, build the index from encodeIdx events |
|
||||||
for n in self.segment_nums: |
|
||||||
log_path = os.path.join(self.seg_path(n), "rlog") |
|
||||||
if os.path.exists(log_path): |
|
||||||
with open(log_path, "rb") as f: |
|
||||||
for evt in capnp_log.Event.read_multiple(f): |
|
||||||
if evt.which() == 'encodeIdx' and evt.encodeIdx.type == 'bigBoxLossless': |
|
||||||
frame_time = evt.logMonoTime / 1.0e9 |
|
||||||
self.rcamera_encode_map[evt.encodeIdx.frameId] = ( |
|
||||||
evt.encodeIdx.segmentNum, evt.encodeIdx.segmentId, |
|
||||||
frame_time) |
|
||||||
|
|
||||||
print("done") |
|
||||||
|
|
||||||
# read the first event to find the start time |
|
||||||
self.reset_to_seg(self.segment_range[0]) |
|
||||||
for evt in self.events(): |
|
||||||
if evt.which() != 'initData': |
|
||||||
self.start_mono = evt.logMonoTime |
|
||||||
break |
|
||||||
self.reset_to_seg(self.segment_range[0]) |
|
||||||
|
|
||||||
|
|
||||||
def seg_path(self, num): |
|
||||||
return self.base_path+"--"+str(num) |
|
||||||
|
|
||||||
def reset_to_seg(self, seg): |
|
||||||
self.cur_seg = seg |
|
||||||
if self.cur_seg_f: |
|
||||||
self.cur_seg_f.close() |
|
||||||
self.cur_seg_f = None |
|
||||||
|
|
||||||
def seek_ts(self, ts): |
|
||||||
seek_seg = int(ts/60) |
|
||||||
if seek_seg < self.segment_range[0] or seek_seg > self.segment_range[1]: |
|
||||||
raise ValueError |
|
||||||
|
|
||||||
self.reset_to_seg(seek_seg) |
|
||||||
target_mono = self.start_mono + int(ts*1e9) |
|
||||||
for evt in self.events(): |
|
||||||
if evt.logMonoTime >= target_mono: |
|
||||||
break |
|
||||||
|
|
||||||
def read_event(self): |
|
||||||
while True: |
|
||||||
if self.cur_seg > self.segment_range[1]: |
|
||||||
return None |
|
||||||
if self.cur_seg_f is None: |
|
||||||
log_path = os.path.join(self.seg_path(self.cur_seg), "rlog") |
|
||||||
if not os.path.exists(log_path): |
|
||||||
print("missing log file!", log_path) |
|
||||||
self.cur_seg += 1 |
|
||||||
continue |
|
||||||
self.cur_seg_f = open(log_path, "rb") |
|
||||||
|
|
||||||
try: |
|
||||||
return capnp_log.Event.read(self.cur_seg_f) |
|
||||||
except capnp.lib.capnp.KjException as e: |
|
||||||
if 'EOF' in str(e): # dumb, but pycapnp does this too |
|
||||||
self.cur_seg_f.close() |
|
||||||
self.cur_seg_f = None |
|
||||||
self.cur_seg += 1 |
|
||||||
else: |
|
||||||
raise |
|
||||||
|
|
||||||
def events(self): |
|
||||||
while True: |
|
||||||
r = self.read_event() |
|
||||||
if r is None: |
|
||||||
break |
|
||||||
yield r |
|
||||||
|
|
||||||
def read_frame(self, frame_id): |
|
||||||
encode_idx = self.rcamera_encode_map.get(frame_id) |
|
||||||
if encode_idx is None: |
|
||||||
return None |
|
||||||
|
|
||||||
seg_num, seg_id, _ = encode_idx |
|
||||||
camera_path = os.path.join(self.seg_path(seg_num), "rcamera") |
|
||||||
if not os.path.exists(camera_path): |
|
||||||
return None |
|
||||||
with YuvData(camera_path, self.rcamera_size) as data: |
|
||||||
return data.read_frame(seg_id) |
|
||||||
|
|
||||||
def close(self): |
|
||||||
if self.cur_seg_f is not None: |
|
||||||
self.cur_seg_f.close() |
|
||||||
|
|
||||||
def __enter__(self): |
|
||||||
return self |
|
||||||
|
|
||||||
def __exit__(self, type, value, traceback): |
|
||||||
self.close() |
|
||||||
@ -1,66 +0,0 @@ |
|||||||
import bisect |
|
||||||
import numpy as np |
|
||||||
from scipy.interpolate import interp1d |
|
||||||
|
|
||||||
|
|
||||||
def deep_interp_0_fast(dx, x, y): |
|
||||||
FIX = False |
|
||||||
if len(y.shape) == 1: |
|
||||||
y = y[:, None] |
|
||||||
FIX = True |
|
||||||
ret = np.zeros((dx.shape[0], y.shape[1])) |
|
||||||
index = list(x) |
|
||||||
for i in range(dx.shape[0]): |
|
||||||
idx = bisect.bisect_left(index, dx[i]) |
|
||||||
if idx == x.shape[0]: |
|
||||||
idx = x.shape[0] - 1 |
|
||||||
ret[i] = y[idx] |
|
||||||
|
|
||||||
if FIX: |
|
||||||
return ret[:, 0] |
|
||||||
else: |
|
||||||
return ret |
|
||||||
|
|
||||||
|
|
||||||
def running_mean(x, N): |
|
||||||
cumsum = np.cumsum(np.insert(x, [0]*(int(N/2)) + [-1]*(N-int(N/2)), [x[0]]*int(N/2) + [x[-1]]*(N-int(N/2)))) |
|
||||||
return (cumsum[N:] - cumsum[:-N]) / N |
|
||||||
|
|
||||||
|
|
||||||
def deep_interp_np(x, xp, fp): |
|
||||||
x = np.atleast_1d(x) |
|
||||||
xp = np.array(xp) |
|
||||||
if len(xp) < 2: |
|
||||||
return np.repeat(fp, len(x), axis=0) |
|
||||||
if min(np.diff(xp)) < 0: |
|
||||||
raise RuntimeError('Bad x array for interpolation') |
|
||||||
j = np.searchsorted(xp, x) - 1 |
|
||||||
j = np.clip(j, 0, len(xp)-2) |
|
||||||
d = np.divide(x - xp[j], xp[j + 1] - xp[j], out=np.ones_like(x, dtype=np.float64), where=xp[j + 1] - xp[j] != 0) |
|
||||||
vals_interp = (fp[j].T*(1 - d)).T + (fp[j + 1].T*d).T |
|
||||||
if len(vals_interp) == 1: |
|
||||||
return vals_interp[0] |
|
||||||
else: |
|
||||||
return vals_interp |
|
||||||
|
|
||||||
|
|
||||||
def clipping_deep_interp(x, xp, fp): |
|
||||||
if len(xp) < 2: |
|
||||||
return deep_interp_np(x, xp, fp) |
|
||||||
bad_idx = np.where(np.diff(xp) < 0)[0] |
|
||||||
if len(bad_idx) > 0: |
|
||||||
if bad_idx[0] ==1: |
|
||||||
return np.zeros([] + list(fp.shape[1:])) |
|
||||||
return deep_interp_np(x, xp[:bad_idx[0]], fp[:bad_idx[0]]) |
|
||||||
else: |
|
||||||
return deep_interp_np(x, xp, fp) |
|
||||||
|
|
||||||
|
|
||||||
def deep_interp(dx, x, y, kind="slinear"): |
|
||||||
return interp1d( |
|
||||||
x, y, |
|
||||||
axis=0, |
|
||||||
kind=kind, |
|
||||||
bounds_error=False, |
|
||||||
fill_value="extrapolate", |
|
||||||
assume_sorted=True)(dx) |
|
||||||
@ -1,78 +0,0 @@ |
|||||||
import os |
|
||||||
import numpy as np |
|
||||||
import random |
|
||||||
|
|
||||||
class SamplingBuffer(): |
|
||||||
def __init__(self, fn, size, write=False): |
|
||||||
self._fn = fn |
|
||||||
self._write = write |
|
||||||
if self._write: |
|
||||||
self._f = open(self._fn, "ab") |
|
||||||
else: |
|
||||||
self._f = open(self._fn, "rb") |
|
||||||
self._size = size |
|
||||||
self._refresh() |
|
||||||
|
|
||||||
def _refresh(self): |
|
||||||
self.cnt = os.path.getsize(self._fn) / self._size |
|
||||||
|
|
||||||
@property |
|
||||||
def count(self): |
|
||||||
self._refresh() |
|
||||||
return self.cnt |
|
||||||
|
|
||||||
def _fetch_one(self, x): |
|
||||||
assert self._write == False |
|
||||||
self._f.seek(x*self._size) |
|
||||||
return self._f.read(self._size) |
|
||||||
|
|
||||||
def sample(self, count, indices = None): |
|
||||||
if indices == None: |
|
||||||
cnt = self.count |
|
||||||
assert cnt != 0 |
|
||||||
indices = map(lambda x: random.randint(0, cnt-1), range(count)) |
|
||||||
return map(self._fetch_one, indices) |
|
||||||
|
|
||||||
def write(self, dat): |
|
||||||
assert self._write == True |
|
||||||
assert (len(dat) % self._size) == 0 |
|
||||||
self._f.write(dat) |
|
||||||
self._f.flush() |
|
||||||
|
|
||||||
class NumpySamplingBuffer(): |
|
||||||
def __init__(self, fn, size, dtype, write=False): |
|
||||||
self._size = size |
|
||||||
self._dtype = dtype |
|
||||||
self._buf = SamplingBuffer(fn, len(np.zeros(size, dtype=dtype).tobytes()), write) |
|
||||||
|
|
||||||
@property |
|
||||||
def count(self): |
|
||||||
return self._buf.count |
|
||||||
|
|
||||||
def write(self, dat): |
|
||||||
self._buf.write(dat.tobytes()) |
|
||||||
|
|
||||||
def sample(self, count, indices = None): |
|
||||||
return np.fromstring(''.join(self._buf.sample(count, indices)), dtype=self._dtype).reshape([count]+list(self._size)) |
|
||||||
|
|
||||||
# TODO: n IOPS needed where n is the Multi |
|
||||||
class MultiNumpySamplingBuffer(): |
|
||||||
def __init__(self, fn, npa, write=False): |
|
||||||
self._bufs = [] |
|
||||||
for i,n in enumerate(npa): |
|
||||||
self._bufs.append(NumpySamplingBuffer(fn + ("_%d" % i), n[0], n[1], write)) |
|
||||||
|
|
||||||
def write(self, dat): |
|
||||||
for b,x in zip(self._bufs, dat): |
|
||||||
b.write(x) |
|
||||||
|
|
||||||
@property |
|
||||||
def count(self): |
|
||||||
return min(map(lambda x: x.count, self._bufs)) |
|
||||||
|
|
||||||
def sample(self, count): |
|
||||||
cnt = self.count |
|
||||||
assert cnt != 0 |
|
||||||
indices = map(lambda x: random.randint(0, cnt-1), range(count)) |
|
||||||
return map(lambda x: x.sample(count, indices), self._bufs) |
|
||||||
|
|
||||||
@ -1,94 +0,0 @@ |
|||||||
import numpy as np |
|
||||||
|
|
||||||
_DESC_FMT = """ |
|
||||||
{} (n={}): |
|
||||||
MEAN={} |
|
||||||
VAR={} |
|
||||||
MIN={} |
|
||||||
MAX={} |
|
||||||
""" |
|
||||||
|
|
||||||
class StatTracker(): |
|
||||||
def __init__(self, name): |
|
||||||
self._name = name |
|
||||||
self._mean = 0. |
|
||||||
self._var = 0. |
|
||||||
self._n = 0 |
|
||||||
self._min = -float("-inf") |
|
||||||
self._max = -float("inf") |
|
||||||
|
|
||||||
@property |
|
||||||
def mean(self): |
|
||||||
return self._mean |
|
||||||
|
|
||||||
@property |
|
||||||
def var(self): |
|
||||||
return (self._n * self._var) / (self._n - 1.) |
|
||||||
|
|
||||||
@property |
|
||||||
def min(self): |
|
||||||
return self._min |
|
||||||
|
|
||||||
@property |
|
||||||
def max(self): |
|
||||||
return self._max |
|
||||||
|
|
||||||
def update(self, samples): |
|
||||||
# https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Parallel_algorithm |
|
||||||
data = samples.reshape(-1) |
|
||||||
n_a = data.size |
|
||||||
mean_a = np.mean(data) |
|
||||||
var_a = np.var(data, ddof=0) |
|
||||||
|
|
||||||
n_b = self._n |
|
||||||
mean_b = self._mean |
|
||||||
|
|
||||||
delta = mean_b - mean_a |
|
||||||
m_a = var_a * (n_a - 1) |
|
||||||
m_b = self._var * (n_b - 1) |
|
||||||
m2 = m_a + m_b + delta**2 * n_a * n_b / (n_a + n_b) |
|
||||||
|
|
||||||
self._var = m2 / (n_a + n_b) |
|
||||||
self._mean = (n_a * mean_a + n_b * mean_b) / (n_a + n_b) |
|
||||||
self._n = n_a + n_b |
|
||||||
|
|
||||||
self._min = min(self._min, np.min(data)) |
|
||||||
self._max = max(self._max, np.max(data)) |
|
||||||
|
|
||||||
def __str__(self): |
|
||||||
return _DESC_FMT.format(self._name, self._n, self._mean, self.var, self._min, |
|
||||||
self._max) |
|
||||||
|
|
||||||
# FIXME(mgraczyk): The variance computation does not work with 1 sample batches. |
|
||||||
class VectorStatTracker(StatTracker): |
|
||||||
def __init__(self, name, dim): |
|
||||||
self._name = name |
|
||||||
self._mean = np.zeros((dim, )) |
|
||||||
self._var = np.zeros((dim, dim)) |
|
||||||
self._n = 0 |
|
||||||
self._min = np.full((dim, ), -float("-inf")) |
|
||||||
self._max = np.full((dim, ), -float("inf")) |
|
||||||
|
|
||||||
@property |
|
||||||
def cov(self): |
|
||||||
return self.var |
|
||||||
|
|
||||||
def update(self, samples): |
|
||||||
n_a = samples.shape[0] |
|
||||||
mean_a = np.mean(samples, axis=0) |
|
||||||
var_a = np.cov(samples, ddof=0, rowvar=False) |
|
||||||
|
|
||||||
n_b = self._n |
|
||||||
mean_b = self._mean |
|
||||||
|
|
||||||
delta = mean_b - mean_a |
|
||||||
m_a = var_a * (n_a - 1) |
|
||||||
m_b = self._var * (n_b - 1) |
|
||||||
m2 = m_a + m_b + delta**2 * n_a * n_b / (n_a + n_b) |
|
||||||
|
|
||||||
self._var = m2 / (n_a + n_b) |
|
||||||
self._mean = (n_a * mean_a + n_b * mean_b) / (n_a + n_b) |
|
||||||
self._n = n_a + n_b |
|
||||||
|
|
||||||
self._min = np.minimum(self._min, np.min(samples, axis=0)) |
|
||||||
self._max = np.maximum(self._max, np.max(samples, axis=0)) |
|
||||||
@ -1,113 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import os |
|
||||||
|
|
||||||
from common.basedir import BASEDIR |
|
||||||
os.environ['BASEDIR'] = BASEDIR |
|
||||||
SCALE = 3 |
|
||||||
|
|
||||||
import argparse |
|
||||||
import zmq |
|
||||||
import pygame |
|
||||||
import numpy as np |
|
||||||
import cv2 |
|
||||||
import sys |
|
||||||
import traceback |
|
||||||
from collections import namedtuple |
|
||||||
|
|
||||||
from cereal import car |
|
||||||
from common.params import Params |
|
||||||
from common.lazy_property import lazy_property |
|
||||||
from cereal.messaging import sub_sock, recv_one_or_none, recv_one |
|
||||||
from cereal.services import service_list |
|
||||||
|
|
||||||
_BB_OFFSET = 290, 332 |
|
||||||
_BB_TO_FULL_FRAME = np.asarray([[1., 0., _BB_OFFSET[0]], [0., 1., _BB_OFFSET[1]], |
|
||||||
[0., 0., 1.]]) |
|
||||||
_FULL_FRAME_TO_BB = np.linalg.inv(_BB_TO_FULL_FRAME) |
|
||||||
_FULL_FRAME_SIZE = 1164, 874 |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def pygame_modules_have_loaded(): |
|
||||||
return pygame.display.get_init() and pygame.font.get_init() |
|
||||||
|
|
||||||
|
|
||||||
def ui_thread(addr, frame_address): |
|
||||||
context = zmq.Context() |
|
||||||
|
|
||||||
pygame.init() |
|
||||||
pygame.font.init() |
|
||||||
assert pygame_modules_have_loaded() |
|
||||||
|
|
||||||
size = (640 * SCALE, 480 * SCALE) |
|
||||||
pygame.display.set_caption("comma one debug UI") |
|
||||||
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF) |
|
||||||
|
|
||||||
camera_surface = pygame.surface.Surface((640 * SCALE, 480 * SCALE), 0, 24).convert() |
|
||||||
|
|
||||||
frame = context.socket(zmq.SUB) |
|
||||||
frame.connect(frame_address or "tcp://%s:%d" % (addr, 'frame')) |
|
||||||
frame.setsockopt(zmq.SUBSCRIBE, "") |
|
||||||
|
|
||||||
img = np.zeros((480, 640, 3), dtype='uint8') |
|
||||||
imgff = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype=np.uint8) |
|
||||||
|
|
||||||
while 1: |
|
||||||
list(pygame.event.get()) |
|
||||||
screen.fill((64, 64, 64)) |
|
||||||
|
|
||||||
# ***** frame ***** |
|
||||||
fpkt = recv_one(frame) |
|
||||||
yuv_img = fpkt.frame.image |
|
||||||
|
|
||||||
if fpkt.frame.transform: |
|
||||||
yuv_transform = np.array(fpkt.frame.transform).reshape(3, 3) |
|
||||||
else: |
|
||||||
# assume frame is flipped |
|
||||||
yuv_transform = np.array([[-1.0, 0.0, _FULL_FRAME_SIZE[0] - 1], |
|
||||||
[0.0, -1.0, _FULL_FRAME_SIZE[1] - 1], [0.0, 0.0, 1.0]]) |
|
||||||
|
|
||||||
if yuv_img and len(yuv_img) == _FULL_FRAME_SIZE[0] * _FULL_FRAME_SIZE[1] * 3 // 2: |
|
||||||
yuv_np = np.frombuffer( |
|
||||||
yuv_img, dtype=np.uint8).reshape(_FULL_FRAME_SIZE[1] * 3 // 2, -1) |
|
||||||
cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff) |
|
||||||
cv2.warpAffine( |
|
||||||
imgff, |
|
||||||
np.dot(yuv_transform, _BB_TO_FULL_FRAME)[:2], (img.shape[1], img.shape[0]), |
|
||||||
dst=img, |
|
||||||
flags=cv2.WARP_INVERSE_MAP) |
|
||||||
else: |
|
||||||
img.fill(0) |
|
||||||
|
|
||||||
height, width = img.shape[:2] |
|
||||||
img_resized = cv2.resize( |
|
||||||
img, (SCALE * width, SCALE * height), interpolation=cv2.INTER_CUBIC) |
|
||||||
# *** blits *** |
|
||||||
pygame.surfarray.blit_array(camera_surface, img_resized.swapaxes(0, 1)) |
|
||||||
screen.blit(camera_surface, (0, 0)) |
|
||||||
|
|
||||||
# this takes time...vsync or something |
|
||||||
pygame.display.flip() |
|
||||||
|
|
||||||
|
|
||||||
def get_arg_parser(): |
|
||||||
parser = argparse.ArgumentParser( |
|
||||||
description="Show replay data in a UI.", |
|
||||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter) |
|
||||||
|
|
||||||
parser.add_argument( |
|
||||||
"ip_address", |
|
||||||
nargs="?", |
|
||||||
default="127.0.0.1", |
|
||||||
help="The ip address on which to receive zmq messages.") |
|
||||||
|
|
||||||
parser.add_argument( |
|
||||||
"--frame-address", |
|
||||||
default=None, |
|
||||||
help="The ip address on which to receive zmq messages.") |
|
||||||
return parser |
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
args = get_arg_parser().parse_args(sys.argv[1:]) |
|
||||||
ui_thread(args.ip_address, args.frame_address) |
|
||||||
@ -1,98 +0,0 @@ |
|||||||
#!/usr/bin/env python |
|
||||||
from common.kalman.ned import ecef2geodetic |
|
||||||
|
|
||||||
import csv |
|
||||||
import numpy as np |
|
||||||
import webbrowser |
|
||||||
import os |
|
||||||
import sys |
|
||||||
import json |
|
||||||
import numpy.linalg as LA |
|
||||||
import gmplot |
|
||||||
from dateutil.parser import parse |
|
||||||
from common.numpy_helpers import deep_interp |
|
||||||
# import cvxpy as cvx |
|
||||||
MPH_TO_MS = 0.44704 |
|
||||||
|
|
||||||
|
|
||||||
def downsample(positions, speeds, start_idx, end_idx, dist): |
|
||||||
# TODO: save headings too |
|
||||||
track = [] |
|
||||||
last_position = positions[start_idx] |
|
||||||
valid_indeces = [] |
|
||||||
track_speeds = [] |
|
||||||
for pi in range(start_idx, end_idx): |
|
||||||
# only save points that are at least 10 cm far away |
|
||||||
if LA.norm(positions[pi] - last_position) >= dist: |
|
||||||
#print LA.norm(positions[pi] - last_position) |
|
||||||
last_position = positions[pi] |
|
||||||
track.append(positions[pi]) |
|
||||||
valid_indeces.append(pi) |
|
||||||
track_speeds.append(speeds[pi]) |
|
||||||
print(-start_idx + end_idx, len(valid_indeces)) |
|
||||||
# this compare the original point count vs the filtered count |
|
||||||
|
|
||||||
track = np.array(track) |
|
||||||
track_speeds = np.array(track_speeds) |
|
||||||
return track, track_speeds |
|
||||||
|
|
||||||
def converter(date): |
|
||||||
|
|
||||||
filename = "/home/batman/one/selfdrive/locationd/liveloc_dumps/" + date + "/canonical.csv" # Point one (OK!) |
|
||||||
|
|
||||||
c = csv.DictReader(open(filename, 'rb'), delimiter=',') |
|
||||||
|
|
||||||
start_time = None |
|
||||||
|
|
||||||
t = [] |
|
||||||
ll_positions = [] |
|
||||||
positions = [] |
|
||||||
sats = [] |
|
||||||
flag = [] |
|
||||||
speeds = [] |
|
||||||
|
|
||||||
for row in c: |
|
||||||
t.append(float(row['pctime'])) |
|
||||||
x = float(row['ecefX']) |
|
||||||
y = float(row['ecefY']) |
|
||||||
z = float(row['ecefZ']) |
|
||||||
ecef = np.array((x, y, z)) |
|
||||||
speeds.append(float(row['velSpeed'])) |
|
||||||
|
|
||||||
pos = ecef2geodetic(ecef) |
|
||||||
ll_positions.append(pos) |
|
||||||
positions.append(ecef) |
|
||||||
|
|
||||||
t = np.array(t) |
|
||||||
ll_positions = np.array(ll_positions) |
|
||||||
positions = np.array(positions) |
|
||||||
|
|
||||||
#distances = ll_positions[:,0:2] - START_POS[:2] |
|
||||||
#i_start = np.argmin(LA.norm(distances, axis=1)) |
|
||||||
|
|
||||||
#for i in range(i_start + 500): |
|
||||||
# distances[i] += np.array([100, 100]) |
|
||||||
#i_end = np.argmin(LA.norm(distances, axis=1)) |
|
||||||
|
|
||||||
i_start = 0 |
|
||||||
i_end = len(positions) |
|
||||||
|
|
||||||
print(i_start, i_end) |
|
||||||
track, track_speeds = downsample(positions, speeds, i_start, i_end, 0.2) |
|
||||||
ll_track = np.array([ecef2geodetic(pos) for pos in track]) |
|
||||||
|
|
||||||
track_struct = {} |
|
||||||
print(track_speeds.shape) |
|
||||||
print(track.shape) |
|
||||||
track_struct['race'] = np.hstack((track, |
|
||||||
np.expand_dims(track_speeds, axis=1), |
|
||||||
np.zeros((len(track_speeds), 1)))) |
|
||||||
|
|
||||||
f = open('/home/batman/one/selfdrive/controls/tracks/loop_city.npy', 'w') |
|
||||||
np.save(f, track_struct) |
|
||||||
f.close() |
|
||||||
print("SAVED!") |
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
converter(sys.argv[1]) |
|
||||||
@ -1,113 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import os |
|
||||||
import zmq |
|
||||||
|
|
||||||
import cereal.messaging as messaging |
|
||||||
from cereal.services import service_list |
|
||||||
|
|
||||||
from panda.lib.panda import Panda |
|
||||||
from hexdump import hexdump |
|
||||||
import time |
|
||||||
|
|
||||||
def raw_panda(): |
|
||||||
p = Panda() |
|
||||||
print(p) |
|
||||||
|
|
||||||
p.set_uart_baud(2, 9600) |
|
||||||
p.set_uart_baud(3, 9600) |
|
||||||
|
|
||||||
p.set_uart_parity(2, 1) |
|
||||||
p.set_uart_parity(3, 1) |
|
||||||
|
|
||||||
p.set_uart_callback(2, 1) |
|
||||||
p.set_uart_callback(3, 1) |
|
||||||
|
|
||||||
idx = 0 |
|
||||||
while 1: |
|
||||||
""" |
|
||||||
dat = p.serial_read(2) |
|
||||||
if len(dat) > 0: |
|
||||||
print "2:", |
|
||||||
hexdump(dat) |
|
||||||
|
|
||||||
dat = p.serial_read(3) |
|
||||||
if len(dat) > 0: |
|
||||||
print "3:", |
|
||||||
hexdump(dat) |
|
||||||
|
|
||||||
print "read done, waiting" |
|
||||||
time.sleep(0.01) |
|
||||||
""" |
|
||||||
|
|
||||||
if idx%2 == 1: |
|
||||||
dat = "\x20\x80\xc0\xa0" |
|
||||||
else: |
|
||||||
dat = "\x00\x80\xc0\xc0" |
|
||||||
p.can_send(0, dat, 8) |
|
||||||
|
|
||||||
for r in p.can_recv(): |
|
||||||
if r[-1] in [8, 9]: |
|
||||||
print(r[-1], r[2].encode("hex")) |
|
||||||
|
|
||||||
time.sleep(0.01) |
|
||||||
idx += 1 |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
#raw_panda() |
|
||||||
#exit(0) |
|
||||||
|
|
||||||
logcan = messaging.sub_sock('can') |
|
||||||
|
|
||||||
t1 = [] |
|
||||||
t2 = [] |
|
||||||
t3 = [] |
|
||||||
|
|
||||||
while len(t1) < 1000 or os.uname()[-1] == "aarch64": |
|
||||||
rr = messaging.recv_sock(logcan, wait=True) |
|
||||||
for c in rr.can: |
|
||||||
if c.src in [9] and len(c.dat) == 5: |
|
||||||
aa = map(lambda x: ord(x)&0x7f, c.dat) |
|
||||||
|
|
||||||
# checksum |
|
||||||
assert (-(aa[0]+aa[1]+aa[2]+aa[3]))&0x7f == aa[4] |
|
||||||
|
|
||||||
#print map(bin, aa[0:4]) |
|
||||||
|
|
||||||
aa[0] &= ~0x20 |
|
||||||
aa[1] &= ~0x20 |
|
||||||
|
|
||||||
st = (aa[0] << 5) + aa[1] |
|
||||||
if st >= 256: |
|
||||||
st = -(512-st) |
|
||||||
|
|
||||||
mt = ((aa[2] >> 3) << 7) + aa[3] |
|
||||||
if mt >= 512: |
|
||||||
mt = -(1024-mt) |
|
||||||
|
|
||||||
print(st, mt) |
|
||||||
t1.append(st) |
|
||||||
t2.append(mt) |
|
||||||
#print map(bin, aa), "apply", st |
|
||||||
|
|
||||||
if c.src in [8] and len(c.dat) == 4: |
|
||||||
aa = map(lambda x: ord(x)&0x7f, c.dat) |
|
||||||
|
|
||||||
# checksum |
|
||||||
assert (-(aa[0]+aa[1]+aa[2]))&0x7f == aa[3] |
|
||||||
|
|
||||||
aa[0] &= ~0x20 |
|
||||||
aa[1] &= ~0x20 |
|
||||||
|
|
||||||
st = (aa[0] << 5) + aa[1] |
|
||||||
if st >= 256: |
|
||||||
st = -(512-st) |
|
||||||
print(aa, "apply", st) |
|
||||||
|
|
||||||
t3.append(st) |
|
||||||
|
|
||||||
import matplotlib.pyplot as plt |
|
||||||
plt.plot(t1) |
|
||||||
plt.plot(t2) |
|
||||||
plt.plot(t3) |
|
||||||
plt.show() |
|
||||||
|
|
||||||
@ -1,79 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
|
|
||||||
import argparse |
|
||||||
import time |
|
||||||
import os |
|
||||||
|
|
||||||
from tqdm import tqdm |
|
||||||
|
|
||||||
from cereal.messaging import PubMaster, recv_one, sub_sock |
|
||||||
from cereal.services import service_list |
|
||||||
from tools.lib.logreader import LogReader |
|
||||||
from xx.chffr.lib.route import Route, RouteSegment |
|
||||||
from tools.lib.route_framereader import RouteFrameReader |
|
||||||
from xx.uncommon.column_store import save_dict_as_column_store |
|
||||||
from xx.pipeline.lib.log_time_series import append_dict |
|
||||||
from selfdrive.test.process_replay.compare_logs import save_log |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
parser = argparse.ArgumentParser(description="Run visiond on segment") |
|
||||||
parser.add_argument("segment_name", help="The segment to run") |
|
||||||
parser.add_argument("output_path", help="The output file") |
|
||||||
|
|
||||||
args = parser.parse_args() |
|
||||||
segment = RouteSegment.from_canonical_name(args.segment_name) |
|
||||||
route = Route(segment._name._route_name) |
|
||||||
|
|
||||||
frame_id_lookup = {} |
|
||||||
frame_reader = RouteFrameReader(route.camera_paths(), None, frame_id_lookup, readahead=True) |
|
||||||
|
|
||||||
msgs = list(LogReader(segment.log_path)) |
|
||||||
|
|
||||||
pm = PubMaster(['liveCalibration', 'frame']) |
|
||||||
model_sock = sub_sock('model') |
|
||||||
|
|
||||||
# Read encodeIdx |
|
||||||
for msg in msgs: |
|
||||||
if msg.which() == 'encodeIdx': |
|
||||||
frame_id_lookup[msg.encodeIdx.frameId] = (msg.encodeIdx.segmentNum, msg.encodeIdx.segmentId) |
|
||||||
|
|
||||||
# Send some livecalibration messages to initalize visiond |
|
||||||
for msg in msgs: |
|
||||||
if msg.which() == 'liveCalibration': |
|
||||||
pm.send('liveCalibration', msg.as_builder()) |
|
||||||
|
|
||||||
time.sleep(1.0) |
|
||||||
values = {} |
|
||||||
|
|
||||||
out_msgs = [] |
|
||||||
for msg in tqdm(msgs): |
|
||||||
w = msg.which() |
|
||||||
|
|
||||||
if w == 'liveCalibration': |
|
||||||
pm.send(w, msg.as_builder()) |
|
||||||
|
|
||||||
if w == 'frame': |
|
||||||
msg = msg.as_builder() |
|
||||||
|
|
||||||
frame_id = msg.frame.frameId |
|
||||||
img = frame_reader.get(frame_id, pix_fmt="rgb24")[:,:,::-1] |
|
||||||
|
|
||||||
msg.frame.image = img.flatten().tobytes() |
|
||||||
pm.send(w, msg) |
|
||||||
|
|
||||||
model = recv_one(model_sock) |
|
||||||
model = model.as_builder() |
|
||||||
model.logMonoTime = 0 |
|
||||||
model = model.as_reader() |
|
||||||
out_msgs.append(model) |
|
||||||
|
|
||||||
save_log(args.output_path, out_msgs) |
|
||||||
|
|
||||||
# tm = model.logMonoTime / 1.0e9 |
|
||||||
# model = model.model |
|
||||||
# append_dict("model/data/path", tm, model.path.to_dict(), values) |
|
||||||
# append_dict("model/data/left_lane", tm, model.leftLane.to_dict(), values) |
|
||||||
# append_dict("model/data/right_lane", tm, model.rightLane.to_dict(), values) |
|
||||||
# append_dict("model/data/lead", tm, model.lead.to_dict(), values) |
|
||||||
|
|
||||||
# save_dict_as_column_store(values, os.path.join(args.output_path, "LiveVisionD", args.segment_name)) |
|
||||||
@ -1,68 +0,0 @@ |
|||||||
#!/usr/bin/env python |
|
||||||
|
|
||||||
import matplotlib |
|
||||||
matplotlib.use('TkAgg') |
|
||||||
import matplotlib.pyplot as plt |
|
||||||
|
|
||||||
import numpy as np |
|
||||||
import zmq |
|
||||||
from cereal.services import service_list |
|
||||||
from selfdrive.config import Conversions as CV |
|
||||||
import cereal.messaging as messaging |
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
live_map_sock = messaging.sub_sock(service_list['liveMapData'].port, conflate=True) |
|
||||||
plan_sock = messaging.sub_sock(service_list['plan'].port, conflate=True) |
|
||||||
|
|
||||||
plt.ion() |
|
||||||
fig = plt.figure(figsize=(8, 16)) |
|
||||||
ax = fig.add_subplot(2, 1, 1) |
|
||||||
ax.set_title('Map') |
|
||||||
|
|
||||||
SCALE = 1000 |
|
||||||
ax.set_xlim([-SCALE, SCALE]) |
|
||||||
ax.set_ylim([-SCALE, SCALE]) |
|
||||||
ax.set_xlabel('x [m]') |
|
||||||
ax.set_ylabel('y [m]') |
|
||||||
ax.grid(True) |
|
||||||
|
|
||||||
points_plt, = ax.plot([0.0], [0.0], "--xk") |
|
||||||
cur, = ax.plot([0.0], [0.0], "xr") |
|
||||||
|
|
||||||
speed_txt = ax.text(-500, 900, '') |
|
||||||
curv_txt = ax.text(-500, 775, '') |
|
||||||
|
|
||||||
ax = fig.add_subplot(2, 1, 2) |
|
||||||
ax.set_title('Curvature') |
|
||||||
curvature_plt, = ax.plot([0.0], [0.0], "--xk") |
|
||||||
ax.set_xlim([0, 500]) |
|
||||||
ax.set_ylim([0, 1e-2]) |
|
||||||
ax.set_xlabel('Distance along path [m]') |
|
||||||
ax.set_ylabel('Curvature [1/m]') |
|
||||||
ax.grid(True) |
|
||||||
|
|
||||||
plt.show() |
|
||||||
|
|
||||||
while True: |
|
||||||
m = messaging.recv_one_or_none(live_map_sock) |
|
||||||
p = messaging.recv_one_or_none(plan_sock) |
|
||||||
if p is not None: |
|
||||||
v = p.plan.vCurvature * CV.MS_TO_MPH |
|
||||||
speed_txt.set_text('Desired curvature speed: %.2f mph' % v) |
|
||||||
|
|
||||||
if m is not None: |
|
||||||
print("Current way id: %d" % m.liveMapData.wayId) |
|
||||||
curv_txt.set_text('Curvature valid: %s Dist: %03.0f m\nSpeedlimit valid: %s Speed: %.0f mph' % |
|
||||||
(str(m.liveMapData.curvatureValid), |
|
||||||
m.liveMapData.distToTurn, |
|
||||||
str(m.liveMapData.speedLimitValid), |
|
||||||
m.liveMapData.speedLimit * CV.MS_TO_MPH)) |
|
||||||
|
|
||||||
points_plt.set_xdata(m.liveMapData.roadX) |
|
||||||
points_plt.set_ydata(m.liveMapData.roadY) |
|
||||||
curvature_plt.set_xdata(m.liveMapData.roadCurvatureX) |
|
||||||
curvature_plt.set_ydata(m.liveMapData.roadCurvature) |
|
||||||
|
|
||||||
fig.canvas.draw() |
|
||||||
fig.canvas.flush_events() |
|
||||||
Loading…
Reference in new issue