def __init__(self, frametypes=FrameType.Color | FrameType.Depth, registration=True): # open the device and start the listener num_devices = fn.enumerateDevices() assert num_devices > 0, "Couldn't find any devices" serial = fn.getDeviceSerialNumber(0) self.device = fn.openDevice(serial, pipeline=pipeline) self.listener = SyncMultiFrameListener(frametypes) self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) self.device.start() # NOTE: must be called after device.start() if registration: ir = self.device.getIrCameraParams() color = self.device.getColorCameraParams() self.registration = Registration(ir, color) # create these here so we don't have to every time we call update() self.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4) # which kinds of frames are we going to be reading? # will be true or false self.need_color = frametypes & FrameType.Color self.need_depth = frametypes & FrameType.Depth self.need_ir = frametypes & FrameType.Ir # initialize our frames self.frames = {} # ensure that we shut down smoothly atexit.register(self.close)
def start(self): if self.device: self.device.start() self.registration = Registration( self.device.getIrCameraParams(), self.device.getColorCameraParams()) return self # for convenience else: raise ConnectionError("Connection to Kinect wasn't established")
def __init__(self): self.pipeline = OpenCLPacketPipeline() fn = Freenect2() self.device = fn.openDefaultDevice(pipeline=self.pipeline) self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) self.device.start() self.registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) self.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4)
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 connect_kinect(self): # Bad, but needed self.fn = Freenect2() num_devices = self.fn.enumerateDevices() if num_devices == 0: print("Kinect not found, check again") self.connected = False return try: self.pipeline = OpenGLPacketPipeline() except: self.pipeline = CpuPacketPipeline() serial = self.fn.getDeviceSerialNumber(0) self.device = self.fn.openDevice(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() print("started") # NOTE: must be called after device.start() self.registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) print("registration") self.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4) self.connected = True
def captureKinect(device, serial,listener, timestamp, dID): ts = timestamp dataID = dID camera="Kinect" filename = dataID+"_"+ts+"_"+camera firmware = device.getFirmwareVersion() firmware = str(firmware).split("'")[1] # NOTE: must be called after device.start() registration = Registration(device.getIrCameraParams(),device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) frames = listener.waitForNewFrame() color = frames["color"] ir = frames["ir"] depth = frames["depth"] registration.apply(color, depth, undistorted, registered,bigdepth=None,color_depth_map=None) colorExposure = color.exposure depthExposure = depth.exposure irExposure = ir.exposure colorGain = color.gain depthGain = depth.gain irGain = ir.gain #key = cv2.waitKey(5000) cv2.imwrite('DataSet/'+filename+"_color.jpg", color.asarray()) cv2.imwrite('DataSet/'+filename+"_depth.jpg", depth.asarray()%256) cv2.imwrite('DataSet/'+filename+"_RGBD.jpg", registered.asarray(np.uint8)) listener.release(frames) with open("KinectDeviceID.txt") as file: deviceID = file.read() process = subprocess.Popen("lsusb -v -d "+deviceID+"|grep \""+deviceID+"\"", stdout=subprocess.PIPE, shell=True) usb = process.stdout.readline() usb = str(usb).split("'")[1] usb = usb.split(':')[0] #print(usb) process.communicate() SensorDetails=[] SensorDetails.append(str(serial).split("'")[1]) SensorDetails.append(firmware) SensorDetails.append("NA") SensorDetails.append(usb) SensorDetails.append("Color: "+str(colorExposure)+" Depth: "+str(depthExposure)+" IR: "+str(irExposure)) SensorDetails.append("Color: "+str(colorGain)+" Depth: "+str(depthGain)+" IR: "+str(irGain)) with open("kinect.txt",'w') as file: for item in SensorDetails: file.write(item+"\n")
def _init_device(self): if _platform == 'Windows': device = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Color | PyKinectV2.FrameSourceTypes_Depth | PyKinectV2.FrameSourceTypes_Infrared) elif _platform == 'Linux': if _pylib: # self.device = Device() fn = Freenect2() num_devices = fn.enumerateDevices() assert num_devices > 0 serial = fn.getDeviceSerialNumber(0) self.device = fn.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.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4) frames = self.listener.waitForNewFrame() self.listener.release(frames) self.device.stop() self.device.close() elif _lib: try: self.device = Device() except: traceback.print_exc() else: print(_platform) raise NotImplementedError
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 initCamera(fn, pipeline): 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) types = (FrameType.Ir | FrameType.Depth) listener = SyncMultiFrameListener(types) device.setIrAndDepthFrameListener(listener) device.startStreams(rgb= False, depth=True) registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) return (device, listener, registration)
class KinectCamera(Camera): def __init__(self): self.pipeline = OpenCLPacketPipeline() fn = Freenect2() self.device = fn.openDefaultDevice(pipeline=self.pipeline) self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) self.device.start() self.registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) self.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4) def getFrames(self): frames = self.listener.waitForNewFrame() self.registration.apply(frames['color'], frames['depth'], self.undistorted, self.registered) frames['registered'] = self.registration return frames def stop(self): self.device.stop() self.device.close()
def setup(self, fn, pipeline, **kwargs): super(KivyCamera, self).__init__(**kwargs) self.bg_image = None self.current_img = None self.current_final_img = None self.current_mask = None serial = fn.getDeviceSerialNumber(0) self.device = fn.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() # 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 need_bigdepth = True need_color_depth_map = False self.bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None self.color_depth_map = np.zeros((424, 512), np.int32).ravel() \ if need_color_depth_map else None self.clipping_distance = 0.5
def fullDataLoop(self): registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) deptharraytest = [{'x': 152, 'y': 100, 'depth': 400}, {'x': 300, 'y': 200, 'depth': 400}, {'x': 350, 'y': 150, 'depth': 400}, {'x': 400, 'y': 300, 'depth': 400}, {'x': 22, 'y': 10, 'depth': 400}, {'x': 144, 'y': 12, 'depth': 400}] depthArray = self.update(registration) rows, cols = depthArray.shape d = self.pointRavel(depthArray, rows, cols) m = self.getMeanDepth(d, rows, cols) b = self.getBody(d, m, rows, cols) s = self.getSkeleton(deptharraytest, m, cols, rows) return {'spine' : s, 'meanDepth' : m}
def run(self, duration): end = time.time() + duration self.device.start() registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) deptharraytest = [{'x':152, 'y':100, 'depth':400}, {'x':300, 'y':200, 'depth':400}, {'x': 350, 'y': 150, 'depth': 400},{'x':400, 'y':300, 'depth':400}, {'x': 22, 'y': 10, 'depth': 400},{'x':144, 'y':12, 'depth':400}] while time.time() < end: depthArray = self.update(registration) rows, cols = depthArray.shape d = self.pointRavel(depthArray, rows, cols) m =self.getMeanDepth(d, rows, cols) b = self.getBody(d, m, rows, cols) s = self.getSkeleton(deptharraytest, m, cols, rows) self.changeInX(s) self.device.stop()
def execute(self): self.device.start() registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) d = self.update(registration) shape = d.shape m = self.getMeanDepth(d, 9) b = self.getBody(d,m) s = self.getSkeleton(d, m) self.changeInX(s) print ('depthArray' + str(len(d.ravel()))) print ('body' + str(len(b))) for body in b: print ("(" + str(body['x']) + "," + str(body['y']) + "): " + str(body['z'])) print (m) self.device.stop()
def find_and_track_kinect(self, name, tracker = "CSRT", face_target_box = DEFAULT_FACE_TARGET_BOX, track_scaling = DEFAULT_SCALING, res = (RGB_W, RGB_H), video_out = True): print("Starting Tracking") target = name fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline = self.pipeline) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Depth) device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) bigdepth = Frame(1920, 1082, 4) trackerObj = None face_process_frame = True bbox = None track_bbox = None head_h = 0 body_left_w = 0 body_right_w = 0 center_w = 0 globalState = "" # Following line creates an avi video stream of daisy's tracking # out = cv2.VideoWriter('daisy_eye.avi', cv2.VideoWriter_fourcc('M','J','P','G'), 10, (960, 540)) # out.write(c) while True: timer = cv2.getTickCount() frames = listener.waitForNewFrame() color = frames["color"] depth = frames["depth"] registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth) bd = np.resize(bigdepth.asarray(np.float32), (1080, 1920)) c = cv2.cvtColor(color.asarray(), cv2.COLOR_RGB2BGR) if self.connected: newTarget = self.alexa_neuron.get('name'); if newTarget != target: target = newTarget listener.release(frames) trackerObj = None face_process_frame = True bbox = None track_bbox = None continue if target is not None and target not in self.known_faces: target = None if target is None: if self.connected: c = self.__scale_frame(c, scale_factor = 0.5) image = cv2.imencode('.jpg', c)[1].tostring() self.web_neuron.update([('image', image)]) listener.release(frames) trackerObj = None face_process_frame = True bbox = None track_bbox = None self.__update_individual_position("WAITING", None, None, None, res) continue face_bbox = None new_track_bbox = None if face_process_frame: small_c = self.__crop_frame(c, face_target_box) face_locations = face_recognition.face_locations(small_c, number_of_times_to_upsample=0, model="cnn") face_encodings = face_recognition.face_encodings(small_c, face_locations) for face_encoding in face_encodings: matches = face_recognition.compare_faces( [self.known_faces[target]], face_encoding, 0.6) if len(matches) > 0 and matches[0]: (top, right, bottom, left) = face_locations[0] left += face_target_box[0] top += face_target_box[1] right += face_target_box[0] bottom += face_target_box[1] face_bbox = (left, top, right, bottom) mid_w = int((left + right) / 2) mid_h = int((top + bottom) / 2) new_track_bbox = self.__body_bbox(bd, mid_w, mid_h, res) break face_process_frame = not face_process_frame overlap_pct = 0 track_area = self.__bbox_area(track_bbox) if track_area > 0 and new_track_bbox: overlap_area = self.__bbox_overlap(new_track_bbox, track_bbox) overlap_pct = min(overlap_area / self.__bbox_area(new_track_bbox), overlap_area / self.__bbox_area(track_bbox)) small_c = self.__scale_frame(c, track_scaling) if new_track_bbox is not None and overlap_pct < CORRECTION_THRESHOLD: bbox = (new_track_bbox[0], new_track_bbox[1], new_track_bbox[2] - new_track_bbox[0], new_track_bbox[3] - new_track_bbox[1]) bbox = self.__scale_bbox(bbox, track_scaling) trackerObj = self.__init_tracker(small_c, bbox, tracker) self.alexa_neuron.update([('tracking', True)]) if trackerObj is None: self.__update_individual_position("WAITING", None, None, None, res) status = False if trackerObj is not None: status, trackerBBox = trackerObj.update(small_c) bbox = (int(trackerBBox[0]), int(trackerBBox[1]), int(trackerBBox[0] + trackerBBox[2]), int(trackerBBox[1] + trackerBBox[3])) if bbox is not None: track_bbox = (bbox[0], bbox[1], bbox[2], bbox[3]) track_bbox = self.__scale_bbox(bbox, 1/track_scaling) w = 0 h = 0 if status: w = track_bbox[0] + int((track_bbox[2] - track_bbox[0])/2) h = track_bbox[1] + int((track_bbox[3] - track_bbox[1])/2) if (w < res[0] and w >= 0 and h < res[1] and h >= 0): distanceAtCenter = bd[h][w] center = (w, h) globalState = self.__update_individual_position(status, track_bbox, center, distanceAtCenter, res) if globalState == "Fail": break fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer) if video_out or self.connected: cv2.line(c, (w, 0), (w, res[1]), (0,255,0), 1) cv2.line(c, (0, h), (res[0], h), (0,255,0), 1) cv2.line(c, (0, head_h), (res[0], head_h), (0,0,0), 1) cv2.line(c, (body_left_w, 0), (body_left_w, res[1]), (0,0,255), 1) cv2.line(c, (body_right_w, 0), (body_right_w, res[1]), (0,0,255), 1) cv2.line(c, (center_w, 0), (center_w, res[1]), (0,0,255), 1) self.__draw_bbox(True, c, face_target_box, (255, 0, 0), "FACE_TARGET") self.__draw_bbox(status, c, track_bbox, (0, 255, 0), tracker) self.__draw_bbox(face_bbox is not None, c, face_bbox, (0, 0, 255), target) self.__draw_bbox(face_bbox is not None, c, new_track_bbox, (0, 255, 255), "BODY") c = self.__scale_frame(c, scale_factor = 0.5) cv2.putText(c, "FPS : " + str(int(fps)), (100,50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,0,255), 1) if not status: failedTrackers = "FAILED: " failedTrackers += tracker + " " cv2.putText(c, failedTrackers, (100, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,142), 1) if self.connected: image = cv2.imencode('.jpg', c)[1].tostring() self.web_neuron.update([('image', image)]) if video_out: cv2.imshow("color", c) listener.release(frames) key = cv2.waitKey(1) & 0xff if key == ord('q'): self.__update_individual_position("STOP", None, None, None, res) break self.so.close() cv2.destroyAllWindows() device.stop() device.close()
def main(): instructions = 0 print "--- Intention classification for communication between a human and a robot ---" if instructions == 1: print "First you will be required to present a facial expression before you will do a head movement." print "If done correctly these gestures will be detected by the robot and it will perform the desired task." raw_input("Press Enter to continue...") if instructions == 1: print "This is the module for facial expression recognition." print "This program can detect the emotions: Happy and Angry." print "The program will look for the expression for 3 seconds." raw_input("To proceed to Facial Expression Recognition press Enter...") predictExp = 0 # Load Facial Expression Recognition trained model print "- Loading FER model... -" #faceExpModel = tf.keras.models.load_model("/home/bjornar/ML_models/FER/Good models(80+)/tf_keras_weights_ninthRev-88percent/tf_keras_weights_ninthRev.hdf5") faceExpModel = tf.keras.models.load_model( "/home/project/Bjorn/tf_keras_weights_ninthRev-88percent/tf_keras_weights_ninthRev.hdf5" ) # Load Face Cascade for Face Detection print "- Loading Face Cascade for Face Detection... -" #cascPath = "/home/bjornar/MScDissertation/TrainingData/FaceDetection/haarcascade_frontalface_default.xml" cascPath = "/home/project/Bjorn/IntentionClassification-Repository/haarcascade_frontalface_default.xml" faceCascade = cv2.CascadeClassifier(cascPath) ## Initializing Head Movement variables # Introduce mark_detector to detect landmarks. mark_detector = MarkDetector() sample_frame = cv2.imread("sample_frame.png") # Setup process and queues for multiprocessing. img_queue = Queue() box_queue = Queue() img_queue.put(sample_frame) box_process = Process(target=get_face, args=( mark_detector, img_queue, box_queue, )) box_process.start() # Introduce pose estimator to solve pose. Get one frame to setup the # estimator according to the image size. height, width = sample_frame.shape[:2] pose_estimator = PoseEstimator(img_size=(height, width)) # Introduce scalar stabilizers for pose. pose_stabilizers = [ Stabilizer(state_num=2, measure_num=1, cov_process=0.1, cov_measure=0.1) for _ in range(6) ] noseMarks = [[0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0]] counter = 0 font = cv2.FONT_HERSHEY_SIMPLEX numXPoints = 0 numYPoints = 0 sumX = 0 sumY = 0 currentTime = 0 previousTime1 = 0 previousTime2 = 0 directionArray = [] moveSequence = [] moves = [] classifyMoves = 0 headPoseDirection = 'emtpy' if camera == 'kinect': ## Initialize Kinect camera print "Initializing camera..." 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 and set logger #logger = createConsoleLogger(LoggerLevel.Debug) setGlobalLogger() 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()) elif camera == 'webcam': #video_capture = cv2.VideoCapture(0) video_capture = cv2.VideoCapture(-1) elif camera == 'video': #video_capture = cv2.VideoCapture(0) video_capture = cv2.VideoCapture( "/home/project/Bjorn/SonyHandycamTest.mp4") ## Facial Expression Recognition variables FER_prediction = [] FERclass = '' FERstart = 0 classifyMoves = 0 ## Head movement variables predictHeadMov = 3 HeadMov = [] HMCclass = '' detectedFaces = [] mark = [] notDone = 0 progressStatus = [0, 0] while notDone in progressStatus: # This waits for each module to finsih if camera == 'kinect': frames = listener.waitForNewFrame() color = frames["color"] color = color.asarray() color = cv2.resize(color, (int(873), int(491))) color = color[0:480, 150:790] color = np.delete(color, np.s_[3::], 2) elif camera == 'webcam': # Capture frame-by-frame ret, color = video_capture.read() elif camera == 'video': # Capture frame-by-frame ret, color = video_capture.read() ## Detect facial expression predictExpNums = [1, 2] if predictExp == 0: while predictExp not in predictExpNums: predictExp = int( raw_input( "\nPress 1 to detect Facial Expression or press 2 to do Head Movement classification." )) if predictExp == 1: predictExp = 1 print "------ Facial Expression Recognition ------" elif predictExp == 2: predictHeadMov = 0 progressStatus[0] = 1 FERstart = time.time() elif predictExp == 1: currentTime = time.time() if currentTime - FERstart < 10: FER_prediction.append( facialExpressionRecogntion(color, faceExpModel, faceCascade)) else: predictExp = 2 FER_prediction = filter(None, FER_prediction) FERclass = mostCommon(FER_prediction) FERclass = FERclass[2:7] predictHeadMov = 0 if FERclass == '': print("Did not detect an expression, try again.") predictExp = 0 else: progressStatus[0] = 1 print "Detected expression: " + str(FERclass) print "Progress: FER DONE" # else: # #cv2.imwrite("sample_frame.png", color) # break ## Detect head movement class if predictHeadMov == 0: predictHeadMov = int( raw_input( "\nPress 1 to do Head Movement classification or 2 to skip." )) if predictHeadMov == 1: predictHeadMov = 1 print "------ Head Movement Classification ------" moveTime = int( raw_input( "How many seconds should I record your movement?")) #moveTime = 2 else: predictHeadMov = 2 timer = time.time() previousTime1 = timer previousTime2 = timer if predictHeadMov == 1: print color.shape color = color[0:480, 0:480] color = cv2.cvtColor(color, cv2.COLOR_BGR2GRAY) cv2.imshow("", color) raw_input() print color.shape # Feed frame to image queue. img_queue.put(color) #pdb.set_trace() # Get face from box queue. facebox = box_queue.get() print color.shape if facebox is not None: # Detect landmarks from image of 128x128. face_img = color[facebox[1]:facebox[3], facebox[0]:facebox[2]] face_img = cv2.resize(face_img, (CNN_INPUT_SIZE, CNN_INPUT_SIZE)) face_img = cv2.cvtColor(face_img, cv2.COLOR_BGR2RGB) marks = mark_detector.detect_marks(face_img) #Convert the marks locations from local CNN to global image. marks *= (facebox[2] - facebox[0]) marks[:, 0] += facebox[0] marks[:, 1] += facebox[1] font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(color, headPoseDirection, (20, 70), font, 1, (0, 255, 0), 4) # # Get average position of nose noseMarksTemp = [] noseMarksTemp.append(marks[30][0]) noseMarksTemp.append(marks[30][1]) noseMarks[0] = noseMarksTemp for i in range(9, 0, -1): noseMarks[i] = noseMarks[i - 1] # Get the direction of head movement headPoseDirection = calculateDirection(noseMarks) directionArray.append(headPoseDirection) if classifyMoves == 0: classifyMoves = 1 timer = time.time() previousTime1 = timer previousTime2 = timer currentTime = time.time() if currentTime - previousTime1 > moveTime and classifyMoves == 1: print "------------------------------------------------" print "Elapsed timer 1: " + str(currentTime - previousTime1) # Get most common direction HMCclass = mostCommon(directionArray) classifyMoves = 2 directionArray = [] previousTime1 = currentTime progressStatus[1] = 1 print "Progress: HMC DONE" else: print "Did not detect a face" elif predictHeadMov == 2: progressStatus[1] = 1 print "Skipped Head Movement Estimation." break # if notDone in progressStatus and predictHeadMov == 2 and predictExp == 2: # print "You seem to have skipped one or more tasks." # inpt = '' # while inpt == '': # inpt = raw_input("To do FER press 1 and to do HMC press 2...") # if input == '1': # predictExp = 1 # elif input == '2': # predictHeadMov cv2.imshow("", color) if camera == 'kinect': listener.release(frames) key = cv2.waitKey(delay=1) if key == ord('q'): break if camera == 'kinect': listener.release(frames) device.stop() device.close() elif camera == 'webcam' or camera == 'video': video_capture.release() cv2.destroyAllWindows() # Clean up the multiprocessing process. box_process.terminate() box_process.join() print "---------------- RESULT ----------------" if FERclass != '': print "Detected facial expression: " + str(FERclass) else: print "Did not detect any expression." if HMCclass != '': print "Detected head movement: " + str(HMCclass) else: print "Did not detect a head movement." print "----------------------------------------" intentionClassification = [FERclass, HMCclass] return intentionClassification
def find_and_track_kinect(name, tracker = "CSRT", min_range = 0, max_range = 1700, face_target_box = DEFAULT_FACE_TARGET_BOX, res = (RGB_W, RGB_H), video_out = True, debug = True): known_faces = {} for person in faces: image = face_recognition.load_image_file(faces[person]) print(person) face_encoding_list = face_recognition.face_encodings(image) if len(face_encoding_list) > 0: known_faces[person] = face_encoding_list[0] else: print("\t Could not find face for person...") fn = Freenect2() num_devices = fn.enumerateDevices() if num_devices == 0: print("No device connected!") serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline = pipeline) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Depth) device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) bigdepth = Frame(1920, 1082, 4) trackerObj = None face_count = 5 face_process_frame = True bbox = None face_bbox = None track_bbox = None while True: timer = cv2.getTickCount() person_found = False frames = listener.waitForNewFrame() color = frames["color"] depth = frames["depth"] registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth) bd = np.resize(bigdepth.asarray(np.float32), (1080, 1920)) c = cv2.cvtColor(color.asarray(), cv2.COLOR_RGB2BGR) __clean_color(c, bd, min_range, max_range) if face_process_frame: small_c = __crop_frame(c, face_target_box) face_locations = face_recognition.face_locations(small_c, model="cnn") face_encodings = face_recognition.face_encodings(small_c, face_locations) for face_encoding in face_encodings: matches = face_recognition.compare_faces( [known_faces[name]], face_encoding, 0.6) if len(matches) > 0 and matches[0]: person_found = True face_count += 1 (top, right, bottom, left) = face_locations[0] left += face_target_box[0] top += face_target_box[1] right += face_target_box[0] bottom += face_target_box[1] face_bbox = (left, top, right, bottom) person_found = True break face_process_frame = not face_process_frame overlap_pct = 0 track_area = __bbox_area(track_bbox) if track_area > 0 and face_bbox: overlap_area = __bbox_overlap(face_bbox, track_bbox) overlap_pct = min(overlap_area / __bbox_area(face_bbox), overlap_area / __bbox_area(track_bbox)) if person_found and face_count >= FACE_COUNT and overlap_pct < CORRECTION_THRESHOLD: bbox = (face_bbox[0], face_bbox[1], face_bbox[2] - face_bbox[0], face_bbox[3] - face_bbox[1]) trackerObj = __init_tracker(c, bbox, tracker) face_count = 0 status = False if trackerObj is not None: status, trackerBBox = trackerObj.update(c) bbox = (int(trackerBBox[0]), int(trackerBBox[1]), int(trackerBBox[0] + trackerBBox[2]), int(trackerBBox[1] + trackerBBox[3])) if bbox is not None: track_bbox = bbox fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer) w = 0 h = 0 if status: w = track_bbox[0] + int((track_bbox[2] - track_bbox[0])/2) h = track_bbox[1] + int((track_bbox[3] - track_bbox[1])/2) if (w < res[0] and w >= 0 and h < res[1] and h >= 0): distanceAtCenter = bd[h][w] __update_individual_position("NONE", track_bbox, distanceAtCenter, res) if video_out: cv2.line(c, (w, 0), (w, int(res[1])), (0,255,0), 1) cv2.line(c, (0, h), \ (int(res[0]), h), (0,255,0), 1) __draw_bbox(True, c, face_target_box, (255, 0, 0), "FACE_TARGET") __draw_bbox(status, c, track_bbox, (0, 255, 0), tracker) __draw_bbox(person_found, c, face_bbox, (0, 0, 255), name) c = __scale_frame(c, scale_factor = 0.5) cv2.putText(c, "FPS : " + str(int(fps)), (100,50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,0,255), 1) if not status: failedTrackers = "FAILED: " failedTrackers += tracker + " " cv2.putText(c, failedTrackers, (100, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,142), 1) cv2.imshow("color", c) listener.release(frames) key = cv2.waitKey(1) & 0xff if key == ord('q'): break device.stop() device.close()
def get_image(imgtopic): image = imgtopic fn = Freenect2() num_devices = fn.enumerateDevices() assert num_devices > 0 serial = fn.getDefaultDeviceSerialNumber() assert serial == fn.getDeviceSerialNumber(0) device = fn.openDevice(serial) assert fn.getDefaultDeviceSerialNumber() == device.getSerialNumber() device.getFirmwareVersion() listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # Registration registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) # undistorted = Frame(512, 424, 4) # registered = Frame(512, 424, 4) # # optional parameters for registration # bigdepth = Frame(1920, 1082, 4) # color_depth_map = np.zeros((424, 512), np.int32) # # test if we can get two frames at least # frames = listener.waitForNewFrame() # listener.release(frames) # frames as a first argment also should work frames = FrameMap() listener.waitForNewFrame(frames) color = frames[FrameType.Color] # ir = frames[FrameType.Ir] # depth = frames[FrameType.Depth] # for frame in [ir, depth]: # assert frame.exposure == 0 # assert frame.gain == 0 # assert frame.gamma == 0 # for frame in [color]: # assert frame.exposure > 0 # assert frame.gain > 0 # assert frame.gamma > 0 # registration.apply(color, depth, undistorted, registered) # registration.apply(color, depth, undistorted, registered, # bigdepth=bigdepth, # color_depth_map=color_depth_map.ravel()) # assert color.width == 1920 # assert color.height == 1080 # assert color.bytes_per_pixel == 4 # assert ir.width == 512 # assert ir.height == 424 # assert ir.bytes_per_pixel == 4 # assert depth.width == 512 # assert depth.height == 424 # assert depth.bytes_per_pixel == 4 # assert color.asarray().shape == (color.height, color.width, 4) # assert ir.asarray().shape == (ir.height, ir.width) # assert depth.asarray(np.float32).shape == (depth.height, depth.width) return color.asarray()
print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=pipeline) # select image to use types = FrameType.Color | FrameType.Depth listener = SyncMultiFrameListener(types) device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # use preset calibration parameters registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) while True: frames = listener.waitForNewFrame() color = frames["color"] ir = frames["ir"] depth = frames["depth"] registration.apply(color, depth, undistorted, registered) ### image processing ### img = cv2.resize(color.asarray(), (int(1920 / 3), int(1080 / 3))) img = cv2.medianBlur(img, 5) grayscale = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
def test_sync_multi_frame(): fn = Freenect2() num_devices = fn.enumerateDevices() assert num_devices > 0 serial = fn.getDefaultDeviceSerialNumber() assert serial == fn.getDeviceSerialNumber(0) device = fn.openDevice(serial) assert fn.getDefaultDeviceSerialNumber() == device.getSerialNumber() device.getFirmwareVersion() listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # Registration registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) # optional parameters for registration bigdepth = Frame(1920, 1082, 4) color_depth_map = np.zeros((424, 512), np.int32) # test if we can get two frames at least frames = listener.waitForNewFrame() listener.release(frames) # frames as a first argment also should work frames = FrameMap() listener.waitForNewFrame(frames) color = frames[FrameType.Color] ir = frames[FrameType.Ir] depth = frames[FrameType.Depth] for frame in [ir, depth]: assert frame.exposure == 0 assert frame.gain == 0 assert frame.gamma == 0 for frame in [color]: assert frame.exposure > 0 assert frame.gain > 0 assert frame.gamma > 0 registration.apply(color, depth, undistorted, registered) # with optinal parameters registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth, color_depth_map=color_depth_map.ravel()) assert color.width == 1920 assert color.height == 1080 assert color.bytes_per_pixel == 4 assert ir.width == 512 assert ir.height == 424 assert ir.bytes_per_pixel == 4 assert depth.width == 512 assert depth.height == 424 assert depth.bytes_per_pixel == 4 assert color.asarray().shape == (color.height, color.width, 4) assert ir.asarray().shape == (ir.height, ir.width) assert depth.asarray(np.float32).shape == (depth.height, depth.width) listener.release(frames) def __test_cannot_determine_type_of_frame(frame): frame.asarray() for frame in [registered, undistorted]: yield raises(ValueError)(__test_cannot_determine_type_of_frame), frame device.stop() device.close()
def test_sync_multi_frame(): fn = Freenect2() num_devices = fn.enumerateDevices() assert num_devices > 0 serial = fn.getDefaultDeviceSerialNumber() assert serial == fn.getDeviceSerialNumber(0) # TODO: tests for openDefaultDevice # device = fn.openDefaultDevice() device = fn.openDevice(serial) assert fn.getDefaultDeviceSerialNumber() == device.getSerialNumber() device.getFirmwareVersion() listener = SyncMultiFrameListener( FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # Registration registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) # test if we can get two frames at least frames = listener.waitForNewFrame() listener.release(frames) frames = listener.waitForNewFrame() color = frames[FrameType.Color] ir = frames[FrameType.Ir] depth = frames[FrameType.Depth] for frame in [ir, depth]: assert frame.exposure == 0 assert frame.gain == 0 assert frame.gamma == 0 for frame in [color]: assert frame.exposure > 0 assert frame.gain > 0 assert frame.gamma > 0 registration.apply(color, depth, undistorted, registered) ### Color ### assert color.width == 1920 assert color.height == 1080 assert color.bytes_per_pixel == 4 assert ir.width == 512 assert ir.height == 424 assert ir.bytes_per_pixel == 4 assert depth.width == 512 assert depth.height == 424 assert depth.bytes_per_pixel == 4 assert color.asarray().shape == (color.height, color.width, 4) assert ir.asarray().shape == (ir.height, ir.width) assert depth.astype(np.float32).shape == (depth.height, depth.width) listener.release(frames) def __test_cannot_determine_type_of_frame(frame): frame.asarray() for frame in [registered, undistorted]: yield raises(ValueError)(__test_cannot_determine_type_of_frame), frame device.stop() device.close()
class Tracker: # X_POSITION = -0.11387496 # Z_POSITION = 1.3 X_POSITION = -0.185 Z_POSITION = 1.5 greenLower = (29, 86, 6) greenUpper = (64, 255, 255) greenLower = (27, 100, 50) greenUpper = (60, 255, 255) 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 connect_camera(self): # Bad, but needed self.camera = cv2.VideoCapture(1) if not self.camera.isOpened(): print("Unable to connect to the camera, check again") time.sleep(5) else: self.connected = True def connect_kinect(self): # Bad, but needed self.fn = Freenect2() num_devices = self.fn.enumerateDevices() if num_devices == 0: print("Kinect not found, check again") self.connected = False return try: self.pipeline = OpenGLPacketPipeline() except: self.pipeline = CpuPacketPipeline() serial = self.fn.getDeviceSerialNumber(0) self.device = self.fn.openDevice(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() print("started") # NOTE: must be called after device.start() self.registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) print("registration") self.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4) self.connected = True def video_thread(self): print('Camera thread started') 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 not self.killed: while self.running: # (grabbed, frame) = self.camera.read() frames = self.listener.waitForNewFrame() frame = frames["color"] ir = frames["ir"] depth = frames["depth"] self.registration.apply(frame, depth, self.undistorted, self.registered, bigdepth=bigdepth, color_depth_map=color_depth_map) # print(frame.width, frame.height, frame.bytes_per_pixel) # print(ir.width, ir.height, ir.bytes_per_pixel) # print(depth.width, depth.height, depth.bytes_per_pixel) # print(frame.asarray().shape, ir.asarray().shape, depth.asarray().shape) # color_image_array = frame.asarray() color_image_array = self.registered.asarray(np.uint8) hsv = cv2.cvtColor(color_image_array, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, self.greenLower, self.greenUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] center = None if len(cnts) > 0: c = max(cnts, key=cv2.contourArea) ((x, y), radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) if radius > 10: cv2.circle(color_image_array, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(color_image_array, center, 5, (0, 0, 255), -1) x, y, z = self.registration.getPointXYZ(self.undistorted, y, x) # print(x, y, z, end='\r') if not math.isnan(y) and not math.isnan(z): self.px.append(y) self.py.append(z) row = tuple(c[c[:, :, 1].argmin()][0])[1] self.max_radius = min(self.max_radius, row) if self.gui: cv2.imshow("Frame", color_image_array) cv2.imshow("Masked", mask) self.listener.release(frames) cv2.waitKey(1) & 0xFF # self.ax.plot(self.px) # self.ax.plot(self.py) # plt.pause(0.01) cv2.destroyAllWindows() print("exit looping") def start(self): self.max_radius = 100000 if self.use_kinect: self.px = [] self.py = [] self.running = True def stop(self): self.running = False def kill(self): self.killed = True if self.use_kinect: self.device.stop() self.device.close() def get_reward(self): if self.use_kinect: if not self.basket: try: peaks = peakutils.indexes(self.px) except Exception as e: print(e) peaks = np.array([]) if peaks.any(): # for xx in peaks: # t.ax.plot(xx, self.px[xx], 'ro') return self.py[peaks[0]] * 100 else: return 0 else: print(np.mean(self.px), np.mean(self.py)) dist = distance(np.array([self.px, self.py]).T, self.X_POSITION, self.Z_POSITION) print("distance", dist, len(self.px)) return dist * 100 else: return self.max_radius
class KinectV2: """ control class for the KinectV2 based on the Python wrappers of the official Microsoft SDK Init the kinect and provides a method that returns the scanned depth image as numpy array. Also we do gaussian blurring to get smoother surfaces. """ def __init__(self): # hard coded class attributes for KinectV2's native resolution self.name = 'kinect_v2' self.depth_width = 512 self.depth_height = 424 self.color_width = 1920 self.color_height = 1080 self.depth = None self.color = None self._init_device() #self.depth = self.get_frame() #self.color = self.get_color() print("KinectV2 initialized.") def _init_device(self): if _platform == 'Windows': device = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Color | PyKinectV2.FrameSourceTypes_Depth | PyKinectV2.FrameSourceTypes_Infrared) elif _platform == 'Linux': if _pylib: # self.device = Device() fn = Freenect2() num_devices = fn.enumerateDevices() assert num_devices > 0 serial = fn.getDeviceSerialNumber(0) self.device = fn.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.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4) frames = self.listener.waitForNewFrame() self.listener.release(frames) self.device.stop() self.device.close() elif _lib: try: self.device = Device() except: traceback.print_exc() else: print(_platform) raise NotImplementedError def _get_linux_frame(self, typ:str='all'): """ Manage to Args: typ: Returns: """ #self.device.start() frames = self.listener.waitForNewFrame(milliseconds=1000) if frames: if typ == 'depth': depth = frames[FrameType.Depth] self.listener.release(frames) return depth.asarray() elif typ == 'ir': ir = frames[FrameType.Ir] self.listener.release(frames) return ir.asarray() if typ == 'color': color = frames[FrameType.Color] self.listener.release(frames) return color.asarray() if typ == 'all': color = frames[FrameType.Color] ir = frames[FrameType.Ir] depth = frames[FrameType.Depth] self.registration.apply(color, depth, self.undistorted, self.registered) self.registration.undistortDepth(depth, self.undistorted) self.listener.release(frames) return depth.asarray(), color.asarray(), ir.asarray() else: raise FileNotFoundError def get_linux_frame(self, typ:str='all'): """ Manage to Args: typ: Returns: """ frames = {} with self.device.running(): for type_, frame in self.device: frames[type_] = frame if FrameType.Color in frames and FrameType.Depth in frames and FrameType.Ir in frames: break if typ == 'depth': depth = frames[FrameType.Depth].to_array() return depth elif typ == 'ir': ir = frames[FrameType.Ir].to_array() return ir elif typ == 'color': color = frames[FrameType.Color].to_array() resolution_camera = self.color_height * self.color_width # resolution camera Kinect V2 palette = numpy.reshape(color, (resolution_camera, 4))[:, [2, 1, 0]] position_palette = numpy.reshape(numpy.arange(0, len(palette), 1), (self.color_height, self.color_width)) color = palette[position_palette] return color else: color = frames[FrameType.Color].to_array() resolution_camera = self.color_height * self.color_width # resolution camera Kinect V2 palette = numpy.reshape(color, (resolution_camera, 4))[:, [2, 1, 0]] position_palette = numpy.reshape(numpy.arange(0, len(palette), 1), (self.color_height, self.color_width)) color = palette[position_palette] depth = frames[FrameType.Depth].to_array() ir = frames[FrameType.Ir].to_array() return color, depth, ir def get_frame(self): """ Args: Returns: 2D Array of the shape(424, 512) containing the depth information of the latest frame in mm """ if _platform =='Windows': depth_flattened = self.device.get_last_depth_frame() self.depth = depth_flattened.reshape( (self.depth_height, self.depth_width)) # reshape the array to 2D with native resolution of the kinectV2 elif _platform =='Linux': self.depth = self.get_linux_frame(typ='depth') return self.depth def get_ir_frame_raw(self): """ Args: Returns: 2D Array of the shape(424, 512) containing the raw infrared intensity in (uint16) of the last frame """ if _platform=='Windows': ir_flattened = self.device.get_last_infrared_frame() self.ir_frame_raw = numpy.flipud( ir_flattened.reshape((self.depth_height, self.depth_width))) # reshape the array to 2D with native resolution of the kinectV2 elif _platform=='Linux': self.ir_frame_raw = self.get_linux_frame(typ='ir') return self.ir_frame_raw def get_ir_frame(self, min=0, max=6000): """ Args: min: minimum intensity value mapped to uint8 (will become 0) default: 0 max: maximum intensity value mapped to uint8 (will become 255) default: 6000 Returns: 2D Array of the shape(424, 512) containing the infrared intensity between min and max mapped to uint8 of the last frame """ ir_frame_raw = self.get_ir_frame_raw() self.ir_frame = numpy.interp(ir_frame_raw, (min, max), (0, 255)).astype('uint8') return self.ir_frame def get_color(self): """ Returns: """ if _platform == 'Windows': color_flattened = self.device.get_last_color_frame() resolution_camera = self.color_height * self.color_width # resolution camera Kinect V2 # Palette of colors in RGB / Cut of 4th column marked as intensity palette = numpy.reshape(numpy.array([color_flattened]), (resolution_camera, 4))[:, [2, 1, 0]] position_palette = numpy.reshape(numpy.arange(0, len(palette), 1), (self.color_height, self.color_width)) self.color = numpy.flipud(palette[position_palette]) elif _platform =='Linux': self.color = self.get_linux_frame(typ='color') return self.color
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()
class Kinect: def __init__(self, kinect_num=0): self.fn = Freenect2() self.serial = None self.device = None self.listener = None self.registration = None self._frames = None # frames cache so that the user can use them before we free them self._bigdepth = Frame(1920, 1082, 4) # malloc'd self._undistorted = Frame(512, 424, 4) self._registered = Frame(512, 424, 4) self._color_dump = Frame(1920, 1080, 4) num_devices = self.fn.enumerateDevices() if num_devices <= kinect_num: raise ConnectionError("No Kinect device at index %d" % kinect_num) self.serial = self.fn.getDeviceSerialNumber(kinect_num) self.device = self.fn.openDevice(self.serial, pipeline=pipeline) self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Depth) # Register listeners self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) def close(self): if self.device: self.device.close() def start(self): if self.device: self.device.start() self.registration = Registration( self.device.getIrCameraParams(), self.device.getColorCameraParams()) return self # for convenience else: raise ConnectionError("Connection to Kinect wasn't established") def stop(self): if self.device: self.device.stop() ### If copy is False for these functoins, make sure to call release_frames ### when you're done with the returned frame ### def get_current_color_frame(self, copy=True): self._frames = self.listener.waitForNewFrame() ret = self._convert_color_frame(self._frames["color"], copy=copy) if copy: self.release_frames() return ret def get_current_depth_frame(self, copy=True): self._frames = self.listener.waitForNewFrame() if copy: ret = self._frames["depth"].asarray().copy() self.release_frames() else: ret = self._frames["depth"].asarray() return ret def get_current_rgbd_frame(self, copy=True): self._frames = self.listener.waitForNewFrame() self.registration.apply(self._frames["color"], self._frames["depth"], self._undistorted, self._registered, bigdepth=self._bigdepth) color = self._convert_color_frame(self._frames["color"], copy=copy) depth = self._convert_bigdepth_frame(self._bigdepth, copy=copy) if copy: self.release_frames() return color, depth def get_current_bigdepth_frame(self, copy=True): self._frames = self.listener.waitForNewFrame() self.registration.apply(self._frames["color"], self._frames["depth"], self._undistorted, self._registered, bigdepth=self._bigdepth) depth = self._convert_bigdepth_frame(self._bigdepth, copy=copy) if copy: self.release_frames() return depth def release_frames(self): self.listener.release(self._frames) # single frame calls def get_single_color_frame(self): self.start() ret = self.get_current_color_frame() self.stop() return ret def get_single_depth_frame(self): self.start() ret = self.get_current_depth_frame() self.stop() return ret def _convert_bigdepth_frame(self, frame, copy=True): if copy: d = self._bigdepth.asarray(np.float32).copy() else: d = self._bigdepth.asarray(np.float32) return d[1:-1, ::-1] def _convert_color_frame(self, frame, copy=True): if copy: img = frame.asarray().copy() else: img = frame.asarray() img = img[:, ::-1] img[..., :3] = img[..., 2::-1] # bgrx -> rgbx return img
def __init__(self, params=None, device_index=0, headless=False, pipeline=None): ''' Kinect: Kinect interface using the libfreenect library. Will query libfreenect for available devices, if none are found will throw a RuntimeError. Otherwise will open the device for collecting color, depth, and ir data. Use kinect.get_frame([<frame type>]) within a typical opencv capture loop to collect data from the kinect. When instantiating, optionally pass a parameters file or dict with the kinect's position and orientation in the worldspace and/or the kinect's intrinsic parameters. An example dictionary is shown below: kinect_params = { "position": { "x": 0, "y": 0, "z": 0.810, "roll": 0, "azimuth": 0, "elevation": -34 } "intrinsic_parameters": { "cx": 256.684, "cy": 207.085, "fx": 366.193, "fy": 366.193 } } These can also be adjusted using the intrinsic_parameters and position properties. ARGUMENTS: params: str or dict Path to a kinect parameters file (.json) or a python dict with position or intrinsic_parameters. device_index: int Use to interface with a specific device if more than one is connected. headless: bool If true, will allow the kinect to collect and process data without a display. Usefull if the kinect is plugged into a server. pipeline: PacketPipeline Optionally pass a pylibfreenect2 pipeline that will be used with the kinect. Possible types are as follows: OpenGLPacketPipeline CpuPacketPipeline OpenCLPacketPipeline CudaPacketPipeline OpenCLKdePacketPipeline Note that this will override the headless argument - Headless requires CUDA or OpenCL pipeline. ''' self.fn = Freenect2() num_devices = self.fn.enumerateDevices() if (num_devices == 0): raise RuntimeError('No device connected!') if (device_index >= num_devices): raise RuntimeError('Device {} not available!'.format(device_index)) self._device_index = device_index # Import pipeline depending on headless state self._headless = headless if (pipeline is None): pipeline = self._import_pipeline(self._headless) print("Packet pipeline:", type(pipeline).__name__) self.device = self.fn.openDevice(self.fn.getDeviceSerialNumber( self._device_index), pipeline=pipeline) # We want all the types types = (FrameType.Color | FrameType.Depth | FrameType.Ir) self.listener = SyncMultiFrameListener(types) self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) self.device.start() # We'll use this to generate registered depth images self.registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) self._camera_position = dotdict({ "x": 0., "y": 0., "z": 0., "roll": 0., "azimuth": 0., "elevation": 0. }) self._camera_params = dotdict({ "cx": self.device.getIrCameraParams().cx, "cy": self.device.getIrCameraParams().cy, "fx": self.device.getIrCameraParams().fx, "fy": self.device.getIrCameraParams().fy }) # Try and load parameters pos, param = self._load_camera_params(params) if (pos is not None): self._camera_position.update(pos) if (pos is not None): self._camera_params.update(param)
class KivyCamera(Image): def __init__(self, **kwargs): super(KivyCamera, self).__init__(**kwargs) def setup(self, fn, pipeline, **kwargs): super(KivyCamera, self).__init__(**kwargs) self.bg_image = None self.current_img = None self.current_final_img = None self.current_mask = None serial = fn.getDeviceSerialNumber(0) self.device = fn.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() # 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 need_bigdepth = True need_color_depth_map = False self.bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None self.color_depth_map = np.zeros((424, 512), np.int32).ravel() \ if need_color_depth_map else None self.clipping_distance = 0.5 def update(self, dt): frames = self.listener.waitForNewFrame() color = frames["color"] depth = frames["depth"] self.registration.apply(color, depth, self.undistorted, self.registered, bigdepth=self.bigdepth, color_depth_map=self.color_depth_map) self.current_img = color.asarray()[:,:,:3] depth_img = self.bigdepth.asarray(np.float32) / 4500. # scale to interval [0,1] if(self.bg_image is not None): if(self.bg_image.shape[2] == 4): # put frame around picture fgMask = self.bg_image[:,:,2]>0 else: # modify background # if needed: denoise filter depth_img = cv2.medianBlur(depth_img, 5) depth_img = cv2.GaussianBlur(depth_img, (9,9), 0) fgMask = ((depth_img > self.clipping_distance) & (depth_img > 0)) fgMask = cv2.resize(fgMask.astype(np.uint8), (1920, 1080)) # if needed: morphological operations #kernel = np.ones((10,10), np.uint8) #fgMask = cv2.morphologyEx(fgMask, cv2.MORPH_CLOSE, kernel) #fgMask = cv2.morphologyEx(fgMask, cv2.MORPH_OPEN, kernel) self.current_mask = np.dstack((fgMask, fgMask, fgMask)).astype(np.bool) # mask is 1 channel, color is 3 channels self.current_final_img = np.where(self.current_mask, self.bg_image[:,:,:3], self.current_img) else: self.current_mask = None self.current_final_img = self.current_img self.listener.release(frames) self.display_img(self.current_final_img) def display_img(self, img): buf1 = cv2.flip(img, 0) buf = buf1.tostring() image_texture = Texture.create(size=(img.shape[1], img.shape[0]), colorfmt='rgb') #(480,640) image_texture.blit_buffer(buf, colorfmt='bgr', bufferfmt='ubyte') # display image from the texture self.texture = image_texture
def publishRGBD(self): fn = Freenect2() num_devices = fn.enumerateDevices() #Exit if device is not found if num_devices == 0: print("No device connected!") sys.exit(1) serial = fn.getDeviceSerialNumber(0) device = fn.openDevice(serial, pipeline=self.pipeline) types = 0 if self.enable_rgb: types |= FrameType.Color if self.enable_depth: types |= (FrameType.Ir | FrameType.Depth) listener = SyncMultiFrameListener(types) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) if self.enable_rgb and self.enable_depth: device.start() else: device.startStreams(rgb=self.enable_rgb, depth=self.enable_depth) # Should be transformed to manually calibrated values. if self.enable_depth: registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) while not rospy.is_shutdown(): frames = listener.waitForNewFrame() if self.enable_rgb: color = frames["color"] if self.enable_depth: ir = frames["ir"] depth = frames["depth"] if self.enable_rgb and self.enable_depth: registration.apply(color, depth, undistorted, registered) elif enable_depth: registration.undistortDepth(depth, undistorted) if self.enable_depth: #cv2.imshow("undistorted", undistorted.asarray(np.float32) / 4500.) self.publishDepth(undistorted) if self.enable_rgb and self.enable_depth: #cv2.imshow("registered", registered.asarray(np.uint8)) self.publishColor(registered) listener.release(frames) device.stop() device.close() sys.exit(0)
if enable_depth: types |= (FrameType.Ir | FrameType.Depth) listener = SyncMultiFrameListener(types) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) if enable_rgb and enable_depth: device.start() else: device.startStreams(rgb=enable_rgb, depth=enable_depth) # NOTE: must be called after device.start() if enable_depth: registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) while True: frames = listener.waitForNewFrame() if enable_rgb: color = frames["color"] if enable_depth: ir = frames["ir"] depth = frames["depth"] if enable_rgb and enable_depth: registration.apply(color, depth, undistorted, registered)
def test_sync_multi_frame(): fn = Freenect2() num_devices = fn.enumerateDevices() assert num_devices > 0 serial = fn.getDefaultDeviceSerialNumber() assert serial == fn.getDeviceSerialNumber(0) device = fn.openDevice(serial) assert fn.getDefaultDeviceSerialNumber() == device.getSerialNumber() device.getFirmwareVersion() listener = SyncMultiFrameListener( FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # Registration registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) # optional parameters for registration bigdepth = Frame(1920, 1082, 4) color_depth_map = np.zeros((424, 512), np.int32) # test if we can get two frames at least frames = listener.waitForNewFrame() listener.release(frames) # frames as a first argment also should work frames = FrameMap() listener.waitForNewFrame(frames) color = frames[FrameType.Color] ir = frames[FrameType.Ir] depth = frames[FrameType.Depth] for frame in [ir, depth]: assert frame.exposure == 0 assert frame.gain == 0 assert frame.gamma == 0 for frame in [color]: assert frame.exposure > 0 assert frame.gain > 0 assert frame.gamma > 0 registration.apply(color, depth, undistorted, registered) # with optinal parameters registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth, color_depth_map=color_depth_map.ravel()) registration.undistortDepth(depth, undistorted) assert color.width == 1920 assert color.height == 1080 assert color.bytes_per_pixel == 4 assert ir.width == 512 assert ir.height == 424 assert ir.bytes_per_pixel == 4 assert depth.width == 512 assert depth.height == 424 assert depth.bytes_per_pixel == 4 assert color.asarray().shape == (color.height, color.width, 4) assert ir.asarray().shape == (ir.height, ir.width) assert depth.asarray(np.float32).shape == (depth.height, depth.width) listener.release(frames) def __test_cannot_determine_type_of_frame(frame): frame.asarray() for frame in [registered, undistorted]: yield raises(ValueError)(__test_cannot_determine_type_of_frame), frame # getPointXYZ x, y, z = registration.getPointXYZ(undistorted, 512 // 2, 424 // 2) if not np.isnan([x, y, z]).any(): assert z > 0 # getPointXYZRGB x, y, z, b, g, r = registration.getPointXYZRGB(undistorted, registered, 512 // 2, 424 // 2) if not np.isnan([x, y, z]).any(): assert z > 0 assert np.isfinite([b, g, r]).all() for pix in [b, g, r]: assert pix >= 0 and pix <= 255 device.stop() device.close()
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 SIZE_AVG_FILTER = 5
class RunCamera(object): # check if it needs to make a picture and save the picture _img_save = False _img_num = 0 _img_done = True 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 draw_color_frame(self, np_frame, target_surface): target_surface.lock() #address = self._kinect.surface_as_array(target_surface.get_buffer()) #ctypes.memmove(address, frame.ctypes.data, frame.size) address = get_address_from_array(target_surface.get_buffer()) ctypes.memmove(address, np_frame.ctypes.data, np_frame.size) del address target_surface.unlock() def run(self): while not self._done: # --- Main event loop for event in pygame.event.get(): # User did something if event.type == pygame.QUIT: # If user clicked close self._done = True # Flag that we are done so we exit this loop elif event.type == pygame.VIDEORESIZE: # window resized self._screen = pygame.display.set_mode(event.dict['size'], pygame.HWSURFACE|pygame.DOUBLEBUF|pygame.RESIZABLE, 32) # --- Getting frames and drawing #if self._kinect.has_new_color_frame(): # frame = self._kinect.get_last_color_frame() # self.draw_color_frame(frame, self._frame_surface) # frame = None frames = self._listener.waitForNewFrame() color_frame = frames["color"] ir = frames["ir"] depth = frames["depth"] self._registration.apply(color_frame, depth, self._undistorted, self._registered, bigdepth=self._bigdepth, color_depth_map=self._color_depth_map) color_arr_view = color_frame.asarray()[:,:,0:3] test_surf = pygame.surfarray.make_surface(color_arr_view) #self.draw_color_frame(color_frame.asarray(), self._frame_surface) self._frame_surface = test_surf # --- When the flag of Saving image is unlocked, so do it if RunCamera._img_save == True: print("entered if statement for image saving") # First to lock it, or it will save too many picture RunCamera._img_save = False # Folder and names of pictures # or we can just use other names of th picture.. name = "pic\\test-" + str(RunCamera._img_num) + ".jpg" # Save the image pygame.image.save(self._frame_surface, name) print "\n \nWe just saved the " + name + "!\n\nLock save-image-processing...\n\n===============================================\n" RunCamera._img_num = RunCamera._img_num + 1 # Tell wether all be done RunCamera._img_done = True # --- copy back buffer surface pixels to the screen, resize it if needed and keep aspect ratio h_to_w = float(self._frame_surface.get_height()) / self._frame_surface.get_width() # --- Screen's width and height target_width = self._screen.get_width() target_height = int(h_to_w * target_width ) surface_to_draw = pygame.transform.scale(self._frame_surface, (target_width, target_height)) self._screen.blit(surface_to_draw, (0,0)) surface_to_draw = None pygame.display.update() # --- Go ahead and update the screen with what we've drawn. pygame.display.flip() # --- Limit to 30 frames per second, try 60 if 30 runs smoothly self._clock.tick(30) self._listener.release(frames) # Close our Kinect sensor, close the window and quit. #self._kinect.close() #self._device.stop() self._device.close() pygame.quit()
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()
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)
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()) h, w = 512, 424 FOVX = 1.232202 #horizontal FOV in radians focal_x = device.getIrCameraParams().fx #focal length x focal_y = device.getIrCameraParams().fy #focal length y principal_x = device.getIrCameraParams().cx #principal point x principal_y = device.getIrCameraParams().cy #principal point y undistorted = Frame(h, w, 4) registered = Frame(h, w, 4) while True: frames = listener.waitForNewFrame() depth_frame = frames["depth"] color = frames["color"]
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