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 __init__(self):
self.start = 0
self.step = 5
self.end = self.step * 500
self.count = 0
self.ren = vtk.vtkRenderer()
args = parse_args(sys.argv[1], sys.argv[2])
# Transforms
self.imu_transforms = get_transforms(args)
self.trans_wrt_imu = self.imu_transforms[self.start:self.end:self.step,
0:3, 3]
self.params = args['params']
self.lidar_params = self.params['lidar']
ml = MultiLane(sys.argv[3], sys.argv[4], 2, 2)
ml.extendLanes()
saveInterp(ml.interp, ml.rightLanes + ml.leftLanes)
ml.filterLaneMarkings()
print 'Adding filtered points'
pts = ml.lanes.copy()
raw_cloud = VtkPointCloud(pts[:, :3], pts[:, 4])
raw_actor = raw_cloud.get_vtk_cloud(zMin=0, zMax=100)
self.ren.AddActor(raw_actor)
try:
npz = np.load('cluster.npz')
print 'Loading clusters from file'
ml.lanes = npz['data']
ml.times = npz['t']
except IOError:
print 'Clustering points'
ml.clusterLanes()
ml.saveLanes('cluster.npz')
ml.sampleLanes()
print 'Adding clustered points'
clusters = ml.lanes.copy()
cluster_cloud = VtkPointCloud(clusters[:, :3], clusters[:, -2])
cluster_actor = cluster_cloud.get_vtk_cloud(zMin=0, zMax=4)
cluster_actor.GetProperty().SetPointSize(10)
self.ren.AddActor(cluster_actor)
print 'Interpolating lanes'
ml.interpolateLanes()
interp_lanes = ml.interp_lanes.copy()
interp_lanes_cloud = VtkPointCloud(interp_lanes[:, :3],
interp_lanes[:, 3])
interp_lanes_actor = interp_lanes_cloud.get_vtk_cloud(zMin=0, zMax=4)
self.ren.AddActor(interp_lanes_actor)
# ml.fixMissingPoints()
# saveClusters(ml.lanes, ml.times, -1, 5)
print 'Adding car'
self.car = load_ply('../mapping/viz/gtr.ply')
self.ren.AddActor(self.car)
self.car.GetProperty().LightingOff()
print 'Rendering'
self.ren.ResetCamera()
self.win = vtk.vtkRenderWindow()
self.ren.SetBackground(0, 0, 0)
self.win.AddRenderer(self.ren)
self.win.SetSize(800, 400)
self.iren = vtk.vtkRenderWindowInteractor()
self.iren.SetRenderWindow(self.win)
mouseInteractor = vtk.vtkInteractorStyleTrackballCamera()
self.iren.SetInteractorStyle(mouseInteractor)
self.iren.Initialize()
# Whether to write video
self.record = False
# Set up time
self.iren.AddObserver('TimerEvent', self.update)
self.timer = self.iren.CreateRepeatingTimer(100)
# Add keypress event
self.iren.AddObserver('KeyPressEvent', self.keyhandler)
self.mode = 'ahead'
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()
def __init__(self):
self.start = EXPORT_START
self.end = EXPORT_START + EXPORT_NUM * EXPORT_STEP
self.step = EXPORT_STEP
self.count = 0
self.ren = vtk.vtkRenderer()
''' Transforms '''
self.imu_transforms = get_transforms()
self.trans_wrt_imu = self.imu_transforms[self.start:self.end:self.step,
0:3, 3]
self.params = LoadParameters('q50_4_3_14_params')
self.radar_params = self.params['radar']
self.lidar_params = self.params['lidar']
''' Radar '''
self.rdr_pts = loadRDRCamMap(MAP_FILE)
self.radar_actors = []
print 'Adding transforms'
gps_cloud = VtkPointCloud(self.trans_wrt_imu[:, 0:3],
0 * self.trans_wrt_imu[:, 0])
self.ren.AddActor(gps_cloud.get_vtk_cloud())
#print 'Adding octomap'
#octomap_actor = load_octomap(OCTOMAP_H5_FILE)
#self.ren.AddActor(octomap_actor)
print 'Adding point cloud'
cloud_actor = load_vtk_cloud(STATIC_VTK_FILE)
self.ren.AddActor(cloud_actor)
#dynamic_actor = load_vtk_cloud(DYNAMIC_VTK_FILE)
#dynamic_actor.GetProperty().SetColor(0, 0, 1)
#dynamic_actor.GetMapper().ScalarVisibilityOff()
#self.ren.AddActor(dynamic_actor)
print 'Adding car'
self.car = load_ply('gtr.ply')
self.ren.AddActor(self.car)
print 'Rendering'
self.ren.ResetCamera()
self.win = vtk.vtkRenderWindow()
self.win.AddRenderer(self.ren)
self.win.SetSize(400, 400)
self.iren = vtk.vtkRenderWindowInteractor()
self.iren.SetRenderWindow(self.win)
mouseInteractor = vtk.vtkInteractorStyleTrackballCamera()
self.iren.SetInteractorStyle(mouseInteractor)
self.iren.Initialize()
# Whether to write video
self.record = False
# Set up time
self.iren.AddObserver('TimerEvent', self.update)
self.timer = self.iren.CreateRepeatingTimer(100)
# Add keypress event
self.iren.AddObserver('KeyPressEvent', self.keyhandler)
self.mode = 'ahead'
self.iren.Start()