class Framer:
"""Creates frames from log entries. Packs together closest scan, pose and camera picture."""
def __init__(self,
filepath,
lidar_name=None,
pose2d_name=None,
pose3d_name=None,
camera_name=None):
self.log = LogIndexedReader(filepath)
self.current = 0
self.pose = [0, 0, 0]
self.pose2d = [0, 0, 0]
self.pose3d = [[0, 0, 0], [1, 0, 0, 0]]
self.scan = []
self.image = None
self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None
names = lookup_stream_names(filepath)
if lidar_name is not None:
self.lidar_id = names.index(lidar_name) + 1
if pose2d_name is not None:
self.pose2d_id = names.index(pose2d_name) + 1
if pose3d_name is not None:
self.pose3d_id = names.index(pose3d_name) + 1
if camera_name is not None:
self.camera_id = names.index(camera_name) + 1
def __enter__(self):
self.log.__enter__()
return self
def __exit__(self, *args):
self.log.__exit__()
def prev(self):
return self._step(-1)
def next(self):
return self._step(1)
def _step(self, direction):
if (self.current + direction) >= len(self.log):
self.log.grow()
while self.current + direction >= 0 and self.current + direction < len(
self.log):
self.current += direction
timestamp, stream_id, data = self.log[self.current]
if stream_id == self.lidar_id:
self.scan = deserialize(data)
return timestamp, self.pose, self.scan, self.image, False
elif stream_id == self.camera_id:
jpeg = deserialize(data)
self.image = pygame.image.load(io.BytesIO(jpeg),
'JPG').convert()
if self.lidar_id is None:
return timestamp, self.pose, self.scan, self.image, False
elif stream_id == self.pose3d_id:
pose3d, orientation = deserialize(data)
assert len(pose3d) == 3
assert len(orientation) == 4
self.pose = [
pose3d[0], pose3d[1],
quaternion.heading(orientation)
]
self.pose3d = [pose3d, orientation]
elif stream_id == self.pose2d_id:
arr = deserialize(data)
assert len(arr) == 3
self.pose = (arr[0] / 1000.0, arr[1] / 1000.0,
math.radians(arr[2] / 100.0))
x, y, heading = self.pose
self.pose = (x * math.cos(g_rotation_offset_rad) -
y * math.sin(g_rotation_offset_rad),
x * math.sin(g_rotation_offset_rad) +
y * math.cos(g_rotation_offset_rad),
heading + g_rotation_offset_rad)
if self.lidar_id is None and self.camera_id is None:
return timestamp, self.pose, self.scan, self.image, False
return timedelta(), self.pose, self.scan, self.image, True
class Framer:
"""Creates frames from log entries. Packs together closest scan, pose and camera picture."""
def __init__(self, filepath, lidar_name=None, lidar2_name=None, pose2d_name=None, pose3d_name=None, camera_name=None,
camera2_name=None, bbox_name=None, joint_name=None, keyframes_name=None, title_name=None):
self.log = LogIndexedReader(filepath)
self.current = 0
self.pose = [0, 0, 0]
self.pose2d = [0, 0, 0]
self.pose3d = [[0, 0, 0],[1, 0, 0, 0]]
self.scan = []
self.scan2 = []
self.image = None
self.image2 = None
self.bbox = None
self.joint = None
self.keyframe = None
self.title = None
self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None
self.lidar2_id = None
self.camera2_id = None
self.bbox_id = None
self.joint_id = None
self.keyframes_id = None
self.title_id = None
names = lookup_stream_names(filepath)
if lidar_name is not None:
self.lidar_id = names.index(lidar_name) + 1
if lidar2_name is not None:
self.lidar2_id = names.index(lidar2_name) + 1
if pose2d_name is not None:
self.pose2d_id = names.index(pose2d_name) + 1
if pose3d_name is not None:
self.pose3d_id = names.index(pose3d_name) + 1
if camera_name is not None:
self.camera_id = names.index(camera_name) + 1
if camera2_name is not None:
self.camera2_id = names.index(camera2_name) + 1
if bbox_name is not None:
self.bbox_id = names.index(bbox_name) + 1
if joint_name is not None:
self.joint_id = names.index(joint_name) + 1
if keyframes_name is not None:
self.keyframes_id = names.index(keyframes_name) + 1
if title_name is not None:
self.title_id = names.index(title_name) + 1
def __enter__(self):
self.log.__enter__()
return self
def __exit__(self, *args):
self.log.__exit__()
def prev(self):
return self._step(-1)
def next(self):
return self._step(1)
def seek(self, desired_timestamp):
print('Seek:', desired_timestamp)
timestamp, stream_id, data = self.log[self.current]
print('current', timestamp)
if timestamp > desired_timestamp:
return timestamp
start = self.current
end = len(self.log) - 1
timestamp, stream_id, data = self.log[end]
print('end', timestamp)
while timestamp < desired_timestamp:
self.log.grow()
end = len(self.log) - 1
timestamp, stream_id, data = self.log[end]
print('end', timestamp)
while start + 1 < end:
mid = (start + end)//2
timestamp, stream_id, data = self.log[mid]
if timestamp < desired_timestamp:
start = mid
else:
end = mid
print('selected', timestamp)
self.current = start
def _step(self, direction):
self.bbox = None
if (self.current + direction) >= len(self.log):
self.log.grow()
while self.current + direction >= 0 and self.current + direction < len(self.log):
self.current += direction
timestamp, stream_id, data = self.log[self.current]
if stream_id == self.keyframes_id:
self.keyframe = True
if stream_id == self.title_id:
self.title = deserialize(data)
if stream_id == self.bbox_id:
self.bbox = deserialize(data)
if stream_id == self.lidar_id:
self.scan = deserialize(data)
keyframe = self.keyframe
self.keyframe = False
return timestamp, self.pose, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False
if stream_id == self.lidar2_id:
self.scan2 = deserialize(data)
elif stream_id == self.camera_id:
self.image = get_image(deserialize(data))
if self.lidar_id is None:
keyframe = self.keyframe
self.keyframe = False
return timestamp, self.pose, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False
elif stream_id == self.camera2_id:
self.image2 = get_image(deserialize(data))
elif stream_id == self.joint_id:
self.joint = deserialize(data)
elif stream_id == self.pose3d_id:
pose3d, orientation = deserialize(data)
assert len(pose3d) == 3
assert len(orientation) == 4
self.pose = [pose3d[0], pose3d[1], quaternion.heading(orientation)]
self.pose3d = [pose3d, orientation]
elif stream_id == self.pose2d_id:
arr = deserialize(data)
assert len(arr) == 3
self.pose = (arr[0]/1000.0, arr[1]/1000.0, math.radians(arr[2]/100.0))
x, y, heading = self.pose
self.pose = (x * math.cos(g_rotation_offset_rad) - y * math.sin(g_rotation_offset_rad),
x * math.sin(g_rotation_offset_rad) + y * math.cos(g_rotation_offset_rad),
heading + g_rotation_offset_rad)
if self.lidar_id is None and self.camera_id is None:
keyframe = self.keyframe
self.keyframe = False
return timestamp, self.pose, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False
return timedelta(), self.pose, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, self.keyframe, self.title, True
class Framer:
"""Creates frames from log entries. Packs together closest scan, pose and camera picture."""
def __init__(self,
filepath,
lidar_name=None,
lidar2_name=None,
pose2d_name=None,
pose3d_name=None,
camera_name=None,
camera2_name=None,
bbox_name=None,
rgbd_name=None,
joint_name=None,
keyframes_name=None,
title_name=None,
lidar_up_name=None,
lidar_down_name=None):
self.log = LogIndexedReader(filepath)
self.current = 0
self.frame = Frame()
self.pose = [0, 0, 0]
self.pose2d = [0, 0, 0]
self.pose3d = [[0, 0, 0], [1, 0, 0, 0]]
self.scan = []
self.scan2 = []
self.image = None
self.image2 = None
self.bbox = None
self.joint = None
self.keyframe = None
self.title = None
self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None
self.lidar2_id = None
self.camera2_id = None
self.bbox_id = None
self.rgbd_id = None
self.joint_id = None
self.keyframes_id = None
self.title_id = []
self.lidar_up_id = None
self.lidar_down_id = None
names = lookup_stream_names(filepath)
if lidar_name is not None:
self.lidar_id = names.index(lidar_name) + 1
if lidar2_name is not None:
self.lidar2_id = names.index(lidar2_name) + 1
if pose2d_name is not None:
self.pose2d_id = names.index(pose2d_name) + 1
if pose3d_name is not None:
self.pose3d_id = names.index(pose3d_name) + 1
if camera_name is not None:
self.camera_id = names.index(camera_name) + 1
if camera2_name is not None:
self.camera2_id = names.index(camera2_name) + 1
if bbox_name is not None:
self.bbox_id = names.index(bbox_name) + 1
if rgbd_name is not None:
self.rgbd_id = names.index(rgbd_name) + 1
if joint_name is not None:
self.joint_id = names.index(joint_name) + 1
if keyframes_name is not None:
self.keyframes_id = names.index(keyframes_name) + 1
if title_name is not None:
title_list = title_name.split(",")
for ttn in title_list:
self.title_id.append(names.index(ttn) + 1)
if lidar_up_name is not None:
self.lidar_up_id = names.index(lidar_up_name) + 1
if lidar_down_name is not None:
self.lidar_down_id = names.index(lidar_down_name) + 1
def __enter__(self):
self.log.__enter__()
return self
def __exit__(self, *args):
self.log.__exit__()
def prev(self):
return self._step(-1)
def next(self):
return self._step(1)
def seek(self, desired_timestamp):
print('Seek:', desired_timestamp)
timestamp, stream_id, data = self.log[self.current]
print('current', timestamp)
if timestamp > desired_timestamp:
return timestamp
start = self.current
end = len(self.log) - 1
timestamp, stream_id, data = self.log[end]
print('end', timestamp)
while timestamp < desired_timestamp:
self.log.grow()
end = len(self.log) - 1
timestamp, stream_id, data = self.log[end]
print('end', timestamp)
while start + 1 < end:
mid = (start + end) // 2
timestamp, stream_id, data = self.log[mid]
if timestamp < desired_timestamp:
start = mid
else:
end = mid
print('selected', timestamp)
self.current = start
def _step(self, direction):
self.bbox = []
self.title = []
if (self.current + direction) >= len(self.log):
self.log.grow()
while self.current + direction >= 0 and self.current + direction < len(
self.log):
self.current += direction
timestamp, stream_id, data = self.log[self.current]
if stream_id == self.keyframes_id:
self.keyframe = True
if stream_id in self.title_id and stream_id != self.bbox_id:
self.title.append(deserialize(data))
if stream_id == self.lidar_id:
self.scan = deserialize(data)
keyframe = self.keyframe
self.keyframe = False
return timestamp, self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False
if stream_id == self.lidar2_id:
self.scan2 = deserialize(data)
if stream_id == self.lidar_up_id:
self.frame.lidar_up = deserialize(data)
if stream_id == self.lidar_down_id:
self.frame.lidar_down = deserialize(data)
elif stream_id == self.camera_id:
self.image, _ = get_image(deserialize(data))
# bounding boxes associated with an image are stored after the image in the log
# therefore, we need to continue reading the log past the image in order to gathering its bounding box data
current = self.current
while current + direction >= 0 and current + direction < len(
self.log):
current += direction
_, new_stream_id, new_data = self.log[current]
if new_stream_id == self.bbox_id:
self.bbox.append(deserialize(new_data))
if new_stream_id in self.title_id:
self.title.append(deserialize(new_data))
if new_stream_id == self.camera_id:
break
if self.lidar_id is None:
keyframe = self.keyframe
self.keyframe = False
return timestamp, self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False
elif stream_id == self.camera2_id:
self.image2, _ = get_image(deserialize(data))
elif stream_id == self.rgbd_id:
_, _, img_data, depth_data = deserialize(data)
self.image, self.image2 = get_image((img_data, depth_data))
if self.lidar_id is None:
keyframe = self.keyframe
self.keyframe = False
return timestamp, self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False
elif stream_id == self.joint_id:
self.joint = deserialize(data)
elif stream_id == self.pose3d_id:
pose3d, orientation = deserialize(data)
assert len(pose3d) == 3
assert len(orientation) == 4
self.pose = [
pose3d[0], pose3d[1],
quaternion.heading(orientation)
]
self.pose3d = [pose3d, orientation]
elif stream_id == self.pose2d_id:
arr = deserialize(data)
assert len(arr) == 3
self.pose = (arr[0] / 1000.0, arr[1] / 1000.0,
math.radians(arr[2] / 100.0))
x, y, heading = self.pose
self.pose = (x * math.cos(g_rotation_offset_rad) -
y * math.sin(g_rotation_offset_rad),
x * math.sin(g_rotation_offset_rad) +
y * math.cos(g_rotation_offset_rad),
heading + g_rotation_offset_rad)
if self.lidar_id is None and self.camera_id is None:
keyframe = self.keyframe
self.keyframe = False
return timestamp, self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False
return timedelta(
), self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, self.keyframe, self.title, True
