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")
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()
class Kinect: # Kinects's intrinsic parameters based on v2 hardware (estimated). CameraParams = { "cx": 254.878, "cy": 205.395, "fx": 365.456, "fy": 365.456, "k1": 0.0905474, "k2": -0.26819, "k3": 0.0950862, "p1": 0.0, "p2": 0.0, } # Kinect's physical orientation in the real world. CameraPosition = { "x": -4, # distance of kinect sensor from wall. "y": 0, # actual position in meters of kinect sensor relative to the viewport's center. "z": 0, # height in meters of actual kinect sensor from the floor. "roll": 0, # angle in degrees of sensor's roll (used for INU input - trig function for this is commented out by default). "azimuth": 0, # sensor's yaw angle in degrees. "elevation": 0, # sensor's pitch angle in degrees. } DEPTH_SHAPE = (424, 512) N_DEPTH_PIXELS = DEPTH_SHAPE[0] * DEPTH_SHAPE[1] COLOR_SHAPE = (360, 640, 3) 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 hasNextFrame(self): return True def close(self): self.device.stop() self.device.close() def get_frames(self): self._update() return self.frames def _update(self): if self.listener.hasNewFrame() or len(self.frames) == 0: _frames = self.listener.waitForNewFrame() if self.need_color: # get the data, then throw out the 4th channel color = _frames['color'].asarray()[:, :, :3] h, w, d = self.COLOR_SHAPE color = cv2.resize(color, (w, h)) self.frames['color'] = color # print('color:' +str(_frames['color'].timestamp)) if self.need_depth: depth = _frames['depth'].asarray() / 4500 self.frames['depth'] = depth # print('depth:' + str(_frames['depth'].timestamp)) if self.need_ir: ir = _frames['ir'].asarray() / 65535. self.frames['ir'] = ir # print('ir:' + str(_frames['ir'].timestamp)) # register the franes if we need to if self.registration and self.need_color and self.need_color: c, d, u, r = _frames['color'], _frames[ 'depth'], self.undistorted, self.registered self.registration.apply(c, d, u, r) self.frames['undistorted'] = self.undistorted.asarray( np.float32) self.frames['registered'] = self.registered.asarray(np.uint8) # we NEED to explicitly deallocate the frames from the listener self.listener.release(_frames) def makePointCloud(self, undistorted, registered=None): # convert depth frame to point cloud pts = self.depthMatrixToPointCloudPos(undistorted) # # Kinect sensor real-world orientation compensation. pts = self.applyCameraMatrixOrientation(pts) # if supplied with registered color image, figure out color of each point if registered is not None: colors = self.registered2points(registered) else: # else default to white RBGA colors = (1.0, 1.0, 1.0, 1.0) pts = pts.astype(np.float32) return (pts, colors) def depthMatrixToPointCloudPos(self, z, scale=1000): # calculate the real-world xyz vertex coordinates from the raw depth data matrix. C, R = np.indices(z.shape) R = np.subtract(R, self.CameraParams['cx']) R = np.multiply(R, z) R = np.divide(R, self.CameraParams['fx'] * scale) C = np.subtract(C, self.CameraParams['cy']) C = np.multiply(C, z) C = np.divide(C, self.CameraParams['fy'] * scale) return np.column_stack((z.ravel() / scale, R.ravel(), -C.ravel())) def applyCameraMatrixOrientation(self, pt): # Kinect Sensor Orientation Compensation # bacically this is a vectorized version of applyCameraOrientation() # uses same trig to rotate a vertex around a gimbal. def rotatePoints(ax1, ax2, deg): # math to rotate vertexes around a center point on a plane. hyp = np.sqrt( pt[:, ax1]**2 + pt[:, ax2]**2 ) # Get the length of the hypotenuse of the real-world coordinate from center of rotation, this is the radius! d_tan = np.arctan2( pt[:, ax2], pt[:, ax1] ) # Calculate the vertexes current angle (returns radians that go from -180 to 180) cur_angle = np.degrees( d_tan ) % 360 # Convert radians to degrees and use modulo to adjust range from 0 to 360. new_angle = np.radians( (cur_angle + deg) % 360 ) # The new angle (in radians) of the vertexes after being rotated by the value of deg. pt[:, ax1] = hyp * np.cos( new_angle) # Calculate the rotated coordinate for this axis. pt[:, ax2] = hyp * np.sin( new_angle) # Calculate the rotated coordinate for this axis. #rotatePoints(1, 2, CameraPosition['roll']) #rotate on the Y&Z plane # Disabled because most tripods don't roll. If an Inertial Nav Unit is available this could be used) if self.CameraPosition['elevation'] != 0: rotatePoints(0, 2, self.CameraPosition['elevation']) #rotate on the X&Z if self.CameraPosition['azimuth'] != 0: rotatePoints(0, 1, self.CameraPosition['azimuth']) #rotate on the X&Y # Apply offsets for height and linear position of the sensor (from viewport's center) if any([ self.CameraPosition['x'], self.CameraPosition['y'], self.CameraPosition['z'] ]): pt[:] += np.float_([ self.CameraPosition['x'], self.CameraPosition['y'], self.CameraPosition['z'] ]) return pt def registered2points(self, registered): # Format the color registration map from an 2D image of BGRA-pixels to a 1D list of RGB pixels colors = np.divide(registered, 255) # values must be between 0.0 - 1.0 colors = colors.reshape( colors.shape[0] * colors.shape[1], 4) # From: Rows X Cols X RGB -to- [[r,g,b],[r,g,b]...] colors = colors[:, :3:] # remove alpha (fourth index) from BGRA to BGR colors = colors[..., ::-1] #BGR to RGB return colors
class Kinect(): 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) def __del__(self): self.device.stop() def __repr__(self): params = { "position": self._camera_position, "intrinsic_parameters": self._camera_params } # Can't really return the pipeline return 'Kinect(' + str(params) + ',' + str(self._device_index) + \ ',' + str(self._headless) + ')' @property def position(self): ''' Return the kinect's stored position. Elements can be referenced using setattr or setitem dunder methods. Can also be used to update the stored dictionary: k = ktb.Kinect() k.position.x >>> 2.0 k.position.x = 1.0 k.position.x >>> 1.0 ''' return self._camera_position @property def intrinsic_parameters(self): ''' Return the kinect's stored position. Elements can be referenced using setattr or setitem dunder methods. Can also be used to update the stored dictionary: k = ktb.Kinect() k.intrinsic_parameters.cx >>> 2.0 k.intrinsic_parameters.cx = 1.0 k.intrinsic_parameters.cx >>> 1.0 ''' return self._camera_params @staticmethod def _import_pipeline(headless=False): ''' _import_pipeline: Imports the pylibfreenect2 pipeline based on whether or not headless mode is enabled. Unfortunately more intelligent importing cannot be implemented (contrary to the example scripts) because the import raises a C exception, not a python one. As a result the only pipelines this function will load are OpenGL or OpenCL. ARGUMENTS: headless: bool whether or not to run kinect in headless mode. ''' if headless: from pylibfreenect2 import OpenCLPacketPipeline pipeline = OpenCLPacketPipeline() else: from pylibfreenect2 import OpenGLPacketPipeline pipeline = OpenGLPacketPipeline() return pipeline @staticmethod def _load_camera_params(params_file=None): ''' _load_camera_params: Optionally give the kinect's position and orientation in the worldspace and/or the kinect's intrinsic parameters by passing a json file. 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 } } Specifically, the dict may contain the "position" or "intrinsic_parameters" keys, with the above fields. ARGUMENTS: params_file: str or dict Path to a kinect parameters file (.json) or a python dict with position or intrinsic_parameters. ''' if (params_file is None): return None, None elif isinstance(params_file, str): try: with open(params_file, 'r') as infile: params_dict = json.load(infile) except FileNotFoundError: print('Kienct configuration file {} not found'.format( params_file)) raise else: params_dict = params_file # If the keys do not exist, return None return params_dict.get('position', None), params_dict.get('intrinsic_parameters', None) def get_frame(self, frame_type=COLOR): ''' get_frame: Returns singleton or list of frames corresponding to input. Frames can be any of the following types: COLOR - returns a 512 x 424 color image, registered to depth image DEPTH - returns a 512 x 424 undistorted depth image (units are m) IR - returns a 512 x 424 ir image IR - returns a 512 x 424 ir image RAW_COLOR - returns a 1920 x 1080 color image RAW_DEPTH - returns a 512 x 424 depth image (units are mm) ARGUMENTS: frame_type: [frame type] or frame type A list of frame types. Output corresponds directly to this list. The default argument will return a single registered color image. ''' def get_single_frame(ft): if (ft == COLOR): f, _ = self._get_registered_frame(frames) return f elif (ft == DEPTH): return self._get_depth_frame(frames) elif (ft == RAW_COLOR): return self._get_raw_color_frame(frames) elif (ft == RAW_DEPTH): return self._get_raw_depth_frame(frames) elif (ft == IR): return self._get_ir_frame(frames) else: raise RuntimeError('Unknown frame type {}'.format(ft)) frames = self.listener.waitForNewFrame() # Multiple types were passed if isinstance(frame_type, int): return_frames = get_single_frame(frame_type) else: return_frames = [None] * len(frame_type) for i, ft in enumerate(frame_type): return_frames[i] = get_single_frame(ft) self.listener.release(frames) return return_frames def get_ptcld(self, roi=None, scale=1000, colorized=False): ''' get_ptcld: Returns a point cloud, generated from depth image. Units are mm by default. ARGUMENTS: roi: [x, y, w, h] If specified, will crop the point cloud according to the input roi. Does not accelerate runtime. scale: int Scales the point cloud such that ptcl = ptcl (m) / scale. ie scale = 1000 returns point cloud in mm. colorized: bool If True, returns color matrix along with point cloud such that if pt = ptcld[x,y,:], the color of that point is color = color[x,y,:] ''' # Get undistorted (and registered) frames frames = self.listener.waitForNewFrame() if (colorized): registered, undistorted = self._get_registered_frame(frames) else: undistorted = self._get_depth_frame(frames) self.listener.release(frames) # Get point cloud ptcld = self._depthMatrixToPointCloudPos(undistorted, self._camera_params, scale=scale) # Adjust point cloud based on real world coordinates if (self._camera_position is not None): ptcld = self._applyCameraMatrixOrientation(ptcld, self._camera_position) # Reshape to correct size ptcld = ptcld.reshape(DEPTH_SHAPE[1], DEPTH_SHAPE[0], 3) # If roi, extract if (roi is not None): if (isinstance(roi, tuple) or isinstance(roi, list)): [y, x, h, w] = roi xmin, xmax = int(x), int(x + w) ymin, ymax = int(y), int(y + h) ptcld = ptcld[xmin:xmax, ymin:ymax, :] if (colorized): registered = registered[xmin:xmax, ymin:ymax, :] else: roi = np.clip(roi, 0, 1) for c in range(3): ptcld[:, :, c] = np.multiply(ptcld[:, :, c], roi) if (colorized): # Format the color registration map - To become the "color" input for the scatterplot's setData() function. colors = np.divide(registered, 255) # values must be between 0.0 - 1.0 colors = colors.reshape( colors.shape[0] * colors.shape[1], 4) # From: Rows X Cols X RGB -to- [[r,g,b],[r,g,b]...] colors = colors[:, : 3:] # remove alpha (fourth index) from BGRA to BGR colors = colors[..., ::-1] #BGR to RGB return ptcld, colors return ptcld @staticmethod def _depthMatrixToPointCloudPos(z, camera_params, scale=1): ''' Credit to: Logic1 https://stackoverflow.com/questions/41241236/vectorizing-the-kinect-real-world-coordinate-processing-algorithm-for-speed ''' # bacically this is a vectorized version of depthToPointCloudPos() # calculate the real-world xyz vertex coordinates from the raw depth data matrix. C, R = np.indices(z.shape) R = np.subtract(R, camera_params['cx']) R = np.multiply(R, z) R = np.divide(R, camera_params['fx'] * scale) C = np.subtract(C, camera_params['cy']) C = np.multiply(C, z) C = np.divide(C, camera_params['fy'] * scale) return np.column_stack((z.ravel() / scale, R.ravel(), -C.ravel())) @staticmethod def _applyCameraMatrixOrientation(pt, camera_position=None): ''' Credit to: Logic1 https://stackoverflow.com/questions/41241236/vectorizing-the-kinect-real-world-coordinate-processing-algorithm-for-speed ''' # Kinect Sensor Orientation Compensation # bacically this is a vectorized version of applyCameraOrientation() # uses same trig to rotate a vertex around a gimbal. def rotatePoints(ax1, ax2, deg): # math to rotate vertexes around a center point on a plane. hyp = np.sqrt( pt[:, ax1]**2 + pt[:, ax2]**2 ) # Get the length of the hypotenuse of the real-world coordinate from center of rotation, this is the radius! d_tan = np.arctan2( pt[:, ax2], pt[:, ax1] ) # Calculate the vertexes current angle (returns radians that go from -180 to 180) cur_angle = np.degrees( d_tan ) % 360 # Convert radians to degrees and use modulo to adjust range from 0 to 360. new_angle = np.radians( (cur_angle + deg) % 360 ) # The new angle (in radians) of the vertexes after being rotated by the value of deg. pt[:, ax1] = hyp * np.cos( new_angle) # Calculate the rotated coordinate for this axis. pt[:, ax2] = hyp * np.sin( new_angle) # Calculate the rotated coordinate for this axis. if (camera_position is not None): rotatePoints( 1, 2, camera_position['roll'] ) #rotate on the Y&Z plane # Usually disabled because most tripods don't roll. If an Inertial Nav Unit is available this could be used) rotatePoints( 0, 2, camera_position['elevation']) #rotate on the X&Z plane rotatePoints(0, 1, camera_position['azimuth']) #rotate on the X&Y # Apply offsets for height and linear position of the sensor (from viewport's center) pt[:] += np.float_([ camera_position['x'], camera_position['y'], camera_position['z'] ]) return pt def record(self, filename, frame_type=COLOR, video_codec='XVID'): ''' record: Records a video of the type specified. If no filename is given, records as a .avi. Do not call this in conjunction with a typical cv2 display loop. ARGUMENTS: filename: (str) Name to save the video with. frame_type: frame type What channel to record (only one type). video_codec: (str) cv2 video codec. ''' # Create the video writer if (frame_type == RAW_COLOR): shape = COLOR_SHAPE else: shape = DEPTH_SHAPE[:2] fourcc = cv2.VideoWriter_fourcc(*video_codec) out = cv2.VideoWriter(filename, fourcc, 25, shape) # Record. On keyboard interrupt close and save. try: while True: frame = self.get_frame(frame_type=frame_type) if (frame_type == RAW_COLOR): frame = frame[:, :, :3] out.write(frame) if (not self._headless): cv2.imshow("KinectVideo", frame) if cv2.waitKey(1) & 0xFF == ord('q'): break out.release() cv2.destroyAllWindows() except KeyboardInterrupt: pass finally: out.release() cv2.destroyAllWindows() @staticmethod def _new_frame(shape): ''' _new_frame: Return a pylibfreenect2 frame with the dimensions specified. Note that Frames are pointers, and as such returning using numpy images created from them directly results in some strange behavior. After using the frame, it is highly recommended to copy the resulting image using np.copy() rather than returning the Frame referencing array directly. ''' return Frame(shape[0], shape[1], shape[2]) @staticmethod def _get_raw_color_frame(frames): ''' _get_raw_color_frame: Return the current rgb image as a cv2 image. ''' return cv2.resize(frames["color"].asarray(), COLOR_SHAPE) @staticmethod def _get_raw_depth_frame(frames): ''' _get_raw_depth_frame: Return the current depth image as a cv2 image. ''' return cv2.resize(frames["depth"].asarray(), DEPTH_SHAPE[:2]) @staticmethod def _get_ir_frame(frames): ''' get_ir_depth_frame: Return the current ir image as a cv2 image. ''' return cv2.resize(frames["ir"].asarray(), DEPTH_SHAPE[:2]) def _get_depth_frame(self, frames): ''' _get_depth_frame: Return the current undistorted depth image. ''' undistorted = self._new_frame(DEPTH_SHAPE) self.registration.undistortDepth(frames["depth"], undistorted) undistorted = np.copy(undistorted.asarray(np.float32)) return undistorted def _get_registered_frame(self, frames): ''' get_registered: Return registered color and undistorted depth image. ''' registered = self._new_frame(DEPTH_SHAPE) undistorted = self._new_frame(DEPTH_SHAPE) self.registration.undistortDepth(frames["depth"], undistorted) self.registration.apply(frames["color"], frames["depth"], undistorted, registered) undistorted = np.copy(undistorted.asarray(np.float32)) registered = np.copy(registered.asarray(np.uint8))[..., :3] return registered, undistorted
face_process_frame = True bbox = None track_bbox = None 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) face_bbox = None new_track_bbox = None face_locations = face_recognition.face_locations( c, number_of_times_to_upsample=0, model="cnn") face_encodings = face_recognition.face_encodings(c, face_locations) for face_encoding in face_encodings: matches = face_recognition.compare_faces([known_faces[target]], face_encoding, 0.6) if len(matches) > 0 and matches[0]:
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()
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
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
os.makedirs(colorDir) bigdepthDir = os.path.join(outputDir, bigdepthDir) if not os.path.exists(bigdepthDir): os.makedirs(bigdepthDir) for x in range(0, framesToCapture): frames = listener.waitForNewFrame() frameNum = "%04d" % (x) color = frames["color"] ir = frames["ir"] depth = frames["depth"] registration.apply(color, depth, undistorted, registered, enable_filter=need_enable_filter, 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.imwrite(os.path.join(outputDir, "ir" + frameNum + ".png"), ir.asarray() / 65535.) cv2.imwrite(os.path.join(outputDir, "depth" + frameNum + ".png"), depth.asarray() / 4500.) # Convert from RGBX or BGRX to RGB or BGR colorArray = color.asarray() print(colorArray.shape) trimColor = np.zeros( (np.size(colorArray,0), np.size(colorArray,1), 3) ) print(trimColor.shape) trimColor[:,:,0] = colorArray[:,:,0]
class Kinect(): ''' Kinect object, it uses pylibfreenect2 as interface to get the frames. The original example was taken from the pylibfreenect2 github repository, at: https://github.com/r9y9/pylibfreenect2/blob/master/examples/selective_streams.py ''' def __init__(self, enable_rgb, enable_depth, need_bigdepth, need_color_depth_map, K=None, D=None): ''' Init method called upon creation of Kinect object ''' # according to the system, it loads the correct pipeline # and prints a log for the user 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() rospy.loginfo(color.BOLD + color.YELLOW + '-- PACKET PIPELINE: ' + str(type(self.pipeline).__name__) + ' --' + color.END) self.enable_rgb = enable_rgb self.enable_depth = enable_depth self.K = K self.D = D # creates the freenect2 device self.fn = Freenect2() # if no kinects are plugged in the system, it quits self.num_devices = self.fn.enumerateDevices() if self.num_devices == 0: rospy.loginfo(color.BOLD + color.RED + '-- ERROR: NO DEVICE CONNECTED!! --' + color.END) sys.exit(1) # otherwise it gets the first one available self.serial = self.fn.getDeviceSerialNumber(0) self.device = self.fn.openDevice(self.serial, pipeline=self.pipeline) # defines the streams to be acquired according to what the user wants types = 0 if self.enable_rgb: types |= FrameType.Color if self.enable_depth: types |= (FrameType.Ir | FrameType.Depth) self.listener = SyncMultiFrameListener(types) # Register listeners if self.enable_rgb: self.device.setColorFrameListener(self.listener) if self.enable_depth: self.device.setIrAndDepthFrameListener(self.listener) if self.enable_rgb and self.enable_depth: self.device.start() else: self.device.startStreams(rgb=self.enable_rgb, depth=self.enable_depth) # NOTE: must be called after device.start() if self.enable_depth: self.registration = Registration( self.device.getIrCameraParams(), self.device.getColorCameraParams()) # last number is bytes per pixel self.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4) # Optinal parameters for registration self.need_bigdepth = need_bigdepth self.need_color_depth_map = need_color_depth_map if self.need_bigdepth: self.bigdepth = Frame(1920, 1082, 4) else: self.bigdepth = None if self.need_color_depth_map: self.color_depth_map = np.zeros((424, 512), np.int32).ravel() else: self.color_depth_map = None def acquire(self): ''' Acquisition method to trigger the Kinect to acquire new frames. ''' # acquires a frame only if it's new frames = self.listener.waitForNewFrame() if self.enable_rgb: self.color = frames["color"] self.color_new = cv2.resize(self.color.asarray(), (int(1920 / 1), int(1080 / 1))) # The image obtained has a fourth dimension which is the alpha value # thus we have to remove it and take only the first three self.color_new = self.color_new[:, :, 0:3] # correct distortion of camera self.correct_distortion() if self.enable_depth: # these only have one dimension, we just need to convert them to arrays # if we want to perform detection on them self.depth = frames["depth"] self.depth_new = cv2.resize(self.depth.asarray() / 4500., (int(512 / 1), int(424 / 1))) self.registration.undistortDepth(self.depth, self.undistorted) self.undistorted_new = cv2.resize( self.undistorted.asarray(np.float32) / 4500., (int(512 / 1), int(424 / 1))) self.ir = frames["ir"] self.ir_new = cv2.resize(self.ir.asarray() / 65535., (int(512 / 1), int(424 / 1))) if self.enable_rgb and self.enable_depth: self.registration.apply(self.color, self.depth, self.undistorted, self.registered, bigdepth=self.bigdepth, color_depth_map=self.color_depth_map) # RGB + D self.registered_new = self.registered.asarray(np.uint8) if self.need_bigdepth: self.bigdepth_new = cv2.resize( self.bigdepth.asarray(np.float32), (int(1920 / 1), int(1082 / 1))) #cv2.imshow("bigdepth", cv2.resize(self.bigdepth.asarray(np.float32), (int(1920 / 1), int(1082 / 1)))) if self.need_color_depth_map: #cv2.imshow("color_depth_map", self.color_depth_map.reshape(424, 512)) self.color_depth_map_new = self.color_depth_map.reshape( 424, 512) # do this anyway to release every acquired frame self.listener.release(frames) def stop(self): ''' Stop method to close device upon exiting the program ''' rospy.loginfo(color.BOLD + color.RED + '\n -- CLOSING DEVICE... --' + color.END) self.device.stop() self.device.close() def correct_distortion(self): ''' Method to correct distortion using camera calibration parameters ''' if self.K is not None and self.D is not None: h, w = self.color_new.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix( self.K, self.D, (w, h), 1, (w, h)) # undistort self.RGBundistorted = cv2.undistort(self.color_new, self.K, self.D, None, newcameramtx) # crop the image x, y, w, h = roi self.RGBundistorted = self.RGBundistorted[y:y + h, x:x + w] self.RGBundistorted = cv2.flip(self.RGBundistorted, 0) self.RGBundistorted = self.RGBundistorted[0:900, 300:1800] #self.RGBundistorted = self.RGBundistorted[0:900, 520:1650] else: rospy.loginfo(color.BOLD + color.RED + '-- ERROR: NO CALIBRATION LOADED!! --' + color.END) self.RGBundistorted = self.color_new
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
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 KinectStreamer: 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 stream_thread(self): undistorted = Frame(RES_DEPTH[0], RES_DEPTH[1], 4) registered = Frame(RES_DEPTH[0], RES_DEPTH[1], 4) while self.run_loop: frames = self.listener.waitForNewFrame() color = frames["color"] ir = frames["ir"] depth = frames["depth"] self.registration.apply(color, depth, undistorted, registered) img_ir = ir.asarray() / 65535. self.lock.acquire() self.img_depth = (depth.asarray() / 4500. * 255).astype(np.uint8) self.img_color = np.copy(color.asarray()[:, ::-1, :3]) self.img_buffer[:, :, :, self.idx_buffer] = self.img_color self.img_registered_color = np.copy(registered.asarray(np.uint8)) self.img_registered_depth = np.copy(undistorted.asarray(np.uint8)) self.lock.release() self.idx_buffer += 1 if self.idx_buffer >= SIZE_AVG_FILTER: self.idx_buffer = 0 cv2.imshow("Kinect Depth", self.img_depth) cv2.imshow( "Kinect Color", cv2.resize(self.img_color, (int(round( 0.3 * RES_COLOR[0])), int(round(0.3 * RES_COLOR[1]))))) cv2.imshow("Kinect R Depth", self.img_registered_depth) cv2.imshow("Kinect R Color", self.img_registered_color) key = cv2.waitKey(1) & 0xFF if key == ord('q') or key == ord('x'): self.run_loop = False break elif key == ord(' '): cv2.imwrite("frame.png", frame) self.listener.release(frames) self.device.stop() self.device.close() def write_thread(self, video_name="output"): fourcc = cv2.VideoWriter_fourcc(*'X264') vid_color = cv2.VideoWriter(video_name + ".avi", fourcc, FPS, RES_COLOR) vid_depth = cv2.VideoWriter(video_name + "_depth.avi", fourcc, FPS, RES_DEPTH, isColor=False) t_curr = time.time() while self.run_loop: t_start = t_curr self.lock.acquire() try: vid_color.write(self.img_color) vid_depth.write(self.img_depth) except: pass self.lock.release() t_curr = time.time() time.sleep(max(1. / FPS - (t_curr - t_start), 0)) vid_color.release() vid_depth.release()
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)
if (bearing >= 0) and (bearing <= 180): if ( heading > bearing and heading < (bearing+180)): print("right") turnRight() print (heading) #time.sleep(0.5) else: print("Left") turnLeft() print (heading) #time.sleep(0.5) moveForward() frames = listener.waitForNewFrame() depth = frames["depth"] registration.apply(depth,registered) 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)))'''
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)
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 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()
def run(self): pipeline = CpuPacketPipeline() print("Packet pipeline:", type(pipeline).__name__) enable_rgb = True enable_depth = True 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) types = 0 if enable_rgb: types |= FrameType.Color 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) count = 0 while True: mutex = Lock() mutex.acquire() 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) elif enable_depth: registration.undistortDepth(depth, undistorted) if enable_depth: cv2.imshow("ir", ir.asarray() / 65535.) nameir = self.makename("ir", count) #cv2.imwrite(nameir, ir.asarray() / 65535.) cv2.imshow("depth", depth.asarray() / 4500.) named = self.makename("depth", count) #cv2.imwrite(named, depth.asarray() / 4500.) cv2.imshow("undistorted", undistorted.asarray(np.float32) / 4500.) nameu = self.makename("undistorted", count) #cv2.imwrite(nameu, undistorted.asarray(np.float32) / 4500.) if enable_rgb: cv2.imshow( "color", cv2.resize(color.asarray(), (int(1920 / 3), int(1080 / 3)))) namec = self.makename("color", count) #cv2.imwrite(namec,cv2.resize(color.asarray(),(int(1920 / 3), int(1080 / 3)))) if enable_rgb and enable_depth: cv2.imshow("registered", registered.asarray(np.uint8)) namer = self.makename("registered", count) #cv2.imwrite(namer,registered.asarray(np.uint8)) mutex.release() count = count + 1 listener.release(frames) key = cv2.waitKey(delay=1) if key == ord('q'): break device.stop() device.close()
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()
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 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()
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() 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))))
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 main(hrnet_c, hrnet_j, hrnet_weights, hrnet_joints_set, single_person, max_batch_size, disable_vidgear, device): if device is not None: device = torch.device(device) else: if torch.cuda.is_available() and True: torch.backends.cudnn.deterministic = True device = torch.device('cuda:0') else: device = torch.device('cpu') print(device) has_display = 'DISPLAY' in os.environ.keys() or sys.platform == 'win32' model = SimpleHRNet( hrnet_c, hrnet_j, hrnet_weights, multiperson=not single_person, max_batch_size=max_batch_size, device=device ) print("Packet pipeline:", type(pipeline).__name__) enable_rgb = True enable_depth = True 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) types = 0 if enable_rgb: types |= FrameType.Color 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) elif enable_depth: registration.undistortDepth(depth, undistorted) if enable_depth: cv2.imshow("ir", ir.asarray() / 65535.) cv2.imshow("depth", depth.asarray() / 4500.) # cv2.imshow("undistorted", undistorted.asarray(np.float32) / 4500.) if enable_rgb and enable_depth: cv2.imshow("registered", registered.asarray(np.uint8)) # color = cv2.resize(color.asarray()[:, :, :3], (int(1920 / 3), int(1080 / 3))) color = registered.asarray(np.uint8)[:, :, :3] color = np.ascontiguousarray(color, dtype=np.uint8) pts = model.predict(color) undistorted_nor = undistorted.asarray(np.float32) / 4500. undistorted_image = cv2.cvtColor(undistorted_nor, cv2.COLOR_GRAY2BGR) for i, pt in enumerate(pts): color = draw_points_and_skeleton(color, pt, joints_dict()[hrnet_joints_set]['skeleton'], person_index=i, joints_color_palette='gist_rainbow', skeleton_color_palette='jet', joints_palette_samples=10) for j, point in enumerate(pt): if point[2]>0.5 and point[0] < undistorted_image.shape[0] and point[1] < undistorted_image.shape[1]: if j == 9: left_wrist_depth = undistorted_nor[int(point[0]), int(point[1])] print('left_wrist_depth',left_wrist_depth) undistorted_image = cv2.circle(undistorted_image, (int(point[1]), int(point[0])), 30, (255, 255, 0),-1) if j == 10: right_wrist_depth = undistorted_nor[int(point[0]), int(point[1])] print('right_wrist_depth', right_wrist_depth) undistorted_image = cv2.circle(undistorted_image, (int(point[1]), int(point[0])), 30, (255, 0, 255),-1) if enable_rgb: cv2.imshow("color", color) cv2.imshow("undistorted", undistorted_image) listener.release(frames) key = cv2.waitKey(delay=1) if key == ord('q'): break device.stop() device.close() sys.exit(0)
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) elif enable_depth: registration.undistortDepth(depth, undistorted) if enable_depth: cv2.imshow("ir", ir.asarray() / 65535.) cv2.imshow("depth", depth.asarray() / 4500.) cv2.imshow("undistorted", undistorted.asarray(np.float32) / 4500.) if enable_rgb: cv2.imshow("color", cv2.resize(color.asarray(), (int(1920 / 3), int(1080 / 3)))) if enable_rgb and enable_depth: cv2.imshow("registered", registered.asarray(np.uint8)) listener.release(frames)
def run(self): self._isrunning = True try: from pylibfreenect2 import OpenGLPacketPipeline pipeline = OpenGLPacketPipeline() except: try: from pylibfreenect2 import OpenCLPacketPipeline pipeline = OpenCLPacketPipeline() except: from pylibfreenect2 import CpuPacketPipeline pipeline = CpuPacketPipeline() if self._debug: print("Packet pipeline:", type(pipeline).__name__) self.fn = Freenect2() num_devices = self.fn.enumerateDevices() if self._debug: print("Number of devices: {}".format(num_devices)) serial = self.fn.getDeviceSerialNumber(0) device = self.fn.openDevice(serial, pipeline=pipeline) listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) if self._debug: print("Init done") device.start() registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) params = device.getIrCameraParams() self.cx = params.cx self.cy = params.cy self.fx = params.fx self.fy = params.fy undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) while self._isrunning: frames = listener.waitForNewFrame() color = frames["color"] ir = frames["ir"] depth = frames["depth"] registration.apply(color, depth, undistorted, registered, bigdepth=None, color_depth_map=None) if self._save_color: self._set_color_image( cv2.cvtColor(color.asarray(), cv2.COLOR_BGR2RGB)) if self._save_depth or self._save_pointcloud: depth_arr = depth.asarray() if self._save_depth: self._set_depth_image(depth_arr) if self._save_ir: self._set_ir_image(ir.asarray()) if self._save_registered or self._save_pointcloud: reg = cv2.cvtColor(registered.asarray(np.uint8), cv2.COLOR_BGR2RGB) if self._save_registered: self._set_registered_image(reg) if self._save_undistorted: und = undistorted.asarray(np.uint8) self._set_undistorted_image( cv2.cvtColor(und, cv2.COLOR_BGR2RGB)) if self._save_pointcloud: self.compute_pointcloud(reg, depth_arr) listener.release(frames) if self._debug: print("Stopping device") device.stop() if self._debug: print("Closing device") device.close() if self._debug: print("Device stopped and closed")
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() 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(
def KinectStream(frame_count, BreakKinect, enable_rgb=True, enable_depth=True): # create folders for the color and depth images, respectively last_path, _ = os.path.split(os.getcwd()) path_color = os.path.join(last_path, "color") path_depth = os.path.join(last_path, "depth") CreateRefrashFolder(path_color) CreateRefrashFolder(path_depth) ''' # if the depth images are needed, you must use OpenGLPacketPipeline for enabling GPU to # render the depth map for so that the real-time capture can be achieved. try: from pylibfreenect2 import OpenGLPacketPipeline pipeline = OpenGLPacketPipeline() except: try: from pylibfreenect2 import OpenCLPacketPipeline pipeline = OpenCLPacketPipeline() except: from pylibfreenect2 import CpuPacketPipeline pipeline = CpuPacketPipeline() ''' if enable_depth: from pylibfreenect2 import OpenGLPacketPipeline pipeline = OpenGLPacketPipeline() else: from pylibfreenect2 import CpuPacketPipeline pipeline = CpuPacketPipeline() print("Packet pipeline:", type(pipeline).__name__) 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) types = 0 if enable_rgb: types |= FrameType.Color 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) # the target size of the resize function SetSize = (540, 360) # SetSize = (1080, 720) while not BreakKinect.value: frame_count.value += 1 file_name = 'image' + str(int(frame_count.value)) + '.jpg' im_path_color = os.path.join(path_color, file_name) im_path_depth = os.path.join(path_depth, file_name) 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, enable_filter=False) elif enable_depth: registration.undistortDepth(depth, undistorted) if enable_rgb: start = time.time() new_frame = cv2.resize(color.asarray(), SetSize) # new_frame = new_frame[:,:,:-1] # new_frame = cv2.cvtColor(new_frame[:,:,:-1], cv2.COLOR_RGB2BGR) new_frame = registered.asarray(np.uint8) # new_frame = cv2.cvtColor(new_frame[:,:,[0,1,2]], cv2.COLOR_RGB2BGR) cv2.imwrite(im_path_color, new_frame[:, :, :-1]) # print("Kinect color:", new_frame.shape) if enable_depth: # depth_frame = cv2.resize(depth.asarray()/ 4500., SetSize) depth = depth.asarray() / 4500. # cv2.imshow("color", new_frame) undist = undistorted.asarray(np.float32) / 4500 * 255 cv2.imwrite(im_path_depth, undist.astype(np.uint8)) print("Kinect depth:", depth.shape) listener.release(frames) device.stop() device.close() # WriteVideo(path, im_pre = 'image', video_name = 'test.avi', fps = 15, size = SetSize) sys.exit(0)
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"] registration.apply(color, depth_frame, undistorted, registered) #convert image color = registered.asarray(np.uint8) color = cv2.flip(color, 1) img = depth_frame.asarray(np.float32) / 4500. imgray = np.uint8(depth_frame.asarray(np.float32) / 255.0) #flip images img = cv2.flip(img, 1) imgray = cv2.flip(imgray, 1) if len(sys.argv) > 1: if sys.argv[1] == "test" and frame_i < num_frames: np.save("test_frames/" + str(frame_i) + ".npy", depth_frame.asarray(np.float32)) frame_i += 1 #begin edge detection
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()