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()