def capture_frames(register=False):
device = Device()
frames = {}
with device.running():
for type_, frame in device:
frames[type_] = frame
if FrameType.Color in frames and FrameType.Depth in frames and FrameType.Ir in frames:
break
rgb, depth, ir = frames[FrameType.Color], frames[FrameType.Depth], frames[
FrameType.Ir]
image = {}
image['rgb'] = rgb.to_array()
image['depth'] = depth.to_array()
image['ir'] = ir.to_array()
b, g, r, x = cv2.split(image['rgb'])
image['rgb'] = cv2.merge([r, g, b])
for type_ in image.keys():
image[type_] = np.rot90(image[type_], 1)
image[type_] = np.fliplr(image[type_])
if register == False:
return image
if register == True:
depth_rect, color_rect = device.registration.apply(rgb,
depth,
enable_filter=False)
return depth_rect, color_rect
def _init_device(self):
"""
creates the self.device parameter to start the stream of frames
Returns:
"""
if _platform == 'Windows':
self.device = PyKinectRuntime.PyKinectRuntime(
PyKinectV2.FrameSourceTypes_Color
| PyKinectV2.FrameSourceTypes_Depth
| PyKinectV2.FrameSourceTypes_Infrared)
elif _platform == 'Linux':
# Threading
self._lock = threading.Lock()
self._thread = None
self._thread_status = 'stopped' # status: 'stopped', 'running'
self.device = Device()
self._color = numpy.zeros((self.color_height, self.color_width, 4))
self._depth = numpy.zeros((self.depth_height, self.depth_width))
self._ir = numpy.zeros((self.depth_height, self.depth_width))
self._run()
logger.info("Searching first frame")
while True:
if not numpy.all(self._depth == 0):
logger.info("First frame found")
break
else:
logger.error(_platform + "not implemented")
raise NotImplementedError
def test_linux_2():
if _platform == 'Windows':
return
from freenect2 import Device, FrameType
# We use numpy to process the raw IR frame
import numpy as np
# We use the Pillow library for saving the captured image
from PIL import Image
# Open default device
device = Device()
# Start the device
with device.running():
# For each received frame...
for type_, frame in device:
# ...stop only when we get an IR frame
if type_ is FrameType.Ir:
break
# Outside of the 'with' block, the device has been stopped again
# The received IR frame is in the range 0 -> 65535. Normalise the
# range to 0 -> 1 and take square root as a simple form of gamma
# correction.
ir_image = frame.to_array()
ir_image /= ir_image.max()
ir_image = np.sqrt(ir_image)
def main():
device = Device()
frames = {}
with device.running():
for type_, frame in device:
frames[type_] = frame
if FrameType.Color in frames and FrameType.Depth in frames:
break
rgb, depth = frames[FrameType.Color], frames[FrameType.Depth]
undistorted, registered, big_depth = device.registration.apply(
rgb, depth, with_big_depth=True)
rgb.to_image().save('output_rgb.png')
big_depth.to_image().save('output_depth.tiff')
with open('output_calib.json', 'w') as fobj:
json.dump({
'color': dict(
(k, getattr(device.color_camera_params, k))
for k in 'fx fy cx cy'.split()),
'ir': dict(
(k, getattr(device.ir_camera_params, k))
for k in 'fx fy cx cy k1 k2 k3 p1 p2'.split()),
}, fobj)
def main():
device = Device()
frames = {}
with device.running():
for type_, frame in device:
frames[type_] = frame
if FrameType.Color in frames and FrameType.Depth in frames:
break
rgb, depth = frames[FrameType.Color], frames[FrameType.Depth]
undistorted, registered = device.registration.apply(rgb, depth)
points_array = device.registration.get_points_xyz_array(undistorted)
undistorted.to_image().save('undistorted_depth.tiff')
registered.to_image().save('registered_rgb.png')
np.savez('points.npz', points=points_array)
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 test_linux_2_1():
if _platform == 'Windows':
return
from freenect2 import Device, FrameType
import numpy as np
# Open the default device and capture a color and depth frame.
device = Device()
frames = {}
with device.running():
for type_, frame in device:
frames[type_] = frame
if FrameType.Color in frames and FrameType.Depth in frames:
break
# Use the factory calibration to undistort the depth frame and register the RGB
# frame onto it.
rgb, depth = frames[FrameType.Color], frames[FrameType.Depth]
undistorted, registered, big_depth = device.registration.apply(
rgb, depth, with_big_depth=True)
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
"""
Capture a single RGB and depth frame and save them to output.pcd in
the libpcl PCD format. View the resulting cloud with:
pcl_viewer output.pcd
"""
from freenect2 import Device, FrameType
import numpy as np
# Open the default device and capture a color and depth frame.
device = Device()
frames = {}
with device.running():
for type_, frame in device:
frames[type_] = frame
if FrameType.Color in frames and FrameType.Depth in frames:
break
# Use the factory calibration to undistort the depth frame and register the RGB
# frame onto it.
rgb, depth = frames[FrameType.Color], frames[FrameType.Depth]
undistorted, registered, big_depth = device.registration.apply(
rgb, depth, with_big_depth=True)
# Combine the depth and RGB data together into a single point cloud.
with open('output.pcd', 'wb') as fobj:
device.registration.write_pcd(fobj, undistorted, registered)
with open('output_big.pcd', 'wb') as fobj:
device.registration.write_big_pcd(fobj, big_depth, rgb)
help="Modify config options using the command-line",
default=None,
nargs=argparse.REMAINDER)
args = parser.parse_args()
# update config file
update_config(cfg, args)
print(torch.cuda.get_device_name(0))
# assert torch.cuda.get_device_name(0) is 'GeForce GTX 1080'
model = get_model('vgg19')
model.load_state_dict(torch.load(args.weight))
model.cuda()
model.float()
model.eval()
device = Device()
device.start()
t = True
if __name__ == "__main__":
# video_capture = cv2.VideoCapture(0)
while True:
# Capture frame-by-frame
if t:
t = False
time.sleep(2)
type_, frame = device.get_next_frame()
if type_ is FrameType.Color: # FrameFormat.BGRX
rgb = frame.to_array().astype(np.uint8)[:,:,0:3]
# cv2.imshow('rgb', rgb[:,:,0:3])
oriImg = rgb
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._init_device()
self.depth = self.get_frame()
self.color = self.get_color()
logger.info("KinectV2 initialized.")
def _init_device(self):
"""
creates the self.device parameter to start the stream of frames
Returns:
"""
if _platform == 'Windows':
self.device = PyKinectRuntime.PyKinectRuntime(
PyKinectV2.FrameSourceTypes_Color
| PyKinectV2.FrameSourceTypes_Depth
| PyKinectV2.FrameSourceTypes_Infrared)
elif _platform == 'Linux':
# Threading
self._lock = threading.Lock()
self._thread = None
self._thread_status = 'stopped' # status: 'stopped', 'running'
self.device = Device()
self._color = numpy.zeros((self.color_height, self.color_width, 4))
self._depth = numpy.zeros((self.depth_height, self.depth_width))
self._ir = numpy.zeros((self.depth_height, self.depth_width))
self._run()
logger.info("Searching first frame")
while True:
if not numpy.all(self._depth == 0):
logger.info("First frame found")
break
else:
logger.error(_platform + "not implemented")
raise NotImplementedError
def _run(self):
"""
Run the thread when _platform is linux
"""
if self._thread_status != 'running':
self._thread_status = 'running'
self._thread = threading.Thread(
target=self._open_kinect_frame_stream,
daemon=True,
)
self._thread.start()
logger.info('Acquiring frames...')
else:
logger.info('Already running.')
def _stop(self):
"""
Stop the thread when _platform is linux
"""
if self._thread_status is not 'stopped':
self._thread_status = 'stopped' # set flag to end thread loop
self._thread.join() # wait for the thread to finish
logger.info('Stopping frame acquisition.')
else:
logger.info('thread was not running.')
def _open_kinect_frame_stream(self):
"""
keep the stream open to adquire new frames when using linux
"""
frames = {}
with self.device.running():
for type_, frame in self.device:
frames[type_] = frame
if FrameType.Color in frames:
self._color = frames[FrameType.Color].to_array()
if FrameType.Depth in frames:
self._depth = frames[FrameType.Depth].to_array()
if FrameType.Ir in frames:
self._ir = frames[FrameType.Ir].to_array()
if self._thread_status != "running":
break
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':
# assert self._thread_status == "running"
self.depth = self._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()
# reshape the array to 2D with native resolution of the kinectV2
self.ir_frame_raw = numpy.flipud(
ir_flattened.reshape((self.depth_height, self.depth_width)))
elif _platform == 'Linux':
# assert self._thread_status == "running"
self.ir_frame_raw = self._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 = numpy.array([self.device.get_last_color_frame()])
elif _platform == 'Linux':
# assert self._thread_status == "running"
color = self._color
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(color, (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])
return self.color
from freenect2 import Device, FrameType
import cv2 as cv
import numpy as np
import requests
from copy import deepcopy
device = Device()
frame_height = 424
frame_width = 512
hallway_door_person_location = {'x1': 220, 'y1': 90, 'x2': 290, 'y2': 260}
doorway_states = {
"kitchen": {
"direction": 0,
"progress": 0
},
"hallway": {
"direction": 0,
"progress": 0
}
}
hallway_door_frame_dist = 3000
device.start()
for frame_type, frame in device:
if frame_type == FrameType.Depth:
frame_mm = frame.to_array()
hallway_door_person = frame_mm[hallway_door_person_location['y1']:
dir_save = cf.dir_save + args.name + '/' + cf.dir_kinect
os.makedirs(dir_save, exist_ok=True)
file_ply = dir_save + cf.save_ply
file_img = dir_save + cf.save_image
file_depth = dir_save + cf.save_depth
def save_images(cam, idx, rgb, depth=None):
cv2.imwrite(file_img.format(cam, idx), rgb)
depth_image = tool.pack_float_to_bmp_bgra(depth)
cv2.imwrite(file_depth.format(cam, idx), depth_image)
# Get Kinect
device = Device()
try:
while True:
frames = {}
with device.running():
for type_, frame in device:
frames[type_] = frame
if FrameType.Color in frames and FrameType.Depth in frames:
break
rgb, depth = frames[FrameType.Color], frames[FrameType.Depth]
rgb = cv2.flip(rgb.to_array(), 1)
depth = cv2.flip(depth.to_array(), 1)
depth_colormap = cv2.applyColorMap(
