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