def __init__(self, use_kinect=False, basket=False, gui=True): self.connected = False self.max_radius = 100000 self.running = False self.killed = False self.gui = gui self.basket = basket self.use_kinect = use_kinect # self.fig, self.ax = plt.subplots() if self.use_kinect: self.px = [] self.py = [] logger = createConsoleLogger(LoggerLevel.Error) setGlobalLogger(logger) while not self.connected: # Bad, but needed if self.use_kinect: self.connect_kinect() else: self.connect_camera() self.thread = threading.Thread(target=self.video_thread) self.thread.start()
def _start_kinect(self): if self.device == 'kinect': freenect.sync_get_depth() #Grabbing a frame initializes the device freenect.sync_get_video() elif self.device == 'kinect2': # a: Identify pipeline to use: 1) OpenGL, 2) OpenCL, 3) CPU try: self.pipeline = FN2.OpenCLPacketPipeline() except: try: self.pipeline = FN2.OpenGLPacketPipeline() except: self.pipeline = FN2.CpuPacketPipeline() self._print('PacketPipeline: ' + type(self.pipeline).__name__) # b: Create and set logger self.logger = FN2.createConsoleLogger(FN2.LoggerLevel.NONE) FN2.setGlobalLogger(self.logger) # c: Identify device and create listener self.fn = FN2.Freenect2() serial = self.fn.getDeviceSerialNumber(0) self.K2device = self.fn.openDevice(serial, pipeline=self.pipeline) self.listener = FN2.SyncMultiFrameListener( FN2.FrameType.Color | FN2.FrameType.Depth) # d: Register listeners self.K2device.setColorFrameListener(self.listener) self.K2device.setIrAndDepthFrameListener(self.listener) # e: Start device and create registration self.K2device.start() self.registration = FN2.Registration(self.K2device.getIrCameraParams(), self.K2device.getColorCameraParams())
def __init__(self, device_id=0): """Create an OpenCV capture object associated with the provided webcam device ID. """ logger = createConsoleLogger(LoggerLevel.Error) setGlobalLogger(logger) self.fn = Freenect2() num_devices = self.fn.enumerateDevices() if num_devices == 0: print("No Kinect!") raise LookupError() serial = self.fn.getDeviceSerialNumber(0) self.device = self.fn.openDevice(serial, pipeline=pipeline) types = 0 types |= FrameType.Color self.listener = SyncMultiFrameListener(types) self.device.setColorFrameListener(self.listener) # Start a thread to continuously capture frames. # This must be done because different layers of buffering in the webcam # and OS drivers will cause you to retrieve old frames if they aren't # continuously read. self._capture_frame = None # Use a lock to prevent access concurrent access to the camera. self._capture_lock = threading.Lock() self._capture_thread = threading.Thread(target=self._grab_frames) self._capture_thread.daemon = True self._capture_thread.start()
def __init__(self, device_no=0): # Package Pipeline try: from pylibfreenect2 import OpenGLPacketPipeline self.pipeline = OpenGLPacketPipeline() except: try: from pylibfreenect2 import OpenCLPacketPipeline self.pipeline = OpenCLPacketPipeline() except: from pylibfreenect2 import CpuPacketPipeline self.pipeline = CpuPacketPipeline() print "Packet pipeline: {}".format(type(self.pipeline).__name__) # Create and set logger logger = createConsoleLogger(LoggerLevel.Error) setGlobalLogger(logger) # Detect Kinect Devices self.fn = Freenect2() num_devices = self.fn.enumerateDevices() if num_devices == 0: print("No device connected!") self.device = None sys.exit(1) # Create Device and Frame Listeners self.serial = self.fn.getDeviceSerialNumber(device_no) self.device = self.fn.openDevice(self.serial, pipeline=self.pipeline) self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register Listeners self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) # Start Device self.device.start() # Set Camera Parameters self._ir_params = self.device.getIrCameraParams() self._color_params = self.device.getColorCameraParams() ''' self._ir_params.fx = cameraParams.params['ir_fx'] self._ir_params.fy = cameraParams.params['ir_fy'] self._ir_params.cx = cameraParams.params['ir_cx'] self._ir_params.cy = cameraParams.params['ir_cy'] self._color_params.fx = cameraParams.params['col_fx'] self._color_params.fy = cameraParams.params['col_fy'] self._color_params.cx = cameraParams.params['col_cx'] self._color_params.cy = cameraParams.params['col_cy'] ''' # Registration self.registration = Registration(self._ir_params, self._color_params) print '[Info] Initialization Finished!'
def start(self): """Starts the Kinect v2 sensor stream. Raises ------ IOError If the Kinect v2 is not detected. """ # open packet pipeline if self._packet_pipeline_mode == Kinect2PacketPipelineMode.OPENGL: self._pipeline = lf2.OpenGLPacketPipeline() elif self._packet_pipeline_mode == Kinect2PacketPipelineMode.CPU: self._pipeline = lf2.CpuPacketPipeline() # setup logger self._logger = lf2.createConsoleLogger(lf2.LoggerLevel.Warning) lf2.setGlobalLogger(self._logger) # check devices self._fn_handle = lf2.Freenect2() self._num_devices = self._fn_handle.enumerateDevices() if self._num_devices == 0: raise IOError( 'Failed to start stream. No Kinect2 devices available!') if self._num_devices <= self._device_num: raise IOError( 'Failed to start stream. Device num %d unavailable!' % (self._device_num)) # open device self._serial = self._fn_handle.getDeviceSerialNumber(self._device_num) self._device = self._fn_handle.openDevice(self._serial, pipeline=self._pipeline) # add device sync modes self._listener = lf2.SyncMultiFrameListener(lf2.FrameType.Color | lf2.FrameType.Ir | lf2.FrameType.Depth) self._device.setColorFrameListener(self._listener) self._device.setIrAndDepthFrameListener(self._listener) # start device self._device.start() # open registration self._registration = None if self._registration_mode == Kinect2RegistrationMode.COLOR_TO_DEPTH: logging.debug('Using color to depth registration') self._registration = lf2.Registration( self._device.getIrCameraParams(), self._device.getColorCameraParams()) self._running = True
def __init__(self): try: from pylibfreenect2 import OpenCLPacketPipeline pipeline = OpenCLPacketPipeline() except: try: from pylibfreenect2 import OpenGLPacketPipeline pipeline = OpenGLPacketPipeline() except: from pylibfreenect2 import CpuPacketPipeline pipeline = CpuPacketPipeline() print("Packet pipeline:", type(pipeline).__name__) # Create and set logger logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger(logger) self.freenect = Freenect2() num_devices = self.freenect.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) serial = self.freenect.getDeviceSerialNumber(0) self.device = self.freenect.openDevice(serial, pipeline=pipeline) self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) self.device.start() self.registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) self.run_loop = True self.lock = threading.Lock() self.img_buffer = np.zeros( (RES_COLOR[1], RES_COLOR[0], 3, SIZE_AVG_FILTER)) self.idx_buffer = 0 # Close on Ctrl-C def signal_handler(signal, frame): self.run_loop = False signal.signal(signal.SIGINT, signal_handler)
def __init__(self, calibration_folder=None): self.freenect = Freenect2() setGlobalLogger(createConsoleLogger(LoggerLevel.NONE)) num_devices = self.freenect.enumerateDevices() if num_devices == 0: raise ValueError("No kinect2 device connected!") serial = self.freenect.getDeviceSerialNumber(0) self.device = self.freenect.openDevice(serial, pipeline=pipeline) self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) self.device.start() self.calibration = {} if calibration_folder is not None: self.calibration = utils.load_calibration(calibration_folder)
def test_logger(): logger_default = createConsoleLoggerWithDefaultLevel() assert isinstance(logger_default, Logger) for level in [ LoggerLevel.NONE, LoggerLevel.Error, LoggerLevel.Warning, LoggerLevel.Info, LoggerLevel.Debug ]: logger = createConsoleLogger(level) setGlobalLogger(logger) assert getGlobalLogger().level() == level # Turn logging off setGlobalLogger(None) # Set to default setGlobalLogger(logger_default) logger = getGlobalLogger() if sys.version_info.major >= 3: message = b"test debugging message" else: message = "test debugging message" logger.log(LoggerLevel.Debug, message)
def test_logger(): logger_default = createConsoleLoggerWithDefaultLevel() assert isinstance(logger_default, Logger) for level in [LoggerLevel.NONE, LoggerLevel.Error, LoggerLevel.Warning, LoggerLevel.Info, LoggerLevel.Debug]: logger = createConsoleLogger(level) setGlobalLogger(logger) assert getGlobalLogger().level() == level # Turn logging off setGlobalLogger(None) # Set to default setGlobalLogger(logger_default) logger = getGlobalLogger() if sys.version_info.major >= 3: message = b"test debugging message" else: message = "test debugging message" logger.log(LoggerLevel.Debug, message)
def __init__(self): self.averageSpineX = 0 self.fn = Freenect2() if self.fn.enumerateDevices() == 0: sys.exit(47) self.serial = self.fn.getDeviceSerialNumber(0) types = 0 types |= FrameType.Color types |= (FrameType.Ir | FrameType.Depth) self.listener = SyncMultiFrameListener(types) self.logger = createConsoleLogger(LoggerLevel.Debug) self.device = self.fn.openDevice(self.serial) self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) self.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4)
def start(self): """Starts the Kinect v2 sensor stream. Raises ------ IOError If the Kinect v2 is not detected. """ # setup logger self._logger = lf2.createConsoleLogger(lf2.LoggerLevel.Warning) lf2.setGlobalLogger(self._logger) # open packet pipeline self._pipeline = None if (self._packet_pipeline_mode == Kinect2PacketPipelineMode.OPENGL or self._packet_pipeline_mode == Kinect2PacketPipelineMode.AUTO): # Try OpenGL packet pipeline first or if specified try: self._pipeline = lf2.OpenGLPacketPipeline() except BaseException: logging.warning("OpenGL not available. " "Defaulting to CPU-based packet pipeline.") if self._pipeline is None and (self._packet_pipeline_mode == Kinect2PacketPipelineMode.OPENCL or self._packet_pipeline_mode == Kinect2PacketPipelineMode.AUTO): # Try OpenCL if available try: self._pipeline = lf2.OpenCLPacketPipeline() except BaseException: logging.warning( "OpenCL not available. Defaulting to CPU packet pipeline.") if (self._pipeline is None or self._packet_pipeline_mode == Kinect2PacketPipelineMode.CPU): # CPU packet pipeline self._pipeline = lf2.CpuPacketPipeline() # check devices self._fn_handle = lf2.Freenect2() self._num_devices = self._fn_handle.enumerateDevices() if self._num_devices == 0: raise IOError( "Failed to start stream. No Kinect2 devices available!") if self._num_devices <= self._device_num: raise IOError( "Failed to start stream. Device num %d unavailable!" % (self._device_num)) # open device self._serial = self._fn_handle.getDeviceSerialNumber(self._device_num) self._device = self._fn_handle.openDevice(self._serial, pipeline=self._pipeline) # add device sync modes self._listener = lf2.SyncMultiFrameListener(lf2.FrameType.Color | lf2.FrameType.Ir | lf2.FrameType.Depth) self._device.setColorFrameListener(self._listener) self._device.setIrAndDepthFrameListener(self._listener) # start device self._device.start() # open registration self._registration = None if self._registration_mode == Kinect2RegistrationMode.COLOR_TO_DEPTH: logging.debug("Using color to depth registration") self._registration = lf2.Registration( self._device.getIrCameraParams(), self._device.getColorCameraParams(), ) self._running = True
class MainApp(App): def build(self): self.root = screenmanager return self.root def on_stop(self): screenmanager.get_screen('screen_capture').ids.cam_id.device.stop() screenmanager.get_screen('screen_capture').ids.cam_id.device.close() if __name__ == '__main__': from pylibfreenect2 import OpenCLPacketPipeline pipeline = OpenCLPacketPipeline() print("Packet pipeline:", type(pipeline).__name__) # Create and set logger logger = createConsoleLogger(LoggerLevel.Warning) #(LoggerLevel.Debug) setGlobalLogger(logger) fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) screenmanager = ScreenManagerLang() screenmanager.add_widget(ScreenStart(name="screen_start")) screenmanager.add_widget(ScreenSelection(name="screen_selection")) screenmanager.add_widget(ScreenPainting(name="screen_painting")) screenmanager.add_widget(ScreenFrame(name="screen_frame")) screenmanager.add_widget(ScreenBackground(name="screen_background")) screenmanager.add_widget(ScreenStyle(name="screen_style"))
def projection(): pipeline = CpuPacketPipeline() print("Packet pipeline:", type(pipeline).__name__) # Create and set logger logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger(logger) fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=pipeline) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # NOTE: must be called after device.start() registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) # Optinal parameters for registration # set True if you need need_bigdepth = True need_color_depth_map = True bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None color_depth_map = np.zeros((424, 512), np.int32).ravel() \ if need_color_depth_map else None color_pub = rospy.Publisher('color', Image, queue_size=10) depth_pub = rospy.Publisher('depth', Image, queue_size=10) ir_pub = rospy.Publisher('ir', Image, queue_size=10) small_depth_pub = rospy.Publisher('small_depth', Image, queue_size=10) rospy.init_node('projection', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): frames = listener.waitForNewFrame() color = frames["color"] ir = frames["ir"] depth = frames["depth"] registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth, color_depth_map=color_depth_map) color_image = CvBridge().cv2_to_imgmsg(color.asarray()) color_pub.publish(color_image) depth_image = CvBridge().cv2_to_imgmsg(bigdepth.asarray(np.float32)) depth_pub.publish(depth_image) ir_image = CvBridge().cv2_to_imgmsg(ir.asarray()) ir_pub.publish(ir_image) small_depth_image = CvBridge().cv2_to_imgmsg(depth.asarray()) small_depth_pub.publish(small_depth_image) rate.sleep() listener.release(frames) key = cv2.waitKey(delay=1) if key == ord('q'): break device.stop() device.close() sys.exit(0)
def main(): try: from pylibfreenect2 import OpenCLPacketPipeline pipeline = OpenCLPacketPipeline() except: from pylibfreenect2 import CpuPacketPipeline pipeline = CpuPacketPipeline() # Create and set logger logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger(logger) fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=pipeline) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # NOTE: must be called after device.start() registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) # Optinal parameters for registration # set True if you need need_bigdepth = False need_color_depth_map = False bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None color_depth_map = np.zeros((424, 512), np.int32).ravel() \ if need_color_depth_map else None point = pcl.PointCloud() visual = pcl.pcl_visualization.CloudViewing() visual.ShowColorCloud(cloud) while True: frames = listener.waitForNewFrame() color = frames["color"] ir = frames["ir"] depth = frames["depth"] registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth, color_depth_map=color_depth_map) # NOTE for visualization: # cv2.imshow without OpenGL backend seems to be quite slow to draw all # things below. Try commenting out some imshow if you don't have a fast # visualization backend. # cv2.imshow("ir", ir.asarray() / 65535.) # cv2.imshow("depth", depth.asarray() / 4500.) # cv2.imshow("color", cv2.resize(color.asarray(), (int(1920 / 3), int(1080 / 3)))) # cv2.imshow("registered", registered.asarray(np.uint8)) # if need_bigdepth: # cv2.imshow("bigdepth", cv2.resize(bigdepth.asarray(np.float32), # (int(1920 / 3), int(1082 / 3)))) # if need_color_depth_map: # cv2.imshow("color_depth_map", color_depth_map.reshape(424, 512)) undistorted_arrray = undistorted.asarray(dtype=np.float32, ndim=2) # registered_array = registered.asarray(dtype=np.uint8) point = pcl.PointCloud(undistorted_arrray) # visual.ShowColorCloud(cloud) listener.release(frames) # key = cv2.waitKey(delay=1) # if key == ord('q'): # break v = True while v: v = not (visual.WasStopped()) device.stop() device.close() sys.exit(0)
def main(): t1=time.time() inputPath = "DataSet/" IDfile = open(IDFilePath,mode='a') try: from pylibfreenect2 import OpenCLPacketPipeline pipeline = OpenCLPacketPipeline() print("Using OpenCL") input() except: try: from pylibfreenect2 import OpenGLPacketPipeline pipeline = OpenGLPacketPipeline() except: from pylibfreenect2 import CpuPacketPipeline pipeline = CpuPacketPipeline() #print("Packet pipeline:", type(pipeline).__name__) # Create and set logger logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger(logger) fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=pipeline) #print("FirmwareVersion: "+ firmware) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() g.InitializeCamera() h.InitializeCameras() t2=time.time() print("Initialization before the loop "+str(t2-t1)) while True: t3=time.time() t_ms = time.time() ts = datetime.datetime.now() print(t_ms) t_ms = t_ms * 1000 t_ms = str(t_ms).split('.')[0] print(t_ms) imageID = t_ms print(str(ts).split('.')[0]) ts = str(ts).split('.')[0] tsList1 = str(ts).split(' ') date = tsList1[0] tm = tsList1[1] print(tsList1) tsListTime = str(tsList1[1]).split(':') tsTime = str(tsListTime[0]) + "." + str(tsListTime[1]) + "." + str(tsListTime[2]) print(tsTime) ts = str(tsList1[0]) + "_" + tsTime print(ts) t4=time.time() print("Starting part of the while loop: "+str(t4-t3)) print("Press \'Enter\' to trigger the acquisition of images or \'q\' to Quit") key = input() print(key) if(not key): t5=time.time() captureKinect(device, serial, listener, ts, imageID) t0=time.time() g.TakeImages(imageID, ts) t9=time.time() h.TakeImages(imageID, ts) t6 = time.time() writeToXML(date, tm, ts, imageID) t7 = time.time() writeIDtoFile(IDfile,imageID) t8=time.time() '''ans = input("Display the images captured? (y/n)") if(ans=='y'): displayImagesCaptured(inputPath, ts, imageID) t9=time.time()''' print("Kinect Capture took "+str(t0-t5)) print("Ensenso took "+str(t9-t0)) print("GiGE took "+str(t6-t9)) print("Writing to XML took "+str(t7-t6)) print("Writing ID took "+ str(t8-t7)) #print("Displaying images took "+str(t6-t5)) print("Total time after pressing enter: "+str(t8-t5)) print("Total loop time: "+str(t8-t3)) elif(key =='q'): ta=time.time() closeKinect(device) g.ShutdownCamera() h.ShutdownCameras() tb=time.time() print("Close "+str(tb-ta)) #print("Overall "+str(tb-t11)) IDfile.close() sys.exit(0) break
cnt_len = cv2.arcLength(cnt, True) cnt = cv2.approxPolyDP(cnt, 0.02 * cnt_len, True) if len(cnt) == 4 and cv2.contourArea( cnt) > 500 and cv2.isContourConvex(cnt): cnt = cnt.reshape(-1, 2) max_cos = np.max([ angle_cos(cnt[i], cnt[(i + 1) % 4], cnt[(i + 2) % 4]) for i in xrange(4) ]) if max_cos < 0.1: squares.append(cnt) return squares # Create and set logger logger = createConsoleLogger(LoggerLevel.Error) setGlobalLogger(logger) fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=pipeline) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners
import cv2 import sys from pylibfreenect2 import Freenect2, SyncMultiFrameListener from pylibfreenect2 import FrameType, Registration, Frame from pylibfreenect2 import createConsoleLogger, setGlobalLogger from pylibfreenect2 import LoggerLevel try: from pylibfreenect2 import OpenGLPacketPipeline pipeline = OpenGLPacketPipeline() except: from pylibfreenect2 import CpuPacketPipeline pipeline = CpuPacketPipeline() # Create and set logger logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger(logger) fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=pipeline) listener = SyncMultiFrameListener( FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners
def CaptureVideo(): logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger(logger) fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=pipeline) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # NOTE: must be called after device.start() registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) # Optinal parameters for registration # set True if you need need_bigdepth = True need_color_depth_map = False bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None color_depth_map = np.zeros((424, 512), np.int32).ravel() \ if need_color_depth_map else None # cap = cv2.VideoCapture(0) res_old = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1] while (True): # Capture frame-by-frame # ret, frame = cap.read() frames = listener.waitForNewFrame() img = frames["color"].asarray()[:, :, :3] frame = cv2.resize(img, (int(1920 / 3), int(1080 / 3))) start = time.time() # Display the resulting frame box, conf, cls = eval_video_detection(net_detection_, frame) p_box = [] for i in range(len(box)): p_box_per_person = {} if int(cls[i]) == 15 and conf[i] > 0.0: p_box_per_person['p1'] = (box[i][0], box[i][1]) p_box_per_person['p2'] = (box[i][2], box[i][3]) p_box_per_person['height_difference'] = box[i][3] - box[i][1] p_box_per_person['weight_difference'] = box[i][2] - box[i][0] p_box_per_person['conf'] = conf[i] p_box_per_person['cls'] = int(cls[i]) p_box.append(p_box_per_person) # print p_box if len(p_box) == 0: cv2.putText(frame, Category_labels_[11], (30, 60), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 0, 255), 2) cv2.imshow('Webcam', frame) else: p_box = sorted(p_box, key=lambda e: e.__getitem__('weight_difference'), reverse=True) p_hh_low = float(p_box[0]['p1'][1]) - (1 / float( (p_box[0]['p2'][1] - p_box[0]['p1'][1])) ) * 2000.0 # 记录剪裁框的位置大小 p_hh_hige = float(p_box[0]['p2'][1]) + (1 / float( (p_box[0]['p2'][1] - p_box[0]['p1'][1]))) * 2000.0 print p_box[0]['p2'][0] - p_box[0]['p1'][0] if p_box[0]['p2'][0] - p_box[0]['p1'][0] > 400: Penalty_ratio_factor = 30000.0 elif p_box[0]['p2'][0] - p_box[0]['p1'][0] > 300: Penalty_ratio_factor = 15000.0 elif p_box[0]['p2'][0] - p_box[0]['p1'][0] > 200: Penalty_ratio_factor = 8000.0 elif p_box[0]['p2'][0] - p_box[0]['p1'][0] > 100: Penalty_ratio_factor = 3000.0 else: Penalty_ratio_factor = 1000.0 p_ww_low = float(p_box[0]['p1'][0]) - (1 / float( (p_box[0]['p2'][0] - p_box[0]['p1'][0])) ) * Penalty_ratio_factor p_ww_hige = float(p_box[0]['p2'][0]) + (1 / float( (p_box[0]['p2'][0] - p_box[0]['p1'][0])) ) * Penalty_ratio_factor cropframe = frame[ int(max(p_hh_low, 0)):int(min(p_hh_hige, frame.shape[0])), int(max(p_ww_low, 0)):int(min(p_ww_hige, frame.shape[1]))] res = eval_video_classification(net_classification_, cropframe, res_old) cv2.imshow('CropWebcam', cropframe) cv2.rectangle(frame, p_box[0]['p1'], p_box[0]['p2'], (0, 0, 255)) p3 = (max(p_box[0]['p1'][0], 15), max(p_box[0]['p1'][1], 15)) title = "%s:%.2f" % (CLASSES[int(15)], conf[i]) cv2.putText(frame, title, p3, cv2.FONT_ITALIC, 0.6, (0, 255, 0), 1) print res res_old = res #print np.max(res) if np.max(res) < 0.9: cv2.putText(frame, Category_labels_[11], (30, 60), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 0, 255), 2) else: Category_res = np.argmax(res) Category_res = (Category_res > 11 and 11 or Category_res) # print Category_res cv2.putText(frame, Category_labels_[Category_res], (30, 60), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 0, 255), 2) cv2.imshow('Webcam', frame) end = time.time() seconds = end - start # Calculate frames per second fps = 1 / seconds print("fps: {0}".format(fps)) # Wait to press 'q' key for break if cv2.waitKey(1) & 0xFF == ord('q'): break listener.release(frames) # When everything done, release the capture # cap.release() device.stop() device.close() cv2.destroyAllWindows() return
body=msg.SerializeToString()) save_frames = False num_frames = 100 frame_i = 0 #clear test frames if there are any left if len(sys.argv) > 1: if sys.argv[1] == "test": files = glob.glob('/test_frames/*') save_frames = True for f in files: os.remove(f) # Create and set logger logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger(logger) fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=pipeline) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners
def __init__(self): try: from pylibfreenect2 import OpenGLPacketPipeline self._pipeline = OpenGLPacketPipeline() except: try: from pylibfreenect2 import OpenCLPacketPipeline self._pipeline = OpenCLPacketPipeline() except: from pylibfreenect2 import CpuPacketPipeline self._pipeline = CpuPacketPipeline() print("Packet pipeline:", type(self._pipeline).__name__) pygame.init() # Used to manage how fast the screen updates self._clock = pygame.time.Clock() # Set the width and height of the screen [width, height] self._screen_width = 500 self._screen_heigth = 300 self._screen = pygame.display.set_mode((self._screen_width, self._screen_heigth), pygame.HWSURFACE|pygame.DOUBLEBUF|pygame.RESIZABLE, 32) # Set the titel of the screen- window pygame.display.set_caption("Kamera") # Loop until the user clicks the close button. self._done = False # Kinect runtime object, we want only color and body frames #self._kinect = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Color) # Create and set logger logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger(logger) self._fn = Freenect2() self._num_devices = self._fn.enumerateDevices() if self._num_devices == 0: print("No device connected!") sys.exit(1) self._serial = self._fn.getDeviceSerialNumber(0) self._device = self._fn.openDevice(self._serial, pipeline=self._pipeline) self._listener = SyncMultiFrameListener( FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners self._device.setColorFrameListener(self._listener) self._device.setIrAndDepthFrameListener(self._listener) self._device.start() # NOTE: must be called after device.start() self._registration = Registration(self._device.getIrCameraParams(), self._device.getColorCameraParams()) self._undistorted = Frame(512, 424, 4) self._registered = Frame(512, 424, 4) # Optinal parameters for registration # set True if you need self._need_bigdepth = False self._need_color_depth_map = False # back buffer surface for getting Kinect color frames, 32bit color, width and height equal to the Kinect color frame size #self._frame_surface = pygame.Surface((self._kinect.color_frame_desc.Width, self._kinect.color_frame_desc.Height), 0, 32) self._bigdepth = Frame(1920, 1082, 4) if self._need_bigdepth else None self._color_depth_map = np.zeros((424, 512), np.int32).ravel() if self._need_color_depth_map else None self._frame_surface = pygame.Surface((self._registered.width, self._registered.height), 0, 32)
def main(lata, longa, latb, longb): lata = conv.convert_gps(float(lata)) longa = conv.convert_gps(float(longa)) latb = conv.convert_gps(float(latb)) longb = conv.convert_gps(float(longb)) try: from pylibfreenect2 import OpenGLPacketPipeline pipeline = OpenGLPacketPipeline() except: try: from pylibfreenect2 import OpenCLPacketPipeline pipeline = OpenCLPacketPipeline() except: from pylibfreenect2 import CpuPacketPipeline pipeline = CpuPacketPipeline() print("Packet pipeline:", type(pipeline).__name__) #create list for gps location of obstacles ############################################################### # Create and set logger logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger(logger) fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=pipeline) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # NOTE: must be called after device.start() registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) # Optinal parameters for registration # set True if you need need_bigdepth = False need_color_depth_map = False bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None color_depth_map = np.zeros((424, 512), np.int32).ravel() \ if need_color_depth_map else None while True: frames = listener.waitForNewFrame() #time.sleep(5) color = frames["color"] ir = frames["ir"] depth = frames["depth"] registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth, color_depth_map=color_depth_map) # NOTE for visualization: # cv2.imshow without OpenGL backend seems to be quite slow to draw all # things below. Try commenting out some imshow if you don't have a fast # visualization backend. #cv2.imshow("ir", ir.asarray() / 65535.) #time.sleep(2) cv2.imshow("depth", depth.asarray() / 4500.) #Comment the coming lines print('center value is:', depth.asarray(np.float32).item((212, 256))) print('down1 value is:', depth.asarray(np.float32).item((270, 256))) '''print('up1 value is:',depth.asarray(np.float32).item((150,256))) print('up2 value is:',depth.asarray(np.float32).item((100,256))) print('down2 value is:',depth.asarray(np.float32).item((350,256))) print('right1 value is:',depth.asarray(np.float32).item((212,300))) print('right2 value is:',depth.asarray(np.float32).item((212,350))) print('left1 value is:',depth.asarray(np.float32).item((212,200))) print('left2 value is:',depth.asarray(np.float32).item((212,150)))''' x = depth.asarray(np.float32) y = np.logical_and(np.greater(x, 1200), np.less(x, 1500)) #print(np.extract(y, x)) no_of_pixels = np.count_nonzero(np.extract(y, x)) print(no_of_pixels) #get gps coordinate at this location######## ''' assumingthegps received is lat,long variables: obs.append([lat,long]) this is to be retrieved in the path planning code. ''' if no_of_pixels > 14000: #approx distance from the camera = 1.5 m print('big Obstacle found, stop!!!') obs.append(findPosition()) elif no_of_pixels > 8000: #approx distance from the camera = 1.5 m print('small Obstacle found!!') obs.append(findPosition()) #cv2.imshow("color", cv2.resize(color.asarray(),(int(1920 / 3), int(1080 / 3)))) #cv2.imshow("registered", registered.asarray(np.uint8)) if need_bigdepth: cv2.imshow( "bigdepth", cv2.resize(bigdepth.asarray(np.float32), (int(1920 / 3), int(1082 / 3)))) if need_color_depth_map: cv2.imshow("color_depth_map", color_depth_map.reshape(424, 512)) listener.release(frames) #time.sleep(20) key = cv2.waitKey(delay=1) & 0xFF if key == ord('q'): break ####################################################### target(latb, longb, lata, longa) #stage3.py main fnc ####################################################### device.stop() device.close()