def __init__(self):
if len(sys.argv) <= 2 or '--help' in sys.argv:
print """Usage:
{name} folder/ video.avi
""".format(name = sys.argv[0])
sys.exit(-1)
args = parse_args(sys.argv[1], sys.argv[2])
self.args = args
bg_file = glob.glob(args['fullname'] + '*bg.npz')[0]
print sys.argv
self.small_step = 5
self.large_step = 10
self.startup_complete = False
##### Grab all the transforms ######
self.absolute = False
(self.imu_transforms_mk1,
self.gps_data_mk1,
self.gps_times_mk1) = get_transforms(args, 'mark1', self.absolute)
(self.imu_transforms_mk2,
self.gps_data_mk2,
self.gps_times_mk2) = get_transforms(args, 'mark2', self.absolute)
self.mk2_t = 0
self.t = self.mk2_to_mk1()
self.cur_imu_transform = self.imu_transforms_mk1[self.t, :,:]
self.imu_kdtree = cKDTree(self.imu_transforms_mk1[:, :3, 3])
self.params = args['params']
self.lidar_params = self.params['lidar']
self.T_from_i_to_l = np.linalg.inv(self.lidar_params['T_from_l_to_i'])
cam_num = args['cam_num']
self.cam_params = self.params['cam'][cam_num]
# Load the MobilEye file
self.mbly_loader = MblyLoader(args)
self.mbly_rot = [0.0, -0.005, -0.006]
self.mbly_T = [5.4, 0.0, -1.9]
# Is the flyover running
self.running = False
# Has the user changed the time
self.manual_change = 0
###### Set up the renderers ######
self.cloud_ren = vtk.vtkRenderer()
self.cloud_ren.SetViewport(0, 0, 1.0, 1.0)
self.cloud_ren.SetBackground(0, 0, 0)
self.img_ren = vtk.vtkRenderer()
# self.img_ren.SetViewport(0.7, 0.0, 1.0, 0.37)
self.img_ren.SetViewport(0.6, 0.6, 1.0, 1.0)
self.img_ren.SetInteractive(False)
self.img_ren.SetBackground(0.1, 0.1, 0.1)
self.win = vtk.vtkRenderWindow()
self.win.StereoCapableWindowOff()
self.win.AddRenderer(self.cloud_ren)
self.win.AddRenderer(self.img_ren)
self.win.SetSize(800, 400)
self.iren = vtk.vtkRenderWindowInteractor()
self.iren.SetRenderWindow(self.win)
###### Cloud Actors ######
print 'Adding raw points'
raw_npz = np.load(bg_file)
pts = raw_npz['data']
raw_cloud = VtkPointCloud(pts[:, :3], np.ones(pts[:, :3].shape) * 255)
raw_actor = raw_cloud.get_vtk_color_cloud()
self.raw_lidar = VTKCloudTree(raw_cloud, raw_actor, build_tree=False)
self.raw_lidar_2d = DataTree(self.raw_lidar.pts[:, :-1], build_tree=False)
self.raw_lidar.actor.GetProperty().SetPointSize(2)
self.raw_lidar.actor.GetProperty().SetOpacity(0.1)
self.raw_lidar.actor.SetPickable(0)
self.cloud_ren.AddActor(self.raw_lidar.actor)
print 'Adding car'
self.car = load_ply('../mapping/viz/gtr.ply')
self.car.SetPickable(0)
self.car.GetProperty().LightingOff()
self.cloud_ren.AddActor(self.car)
self.mbly_vtk_boxes = []
# Car: 0, Truck: 1, Bike: 2, Other: 3-7
red = np.array((1, 0, 0))
green = np.array((0, 1, 0))
blue = np.array((0, 0, 1))
white = red+green+blue
self.mbly_obj_colors = [red, green, blue, white]
self.mbly_vtk_lanes = []
# Dashed: 0, Solid: 1, undecided: 2, Edge: 3, Double: 4, Botts_Dots: 5
self.mbly_lane_color = [green, green, red, blue, white, green]
self.mbly_lane_size = [2, 3, 1, 1, 2, 1]
self.mbly_lane_subsamp = [20, 1, 1, 1, 1, 40]
# Use our custom mouse interactor
self.interactor = LaneInteractorStyle(self.iren, self.cloud_ren, self)
self.iren.SetInteractorStyle(self.interactor)
###### 2D Projection Actors ######
self.video_reader = VideoReader(args['video'])
self.img_actor = None
self.I = None
###### Add Callbacks ######
print 'Rendering'
self.iren.Initialize()
# Set up time
self.iren.AddObserver('TimerEvent', self.update)
self.timer = self.iren.CreateRepeatingTimer(10)
self.iren.Start()
def __init__(self):
if len(sys.argv) <= 2 or '--help' in sys.argv:
print """Usage:
{name} folder/ video.avi
""".format(name=sys.argv[0])
sys.exit(-1)
args = parse_args(sys.argv[1], sys.argv[2])
self.args = args
bg_file = glob.glob(args['fullname'] + '*bg.npz')[0]
print sys.argv
self.small_step = 5
self.large_step = 10
self.startup_complete = False
##### Grab all the transforms ######
self.absolute = False
(self.imu_transforms_mk1, self.gps_data_mk1,
self.gps_times_mk1) = get_transforms(args, 'mark1', self.absolute)
(self.imu_transforms_mk2, self.gps_data_mk2,
self.gps_times_mk2) = get_transforms(args, 'mark2', self.absolute)
self.mk2_t = 0
self.t = self.mk2_to_mk1()
self.cur_imu_transform = self.imu_transforms_mk1[self.t, :, :]
self.imu_kdtree = cKDTree(self.imu_transforms_mk1[:, :3, 3])
self.params = args['params']
self.lidar_params = self.params['lidar']
self.T_from_i_to_l = np.linalg.inv(self.lidar_params['T_from_l_to_i'])
cam_num = args['cam_num']
self.cam_params = self.params['cam'][cam_num]
# Load the MobilEye file
self.mbly_loader = MblyLoader(args)
self.mbly_rot = [0.0, -0.005, -0.006]
self.mbly_T = [5.4, 0.0, -1.9]
# Is the flyover running
self.running = False
# Has the user changed the time
self.manual_change = 0
###### Set up the renderers ######
self.cloud_ren = vtk.vtkRenderer()
self.cloud_ren.SetViewport(0, 0, 1.0, 1.0)
self.cloud_ren.SetBackground(0, 0, 0)
self.img_ren = vtk.vtkRenderer()
# self.img_ren.SetViewport(0.7, 0.0, 1.0, 0.37)
self.img_ren.SetViewport(0.6, 0.6, 1.0, 1.0)
self.img_ren.SetInteractive(False)
self.img_ren.SetBackground(0.1, 0.1, 0.1)
self.win = vtk.vtkRenderWindow()
self.win.StereoCapableWindowOff()
self.win.AddRenderer(self.cloud_ren)
self.win.AddRenderer(self.img_ren)
self.win.SetSize(800, 400)
self.iren = vtk.vtkRenderWindowInteractor()
self.iren.SetRenderWindow(self.win)
###### Cloud Actors ######
print 'Adding raw points'
raw_npz = np.load(bg_file)
pts = raw_npz['data']
raw_cloud = VtkPointCloud(pts[:, :3], np.ones(pts[:, :3].shape) * 255)
raw_actor = raw_cloud.get_vtk_color_cloud()
self.raw_lidar = VTKCloudTree(raw_cloud, raw_actor, build_tree=False)
self.raw_lidar_2d = DataTree(self.raw_lidar.pts[:, :-1],
build_tree=False)
self.raw_lidar.actor.GetProperty().SetPointSize(2)
self.raw_lidar.actor.GetProperty().SetOpacity(0.1)
self.raw_lidar.actor.SetPickable(0)
self.cloud_ren.AddActor(self.raw_lidar.actor)
print 'Adding car'
self.car = load_ply('../mapping/viz/gtr.ply')
self.car.SetPickable(0)
self.car.GetProperty().LightingOff()
self.cloud_ren.AddActor(self.car)
self.mbly_vtk_boxes = []
# Car: 0, Truck: 1, Bike: 2, Other: 3-7
red = np.array((1, 0, 0))
green = np.array((0, 1, 0))
blue = np.array((0, 0, 1))
white = red + green + blue
self.mbly_obj_colors = [red, green, blue, white]
self.mbly_vtk_lanes = []
# Dashed: 0, Solid: 1, undecided: 2, Edge: 3, Double: 4, Botts_Dots: 5
self.mbly_lane_color = [green, green, red, blue, white, green]
self.mbly_lane_size = [2, 3, 1, 1, 2, 1]
self.mbly_lane_subsamp = [20, 1, 1, 1, 1, 40]
# Use our custom mouse interactor
self.interactor = LaneInteractorStyle(self.iren, self.cloud_ren, self)
self.iren.SetInteractorStyle(self.interactor)
###### 2D Projection Actors ######
self.video_reader = VideoReader(args['video'])
self.img_actor = None
self.I = None
###### Add Callbacks ######
print 'Rendering'
self.iren.Initialize()
# Set up time
self.iren.AddObserver('TimerEvent', self.update)
self.timer = self.iren.CreateRepeatingTimer(10)
self.iren.Start()
class Blockworld:
def __init__(self):
if len(sys.argv) <= 2 or '--help' in sys.argv:
print """Usage:
{name} folder/ video.avi
""".format(name = sys.argv[0])
sys.exit(-1)
args = parse_args(sys.argv[1], sys.argv[2])
self.args = args
bg_file = glob.glob(args['fullname'] + '*bg.npz')[0]
print sys.argv
self.small_step = 5
self.large_step = 10
self.startup_complete = False
##### Grab all the transforms ######
self.absolute = False
(self.imu_transforms_mk1,
self.gps_data_mk1,
self.gps_times_mk1) = get_transforms(args, 'mark1', self.absolute)
(self.imu_transforms_mk2,
self.gps_data_mk2,
self.gps_times_mk2) = get_transforms(args, 'mark2', self.absolute)
self.mk2_t = 0
self.t = self.mk2_to_mk1()
self.cur_imu_transform = self.imu_transforms_mk1[self.t, :,:]
self.imu_kdtree = cKDTree(self.imu_transforms_mk1[:, :3, 3])
self.params = args['params']
self.lidar_params = self.params['lidar']
self.T_from_i_to_l = np.linalg.inv(self.lidar_params['T_from_l_to_i'])
cam_num = args['cam_num']
self.cam_params = self.params['cam'][cam_num]
# Load the MobilEye file
self.mbly_loader = MblyLoader(args)
self.mbly_rot = [0.0, -0.005, -0.006]
self.mbly_T = [5.4, 0.0, -1.9]
# Is the flyover running
self.running = False
# Has the user changed the time
self.manual_change = 0
###### Set up the renderers ######
self.cloud_ren = vtk.vtkRenderer()
self.cloud_ren.SetViewport(0, 0, 1.0, 1.0)
self.cloud_ren.SetBackground(0, 0, 0)
self.img_ren = vtk.vtkRenderer()
# self.img_ren.SetViewport(0.7, 0.0, 1.0, 0.37)
self.img_ren.SetViewport(0.6, 0.6, 1.0, 1.0)
self.img_ren.SetInteractive(False)
self.img_ren.SetBackground(0.1, 0.1, 0.1)
self.win = vtk.vtkRenderWindow()
self.win.StereoCapableWindowOff()
self.win.AddRenderer(self.cloud_ren)
self.win.AddRenderer(self.img_ren)
self.win.SetSize(800, 400)
self.iren = vtk.vtkRenderWindowInteractor()
self.iren.SetRenderWindow(self.win)
###### Cloud Actors ######
print 'Adding raw points'
raw_npz = np.load(bg_file)
pts = raw_npz['data']
raw_cloud = VtkPointCloud(pts[:, :3], np.ones(pts[:, :3].shape) * 255)
raw_actor = raw_cloud.get_vtk_color_cloud()
self.raw_lidar = VTKCloudTree(raw_cloud, raw_actor, build_tree=False)
self.raw_lidar_2d = DataTree(self.raw_lidar.pts[:, :-1], build_tree=False)
self.raw_lidar.actor.GetProperty().SetPointSize(2)
self.raw_lidar.actor.GetProperty().SetOpacity(0.1)
self.raw_lidar.actor.SetPickable(0)
self.cloud_ren.AddActor(self.raw_lidar.actor)
print 'Adding car'
self.car = load_ply('../mapping/viz/gtr.ply')
self.car.SetPickable(0)
self.car.GetProperty().LightingOff()
self.cloud_ren.AddActor(self.car)
self.mbly_vtk_boxes = []
# Car: 0, Truck: 1, Bike: 2, Other: 3-7
red = np.array((1, 0, 0))
green = np.array((0, 1, 0))
blue = np.array((0, 0, 1))
white = red+green+blue
self.mbly_obj_colors = [red, green, blue, white]
self.mbly_vtk_lanes = []
# Dashed: 0, Solid: 1, undecided: 2, Edge: 3, Double: 4, Botts_Dots: 5
self.mbly_lane_color = [green, green, red, blue, white, green]
self.mbly_lane_size = [2, 3, 1, 1, 2, 1]
self.mbly_lane_subsamp = [20, 1, 1, 1, 1, 40]
# Use our custom mouse interactor
self.interactor = LaneInteractorStyle(self.iren, self.cloud_ren, self)
self.iren.SetInteractorStyle(self.interactor)
###### 2D Projection Actors ######
self.video_reader = VideoReader(args['video'])
self.img_actor = None
self.I = None
###### Add Callbacks ######
print 'Rendering'
self.iren.Initialize()
# Set up time
self.iren.AddObserver('TimerEvent', self.update)
self.timer = self.iren.CreateRepeatingTimer(10)
self.iren.Start()
def mk2_to_mk1(self, mk2_idx=-1):
if mk2_idx == -1:
mk2_idx = self.mk2_t
return mk2_to_mk1(mk2_idx, self.gps_times_mk1, self.gps_times_mk2)
def getCameraPosition(self, t, focus=100):
offset = np.array([-75.0, 0, 25.0]) / 4
# Rotate the camera so it is behind the car
position = np.dot(self.imu_transforms_mk1[t, 0:3, 0:3], offset)
position += self.imu_transforms_mk1[t, 0:3, 3] + position
# Focus 10 frames in front of the car
focal_point = self.imu_transforms_mk1[t + focus, 0:3, 3]
return position, focal_point
def finished(self, focus=100):
return self.mk2_t + 2 * focus > self.video_reader.total_frame_count
def update(self, iren, event):
# Get the cameras
cloud_cam = self.cloud_ren.GetActiveCamera()
img_cam = self.img_ren.GetActiveCamera()
# Initialization
if not self.startup_complete:
cloud_cam.SetViewUp(0, 0, 1)
self.mk2_t = 1
self.t = self.mk2_to_mk1()
self.startup_complete = True
self.manual_change = -1 # Force an update for the camera
# Update the time (arrow keys also update time)
if self.running:
self.mk2_t += self.large_step
if self.finished():
self.mk2_t -= self.large_step
if self.running == True:
self.interactor.KeyHandler(key='space')
# Get the correct gps time (mk2 is camera time)
self.t = self.mk2_to_mk1()
self.cur_imu_transform = self.imu_transforms_mk1[self.t, :, :]
# Get the correct frame to show
(success, self.I) = self.video_reader.getFrame(self.mk2_t)
# Update the gps time
self.cur_gps_time = self.gps_times_mk2[self.mk2_t]
# Make sure the calibration has been updated
self.mbly_R = euler_matrix(*self.mbly_rot)[:3, :3]
if self.running or self.manual_change:
# Set camera position to in front of the car
position, focal_point = self.getCameraPosition(self.t)
cloud_cam.SetPosition(position)
cloud_cam.SetFocalPoint(focal_point)
# Update the car position
transform = vtk_transform_from_np(self.cur_imu_transform)
transform.RotateZ(90)
transform.Translate(-2, -3, -2)
self.car.SetUserTransform(transform)
# If the user caused a manual change, reset it
self.manual_change = 0
# Copy the image so we can project points onto it
I = self.I.copy()
# Add the lanes to the cloud
mbly_lanes = self.mbly_loader.loadLane(self.cur_gps_time)
lanes_wrt_mbly = self.mblyLaneAsNp(mbly_lanes)
self.addLaneToCloud(lanes_wrt_mbly)
# Add the lanes to the image copy
I = self.addLaneToImg(I, lanes_wrt_mbly)
# Add the objects (cars) to the cloud
mbly_objs = self.mbly_loader.loadObj(self.cur_gps_time)
objs_wrt_mbly = self.mblyObjAsNp(mbly_objs)
self.addObjToCloud(objs_wrt_mbly)
# Add the lanes to the image copy
I = self.addObjToImg(I, objs_wrt_mbly)
vtkimg = VtkImage(I)
self.img_ren.RemoveActor(self.img_actor)
self.img_actor = vtkimg.get_vtk_image()
self.img_ren.AddActor(self.img_actor)
# We need to draw the image before we run ResetCamera or else
# the image is too small
self.img_ren.ResetCamera()
img_cam.SetClippingRange(100, 100000) # These units are pixels
img_cam.Dolly(2.00)
self.iren.GetRenderWindow().Render()
def xformMblyToGlobal(self, pts_wrt_mbly):
# TODO: Need to tranform from imu to gps frame of reference
T_l_to_i = self.params['lidar']['T_from_l_to_i']
T_i_to_world = self.imu_transforms_mk1[self.t, 0:3, 0:4]
pts = pts_wrt_mbly
# Puts points in lidar FOR
pts_wrt_lidar = T_from_mbly_to_lidar(pts_wrt_mbly, self.mbly_T, \
self.mbly_R[:3,:3])
# Make points homogoneous
hom_pts = np.hstack((pts_wrt_lidar[:, :3], np.ones((pts.shape[0], 1))))
# Put in imu FOR
pts_wrt_imu = np.dot(T_l_to_i, hom_pts.T).T
# Put in global FOR
pts_wrt_world = np.dot(T_i_to_world, pts_wrt_imu.T).T
# Add metadata back to output
pts_wrt_world = np.hstack((pts_wrt_world, pts[:, 3:]))
return pts_wrt_world
def mblyObjAsNp(self, mbly_objs):
"""Turns a mobileye object pb message into a numpy array with format:
[x, y, .7, length, width, type]
"""
pts_wrt_mbly = []
for obj in mbly_objs:
pt_wrt_mbly = [obj.pos_x, obj.pos_y, .7, obj.length, \
obj.width, obj.obj_type]
pts_wrt_mbly.append(pt_wrt_mbly)
return np.array(pts_wrt_mbly)
def mblyLaneAsNp(self, mbly_lane):
"""Turns a mobileye lane into a numpy array with format:
[C0, C1, C2, C3, lane_id, lane_type, view_range]
Y = C3*X^3 + C2*X^2 + C1*X + C0.
X is longitudinal distance from camera (positive right!)
Y is lateral distance from camera
lane_id is between -2 and 2, with -2 being the farthest left,
and 2 being the farthest right lane. There is no 0 id.
"""
lanes_wrt_mbly = []
for l in mbly_lane:
lane_wrt_mbly = [l.C0, l.C1, l.C2, l.C3, l.lane_id, l.lane_type, \
l.view_range]
lanes_wrt_mbly.append(lane_wrt_mbly)
return np.array(lanes_wrt_mbly)
def addObjToCloud(self, objs_wrt_mbly):
""" Add the mobileye returns to the 3d scene """
mbly_vtk_boxes = []
car_rot = self.imu_transforms_mk1[self.t, :, :3]
objs_wrt_world = self.xformMblyToGlobal(objs_wrt_mbly)
# Draw each box
car_rot = euler_from_matrix(car_rot)[2] * 180 / math.pi
for o in objs_wrt_world:
# Create the vtk object
box = VtkBoundingBox(o)
actor = box.get_vtk_box(car_rot)
color = self.mbly_obj_colors[int(o[5])]
actor.GetProperty().SetColor(*color)
mbly_vtk_boxes.append(box)
# Remove old boxes
for vtk_box in self.mbly_vtk_boxes:
self.cloud_ren.RemoveActor(vtk_box.actor)
# Update to new actors
self.mbly_vtk_boxes = mbly_vtk_boxes
# Draw new boxes
for vtk_box in self.mbly_vtk_boxes:
self.cloud_ren.AddActor(vtk_box.actor)
def getLanePointsFromModel(self, lane_wrt_mbly):
view_range = lane_wrt_mbly[6]
if view_range == 0:
return None
num_pts = view_range * 3
X = np.linspace(0, view_range, num=num_pts)
# from model: Y = C3*X^3 + C2*X^2 + C1*X + C0.
X = np.vstack((np.ones(X.shape), X, np.power(X, 2), np.power(X, 3)))
# Mbly uses Y-right as positive, we use Y-left as positive
Y = -1 * np.dot(lane_wrt_mbly[:4], X)
lane_pts_wrt_mbly = np.vstack((X[1, :], Y, np.zeros((1, num_pts)))).T
return lane_pts_wrt_mbly
def addLaneToCloud(self, lane_wrt_mbly):
for lane in self.mbly_vtk_lanes:
self.cloud_ren.RemoveActor(lane.actor)
self.mbly_vtk_lanes = []
for i in xrange(lane_wrt_mbly.shape[0]):
type = int(lane_wrt_mbly[i, 5])
color = self.mbly_lane_color[type] * 255
size = self.mbly_lane_size[type]
subsamp = self.mbly_lane_subsamp[type]
lane_pts_wrt_mbly = self.getLanePointsFromModel(lane_wrt_mbly[i, :])
if lane_pts_wrt_mbly is None:
continue
pts_wrt_global = self.xformMblyToGlobal(lane_pts_wrt_mbly)
pts_wrt_global = pts_wrt_global[::subsamp]
num_pts = pts_wrt_global.shape[0]
vtk_lane = VtkPointCloud(pts_wrt_global, np.tile(color, (num_pts, 1)))
actor = vtk_lane.get_vtk_color_cloud()
actor.GetProperty().SetPointSize(size)
self.mbly_vtk_lanes.append(vtk_lane)
self.cloud_ren.AddActor(actor)
def addObjToImg(self, I, objs_wrt_mbly):
"""Takes an image and the mbly objects. Converts the objects into corners of a
bounding box and draws them on the image
"""
if objs_wrt_mbly.shape[0] == 0:
return None
pix = []
width = objs_wrt_mbly[:, 4]
# Assuming the point in obs_wrt_mbly are the center of the object, draw
# a box .5 m below, .5 m above, -.5*width left, .5*width right. Keep the
# same z position
for z in [-.5, .5]:
for y in [-.5, .5]:
offset = np.zeros((width.shape[0], 3))
offset[:, 1] = width*y
offset[:, 2] = z
pt = objs_wrt_mbly[:, :3] + offset
proj_pt = projectPoints(pt, self.args, self.mbly_T, self.mbly_R)
pix.append(proj_pt[:, 3:])
pix = np.array(pix, dtype=np.int32)
pix = np.swapaxes(pix, 0, 1)
# Draw a line between projected points
for i, corner in enumerate(pix):
# Get the color of the box and convert RGB -> BGR
color = self.mbly_obj_colors[int(objs_wrt_mbly[i, 5])][::-1] * 255
corner = tuple(map(tuple, corner))
cv2.rectangle(I, corner[0], corner[3], color, 2)
return I
def addLaneToImg(self, I, lanes_wrt_mbly):
"""Takes an image and the 3d lane points. Projects these points onto the image
"""
if lanes_wrt_mbly.shape[0] == 0:
return None
pix = []
for i in xrange(len(self.mbly_vtk_lanes)):
type = int(lanes_wrt_mbly[i, 5])
color = self.mbly_lane_color[type] * 255
size = self.mbly_lane_size[type]
subsamp = self.mbly_lane_subsamp[type]
pts = self.getLanePointsFromModel(lanes_wrt_mbly[i])[::subsamp]
if pts is None:
continue
proj_pts = projectPoints(pts, self.args, self.mbly_T, self.mbly_R)
proj_pts = proj_pts[:, 3:].astype(np.int32, copy = False)
img_mask = (proj_pts[:, 0] < 1280) & (proj_pts[:, 0] >= 0) &\
(proj_pts[:, 1] < 800) & (proj_pts[:, 1] >= 0)
proj_pts = proj_pts[img_mask]
for pt_i in xrange(proj_pts.shape[0]):
# cv2 only takes tuples at points
pt = tuple(proj_pts[pt_i, :])
# Make sure to convert to bgr
cv2.circle(I, pt, size, color[::-1], thickness=-size)
return I
class Blockworld:
def __init__(self):
if len(sys.argv) <= 2 or '--help' in sys.argv:
print """Usage:
{name} folder/ video.avi
""".format(name=sys.argv[0])
sys.exit(-1)
args = parse_args(sys.argv[1], sys.argv[2])
self.args = args
bg_file = glob.glob(args['fullname'] + '*bg.npz')[0]
print sys.argv
self.small_step = 5
self.large_step = 10
self.startup_complete = False
##### Grab all the transforms ######
self.absolute = False
(self.imu_transforms_mk1, self.gps_data_mk1,
self.gps_times_mk1) = get_transforms(args, 'mark1', self.absolute)
(self.imu_transforms_mk2, self.gps_data_mk2,
self.gps_times_mk2) = get_transforms(args, 'mark2', self.absolute)
self.mk2_t = 0
self.t = self.mk2_to_mk1()
self.cur_imu_transform = self.imu_transforms_mk1[self.t, :, :]
self.imu_kdtree = cKDTree(self.imu_transforms_mk1[:, :3, 3])
self.params = args['params']
self.lidar_params = self.params['lidar']
self.T_from_i_to_l = np.linalg.inv(self.lidar_params['T_from_l_to_i'])
cam_num = args['cam_num']
self.cam_params = self.params['cam'][cam_num]
# Load the MobilEye file
self.mbly_loader = MblyLoader(args)
self.mbly_rot = [0.0, -0.005, -0.006]
self.mbly_T = [5.4, 0.0, -1.9]
# Is the flyover running
self.running = False
# Has the user changed the time
self.manual_change = 0
###### Set up the renderers ######
self.cloud_ren = vtk.vtkRenderer()
self.cloud_ren.SetViewport(0, 0, 1.0, 1.0)
self.cloud_ren.SetBackground(0, 0, 0)
self.img_ren = vtk.vtkRenderer()
# self.img_ren.SetViewport(0.7, 0.0, 1.0, 0.37)
self.img_ren.SetViewport(0.6, 0.6, 1.0, 1.0)
self.img_ren.SetInteractive(False)
self.img_ren.SetBackground(0.1, 0.1, 0.1)
self.win = vtk.vtkRenderWindow()
self.win.StereoCapableWindowOff()
self.win.AddRenderer(self.cloud_ren)
self.win.AddRenderer(self.img_ren)
self.win.SetSize(800, 400)
self.iren = vtk.vtkRenderWindowInteractor()
self.iren.SetRenderWindow(self.win)
###### Cloud Actors ######
print 'Adding raw points'
raw_npz = np.load(bg_file)
pts = raw_npz['data']
raw_cloud = VtkPointCloud(pts[:, :3], np.ones(pts[:, :3].shape) * 255)
raw_actor = raw_cloud.get_vtk_color_cloud()
self.raw_lidar = VTKCloudTree(raw_cloud, raw_actor, build_tree=False)
self.raw_lidar_2d = DataTree(self.raw_lidar.pts[:, :-1],
build_tree=False)
self.raw_lidar.actor.GetProperty().SetPointSize(2)
self.raw_lidar.actor.GetProperty().SetOpacity(0.1)
self.raw_lidar.actor.SetPickable(0)
self.cloud_ren.AddActor(self.raw_lidar.actor)
print 'Adding car'
self.car = load_ply('../mapping/viz/gtr.ply')
self.car.SetPickable(0)
self.car.GetProperty().LightingOff()
self.cloud_ren.AddActor(self.car)
self.mbly_vtk_boxes = []
# Car: 0, Truck: 1, Bike: 2, Other: 3-7
red = np.array((1, 0, 0))
green = np.array((0, 1, 0))
blue = np.array((0, 0, 1))
white = red + green + blue
self.mbly_obj_colors = [red, green, blue, white]
self.mbly_vtk_lanes = []
# Dashed: 0, Solid: 1, undecided: 2, Edge: 3, Double: 4, Botts_Dots: 5
self.mbly_lane_color = [green, green, red, blue, white, green]
self.mbly_lane_size = [2, 3, 1, 1, 2, 1]
self.mbly_lane_subsamp = [20, 1, 1, 1, 1, 40]
# Use our custom mouse interactor
self.interactor = LaneInteractorStyle(self.iren, self.cloud_ren, self)
self.iren.SetInteractorStyle(self.interactor)
###### 2D Projection Actors ######
self.video_reader = VideoReader(args['video'])
self.img_actor = None
self.I = None
###### Add Callbacks ######
print 'Rendering'
self.iren.Initialize()
# Set up time
self.iren.AddObserver('TimerEvent', self.update)
self.timer = self.iren.CreateRepeatingTimer(10)
self.iren.Start()
def mk2_to_mk1(self, mk2_idx=-1):
if mk2_idx == -1:
mk2_idx = self.mk2_t
return mk2_to_mk1(mk2_idx, self.gps_times_mk1, self.gps_times_mk2)
def getCameraPosition(self, t, focus=100):
offset = np.array([-75.0, 0, 25.0]) / 4
# Rotate the camera so it is behind the car
position = np.dot(self.imu_transforms_mk1[t, 0:3, 0:3], offset)
position += self.imu_transforms_mk1[t, 0:3, 3] + position
# Focus 10 frames in front of the car
focal_point = self.imu_transforms_mk1[t + focus, 0:3, 3]
return position, focal_point
def finished(self, focus=100):
return self.mk2_t + 2 * focus > self.video_reader.total_frame_count
def update(self, iren, event):
# Get the cameras
cloud_cam = self.cloud_ren.GetActiveCamera()
img_cam = self.img_ren.GetActiveCamera()
# Initialization
if not self.startup_complete:
cloud_cam.SetViewUp(0, 0, 1)
self.mk2_t = 1
self.t = self.mk2_to_mk1()
self.startup_complete = True
self.manual_change = -1 # Force an update for the camera
# Update the time (arrow keys also update time)
if self.running:
self.mk2_t += self.large_step
if self.finished():
self.mk2_t -= self.large_step
if self.running == True:
self.interactor.KeyHandler(key='space')
# Get the correct gps time (mk2 is camera time)
self.t = self.mk2_to_mk1()
self.cur_imu_transform = self.imu_transforms_mk1[self.t, :, :]
# Get the correct frame to show
(success, self.I) = self.video_reader.getFrame(self.mk2_t)
# Update the gps time
self.cur_gps_time = self.gps_times_mk2[self.mk2_t]
# Make sure the calibration has been updated
self.mbly_R = euler_matrix(*self.mbly_rot)[:3, :3]
if self.running or self.manual_change:
# Set camera position to in front of the car
position, focal_point = self.getCameraPosition(self.t)
cloud_cam.SetPosition(position)
cloud_cam.SetFocalPoint(focal_point)
# Update the car position
transform = vtk_transform_from_np(self.cur_imu_transform)
transform.RotateZ(90)
transform.Translate(-2, -3, -2)
self.car.SetUserTransform(transform)
# If the user caused a manual change, reset it
self.manual_change = 0
# Copy the image so we can project points onto it
I = self.I.copy()
# Add the lanes to the cloud
mbly_lanes = self.mbly_loader.loadLane(self.cur_gps_time)
lanes_wrt_mbly = self.mblyLaneAsNp(mbly_lanes)
self.addLaneToCloud(lanes_wrt_mbly)
# Add the lanes to the image copy
I = self.addLaneToImg(I, lanes_wrt_mbly)
# Add the objects (cars) to the cloud
mbly_objs = self.mbly_loader.loadObj(self.cur_gps_time)
objs_wrt_mbly = self.mblyObjAsNp(mbly_objs)
self.addObjToCloud(objs_wrt_mbly)
# Add the lanes to the image copy
I = self.addObjToImg(I, objs_wrt_mbly)
vtkimg = VtkImage(I)
self.img_ren.RemoveActor(self.img_actor)
self.img_actor = vtkimg.get_vtk_image()
self.img_ren.AddActor(self.img_actor)
# We need to draw the image before we run ResetCamera or else
# the image is too small
self.img_ren.ResetCamera()
img_cam.SetClippingRange(100, 100000) # These units are pixels
img_cam.Dolly(2.00)
self.iren.GetRenderWindow().Render()
def xformMblyToGlobal(self, pts_wrt_mbly):
# TODO: Need to tranform from imu to gps frame of reference
T_l_to_i = self.params['lidar']['T_from_l_to_i']
T_i_to_world = self.imu_transforms_mk1[self.t, 0:3, 0:4]
pts = pts_wrt_mbly
# Puts points in lidar FOR
pts_wrt_lidar = T_from_mbly_to_lidar(pts_wrt_mbly, self.mbly_T, \
self.mbly_R[:3,:3])
# Make points homogoneous
hom_pts = np.hstack((pts_wrt_lidar[:, :3], np.ones((pts.shape[0], 1))))
# Put in imu FOR
pts_wrt_imu = np.dot(T_l_to_i, hom_pts.T).T
# Put in global FOR
pts_wrt_world = np.dot(T_i_to_world, pts_wrt_imu.T).T
# Add metadata back to output
pts_wrt_world = np.hstack((pts_wrt_world, pts[:, 3:]))
return pts_wrt_world
def mblyObjAsNp(self, mbly_objs):
"""Turns a mobileye object pb message into a numpy array with format:
[x, y, .7, length, width, type]
"""
pts_wrt_mbly = []
for obj in mbly_objs:
pt_wrt_mbly = [obj.pos_x, obj.pos_y, .7, obj.length, \
obj.width, obj.obj_type]
pts_wrt_mbly.append(pt_wrt_mbly)
return np.array(pts_wrt_mbly)
def mblyLaneAsNp(self, mbly_lane):
"""Turns a mobileye lane into a numpy array with format:
[C0, C1, C2, C3, lane_id, lane_type, view_range]
Y = C3*X^3 + C2*X^2 + C1*X + C0.
X is longitudinal distance from camera (positive right!)
Y is lateral distance from camera
lane_id is between -2 and 2, with -2 being the farthest left,
and 2 being the farthest right lane. There is no 0 id.
"""
lanes_wrt_mbly = []
for l in mbly_lane:
lane_wrt_mbly = [l.C0, l.C1, l.C2, l.C3, l.lane_id, l.lane_type, \
l.view_range]
lanes_wrt_mbly.append(lane_wrt_mbly)
return np.array(lanes_wrt_mbly)
def addObjToCloud(self, objs_wrt_mbly):
""" Add the mobileye returns to the 3d scene """
mbly_vtk_boxes = []
car_rot = self.imu_transforms_mk1[self.t, :, :3]
objs_wrt_world = self.xformMblyToGlobal(objs_wrt_mbly)
# Draw each box
car_rot = euler_from_matrix(car_rot)[2] * 180 / math.pi
for o in objs_wrt_world:
# Create the vtk object
box = VtkBoundingBox(o)
actor = box.get_vtk_box(car_rot)
color = self.mbly_obj_colors[int(o[5])]
actor.GetProperty().SetColor(*color)
mbly_vtk_boxes.append(box)
# Remove old boxes
for vtk_box in self.mbly_vtk_boxes:
self.cloud_ren.RemoveActor(vtk_box.actor)
# Update to new actors
self.mbly_vtk_boxes = mbly_vtk_boxes
# Draw new boxes
for vtk_box in self.mbly_vtk_boxes:
self.cloud_ren.AddActor(vtk_box.actor)
def getLanePointsFromModel(self, lane_wrt_mbly):
view_range = lane_wrt_mbly[6]
if view_range == 0:
return None
num_pts = view_range * 3
X = np.linspace(0, view_range, num=num_pts)
# from model: Y = C3*X^3 + C2*X^2 + C1*X + C0.
X = np.vstack((np.ones(X.shape), X, np.power(X, 2), np.power(X, 3)))
# Mbly uses Y-right as positive, we use Y-left as positive
Y = -1 * np.dot(lane_wrt_mbly[:4], X)
lane_pts_wrt_mbly = np.vstack((X[1, :], Y, np.zeros((1, num_pts)))).T
return lane_pts_wrt_mbly
def addLaneToCloud(self, lane_wrt_mbly):
for lane in self.mbly_vtk_lanes:
self.cloud_ren.RemoveActor(lane.actor)
self.mbly_vtk_lanes = []
for i in xrange(lane_wrt_mbly.shape[0]):
type = int(lane_wrt_mbly[i, 5])
color = self.mbly_lane_color[type] * 255
size = self.mbly_lane_size[type]
subsamp = self.mbly_lane_subsamp[type]
lane_pts_wrt_mbly = self.getLanePointsFromModel(
lane_wrt_mbly[i, :])
if lane_pts_wrt_mbly is None:
continue
pts_wrt_global = self.xformMblyToGlobal(lane_pts_wrt_mbly)
pts_wrt_global = pts_wrt_global[::subsamp]
num_pts = pts_wrt_global.shape[0]
vtk_lane = VtkPointCloud(pts_wrt_global,
np.tile(color, (num_pts, 1)))
actor = vtk_lane.get_vtk_color_cloud()
actor.GetProperty().SetPointSize(size)
self.mbly_vtk_lanes.append(vtk_lane)
self.cloud_ren.AddActor(actor)
def addObjToImg(self, I, objs_wrt_mbly):
"""Takes an image and the mbly objects. Converts the objects into corners of a
bounding box and draws them on the image
"""
if objs_wrt_mbly.shape[0] == 0:
return None
pix = []
width = objs_wrt_mbly[:, 4]
# Assuming the point in obs_wrt_mbly are the center of the object, draw
# a box .5 m below, .5 m above, -.5*width left, .5*width right. Keep the
# same z position
for z in [-.5, .5]:
for y in [-.5, .5]:
offset = np.zeros((width.shape[0], 3))
offset[:, 1] = width * y
offset[:, 2] = z
pt = objs_wrt_mbly[:, :3] + offset
proj_pt = projectPoints(pt, self.args, self.mbly_T,
self.mbly_R)
pix.append(proj_pt[:, 3:])
pix = np.array(pix, dtype=np.int32)
pix = np.swapaxes(pix, 0, 1)
# Draw a line between projected points
for i, corner in enumerate(pix):
# Get the color of the box and convert RGB -> BGR
color = self.mbly_obj_colors[int(objs_wrt_mbly[i, 5])][::-1] * 255
corner = tuple(map(tuple, corner))
cv2.rectangle(I, corner[0], corner[3], color, 2)
return I
def addLaneToImg(self, I, lanes_wrt_mbly):
"""Takes an image and the 3d lane points. Projects these points onto the image
"""
if lanes_wrt_mbly.shape[0] == 0:
return None
pix = []
for i in xrange(len(self.mbly_vtk_lanes)):
type = int(lanes_wrt_mbly[i, 5])
color = self.mbly_lane_color[type] * 255
size = self.mbly_lane_size[type]
subsamp = self.mbly_lane_subsamp[type]
pts = self.getLanePointsFromModel(lanes_wrt_mbly[i])[::subsamp]
if pts is None:
continue
proj_pts = projectPoints(pts, self.args, self.mbly_T, self.mbly_R)
proj_pts = proj_pts[:, 3:].astype(np.int32, copy=False)
img_mask = (proj_pts[:, 0] < 1280) & (proj_pts[:, 0] >= 0) &\
(proj_pts[:, 1] < 800) & (proj_pts[:, 1] >= 0)
proj_pts = proj_pts[img_mask]
for pt_i in xrange(proj_pts.shape[0]):
# cv2 only takes tuples at points
pt = tuple(proj_pts[pt_i, :])
# Make sure to convert to bgr
cv2.circle(I, pt, size, color[::-1], thickness=-size)
return I
