예제 #1
0
    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)
예제 #2
0
    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)
예제 #3
0
    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()
예제 #4
0
    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()
예제 #5
0
    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()
예제 #6
0
    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()
예제 #7
0
    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()
예제 #8
0
    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()