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) key = cv2.waitKey(delay=1) if key == ord('q'):
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()
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
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 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()
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
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)
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)
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
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()