class Kinect2:
def __init__(self, calibration_folder=None):
self.freenect = Freenect2()
setGlobalLogger(createConsoleLogger(LoggerLevel.NONE))
num_devices = self.freenect.enumerateDevices()
if num_devices == 0:
raise ValueError("No kinect2 device connected!")
serial = self.freenect.getDeviceSerialNumber(0)
self.device = self.freenect.openDevice(serial, pipeline=pipeline)
self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
self.device.start()
self.calibration = {}
if calibration_folder is not None:
self.calibration = utils.load_calibration(calibration_folder)
def snapshot(self):
frames = self.listener.waitForNewFrame()
color = frames["color"]
depth = frames["depth"]
ir = frames["ir"]
color = color.asarray(np.uint8).copy()
depth = depth.asarray(np.float32).copy()
ir = ir.asarray(np.float32).copy()
# flip images
color = color[:, ::-1]
ir = ir[:, ::-1]
depth = depth[:, ::-1]
color = color[..., :3]
# color = color[..., ::-1]
ir = self._convert_ir(ir)
self.listener.release(frames)
return color, depth, ir
def _convert_ir(self, ir):
"""
convertIr function
https://github.com/code-iai/iai_kinect2/blob/master/kinect2_calibration/src/kinect2_calibration.cpp
"""
ir_min, ir_max = ir.min(), ir.max()
ir_convert = (ir - ir_min) / (ir.max() - ir.min())
ir_convert *= 255
ir_convert = np.tile(ir_convert[..., None], (1, 1, 3))
return ir_convert.astype(np.uint8)
def run(self):
"""
main function to start Kinect and read depth information from Kinect, show video and save it
"""
Fn = Freenect2()
num_devices = Fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = Fn.getDeviceSerialNumber(0)
device = Fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(FrameType.Depth)
device.setIrAndDepthFrameListener(listener)
device.start()
fourcc = cv2.VideoWriter_fourcc('m', 'j', 'p', 'g')
depth_video = cv2.VideoWriter(
'Depth.avi', fourcc, 30.0, (512, 424), 0)
i = 0
while not self._done:
frames = listener.waitForNewFrame()
depth = frames["depth"]
depth = depth.asarray().clip(0, 4096)
hand_contour = self._find_contour(depth)
darks = np.zeros((424, 512), dtype=np.uint8)
if cv2.contourArea(hand_contour[0]) < 1000 or cv2.contourArea(hand_contour[0]) > 5000:
self.cover = np.uint8(depth/16.)
else:
seg_depth = self._segment(depth, hand_contour, darks)
np.save('./offline/data/seg_depth%d.npy' % i, seg_depth)
i += 1
cv2.imshow("depth", self.cover)
depth_video.write(self.cover)
listener.release(frames)
key = cv2.waitKey(delay=1)
if key == ord('q'):
self._done = True
depth_video.release()
device.stop()
device.close()
sys.exit()
def getcameraimgbykinect(self, basedir):
try:
from pylibfreenect2 import OpenGLPacketPipeline
pipeline = OpenGLPacketPipeline()
except:
try:
from pylibfreenect2 import OpenCLPacketPipeline
pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
pipeline = CpuPacketPipeline()
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
return
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
types = 0
types |= FrameType.Color
listener = SyncMultiFrameListener(types)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.startStreams(rgb=True, depth=False)
while True:
frames = listener.waitForNewFrame()
color = frames["color"]
colorimg = cv2.resize(color.asarray(), (200,150))
(shortDate, longDate)=self.getDateStr()
file_dir = basedir+"/"+shortDate
if not os.path.exists(file_dir):
os.makedirs(file_dir)
new_filename = Pic_str().create_uuid() + '.jpg'
fpath = os.path.join(file_dir, new_filename)
# print(fpath )
cv2.imwrite(fpath, colorimg, [cv2.IMWRITE_JPEG_QUALITY,70])
listener.release(frames)
isfire = self.fireDetect.detect_image(fpath)
self.sendUtils.sendReqtoServer("园区厂区仓库","网络摄像头",longDate,isfire,shortDate+"/"+new_filename)
time.sleep(3)
device.stop()
device.close()
return
def service_func():
try:
from pylibfreenect2 import OpenGLPacketPipeline
pipeline = OpenGLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
pipeline = CpuPacketPipeline()
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(FrameType.Ir)
device.setIrAndDepthFrameListener(listener)
directory = os.path.dirname(os.path.realpath('__file__'))
try:
while 1:
device.start()
count=0
for i in range(1,5):
frames = listener.waitForNewFrame()
ir = frames["ir"]
tmpimg = ir.asarray() / 256.
tmp = tmpimg.astype(int)
path = directory +"/kinect.png"
print path
cv2.imwrite(path, tmp)
listener.release(frames)
if i < 4 :
time.sleep(30-time.localtime().tm_sec%30)
device.stop()
time.sleep(5)
time.sleep((90-time.localtime().tm_sec)%30)
except KeyboardInterrupt:
device.close()
exit
class Kinect:
def __init__(self):
self.fn = Freenect2()
num_devices = self.fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = self.fn.getDeviceSerialNumber(0)
self.device = self.fn.openDevice(serial, pipeline=pipeline)
self.listener = SyncMultiFrameListener(
FrameType.Color)
# Register listeners
self.device.setColorFrameListener(self.listener)
# device.setIrAndDepthFrameListener(listener)
self.device.start()
# NOTE: must be called after device.start()
registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
def __del__(self):
self.device.stop()
self.device.close()
def get_image(self):
frames = self.listener.waitForNewFrame()
color = frames["color"].asarray()[:,::-1,:3]
self.listener.release(frames)
# if np.sum(color[-1,:,:]) == 0:
# return None
return color
def __test(enable_rgb, enable_depth):
fn = Freenect2()
num_devices = fn.enumerateDevices()
assert num_devices > 0
device = fn.openDefaultDevice()
types = 0
if enable_rgb:
types |= FrameType.Color
if enable_depth:
types |= (FrameType.Ir | FrameType.Depth)
listener = SyncMultiFrameListener(types)
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.startStreams(rgb=enable_rgb, depth=enable_depth)
# test if we can get one frame at least
frames = listener.waitForNewFrame()
listener.release(frames)
device.stop()
device.close()
def __test(enable_rgb, enable_depth):
fn = Freenect2()
num_devices = fn.enumerateDevices()
assert num_devices > 0
device = fn.openDefaultDevice()
types = 0
if enable_rgb:
types |= FrameType.Color
if enable_depth:
types |= (FrameType.Ir | FrameType.Depth)
listener = SyncMultiFrameListener(types)
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.startStreams(rgb=enable_rgb, depth=enable_depth)
# test if we can get one frame at least
frames = listener.waitForNewFrame()
listener.release(frames)
device.stop()
device.close()
def CaptureVideo():
logger = createConsoleLogger(LoggerLevel.Debug)
setGlobalLogger(logger)
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# Optinal parameters for registration
# set True if you need
need_bigdepth = True
need_color_depth_map = False
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
# cap = cv2.VideoCapture(0)
res_old = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]
while (True):
# Capture frame-by-frame
# ret, frame = cap.read()
frames = listener.waitForNewFrame()
img = frames["color"].asarray()[:, :, :3]
frame = cv2.resize(img, (int(1920 / 3), int(1080 / 3)))
start = time.time()
# Display the resulting frame
box, conf, cls = eval_video_detection(net_detection_, frame)
p_box = []
for i in range(len(box)):
p_box_per_person = {}
if int(cls[i]) == 15 and conf[i] > 0.0:
p_box_per_person['p1'] = (box[i][0], box[i][1])
p_box_per_person['p2'] = (box[i][2], box[i][3])
p_box_per_person['height_difference'] = box[i][3] - box[i][1]
p_box_per_person['weight_difference'] = box[i][2] - box[i][0]
p_box_per_person['conf'] = conf[i]
p_box_per_person['cls'] = int(cls[i])
p_box.append(p_box_per_person)
# print p_box
if len(p_box) == 0:
cv2.putText(frame, Category_labels_[11], (30, 60),
cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 0, 255), 2)
cv2.imshow('Webcam', frame)
else:
p_box = sorted(p_box,
key=lambda e: e.__getitem__('weight_difference'),
reverse=True)
p_hh_low = float(p_box[0]['p1'][1]) - (1 / float(
(p_box[0]['p2'][1] - p_box[0]['p1'][1]))
) * 2000.0 # 记录剪裁框的位置大小
p_hh_hige = float(p_box[0]['p2'][1]) + (1 / float(
(p_box[0]['p2'][1] - p_box[0]['p1'][1]))) * 2000.0
print p_box[0]['p2'][0] - p_box[0]['p1'][0]
if p_box[0]['p2'][0] - p_box[0]['p1'][0] > 400:
Penalty_ratio_factor = 30000.0
elif p_box[0]['p2'][0] - p_box[0]['p1'][0] > 300:
Penalty_ratio_factor = 15000.0
elif p_box[0]['p2'][0] - p_box[0]['p1'][0] > 200:
Penalty_ratio_factor = 8000.0
elif p_box[0]['p2'][0] - p_box[0]['p1'][0] > 100:
Penalty_ratio_factor = 3000.0
else:
Penalty_ratio_factor = 1000.0
p_ww_low = float(p_box[0]['p1'][0]) - (1 / float(
(p_box[0]['p2'][0] - p_box[0]['p1'][0]))
) * Penalty_ratio_factor
p_ww_hige = float(p_box[0]['p2'][0]) + (1 / float(
(p_box[0]['p2'][0] - p_box[0]['p1'][0]))
) * Penalty_ratio_factor
cropframe = frame[
int(max(p_hh_low, 0)):int(min(p_hh_hige, frame.shape[0])),
int(max(p_ww_low, 0)):int(min(p_ww_hige, frame.shape[1]))]
res = eval_video_classification(net_classification_, cropframe,
res_old)
cv2.imshow('CropWebcam', cropframe)
cv2.rectangle(frame, p_box[0]['p1'], p_box[0]['p2'], (0, 0, 255))
p3 = (max(p_box[0]['p1'][0], 15), max(p_box[0]['p1'][1], 15))
title = "%s:%.2f" % (CLASSES[int(15)], conf[i])
cv2.putText(frame, title, p3, cv2.FONT_ITALIC, 0.6, (0, 255, 0), 1)
print res
res_old = res
#print np.max(res)
if np.max(res) < 0.9:
cv2.putText(frame, Category_labels_[11], (30, 60),
cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 0, 255), 2)
else:
Category_res = np.argmax(res)
Category_res = (Category_res > 11 and 11 or Category_res)
# print Category_res
cv2.putText(frame, Category_labels_[Category_res], (30, 60),
cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 0, 255), 2)
cv2.imshow('Webcam', frame)
end = time.time()
seconds = end - start
# Calculate frames per second
fps = 1 / seconds
print("fps: {0}".format(fps))
# Wait to press 'q' key for break
if cv2.waitKey(1) & 0xFF == ord('q'):
break
listener.release(frames)
# When everything done, release the capture
# cap.release()
device.stop()
device.close()
cv2.destroyAllWindows()
return
class Kinect:
fn = None
device = None
serial = None
listener = None
connected = False
# Frame-Filter Variables
SCALE = 2
OUTPUT_RESOLUTION = 227
BOX_FILTER_SIZE = 3
def __init__(self):
# Import the pipeline which will be used with the kinect
try:
from pylibfreenect2 import OpenCLPacketPipeline
self.pipeline = OpenCLPacketPipeline()
except:
try:
from pylibfreenect2 import OpenGLPacketPipeline
self.pipeline = OpenGLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
self.pipeline = CpuPacketPipeline()
print("Packet pipeline:", type(self.pipeline).__name__)
self.fn = Freenect2()
num_devices = self.fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
# Get the serial number of the kinect
self.serial = self.fn.getDeviceSerialNumber(0)
self.image_counter = 1
# Connects to the kinect and starts the stream
def connect(self):
# Connect to kinect
self.device = self.fn.openDevice(self.serial, pipeline=self.pipeline)
# Define RGB listener
self.listener = SyncMultiFrameListener(FrameType.Color)
# Register listeners
self.device.setColorFrameListener(self.listener)
# Get a frame from the listener
self.device.startStreams(rgb=True, depth=False)
self.connected = True
# Takes an image out of the stream and saves it
def get_picture(self, name=None):
# Get a frame from the listener
frames = self.listener.waitForNewFrame()
# Extract the RBG frame
color = frames["color"]
_, filtered_color_frame = self.__filter_frame(color)
# Save the frame as picture
if name is None:
cv2.imwrite(
"classification/pictures/new/" + str(self.image_counter) +
".jpeg", filtered_color_frame)
self.image_counter += 1
else:
cv2.imwrite("classification/pictures/" + str(name) + ".jpeg",
filtered_color_frame)
# Release the frame from the listener
self.listener.release(frames)
def start_video(self):
while True:
# Get a frame from the listener
frames = self.listener.waitForNewFrame()
# Extract the RBG frame
color = frames["color"]
cut_frame, _ = self.__filter_frame(color, to_stream=True)
# Show the cut stream of pictures
cv2.imshow("cut", cut_frame)
# Release the frame from the listener
self.listener.release(frames)
# Press q to exit loop and close all cv2-windows
key = cv2.waitKey(delay=1)
if key == ord('q'):
cv2.destroyAllWindows()
break
# Filter the frame of the kinect to the desired scale of frame-filter variables
def __filter_frame(self, frame, to_stream=False):
# original frame resolution: (1080, 1920, 4)
assert type(frame) is Frame
# the desired pixel grid to get the desired resolution in the end
cut_size = self.OUTPUT_RESOLUTION * self.SCALE + self.BOX_FILTER_SIZE - 1
# the amount of rows/columns to cut away
cut_rows = int((1080 - cut_size) / 2)
cut_columns = int((1920 - cut_size) / 2)
# the width of the filter from the middle to the edge
filter_edge = (self.BOX_FILTER_SIZE - 1) / 2
frame = frame.asarray()
filtered_frame = np.zeros(
[self.OUTPUT_RESOLUTION, self.OUTPUT_RESOLUTION, 4], dtype=int)
cut_frame = frame[cut_rows:cut_rows + cut_size,
cut_columns:cut_columns + cut_size]
if self.BOX_FILTER_SIZE != 1 and not to_stream:
# go through complete output array
for i in range(self.OUTPUT_RESOLUTION):
for j in range(self.OUTPUT_RESOLUTION):
# use for every output pixel the whole filter
for m in range(-filter_edge, filter_edge + 1):
for n in range(-filter_edge, filter_edge + 1):
filtered_frame[i, j] += cut_frame[i * self.SCALE +
filter_edge + m,
j * self.SCALE +
filter_edge + n]
filtered_frame[i, j] = filtered_frame[i, j] / (
self.BOX_FILTER_SIZE * self.BOX_FILTER_SIZE)
return cut_frame, filtered_frame
# Terminates the connection to the kinect
def disconnect(self):
self.device.stop()
self.device.close()
self.connected = False
def test_sync_multi_frame():
fn = Freenect2()
num_devices = fn.enumerateDevices()
assert num_devices > 0
serial = fn.getDefaultDeviceSerialNumber()
assert serial == fn.getDeviceSerialNumber(0)
# TODO: tests for openDefaultDevice
# device = fn.openDefaultDevice()
device = fn.openDevice(serial)
assert fn.getDefaultDeviceSerialNumber() == device.getSerialNumber()
device.getFirmwareVersion()
listener = SyncMultiFrameListener(
FrameType.Color | FrameType.Ir | FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# Registration
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# test if we can get two frames at least
frames = listener.waitForNewFrame()
listener.release(frames)
frames = listener.waitForNewFrame()
color = frames[FrameType.Color]
ir = frames[FrameType.Ir]
depth = frames[FrameType.Depth]
for frame in [ir, depth]:
assert frame.exposure == 0
assert frame.gain == 0
assert frame.gamma == 0
for frame in [color]:
assert frame.exposure > 0
assert frame.gain > 0
assert frame.gamma > 0
registration.apply(color, depth, undistorted, registered)
### Color ###
assert color.width == 1920
assert color.height == 1080
assert color.bytes_per_pixel == 4
assert ir.width == 512
assert ir.height == 424
assert ir.bytes_per_pixel == 4
assert depth.width == 512
assert depth.height == 424
assert depth.bytes_per_pixel == 4
assert color.asarray().shape == (color.height, color.width, 4)
assert ir.asarray().shape == (ir.height, ir.width)
assert depth.astype(np.float32).shape == (depth.height, depth.width)
listener.release(frames)
def __test_cannot_determine_type_of_frame(frame):
frame.asarray()
for frame in [registered, undistorted]:
yield raises(ValueError)(__test_cannot_determine_type_of_frame), frame
device.stop()
device.close()
class KinectV2:
"""
control class for the KinectV2 based on the Python wrappers of the official Microsoft SDK
Init the kinect and provides a method that returns the scanned depth image as numpy array.
Also we do gaussian blurring to get smoother surfaces.
"""
def __init__(self):
# hard coded class attributes for KinectV2's native resolution
self.name = 'kinect_v2'
self.depth_width = 512
self.depth_height = 424
self.color_width = 1920
self.color_height = 1080
self.depth = None
self.color = None
self._init_device()
#self.depth = self.get_frame()
#self.color = self.get_color()
print("KinectV2 initialized.")
def _init_device(self):
if _platform == 'Windows':
device = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Color |
PyKinectV2.FrameSourceTypes_Depth |
PyKinectV2.FrameSourceTypes_Infrared)
elif _platform == 'Linux':
if _pylib:
# self.device = Device()
fn = Freenect2()
num_devices = fn.enumerateDevices()
assert num_devices > 0
serial = fn.getDeviceSerialNumber(0)
self.device = fn.openDevice(serial, pipeline=pipeline)
self.listener = SyncMultiFrameListener(
FrameType.Color |
FrameType.Ir |
FrameType.Depth)
# Register listeners
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
self.device.start()
self.registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams())
self.undistorted = Frame(512, 424, 4)
self.registered = Frame(512, 424, 4)
frames = self.listener.waitForNewFrame()
self.listener.release(frames)
self.device.stop()
self.device.close()
elif _lib:
try:
self.device = Device()
except:
traceback.print_exc()
else:
print(_platform)
raise NotImplementedError
def _get_linux_frame(self, typ:str='all'):
"""
Manage to
Args:
typ:
Returns:
"""
#self.device.start()
frames = self.listener.waitForNewFrame(milliseconds=1000)
if frames:
if typ == 'depth':
depth = frames[FrameType.Depth]
self.listener.release(frames)
return depth.asarray()
elif typ == 'ir':
ir = frames[FrameType.Ir]
self.listener.release(frames)
return ir.asarray()
if typ == 'color':
color = frames[FrameType.Color]
self.listener.release(frames)
return color.asarray()
if typ == 'all':
color = frames[FrameType.Color]
ir = frames[FrameType.Ir]
depth = frames[FrameType.Depth]
self.registration.apply(color, depth, self.undistorted, self.registered)
self.registration.undistortDepth(depth, self.undistorted)
self.listener.release(frames)
return depth.asarray(), color.asarray(), ir.asarray()
else:
raise FileNotFoundError
def get_linux_frame(self, typ:str='all'):
"""
Manage to
Args:
typ:
Returns:
"""
frames = {}
with self.device.running():
for type_, frame in self.device:
frames[type_] = frame
if FrameType.Color in frames and FrameType.Depth in frames and FrameType.Ir in frames:
break
if typ == 'depth':
depth = frames[FrameType.Depth].to_array()
return depth
elif typ == 'ir':
ir = frames[FrameType.Ir].to_array()
return ir
elif typ == 'color':
color = frames[FrameType.Color].to_array()
resolution_camera = self.color_height * self.color_width # resolution camera Kinect V2
palette = numpy.reshape(color, (resolution_camera, 4))[:, [2, 1, 0]]
position_palette = numpy.reshape(numpy.arange(0, len(palette), 1), (self.color_height, self.color_width))
color = palette[position_palette]
return color
else:
color = frames[FrameType.Color].to_array()
resolution_camera = self.color_height * self.color_width # resolution camera Kinect V2
palette = numpy.reshape(color, (resolution_camera, 4))[:, [2, 1, 0]]
position_palette = numpy.reshape(numpy.arange(0, len(palette), 1), (self.color_height, self.color_width))
color = palette[position_palette]
depth = frames[FrameType.Depth].to_array()
ir = frames[FrameType.Ir].to_array()
return color, depth, ir
def get_frame(self):
"""
Args:
Returns:
2D Array of the shape(424, 512) containing the depth information of the latest frame in mm
"""
if _platform =='Windows':
depth_flattened = self.device.get_last_depth_frame()
self.depth = depth_flattened.reshape(
(self.depth_height, self.depth_width)) # reshape the array to 2D with native resolution of the kinectV2
elif _platform =='Linux':
self.depth = self.get_linux_frame(typ='depth')
return self.depth
def get_ir_frame_raw(self):
"""
Args:
Returns:
2D Array of the shape(424, 512) containing the raw infrared intensity in (uint16) of the last frame
"""
if _platform=='Windows':
ir_flattened = self.device.get_last_infrared_frame()
self.ir_frame_raw = numpy.flipud(
ir_flattened.reshape((self.depth_height,
self.depth_width))) # reshape the array to 2D with native resolution of the kinectV2
elif _platform=='Linux':
self.ir_frame_raw = self.get_linux_frame(typ='ir')
return self.ir_frame_raw
def get_ir_frame(self, min=0, max=6000):
"""
Args:
min: minimum intensity value mapped to uint8 (will become 0) default: 0
max: maximum intensity value mapped to uint8 (will become 255) default: 6000
Returns:
2D Array of the shape(424, 512) containing the infrared intensity between min and max mapped to uint8 of the last frame
"""
ir_frame_raw = self.get_ir_frame_raw()
self.ir_frame = numpy.interp(ir_frame_raw, (min, max), (0, 255)).astype('uint8')
return self.ir_frame
def get_color(self):
"""
Returns:
"""
if _platform == 'Windows':
color_flattened = self.device.get_last_color_frame()
resolution_camera = self.color_height * self.color_width # resolution camera Kinect V2
# Palette of colors in RGB / Cut of 4th column marked as intensity
palette = numpy.reshape(numpy.array([color_flattened]), (resolution_camera, 4))[:, [2, 1, 0]]
position_palette = numpy.reshape(numpy.arange(0, len(palette), 1), (self.color_height, self.color_width))
self.color = numpy.flipud(palette[position_palette])
elif _platform =='Linux':
self.color = self.get_linux_frame(typ='color')
return self.color
class VideoCapture:
def __init__(self, video_source):
self.video_source = video_source
if video_source == 'kinectv2':
self.listener = None
self.device = None
self.fn = None
self.frames = None
self.startKinectV2Dev()
def startKinectV2Dev(self):
from pylibfreenect2 import Freenect2, SyncMultiFrameListener, FrameType
try:
from pylibfreenect2 import OpenGLPacketPipeline
pipeline = OpenGLPacketPipeline()
except:
try:
from pylibfreenect2 import OpenCLPacketPipeline
pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
pipeline = CpuPacketPipeline()
print("LIBFREENECT2::Packet pipeline:",
type(pipeline).__name__)
self.fn = Freenect2()
num_devices = self.fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = self.fn.getDeviceSerialNumber(0)
self.device = self.fn.openDevice(serial, pipeline=pipeline)
self.listener = SyncMultiFrameListener(
FrameType.Color) # listen,ing only to rgb frames
# Register listeners
self.device.setColorFrameListener(self.listener)
self.device.start()
def stopKinectV2Dev(self):
self.device.stop()
self.device.close()
def stopVideoFeed(self):
if self.video_source == 'kinectv2':
self.stopKinectV2Dev()
def getKinectV1RgbFrame(self):
import freenect
array, _ = freenect.sync_get_video()
rgb_frame = cv2.cvtColor(array, cv2.COLOR_RGB2BGR)
return rgb_frame
def getKinectV2RgbFrame(self):
frames = self.listener.waitForNewFrame()
return frames
def getRgbFrame(self):
rgb_frame = None
if self.video_source == 'kinectv1':
import freenect
rgb_frame = self.getKinectV1RgbFrame()
elif self.video_source == 'kinectv2':
from pylibfreenect2 import Freenect2, SyncMultiFrameListener, FrameType
rgb_frame = self.getKinectV2RgbFrame()
return rgb_frame
def getNextFrame(self):
print "video source ", self.video_source
if self.video_source == 'kinectv1':
rgb_frame = self.getKinectV1RgbFrame()
elif self.video_source == 'kinectv2':
if self.frames:
self.listener.release(self.frames)
self.frames = self.getKinectV2RgbFrame()
resised_rgb_frame = cv2.resize(self.frames["color"].asarray(),
(int(1920), int(1080)))
rgb_frame = resised_rgb_frame[:, ::-1, :]
else:
print "no video feed"
return
frame = cv2.GaussianBlur(rgb_frame, (7, 7), 0)
return frame
def launchVideoForColorCalib(self):
while (1):
frame = self.getNextFrame()
# Convert the image to hsv space and find range of colors
#hsvFrame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# self.detectReward(frame, hsvFrame)
# Show the original and processed feed
hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
cv2.imshow('Original feed', frame)
cv2.setMouseCallback('Original feed', self.onMouseClick, hsv_frame)
# cv2.imshow('Original feed', hsv_frame)
# if 'q' is pressed then exit the loop
if cv2.waitKey(33) == ord('q'):
break
# Destroy all windows exit the program
cv2.destroyAllWindows()
def find_and_track_kinect(self, name, tracker = "CSRT",
face_target_box = DEFAULT_FACE_TARGET_BOX,
track_scaling = DEFAULT_SCALING,
res = (RGB_W, RGB_H), video_out = True):
print("Starting Tracking")
target = name
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline = self.pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Depth)
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
bigdepth = Frame(1920, 1082, 4)
trackerObj = None
face_process_frame = True
bbox = None
track_bbox = None
head_h = 0
body_left_w = 0
body_right_w = 0
center_w = 0
globalState = ""
# Following line creates an avi video stream of daisy's tracking
# out = cv2.VideoWriter('daisy_eye.avi', cv2.VideoWriter_fourcc('M','J','P','G'), 10, (960, 540))
# out.write(c)
while True:
timer = cv2.getTickCount()
frames = listener.waitForNewFrame()
color = frames["color"]
depth = frames["depth"]
registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth)
bd = np.resize(bigdepth.asarray(np.float32), (1080, 1920))
c = cv2.cvtColor(color.asarray(), cv2.COLOR_RGB2BGR)
if self.connected:
newTarget = self.alexa_neuron.get('name');
if newTarget != target:
target = newTarget
listener.release(frames)
trackerObj = None
face_process_frame = True
bbox = None
track_bbox = None
continue
if target is not None and target not in self.known_faces:
target = None
if target is None:
if self.connected:
c = self.__scale_frame(c, scale_factor = 0.5)
image = cv2.imencode('.jpg', c)[1].tostring()
self.web_neuron.update([('image', image)])
listener.release(frames)
trackerObj = None
face_process_frame = True
bbox = None
track_bbox = None
self.__update_individual_position("WAITING", None, None, None, res)
continue
face_bbox = None
new_track_bbox = None
if face_process_frame:
small_c = self.__crop_frame(c, face_target_box)
face_locations = face_recognition.face_locations(small_c, number_of_times_to_upsample=0, model="cnn")
face_encodings = face_recognition.face_encodings(small_c, face_locations)
for face_encoding in face_encodings:
matches = face_recognition.compare_faces(
[self.known_faces[target]], face_encoding, 0.6)
if len(matches) > 0 and matches[0]:
(top, right, bottom, left) = face_locations[0]
left += face_target_box[0]
top += face_target_box[1]
right += face_target_box[0]
bottom += face_target_box[1]
face_bbox = (left, top, right, bottom)
mid_w = int((left + right) / 2)
mid_h = int((top + bottom) / 2)
new_track_bbox = self.__body_bbox(bd, mid_w, mid_h, res)
break
face_process_frame = not face_process_frame
overlap_pct = 0
track_area = self.__bbox_area(track_bbox)
if track_area > 0 and new_track_bbox:
overlap_area = self.__bbox_overlap(new_track_bbox, track_bbox)
overlap_pct = min(overlap_area / self.__bbox_area(new_track_bbox),
overlap_area / self.__bbox_area(track_bbox))
small_c = self.__scale_frame(c, track_scaling)
if new_track_bbox is not None and overlap_pct < CORRECTION_THRESHOLD:
bbox = (new_track_bbox[0],
new_track_bbox[1],
new_track_bbox[2] - new_track_bbox[0],
new_track_bbox[3] - new_track_bbox[1])
bbox = self.__scale_bbox(bbox, track_scaling)
trackerObj = self.__init_tracker(small_c, bbox, tracker)
self.alexa_neuron.update([('tracking', True)])
if trackerObj is None:
self.__update_individual_position("WAITING", None, None, None, res)
status = False
if trackerObj is not None:
status, trackerBBox = trackerObj.update(small_c)
bbox = (int(trackerBBox[0]),
int(trackerBBox[1]),
int(trackerBBox[0] + trackerBBox[2]),
int(trackerBBox[1] + trackerBBox[3]))
if bbox is not None:
track_bbox = (bbox[0], bbox[1], bbox[2], bbox[3])
track_bbox = self.__scale_bbox(bbox, 1/track_scaling)
w = 0
h = 0
if status:
w = track_bbox[0] + int((track_bbox[2] - track_bbox[0])/2)
h = track_bbox[1] + int((track_bbox[3] - track_bbox[1])/2)
if (w < res[0] and w >= 0 and h < res[1] and h >= 0):
distanceAtCenter = bd[h][w]
center = (w, h)
globalState = self.__update_individual_position(status, track_bbox, center, distanceAtCenter, res)
if globalState == "Fail":
break
fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer)
if video_out or self.connected:
cv2.line(c, (w, 0), (w, res[1]), (0,255,0), 1)
cv2.line(c, (0, h), (res[0], h), (0,255,0), 1)
cv2.line(c, (0, head_h), (res[0], head_h), (0,0,0), 1)
cv2.line(c, (body_left_w, 0), (body_left_w, res[1]), (0,0,255), 1)
cv2.line(c, (body_right_w, 0), (body_right_w, res[1]), (0,0,255), 1)
cv2.line(c, (center_w, 0), (center_w, res[1]), (0,0,255), 1)
self.__draw_bbox(True, c, face_target_box, (255, 0, 0), "FACE_TARGET")
self.__draw_bbox(status, c, track_bbox, (0, 255, 0), tracker)
self.__draw_bbox(face_bbox is not None, c, face_bbox, (0, 0, 255), target)
self.__draw_bbox(face_bbox is not None, c, new_track_bbox, (0, 255, 255), "BODY")
c = self.__scale_frame(c, scale_factor = 0.5)
cv2.putText(c, "FPS : " + str(int(fps)), (100,50),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,0,255), 1)
if not status:
failedTrackers = "FAILED: "
failedTrackers += tracker + " "
cv2.putText(c, failedTrackers, (100, 80),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,142), 1)
if self.connected:
image = cv2.imencode('.jpg', c)[1].tostring()
self.web_neuron.update([('image', image)])
if video_out:
cv2.imshow("color", c)
listener.release(frames)
key = cv2.waitKey(1) & 0xff
if key == ord('q'):
self.__update_individual_position("STOP", None, None, None, res)
break
self.so.close()
cv2.destroyAllWindows()
device.stop()
device.close()
def main():
instructions = 0
print "--- Intention classification for communication between a human and a robot ---"
if instructions == 1:
print "First you will be required to present a facial expression before you will do a head movement."
print "If done correctly these gestures will be detected by the robot and it will perform the desired task."
raw_input("Press Enter to continue...")
if instructions == 1:
print "This is the module for facial expression recognition."
print "This program can detect the emotions: Happy and Angry."
print "The program will look for the expression for 3 seconds."
raw_input("To proceed to Facial Expression Recognition press Enter...")
predictExp = 0
# Load Facial Expression Recognition trained model
print "- Loading FER model... -"
#faceExpModel = tf.keras.models.load_model("/home/bjornar/ML_models/FER/Good models(80+)/tf_keras_weights_ninthRev-88percent/tf_keras_weights_ninthRev.hdf5")
faceExpModel = tf.keras.models.load_model(
"/home/project/Bjorn/tf_keras_weights_ninthRev-88percent/tf_keras_weights_ninthRev.hdf5"
)
# Load Face Cascade for Face Detection
print "- Loading Face Cascade for Face Detection... -"
#cascPath = "/home/bjornar/MScDissertation/TrainingData/FaceDetection/haarcascade_frontalface_default.xml"
cascPath = "/home/project/Bjorn/IntentionClassification-Repository/haarcascade_frontalface_default.xml"
faceCascade = cv2.CascadeClassifier(cascPath)
## Initializing Head Movement variables
# Introduce mark_detector to detect landmarks.
mark_detector = MarkDetector()
sample_frame = cv2.imread("sample_frame.png")
# Setup process and queues for multiprocessing.
img_queue = Queue()
box_queue = Queue()
img_queue.put(sample_frame)
box_process = Process(target=get_face,
args=(
mark_detector,
img_queue,
box_queue,
))
box_process.start()
# Introduce pose estimator to solve pose. Get one frame to setup the
# estimator according to the image size.
height, width = sample_frame.shape[:2]
pose_estimator = PoseEstimator(img_size=(height, width))
# Introduce scalar stabilizers for pose.
pose_stabilizers = [
Stabilizer(state_num=2,
measure_num=1,
cov_process=0.1,
cov_measure=0.1) for _ in range(6)
]
noseMarks = [[0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0],
[0, 0], [0, 0], [0, 0]]
counter = 0
font = cv2.FONT_HERSHEY_SIMPLEX
numXPoints = 0
numYPoints = 0
sumX = 0
sumY = 0
currentTime = 0
previousTime1 = 0
previousTime2 = 0
directionArray = []
moveSequence = []
moves = []
classifyMoves = 0
headPoseDirection = 'emtpy'
if camera == 'kinect':
## Initialize Kinect camera
print "Initializing camera..."
try:
from pylibfreenect2 import OpenGLPacketPipeline
pipeline = OpenGLPacketPipeline()
except:
try:
from pylibfreenect2 import OpenCLPacketPipeline
pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
pipeline = CpuPacketPipeline()
#print("Packet pipeline:", type(pipeline).__name__)
# Create and set logger
#logger = createConsoleLogger(LoggerLevel.Debug)
setGlobalLogger()
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("- No device connected! -")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
elif camera == 'webcam':
#video_capture = cv2.VideoCapture(0)
video_capture = cv2.VideoCapture(-1)
elif camera == 'video':
#video_capture = cv2.VideoCapture(0)
video_capture = cv2.VideoCapture(
"/home/project/Bjorn/SonyHandycamTest.mp4")
## Facial Expression Recognition variables
FER_prediction = []
FERclass = ''
FERstart = 0
classifyMoves = 0
## Head movement variables
predictHeadMov = 3
HeadMov = []
HMCclass = ''
detectedFaces = []
mark = []
notDone = 0
progressStatus = [0, 0]
while notDone in progressStatus: # This waits for each module to finsih
if camera == 'kinect':
frames = listener.waitForNewFrame()
color = frames["color"]
color = color.asarray()
color = cv2.resize(color, (int(873), int(491)))
color = color[0:480, 150:790]
color = np.delete(color, np.s_[3::], 2)
elif camera == 'webcam':
# Capture frame-by-frame
ret, color = video_capture.read()
elif camera == 'video':
# Capture frame-by-frame
ret, color = video_capture.read()
## Detect facial expression
predictExpNums = [1, 2]
if predictExp == 0:
while predictExp not in predictExpNums:
predictExp = int(
raw_input(
"\nPress 1 to detect Facial Expression or press 2 to do Head Movement classification."
))
if predictExp == 1:
predictExp = 1
print "------ Facial Expression Recognition ------"
elif predictExp == 2:
predictHeadMov = 0
progressStatus[0] = 1
FERstart = time.time()
elif predictExp == 1:
currentTime = time.time()
if currentTime - FERstart < 10:
FER_prediction.append(
facialExpressionRecogntion(color, faceExpModel,
faceCascade))
else:
predictExp = 2
FER_prediction = filter(None, FER_prediction)
FERclass = mostCommon(FER_prediction)
FERclass = FERclass[2:7]
predictHeadMov = 0
if FERclass == '':
print("Did not detect an expression, try again.")
predictExp = 0
else:
progressStatus[0] = 1
print "Detected expression: " + str(FERclass)
print "Progress: FER DONE"
# else:
# #cv2.imwrite("sample_frame.png", color)
# break
## Detect head movement class
if predictHeadMov == 0:
predictHeadMov = int(
raw_input(
"\nPress 1 to do Head Movement classification or 2 to skip."
))
if predictHeadMov == 1:
predictHeadMov = 1
print "------ Head Movement Classification ------"
moveTime = int(
raw_input(
"How many seconds should I record your movement?"))
#moveTime = 2
else:
predictHeadMov = 2
timer = time.time()
previousTime1 = timer
previousTime2 = timer
if predictHeadMov == 1:
print color.shape
color = color[0:480, 0:480]
color = cv2.cvtColor(color, cv2.COLOR_BGR2GRAY)
cv2.imshow("", color)
raw_input()
print color.shape
# Feed frame to image queue.
img_queue.put(color)
#pdb.set_trace()
# Get face from box queue.
facebox = box_queue.get()
print color.shape
if facebox is not None:
# Detect landmarks from image of 128x128.
face_img = color[facebox[1]:facebox[3], facebox[0]:facebox[2]]
face_img = cv2.resize(face_img,
(CNN_INPUT_SIZE, CNN_INPUT_SIZE))
face_img = cv2.cvtColor(face_img, cv2.COLOR_BGR2RGB)
marks = mark_detector.detect_marks(face_img)
#Convert the marks locations from local CNN to global image.
marks *= (facebox[2] - facebox[0])
marks[:, 0] += facebox[0]
marks[:, 1] += facebox[1]
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(color, headPoseDirection, (20, 70), font, 1,
(0, 255, 0), 4)
# # Get average position of nose
noseMarksTemp = []
noseMarksTemp.append(marks[30][0])
noseMarksTemp.append(marks[30][1])
noseMarks[0] = noseMarksTemp
for i in range(9, 0, -1):
noseMarks[i] = noseMarks[i - 1]
# Get the direction of head movement
headPoseDirection = calculateDirection(noseMarks)
directionArray.append(headPoseDirection)
if classifyMoves == 0:
classifyMoves = 1
timer = time.time()
previousTime1 = timer
previousTime2 = timer
currentTime = time.time()
if currentTime - previousTime1 > moveTime and classifyMoves == 1:
print "------------------------------------------------"
print "Elapsed timer 1: " + str(currentTime -
previousTime1)
# Get most common direction
HMCclass = mostCommon(directionArray)
classifyMoves = 2
directionArray = []
previousTime1 = currentTime
progressStatus[1] = 1
print "Progress: HMC DONE"
else:
print "Did not detect a face"
elif predictHeadMov == 2:
progressStatus[1] = 1
print "Skipped Head Movement Estimation."
break
# if notDone in progressStatus and predictHeadMov == 2 and predictExp == 2:
# print "You seem to have skipped one or more tasks."
# inpt = ''
# while inpt == '':
# inpt = raw_input("To do FER press 1 and to do HMC press 2...")
# if input == '1':
# predictExp = 1
# elif input == '2':
# predictHeadMov
cv2.imshow("", color)
if camera == 'kinect':
listener.release(frames)
key = cv2.waitKey(delay=1)
if key == ord('q'):
break
if camera == 'kinect':
listener.release(frames)
device.stop()
device.close()
elif camera == 'webcam' or camera == 'video':
video_capture.release()
cv2.destroyAllWindows()
# Clean up the multiprocessing process.
box_process.terminate()
box_process.join()
print "---------------- RESULT ----------------"
if FERclass != '':
print "Detected facial expression: " + str(FERclass)
else:
print "Did not detect any expression."
if HMCclass != '':
print "Detected head movement: " + str(HMCclass)
else:
print "Did not detect a head movement."
print "----------------------------------------"
intentionClassification = [FERclass, HMCclass]
return intentionClassification
def 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)
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 run(self):
self._isrunning = True
try:
from pylibfreenect2 import OpenGLPacketPipeline
pipeline = OpenGLPacketPipeline()
except:
try:
from pylibfreenect2 import OpenCLPacketPipeline
pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
pipeline = CpuPacketPipeline()
if self._debug:
print("Packet pipeline:", type(pipeline).__name__)
self.fn = Freenect2()
num_devices = self.fn.enumerateDevices()
if self._debug:
print("Number of devices: {}".format(num_devices))
serial = self.fn.getDeviceSerialNumber(0)
device = self.fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
if self._debug:
print("Init done")
device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
params = device.getIrCameraParams()
self.cx = params.cx
self.cy = params.cy
self.fx = params.fx
self.fy = params.fy
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
while self._isrunning:
frames = listener.waitForNewFrame()
color = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
registration.apply(color,
depth,
undistorted,
registered,
bigdepth=None,
color_depth_map=None)
if self._save_color:
self._set_color_image(
cv2.cvtColor(color.asarray(), cv2.COLOR_BGR2RGB))
if self._save_depth or self._save_pointcloud:
depth_arr = depth.asarray()
if self._save_depth:
self._set_depth_image(depth_arr)
if self._save_ir:
self._set_ir_image(ir.asarray())
if self._save_registered or self._save_pointcloud:
reg = cv2.cvtColor(registered.asarray(np.uint8),
cv2.COLOR_BGR2RGB)
if self._save_registered:
self._set_registered_image(reg)
if self._save_undistorted:
und = undistorted.asarray(np.uint8)
self._set_undistorted_image(
cv2.cvtColor(und, cv2.COLOR_BGR2RGB))
if self._save_pointcloud:
self.compute_pointcloud(reg, depth_arr)
listener.release(frames)
if self._debug:
print("Stopping device")
device.stop()
if self._debug:
print("Closing device")
device.close()
if self._debug:
print("Device stopped and closed")
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):
''' 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
# creates the freenect2 device
self.fn = Freenect2()
# if no kinects are plugged in the system, it quits
num_devices = self.fn.enumerateDevices()
if 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)
self.color_hsv = 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]
# the kinect sensor mirrors the images, so we have to flip them back
self.color_new = cv2.flip(self.color_new, 1)
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.)
self.depth_new = cv2.flip(self.depth_new, 1)
# ir stream, not needed for detection purposes right now
#self.ir = frames["ir"]
#self.ir_new = cv2.resize(self.ir.asarray() / 65535.)
#self.ir_new = cv2.flip(self.ir_new, 1)
''' # This portion is needed if you want to print also the
# registered (RGB+D) depth and the undistorted depth.
if self.enable_rgb and self.enable_depth:
self.registration.apply(self.color, self.depth, self.undistorted, self.registered)
self.registered_new = self.registered.asarray(np.uint8)
self.registered_new = cv2.flip(self.registered_new, 1)
elif self.enable_depth:
self.registration.undistortDepth(self.depth, self.undistorted)
self.undistorted_new = cv2.resize(self.undistorted.asarray(np.float32) / 4500.)
self.undistorted_new = cv2.flip(self.undistorted_new, 1) '''
# do this anyway to release every acquired frame
self.listener.release(frames)
def rgb2hsv(self):
'''Function that converts the RGB input frame in HSV '''
r = self.color_new[:, :, 0] / 255.0
g = self.color_new[:, :, 1] / 255.0
b = self.color_new[:, :, 2] / 255.0
mx = max(r, g, b)
mn = min(r, g, b)
df = mx - mn
if mx == mn:
h = 0
elif mx == r:
h = (60 * ((g - b) / df) + 360) % 360
elif mx == g:
h = (60 * ((b - r) / df) + 120) % 360
elif mx == b:
h = (60 * ((r - g) / df) + 240) % 360
if mx == 0:
s = 0
else:
s = df / mx
v = mx
self.color_hsv = np.dstack((h, s, v))
def main():
try:
from pylibfreenect2 import OpenCLPacketPipeline
pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
pipeline = CpuPacketPipeline()
# Create and set logger
logger = createConsoleLogger(LoggerLevel.Debug)
setGlobalLogger(logger)
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# Optinal parameters for registration
# set True if you need
need_bigdepth = False
need_color_depth_map = False
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
point = pcl.PointCloud()
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowColorCloud(cloud)
while True:
frames = listener.waitForNewFrame()
color = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
registration.apply(color,
depth,
undistorted,
registered,
bigdepth=bigdepth,
color_depth_map=color_depth_map)
# NOTE for visualization:
# cv2.imshow without OpenGL backend seems to be quite slow to draw all
# things below. Try commenting out some imshow if you don't have a fast
# visualization backend.
# cv2.imshow("ir", ir.asarray() / 65535.)
# cv2.imshow("depth", depth.asarray() / 4500.)
# cv2.imshow("color", cv2.resize(color.asarray(), (int(1920 / 3), int(1080 / 3))))
# cv2.imshow("registered", registered.asarray(np.uint8))
# if need_bigdepth:
# cv2.imshow("bigdepth", cv2.resize(bigdepth.asarray(np.float32),
# (int(1920 / 3), int(1082 / 3))))
# if need_color_depth_map:
# cv2.imshow("color_depth_map", color_depth_map.reshape(424, 512))
undistorted_arrray = undistorted.asarray(dtype=np.float32, ndim=2)
# registered_array = registered.asarray(dtype=np.uint8)
point = pcl.PointCloud(undistorted_arrray)
# visual.ShowColorCloud(cloud)
listener.release(frames)
# key = cv2.waitKey(delay=1)
# if key == ord('q'):
# break
v = True
while v:
v = not (visual.WasStopped())
device.stop()
device.close()
sys.exit(0)
class DeviceFrame(TabFrame):
persp = np.asarray([[0.75, 0, 120 + 15], [0, 0.99, 0], [0, 0, 1]],
dtype=np.float32)
def __init__(self, master, freenect, serial, publish):
TabFrame.__init__(self, master)
self.pack(fill=tk.BOTH, expand=True)
self._freenect = freenect
self._serial = serial
self._device_index = self.__device_list_index()
self._device = None
self._listener = None
self._opened = False
self._playing = False
self.frames = FrameMap()
self.image_buffer = {
'color': (None, None, None),
'ir': (None, None, None),
'depth': (None, None, None)
}
cam = TabFrame(self, tk.TOP)
self.add(' Camera ', cam)
color = VideoUI(cam, self, lambda: self.get_image_color())
cam.add(' RGB ', color)
color.canvas.bind("<Button-1>", lambda _: self.capture())
ir = VideoUI(cam, self, lambda: self.get_image_ir())
cam.add(' IR ', ir)
ir.canvas.bind('<Motion>', self.motion)
ir.canvas.bind("<Button-1>", lambda _: self.capture())
depth = VideoUI(cam, self, lambda: self.get_image_depth())
cam.add(' Depth ', depth)
depth.canvas.bind('<Motion>', self.motion)
depth.canvas.bind("<Button-1>", lambda _: self.capture())
filt = TabFrame(self, tk.TOP)
self.add(' Filter ', filt)
sob = VideoUI(filt, self, lambda: self.get_image_ir(filters=(sobel, )))
filt.add(' Sobel ', sob)
mas = VideoUI(filt, self,
lambda: self.get_image_ir(filters=(masking, )))
filt.add(' Masking ', mas)
can = VideoUI(filt, self, lambda: self.get_image_ir(filters=(canny, )))
filt.add(' Canny ', can)
hou = VideoUI(filt, self, lambda: self.get_image_ir(filters=(hough, )))
filt.add(' Hough ', hou)
if 'matplotlib' in sys.modules:
plot = TabFrame(self, tk.TOP)
self.add(' 3D ', plot)
plot_pc = PlotFrame(plot, self,
(lambda: self.get_image_depth(), None),
(920, 580, (0, 5000)))
plot.add(' Point Cloud ', plot_pc)
plot_dm = PlotFrame(
plot, self,
(lambda: self.get_image_depth(), lambda: self.get_image_ir()),
(920, 580, (0, 5000)))
plot.add(' Depthmap ', plot_dm)
plot_cm = PlotFrame(plot, self, (lambda: self.get_image_depth(),
lambda: self.get_image_color()),
(920, 580, (0, 5000)))
plot.add(' Colormap ', plot_cm)
#if 'moderngl' in sys.modules:
# gl = TabFrame(plot, tk.TOP)
# plot.add(' OpenGL ', gl)
track = TabFrame(self, tk.TOP)
self.add(' Tracker ', track)
boos = TrackerUI(track, self, lambda: self.get_image_color(), publish,
1)
track.add(' Boosting ', boos)
colo = TrackerUI(track, self, lambda: self.get_image_color(), publish,
2)
track.add(' Color ', colo)
#if 'flask' in sys.modules:
# stream = TabFrame(self, tk.TOP)
# self.add(' Stream ', stream)
self._publish = publish
self.refresh()
def __device_list_index(self):
num_devs = self._freenect.enumerateDevices()
devs = [
self._freenect.getDeviceSerialNumber(i) for i in range(num_devs)
]
return devs.index(self._serial)
def motion(self, event):
msg = " x={}px y={}px ".format(event.x, event.y)
_, _, buffer = self.get_image_depth()
if buffer is not None:
w, h = buffer.shape[::-1]
if buffer[event.y % h][event.x % w] > 0:
msg = msg + " d={}mm".format(buffer[event.y % h][event.x % w])
if self._publish is not None: self._publish(msg)
return None
def open(self):
self._listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
self._device = self._freenect.openDevice(self._serial,
pipeline=pipeline)
device_index = self.__device_list_index()
if self._device_index != device_index: # keep track of changes in the device list
self._device_index = device_index
self._device.close()
self._listener.release(self.frames)
self.open()
return
self._device.setColorFrameListener(self._listener)
self._device.setIrAndDepthFrameListener(self._listener)
self._device.start()
self._opened = True
self._playing = False
def opened(self):
return self._opened
def play(self):
if not self._opened: return False
self._playing = True
return True
def playing(self):
return self._playing
def stop(self):
if not self._opened: return False
self._playing = False
return True
def close(self):
if not self._opened: return
self._device.close()
self._opened = False
self._playing = False
def refresh(self):
if self._playing:
self._listener.release(self.frames)
self.image_buffer = {
key: (None, None, None)
for key in self.image_buffer
} # reset image buffer
self.frames = self._listener.waitForNewFrame()
self.after(30, self.refresh)
def get_image_color(self, filters=()):
color, _, _ = self.image_buffer['color']
if color is not None: return self.image_buffer['color']
color = self.frames['color']
color = color.asarray(dtype=np.uint8)
color = cv.resize(color, (1920 // 2, 1080 // 2))
color = cv.cvtColor(color, cv.COLOR_BGRA2RGBA)
#color = np.rot90(color)
#color = np.rot90(color, 3)
for f in filters:
color, *_ = f(color)
return self.to_image('color', color)
def get_image_ir(self, filters=()):
ir, _, _ = self.image_buffer['ir']
if ir is not None: return self.image_buffer['ir']
ir = self.frames['ir']
ir = ir.asarray(dtype=np.float32)[
32:-32] # remove 32 lines top and bottom (in rgb out of view)
dsize = (1920 // 2, 1080 // 2)
ir = cv.resize(ir, dsize)
ir = ir / 65535
ir = np.asarray(ir * 255, dtype=np.uint8)
ir = cv.warpPerspective(ir,
DeviceFrame.persp,
None,
borderMode=cv.BORDER_CONSTANT,
borderValue=255)
for f in filters:
ir, *_ = f(ir)
return self.to_image('ir', ir)
def get_image_depth(self, d_min=0, d_max=5000, filters=()):
depth, _, _ = self.image_buffer['depth']
if depth is not None: return self.image_buffer['depth']
depth = self.frames['depth']
depth = depth.asarray(dtype=np.float32)[
32:-32] # remove 32 lines top and bottom (in rgb out of view)
depth = cv.resize(depth, (1920 // 2, 1080 // 2))
depth = cv.warpPerspective(depth,
DeviceFrame.persp,
None,
borderMode=cv.BORDER_CONSTANT,
borderValue=d_max)
buffer = depth.astype(int)
buffer[buffer == d_max], buffer[buffer == d_min] = -1, -1
depth = (depth - d_min) / (d_max - d_min)
depth = np.asarray(depth * 255, dtype=np.uint8)
for f in filters:
depth, *_ = f(depth)
return self.to_image('depth', depth, buffer)
def to_image(self, key, arr, arg=None):
self.image_buffer[key] = (PIL.Image.fromarray(arr), arr, arg)
return self.image_buffer[key]
def capture(self):
timezone = (int(-time.timezone / 3600) + time.daylight) % 25
tz_abbr = 'ZABCDEFGHIKLMNOPQRSTUVWXY'
timestamp = time.strftime('%Y%m%d' + tz_abbr[timezone] + '%H%M%S')
self.save(self.get_image_color()[0], timestamp, 'color')
self.save(self.get_image_ir()[0], timestamp, 'ir')
self.save(self.get_image_depth()[0], timestamp, 'depth')
@classmethod
def save(cls, img, timestamp, name, ext='tiff'):
scriptdir = os.path.dirname(__file__)
timedir = os.path.join(scriptdir, 'capture', str(timestamp))
if not os.path.exists(timedir): os.makedirs(timedir)
file = os.path.join(timedir, name + '.' + ext)
img.save(file, ext)
class OpenCVCapture(object):
def __init__(self, device_id=0):
"""Create an OpenCV capture object associated with the provided webcam
device ID.
"""
logger = createConsoleLogger(LoggerLevel.Error)
setGlobalLogger(logger)
self.fn = Freenect2()
num_devices = self.fn.enumerateDevices()
if num_devices == 0:
print("No Kinect!")
raise LookupError()
serial = self.fn.getDeviceSerialNumber(0)
self.device = self.fn.openDevice(serial, pipeline=pipeline)
types = 0
types |= FrameType.Color
self.listener = SyncMultiFrameListener(types)
self.device.setColorFrameListener(self.listener)
# Start a thread to continuously capture frames.
# This must be done because different layers of buffering in the webcam
# and OS drivers will cause you to retrieve old frames if they aren't
# continuously read.
self._capture_frame = None
# Use a lock to prevent access concurrent access to the camera.
self._capture_lock = threading.Lock()
self._capture_thread = threading.Thread(target=self._grab_frames)
self._capture_thread.daemon = True
self._capture_thread.start()
def _grab_frames(self):
self.device.startStreams(rgb=True, depth=False)
while True:
frames = self.listener.waitForNewFrame()
color = frames["color"]
with self._capture_lock:
self._capture_frame = None
self._capture_frame = cv2.resize(
color.asarray(), (int(1920 / 3), int(1080 / 3))).copy()
self.listener.release(frames)
time.sleep(1.0 / CAPTURE_HZ)
def read(self):
"""Read a single frame from the camera and return the data as an OpenCV
image (which is a numpy array).
"""
frame = None
with self._capture_lock:
frame = self._capture_frame
# If there are problems, keep retrying until an image can be read.
while frame is None:
time.sleep(0)
with self._capture_lock:
frame = self._capture_frame
# Return the capture image data.
return frame
def stop(self):
print('{"status":"Terminating..."}')
self.device.stop()
self.device.close()
def test_sync_multi_frame():
fn = Freenect2()
num_devices = fn.enumerateDevices()
assert num_devices > 0
serial = fn.getDefaultDeviceSerialNumber()
assert serial == fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial)
assert fn.getDefaultDeviceSerialNumber() == device.getSerialNumber()
device.getFirmwareVersion()
listener = SyncMultiFrameListener(
FrameType.Color | FrameType.Ir | FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# Registration
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# optional parameters for registration
bigdepth = Frame(1920, 1082, 4)
color_depth_map = np.zeros((424, 512), np.int32)
# test if we can get two frames at least
frames = listener.waitForNewFrame()
listener.release(frames)
# frames as a first argment also should work
frames = FrameMap()
listener.waitForNewFrame(frames)
color = frames[FrameType.Color]
ir = frames[FrameType.Ir]
depth = frames[FrameType.Depth]
for frame in [ir, depth]:
assert frame.exposure == 0
assert frame.gain == 0
assert frame.gamma == 0
for frame in [color]:
assert frame.exposure > 0
assert frame.gain > 0
assert frame.gamma > 0
registration.apply(color, depth, undistorted, registered)
# with optinal parameters
registration.apply(color, depth, undistorted, registered,
bigdepth=bigdepth,
color_depth_map=color_depth_map.ravel())
registration.undistortDepth(depth, undistorted)
assert color.width == 1920
assert color.height == 1080
assert color.bytes_per_pixel == 4
assert ir.width == 512
assert ir.height == 424
assert ir.bytes_per_pixel == 4
assert depth.width == 512
assert depth.height == 424
assert depth.bytes_per_pixel == 4
assert color.asarray().shape == (color.height, color.width, 4)
assert ir.asarray().shape == (ir.height, ir.width)
assert depth.asarray(np.float32).shape == (depth.height, depth.width)
listener.release(frames)
def __test_cannot_determine_type_of_frame(frame):
frame.asarray()
for frame in [registered, undistorted]:
yield raises(ValueError)(__test_cannot_determine_type_of_frame), frame
# getPointXYZ
x, y, z = registration.getPointXYZ(undistorted, 512 // 2, 424 // 2)
if not np.isnan([x, y, z]).any():
assert z > 0
# getPointXYZRGB
x, y, z, b, g, r = registration.getPointXYZRGB(undistorted, registered,
512 // 2, 424 // 2)
if not np.isnan([x, y, z]).any():
assert z > 0
assert np.isfinite([b, g, r]).all()
for pix in [b, g, r]:
assert pix >= 0 and pix <= 255
device.stop()
device.close()
class kinectDevice:
def __init__(self, device_no=0):
# Package Pipeline
try:
from pylibfreenect2 import OpenGLPacketPipeline
self.pipeline = OpenGLPacketPipeline()
except:
try:
from pylibfreenect2 import OpenCLPacketPipeline
self.pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
self.pipeline = CpuPacketPipeline()
print "Packet pipeline: {}".format(type(self.pipeline).__name__)
# Create and set logger
logger = createConsoleLogger(LoggerLevel.Error)
setGlobalLogger(logger)
# Detect Kinect Devices
self.fn = Freenect2()
num_devices = self.fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
self.device = None
sys.exit(1)
# Create Device and Frame Listeners
self.serial = self.fn.getDeviceSerialNumber(device_no)
self.device = self.fn.openDevice(self.serial, pipeline=self.pipeline)
self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
# Register Listeners
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
# Start Device
self.device.start()
# Set Camera Parameters
self._ir_params = self.device.getIrCameraParams()
self._color_params = self.device.getColorCameraParams()
'''
self._ir_params.fx = cameraParams.params['ir_fx']
self._ir_params.fy = cameraParams.params['ir_fy']
self._ir_params.cx = cameraParams.params['ir_cx']
self._ir_params.cy = cameraParams.params['ir_cy']
self._color_params.fx = cameraParams.params['col_fx']
self._color_params.fy = cameraParams.params['col_fy']
self._color_params.cx = cameraParams.params['col_cx']
self._color_params.cy = cameraParams.params['col_cy']
'''
# Registration
self.registration = Registration(self._ir_params, self._color_params)
print '[Info] Initialization Finished!'
# Get New Frame (in "Frame" type)
def getNewFrame(self):
# Get Raw Data from Listener
self.frames = self.listener.waitForNewFrame()
# Get Depth Frame and Color Frame
depth = self.frames['depth']
color = self.frames['color']
ir = self.frames['ir']
return depth, color, ir
# Release Frame
def releaseFrame(self):
self.listener.release(self.frames)
def __del__(self):
# Stop and Close Device
if self.device != None:
self.device.stop()
self.device.close()
#
# Get Parameters of KinectDevice
#
# Infrared Camera
@property
def ir_params(self):
return self._ir_params
# Color Camera
@property
def color_params(self):
return self._color_params
class KivyCamera(Image):
def __init__(self, **kwargs):
super(KivyCamera, self).__init__(**kwargs)
def setup(self, fn, pipeline, **kwargs):
super(KivyCamera, self).__init__(**kwargs)
self.bg_image = None
self.current_img = None
self.current_final_img = None
self.current_mask = None
serial = fn.getDeviceSerialNumber(0)
self.device = fn.openDevice(serial, pipeline=pipeline)
self.listener = SyncMultiFrameListener(
FrameType.Color | FrameType.Ir | FrameType.Depth)
# Register listeners
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
self.device.start()
# NOTE: must be called after device.start()
self.registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
self.undistorted = Frame(512, 424, 4)
self.registered = Frame(512, 424, 4)
# Optinal parameters for registration
# set True if you need
need_bigdepth = True
need_color_depth_map = False
self.bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
self.color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
self.clipping_distance = 0.5
def update(self, dt):
frames = self.listener.waitForNewFrame()
color = frames["color"]
depth = frames["depth"]
self.registration.apply(color, depth, self.undistorted, self.registered,
bigdepth=self.bigdepth,
color_depth_map=self.color_depth_map)
self.current_img = color.asarray()[:,:,:3]
depth_img = self.bigdepth.asarray(np.float32) / 4500. # scale to interval [0,1]
if(self.bg_image is not None):
if(self.bg_image.shape[2] == 4): # put frame around picture
fgMask = self.bg_image[:,:,2]>0
else: # modify background
# if needed: denoise filter
depth_img = cv2.medianBlur(depth_img, 5)
depth_img = cv2.GaussianBlur(depth_img, (9,9), 0)
fgMask = ((depth_img > self.clipping_distance) & (depth_img > 0))
fgMask = cv2.resize(fgMask.astype(np.uint8), (1920, 1080))
# if needed: morphological operations
#kernel = np.ones((10,10), np.uint8)
#fgMask = cv2.morphologyEx(fgMask, cv2.MORPH_CLOSE, kernel)
#fgMask = cv2.morphologyEx(fgMask, cv2.MORPH_OPEN, kernel)
self.current_mask = np.dstack((fgMask, fgMask, fgMask)).astype(np.bool) # mask is 1 channel, color is 3 channels
self.current_final_img = np.where(self.current_mask, self.bg_image[:,:,:3], self.current_img)
else:
self.current_mask = None
self.current_final_img = self.current_img
self.listener.release(frames)
self.display_img(self.current_final_img)
def display_img(self, img):
buf1 = cv2.flip(img, 0)
buf = buf1.tostring()
image_texture = Texture.create(size=(img.shape[1], img.shape[0]), colorfmt='rgb') #(480,640)
image_texture.blit_buffer(buf, colorfmt='bgr', bufferfmt='ubyte')
# display image from the texture
self.texture = image_texture
def main(self):
"""
main part of the program
"""
# self.start_kinect()
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)
# select the depth data only
listener = SyncMultiFrameListener(FrameType.Depth)
device.setIrAndDepthFrameListener(listener)
device.start()
# get camera focal
IRP = device.getIrCameraParams()
Focal = IRP.fx
netR, optimizer = self.network_config()
# switch to evaluate mode
torch.cuda.synchronize()
netR.eval()
print("net start")
# record videos
fourcc = cv2.VideoWriter_fourcc('m', 'j', 'p', 'g')
depth_video = cv2.VideoWriter('Depth.avi', fourcc, 15, (512, 424))
seg_video = cv2.VideoWriter('Seg.avi', fourcc, 15, (512, 424), 0)
try:
os.mkdir('./results')
os.mkdir('./results/dp')
os.mkdir('./results/pc')
os.mkdir('./results/out')
os.mkdir('./results/off')
os.mkdir('./results/max')
except:
pass
# Get depth stream and estimate the joints location
# count = 0
while not self.done:
# get depth data: 424*512 size
frames = listener.waitForNewFrame()
depth = frames["depth"]
depth = depth.asarray().clip(0, 4080) # 16*255=4080
# convert depth to gray and RGB image
gray_image = np.uint8(depth / 16.)
depth_image = np.empty(
[gray_image.shape[0], gray_image.shape[1], 3], dtype=np.uint8)
depth_image[:, :, 0] = gray_image
depth_image[:, :, 1] = gray_image
depth_image[:, :, 2] = gray_image
hand_contour = self.find_contour(gray_image)
darks = np.zeros((424, 512), dtype=np.uint8)
exist = True
try:
hand_area = cv2.contourArea(hand_contour[0])
if hand_area < 700 or hand_area > 5000:
exist = False
except:
exist = False
if exist:
seg_depth, p_max, p_min = self.segmentation(
depth, hand_contour, darks)
# np.save('./results/dp/depth%d.npy' % count, seg_depth)
# show_image = self.cover
pre = preproces(seg_depth, Focal)
point_clouds, max_bb3d_len, offset, location = pre.run()
# np.save('./results/pc/points_cloud%d.npy' % count, point_clouds)
# joints = self.estimate(netR, optimizer, point_clouds)
point_cloud = np.empty(
[1, opt.SAMPLE_NUM, opt.INPUT_FEATURE_NUM],
dtype=np.float32)
point_cloud[0, :, :] = point_clouds
point_cloud = torch.from_numpy(point_cloud)
point_cloud = point_cloud.cuda()
inputs_level1, inputs_level1_center = group_points(
point_cloud, opt)
inputs_level1 = Variable(inputs_level1, requires_grad=False)
inputs_level1_center = Variable(inputs_level1_center,
requires_grad=False)
# compute output
optimizer.zero_grad()
estimation = netR(inputs_level1, inputs_level1_center)
torch.cuda.synchronize()
# get estimation and save it
joints = estimation.data.cpu()
# np.save('./results/out/results%d.npy' % count, joints)
# np.save('./results/off/offset%d.npy' % count, offset)
# np.save('./results/max/max_bb3d_len%d.npy' % count, max_bb3d_len)
# count += 1
joint = joints.view(-1, 3).numpy()
# location rematch
joint = joint + offset
p_middel = (p_max + p_min) / 2
for i in range(len(joint)):
joint[i, 0] = (joint[i, 0] * Focal) /\
joint[i, 2]+p_middel[0]
joint[i, 1] = (joint[i, 1] * Focal) /\
joint[i, 2]+p_middel[1]
# print('focal:', Focal, '\nmax_bb:', max_bb3d_len)
show_image = self.draw_joints(depth_image,
joint.astype(np.int32))
cover = self.cover
else:
show_image = depth_image
cover = darks
cv2.imshow('cover', cover)
cv2.imshow("results", show_image)
seg_video.write(cover)
depth_video.write(show_image)
listener.release(frames)
key = cv2.waitKey(delay=1)
if key == ord('q'):
self._done = True
seg_video.release()
depth_video.release()
device.stop()
device.close()
sys.exit()
def find_and_track_kinect(name, tracker = "CSRT",
min_range = 0, max_range = 1700,
face_target_box = DEFAULT_FACE_TARGET_BOX,
res = (RGB_W, RGB_H),
video_out = True, debug = True):
known_faces = {}
for person in faces:
image = face_recognition.load_image_file(faces[person])
print(person)
face_encoding_list = face_recognition.face_encodings(image)
if len(face_encoding_list) > 0:
known_faces[person] = face_encoding_list[0]
else:
print("\t Could not find face for person...")
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline = pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Depth)
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
bigdepth = Frame(1920, 1082, 4)
trackerObj = None
face_count = 5
face_process_frame = True
bbox = None
face_bbox = None
track_bbox = None
while True:
timer = cv2.getTickCount()
person_found = False
frames = listener.waitForNewFrame()
color = frames["color"]
depth = frames["depth"]
registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth)
bd = np.resize(bigdepth.asarray(np.float32), (1080, 1920))
c = cv2.cvtColor(color.asarray(), cv2.COLOR_RGB2BGR)
__clean_color(c, bd, min_range, max_range)
if face_process_frame:
small_c = __crop_frame(c, face_target_box)
face_locations = face_recognition.face_locations(small_c, model="cnn")
face_encodings = face_recognition.face_encodings(small_c, face_locations)
for face_encoding in face_encodings:
matches = face_recognition.compare_faces(
[known_faces[name]], face_encoding, 0.6)
if len(matches) > 0 and matches[0]:
person_found = True
face_count += 1
(top, right, bottom, left) = face_locations[0]
left += face_target_box[0]
top += face_target_box[1]
right += face_target_box[0]
bottom += face_target_box[1]
face_bbox = (left, top, right, bottom)
person_found = True
break
face_process_frame = not face_process_frame
overlap_pct = 0
track_area = __bbox_area(track_bbox)
if track_area > 0 and face_bbox:
overlap_area = __bbox_overlap(face_bbox, track_bbox)
overlap_pct = min(overlap_area / __bbox_area(face_bbox),
overlap_area / __bbox_area(track_bbox))
if person_found and face_count >= FACE_COUNT and overlap_pct < CORRECTION_THRESHOLD:
bbox = (face_bbox[0],
face_bbox[1],
face_bbox[2] - face_bbox[0],
face_bbox[3] - face_bbox[1])
trackerObj = __init_tracker(c, bbox, tracker)
face_count = 0
status = False
if trackerObj is not None:
status, trackerBBox = trackerObj.update(c)
bbox = (int(trackerBBox[0]),
int(trackerBBox[1]),
int(trackerBBox[0] + trackerBBox[2]),
int(trackerBBox[1] + trackerBBox[3]))
if bbox is not None:
track_bbox = bbox
fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer)
w = 0
h = 0
if status:
w = track_bbox[0] + int((track_bbox[2] - track_bbox[0])/2)
h = track_bbox[1] + int((track_bbox[3] - track_bbox[1])/2)
if (w < res[0] and w >= 0 and h < res[1] and h >= 0):
distanceAtCenter = bd[h][w]
__update_individual_position("NONE", track_bbox, distanceAtCenter, res)
if video_out:
cv2.line(c, (w, 0),
(w, int(res[1])), (0,255,0), 1)
cv2.line(c, (0, h), \
(int(res[0]), h), (0,255,0), 1)
__draw_bbox(True, c, face_target_box, (255, 0, 0), "FACE_TARGET")
__draw_bbox(status, c, track_bbox, (0, 255, 0), tracker)
__draw_bbox(person_found, c, face_bbox, (0, 0, 255), name)
c = __scale_frame(c, scale_factor = 0.5)
cv2.putText(c, "FPS : " + str(int(fps)), (100,50),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,0,255), 1)
if not status:
failedTrackers = "FAILED: "
failedTrackers += tracker + " "
cv2.putText(c, failedTrackers, (100, 80),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,142), 1)
cv2.imshow("color", c)
listener.release(frames)
key = cv2.waitKey(1) & 0xff
if key == ord('q'):
break
device.stop()
device.close()
def test_sync_multi_frame():
fn = Freenect2()
num_devices = fn.enumerateDevices()
assert num_devices > 0
serial = fn.getDefaultDeviceSerialNumber()
assert serial == fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial)
assert fn.getDefaultDeviceSerialNumber() == device.getSerialNumber()
device.getFirmwareVersion()
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# Registration
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# optional parameters for registration
bigdepth = Frame(1920, 1082, 4)
color_depth_map = np.zeros((424, 512), np.int32)
# test if we can get two frames at least
frames = listener.waitForNewFrame()
listener.release(frames)
# frames as a first argment also should work
frames = FrameMap()
listener.waitForNewFrame(frames)
color = frames[FrameType.Color]
ir = frames[FrameType.Ir]
depth = frames[FrameType.Depth]
for frame in [ir, depth]:
assert frame.exposure == 0
assert frame.gain == 0
assert frame.gamma == 0
for frame in [color]:
assert frame.exposure > 0
assert frame.gain > 0
assert frame.gamma > 0
registration.apply(color, depth, undistorted, registered)
# with optinal parameters
registration.apply(color,
depth,
undistorted,
registered,
bigdepth=bigdepth,
color_depth_map=color_depth_map.ravel())
assert color.width == 1920
assert color.height == 1080
assert color.bytes_per_pixel == 4
assert ir.width == 512
assert ir.height == 424
assert ir.bytes_per_pixel == 4
assert depth.width == 512
assert depth.height == 424
assert depth.bytes_per_pixel == 4
assert color.asarray().shape == (color.height, color.width, 4)
assert ir.asarray().shape == (ir.height, ir.width)
assert depth.asarray(np.float32).shape == (depth.height, depth.width)
listener.release(frames)
def __test_cannot_determine_type_of_frame(frame):
frame.asarray()
for frame in [registered, undistorted]:
yield raises(ValueError)(__test_cannot_determine_type_of_frame), frame
device.stop()
device.close()
class KinectStreamer:
def __init__(self):
try:
from pylibfreenect2 import OpenCLPacketPipeline
pipeline = OpenCLPacketPipeline()
except:
try:
from pylibfreenect2 import OpenGLPacketPipeline
pipeline = OpenGLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
pipeline = CpuPacketPipeline()
print("Packet pipeline:", type(pipeline).__name__)
# Create and set logger
logger = createConsoleLogger(LoggerLevel.Debug)
setGlobalLogger(logger)
self.freenect = Freenect2()
num_devices = self.freenect.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = self.freenect.getDeviceSerialNumber(0)
self.device = self.freenect.openDevice(serial, pipeline=pipeline)
self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
# Register listeners
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
self.device.start()
self.registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
self.run_loop = True
self.lock = threading.Lock()
self.img_buffer = np.zeros(
(RES_COLOR[1], RES_COLOR[0], 3, SIZE_AVG_FILTER))
self.idx_buffer = 0
# Close on Ctrl-C
def signal_handler(signal, frame):
self.run_loop = False
signal.signal(signal.SIGINT, signal_handler)
def stream_thread(self):
undistorted = Frame(RES_DEPTH[0], RES_DEPTH[1], 4)
registered = Frame(RES_DEPTH[0], RES_DEPTH[1], 4)
while self.run_loop:
frames = self.listener.waitForNewFrame()
color = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
self.registration.apply(color, depth, undistorted, registered)
img_ir = ir.asarray() / 65535.
self.lock.acquire()
self.img_depth = (depth.asarray() / 4500. * 255).astype(np.uint8)
self.img_color = np.copy(color.asarray()[:, ::-1, :3])
self.img_buffer[:, :, :, self.idx_buffer] = self.img_color
self.img_registered_color = np.copy(registered.asarray(np.uint8))
self.img_registered_depth = np.copy(undistorted.asarray(np.uint8))
self.lock.release()
self.idx_buffer += 1
if self.idx_buffer >= SIZE_AVG_FILTER:
self.idx_buffer = 0
cv2.imshow("Kinect Depth", self.img_depth)
cv2.imshow(
"Kinect Color",
cv2.resize(self.img_color, (int(round(
0.3 * RES_COLOR[0])), int(round(0.3 * RES_COLOR[1])))))
cv2.imshow("Kinect R Depth", self.img_registered_depth)
cv2.imshow("Kinect R Color", self.img_registered_color)
key = cv2.waitKey(1) & 0xFF
if key == ord('q') or key == ord('x'):
self.run_loop = False
break
elif key == ord(' '):
cv2.imwrite("frame.png", frame)
self.listener.release(frames)
self.device.stop()
self.device.close()
def write_thread(self, video_name="output"):
fourcc = cv2.VideoWriter_fourcc(*'X264')
vid_color = cv2.VideoWriter(video_name + ".avi", fourcc, FPS,
RES_COLOR)
vid_depth = cv2.VideoWriter(video_name + "_depth.avi",
fourcc,
FPS,
RES_DEPTH,
isColor=False)
t_curr = time.time()
while self.run_loop:
t_start = t_curr
self.lock.acquire()
try:
vid_color.write(self.img_color)
vid_depth.write(self.img_depth)
except:
pass
self.lock.release()
t_curr = time.time()
time.sleep(max(1. / FPS - (t_curr - t_start), 0))
vid_color.release()
vid_depth.release()
class Kinect:
def __init__(self, kinect_num=0):
self.fn = Freenect2()
self.serial = None
self.device = None
self.listener = None
self.registration = None
self._frames = None # frames cache so that the user can use them before we free them
self._bigdepth = Frame(1920, 1082, 4) # malloc'd
self._undistorted = Frame(512, 424, 4)
self._registered = Frame(512, 424, 4)
self._color_dump = Frame(1920, 1080, 4)
num_devices = self.fn.enumerateDevices()
if num_devices <= kinect_num:
raise ConnectionError("No Kinect device at index %d" % kinect_num)
self.serial = self.fn.getDeviceSerialNumber(kinect_num)
self.device = self.fn.openDevice(self.serial, pipeline=pipeline)
self.listener = SyncMultiFrameListener(FrameType.Color
| FrameType.Depth)
# Register listeners
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
def close(self):
if self.device:
self.device.close()
def start(self):
if self.device:
self.device.start()
self.registration = Registration(
self.device.getIrCameraParams(),
self.device.getColorCameraParams())
return self # for convenience
else:
raise ConnectionError("Connection to Kinect wasn't established")
def stop(self):
if self.device:
self.device.stop()
### If copy is False for these functoins, make sure to call release_frames
### when you're done with the returned frame ###
def get_current_color_frame(self, copy=True):
self._frames = self.listener.waitForNewFrame()
ret = self._convert_color_frame(self._frames["color"], copy=copy)
if copy:
self.release_frames()
return ret
def get_current_depth_frame(self, copy=True):
self._frames = self.listener.waitForNewFrame()
if copy:
ret = self._frames["depth"].asarray().copy()
self.release_frames()
else:
ret = self._frames["depth"].asarray()
return ret
def get_current_rgbd_frame(self, copy=True):
self._frames = self.listener.waitForNewFrame()
self.registration.apply(self._frames["color"],
self._frames["depth"],
self._undistorted,
self._registered,
bigdepth=self._bigdepth)
color = self._convert_color_frame(self._frames["color"], copy=copy)
depth = self._convert_bigdepth_frame(self._bigdepth, copy=copy)
if copy:
self.release_frames()
return color, depth
def get_current_bigdepth_frame(self, copy=True):
self._frames = self.listener.waitForNewFrame()
self.registration.apply(self._frames["color"],
self._frames["depth"],
self._undistorted,
self._registered,
bigdepth=self._bigdepth)
depth = self._convert_bigdepth_frame(self._bigdepth, copy=copy)
if copy:
self.release_frames()
return depth
def release_frames(self):
self.listener.release(self._frames)
# single frame calls
def get_single_color_frame(self):
self.start()
ret = self.get_current_color_frame()
self.stop()
return ret
def get_single_depth_frame(self):
self.start()
ret = self.get_current_depth_frame()
self.stop()
return ret
def _convert_bigdepth_frame(self, frame, copy=True):
if copy:
d = self._bigdepth.asarray(np.float32).copy()
else:
d = self._bigdepth.asarray(np.float32)
return d[1:-1, ::-1]
def _convert_color_frame(self, frame, copy=True):
if copy:
img = frame.asarray().copy()
else:
img = frame.asarray()
img = img[:, ::-1]
img[..., :3] = img[..., 2::-1] # bgrx -> rgbx
return img
class Tracker:
# X_POSITION = -0.11387496
# Z_POSITION = 1.3
X_POSITION = -0.185
Z_POSITION = 1.5
greenLower = (29, 86, 6)
greenUpper = (64, 255, 255)
greenLower = (27, 100, 50)
greenUpper = (60, 255, 255)
def __init__(self, use_kinect=False, basket=False, gui=True):
self.connected = False
self.max_radius = 100000
self.running = False
self.killed = False
self.gui = gui
self.basket = basket
self.use_kinect = use_kinect
# self.fig, self.ax = plt.subplots()
if self.use_kinect:
self.px = []
self.py = []
logger = createConsoleLogger(LoggerLevel.Error)
setGlobalLogger(logger)
while not self.connected:
# Bad, but needed
if self.use_kinect:
self.connect_kinect()
else:
self.connect_camera()
self.thread = threading.Thread(target=self.video_thread)
self.thread.start()
def connect_camera(self):
# Bad, but needed
self.camera = cv2.VideoCapture(1)
if not self.camera.isOpened():
print("Unable to connect to the camera, check again")
time.sleep(5)
else:
self.connected = True
def connect_kinect(self):
# Bad, but needed
self.fn = Freenect2()
num_devices = self.fn.enumerateDevices()
if num_devices == 0:
print("Kinect not found, check again")
self.connected = False
return
try:
self.pipeline = OpenGLPacketPipeline()
except:
self.pipeline = CpuPacketPipeline()
serial = self.fn.getDeviceSerialNumber(0)
self.device = self.fn.openDevice(serial, pipeline=self.pipeline)
self.listener = SyncMultiFrameListener(
FrameType.Color | FrameType.Ir | FrameType.Depth)
# Register listeners
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
self.device.start()
print("started")
# NOTE: must be called after device.start()
self.registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
print("registration")
self.undistorted = Frame(512, 424, 4)
self.registered = Frame(512, 424, 4)
self.connected = True
def video_thread(self):
print('Camera thread started')
need_bigdepth = False
need_color_depth_map = False
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
while not self.killed:
while self.running:
# (grabbed, frame) = self.camera.read()
frames = self.listener.waitForNewFrame()
frame = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
self.registration.apply(frame, depth, self.undistorted, self.registered,
bigdepth=bigdepth,
color_depth_map=color_depth_map)
# print(frame.width, frame.height, frame.bytes_per_pixel)
# print(ir.width, ir.height, ir.bytes_per_pixel)
# print(depth.width, depth.height, depth.bytes_per_pixel)
# print(frame.asarray().shape, ir.asarray().shape, depth.asarray().shape)
# color_image_array = frame.asarray()
color_image_array = self.registered.asarray(np.uint8)
hsv = cv2.cvtColor(color_image_array, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.greenLower, self.greenUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]),
int(M["m01"] / M["m00"]))
if radius > 10:
cv2.circle(color_image_array, (int(x), int(y)), int(radius),
(0, 255, 255), 2)
cv2.circle(color_image_array, center, 5, (0, 0, 255), -1)
x, y, z = self.registration.getPointXYZ(self.undistorted, y, x)
# print(x, y, z, end='\r')
if not math.isnan(y) and not math.isnan(z):
self.px.append(y)
self.py.append(z)
row = tuple(c[c[:, :, 1].argmin()][0])[1]
self.max_radius = min(self.max_radius, row)
if self.gui:
cv2.imshow("Frame", color_image_array)
cv2.imshow("Masked", mask)
self.listener.release(frames)
cv2.waitKey(1) & 0xFF
# self.ax.plot(self.px)
# self.ax.plot(self.py)
# plt.pause(0.01)
cv2.destroyAllWindows()
print("exit looping")
def start(self):
self.max_radius = 100000
if self.use_kinect:
self.px = []
self.py = []
self.running = True
def stop(self):
self.running = False
def kill(self):
self.killed = True
if self.use_kinect:
self.device.stop()
self.device.close()
def get_reward(self):
if self.use_kinect:
if not self.basket:
try:
peaks = peakutils.indexes(self.px)
except Exception as e:
print(e)
peaks = np.array([])
if peaks.any():
# for xx in peaks:
# t.ax.plot(xx, self.px[xx], 'ro')
return self.py[peaks[0]] * 100
else:
return 0
else:
print(np.mean(self.px), np.mean(self.py))
dist = distance(np.array([self.px, self.py]).T, self.X_POSITION, self.Z_POSITION)
print("distance", dist, len(self.px))
return dist * 100
else:
return self.max_radius
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'):
break
device.stop()
device.close()
sys.exit(0)
plt.plot(x, y)
a = list(range(511))
while True:
frames = listener.waitForNewFrame()
depth = frames["depth"]
dep = depth.asarray()
dep = dep[212:]
dep_display = dep / 4096.
data = dep
y = []
for i in a:
d = estimate_coef(data[:, i], data[:, i + 1])
#plot_regression_line(data[:,1],data[:,2],d)
#print (i)
make_fig()
cv2.imshow("depth", dep_display)
listener.release(frames)
key = cv2.waitKey(50)
if key == ord('q'):
break
device.stop()
device.close()
sys.exit(0)
class Kinect:
def __init__(self):
self.averageSpineX = 0
self.fn = Freenect2()
if self.fn.enumerateDevices() == 0:
sys.exit(47)
self.serial = self.fn.getDeviceSerialNumber(0)
types = 0
types |= FrameType.Color
types |= (FrameType.Ir | FrameType.Depth)
self.listener = SyncMultiFrameListener(types)
self.logger = createConsoleLogger(LoggerLevel.Debug)
self.device = self.fn.openDevice(self.serial)
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
self.undistorted = Frame(512, 424, 4)
self.registered = Frame(512, 424, 4)
def valueBounded(self, checkValue, absoluteValue):
if ((-absoluteValue <= checkValue >= absoluteValue)):
return True
else:
return False
def valueUnbounded(self, checkValue, absoluteValue):
if ((checkValue > absoluteValue) | (checkValue < -absoluteValue)):
return True
else:
return False
def valuePlusMinusDifferential(self, checkValue, testValue, Differential):
if((checkValue < testValue + Differential) & (checkValue > testValue - Differential)):
return True
else:
return False
def update(self, registration):
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
frames = self.listener.waitForNewFrame()
depth = frames["depth"]
color = frames["color"]
d = self.undistorted.asarray(dtype=np.float32)
registration.apply(color, depth, self.undistorted, self.registered)
self.listener.release(frames)
return d
# calculate the average and skeleton points of a depth array, those
# values plus the depth array
# calculate mean depth of depth array, used to find skeleton points.
def getMeanDepth(self, depth, rows, cols):
total = 0
sumDepth = 0
for row in range(rows):
for col in range(cols):
try:
offset = row + col * (rows)
except IndexError as e:
print (e)
#if depth[offset]['depth'] < DANGER_THRESHOLD:
total += 1
sumDepth += depth[offset]['depth']
return (sumDepth / total)
# calculate the skeleton points of the depth array
def getSkeleton(self, depthArray, average, rows, cols):
topY = 0
leftX = rows
bottomY = cols
rightX = 0
for d in depthArray:
#print depthArray[offset]
if (d['x'] > rightX):
rightX = d['x']
if (d['x'] < leftX):
leftX = d['x']
if (d['y'] < bottomY):
bottomY = d['y']
if (d['y'] > topY):
topY = d['y']
averageX = (leftX + rightX) / 2
returnValues = {'averageX': averageX,
'topY': topY,
'bottomY': bottomY,
'rightX': rightX,
'leftX': leftX}
return returnValues
def changeInX(self, spine):
print ("changeInX")
if self.averageSpineX == 0:
self.averageSpineX = spine['averageX']
elif (self.valueBounded((self.averageSpineX - spine['averageX']), X_CHANGE_THRESHOLD)):
self.averageSpineX = ((self.averageSpineX + spine['averageX']) / 2)
else:
self.checkDrowning()
# Check to see if the difference between the averageSpineX and the last
# analyzed averageX is less below a threshold. If it is, the loop
# continues. If it runs for 20 seconds, it will set the drowning flag to
# true. If falsePositive gets to be too high, DORi will no longer
# assume the victim is a drowning risk
def checkDrowning(self):
print ("checkDrowning")
drowningRisk = True
drowning = False
falsePositive = 0
# 20 seconds from start of def #
timeLimit = time.time() + DROWN_TIME
self.device.start()
while drowningRisk:
print ('checking...')
if time.time() > timeLimit:
drowningRisk = False
drowning = True
data = self.fullDataLoop()
if (self.valueUnbounded((self.averageSpineX - data['spine']['averageX']), X_CHANGE_THRESHOLD)):
falsePositive += 1
if falsePositive > 100:
drowningRisk = False
else:
continue
if drowning:
self.device.stop()
print ("This guy is for sure drowning")
sys.exit(47)
self.device.stop()
def depthMatrixToPointCloudPos2(self, depthArray):
R, C = depthArray.shape
R -= KINECT_SPECS['cx']
R *= depthArray
R /= KINECT_SPECS['fx']
C -= KINECT_SPECS['cy']
C *= depthArray
C /= KINECT_SPECS['fy']
return np.column_stack((depthArray.ravel(), R.ravel(), C.ravel()))
def getBody(self, depthArray, average, rows, cols):
body = []
for d in depthArray:
if self.valuePlusMinusDifferential(d['depth'], average, BODY_DEPTH_THRESHOLD):
body.append(d)
return body
def pointRavel(self, unraveledArray, rows, cols):
raveledArray = []
d = unraveledArray.ravel()
for row in range(rows):
for col in range(cols):
try:
offset = row + col * (rows)
raveledArray.append({'x': row, 'y': col, 'depth': d[offset]})
except IndexError as e:
print (e)
return raveledArray
def fullDataLoop(self):
registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
deptharraytest = [{'x': 152, 'y': 100, 'depth': 400}, {'x': 300, 'y': 200, 'depth': 400},
{'x': 350, 'y': 150, 'depth': 400}, {'x': 400, 'y': 300, 'depth': 400},
{'x': 22, 'y': 10, 'depth': 400}, {'x': 144, 'y': 12, 'depth': 400}]
depthArray = self.update(registration)
rows, cols = depthArray.shape
d = self.pointRavel(depthArray, rows, cols)
m = self.getMeanDepth(d, rows, cols)
b = self.getBody(d, m, rows, cols)
s = self.getSkeleton(deptharraytest, m, cols, rows)
return {'spine' : s, 'meanDepth' : m}
def run(self, duration):
end = time.time() + duration
self.device.start()
registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
deptharraytest = [{'x':152, 'y':100, 'depth':400}, {'x':300, 'y':200, 'depth':400},
{'x': 350, 'y': 150, 'depth': 400},{'x':400, 'y':300, 'depth':400},
{'x': 22, 'y': 10, 'depth': 400},{'x':144, 'y':12, 'depth':400}]
while time.time() < end:
depthArray = self.update(registration)
rows, cols = depthArray.shape
d = self.pointRavel(depthArray, rows, cols)
m =self.getMeanDepth(d, rows, cols)
b = self.getBody(d, m, rows, cols)
s = self.getSkeleton(deptharraytest, m, cols, rows)
self.changeInX(s)
self.device.stop()
def execute(self):
self.device.start()
registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
d = self.update(registration)
shape = d.shape
m = self.getMeanDepth(d, 9)
b = self.getBody(d,m)
s = self.getSkeleton(d, m)
self.changeInX(s)
print ('depthArray' + str(len(d.ravel())))
print ('body' + str(len(b)))
for body in b:
print ("(" + str(body['x']) + "," + str(body['y']) + "): " + str(body['z']))
print (m)
self.device.stop()
def exit(self):
self.device.stop()
self.device.close()
def projection():
pipeline = CpuPacketPipeline()
print("Packet pipeline:", type(pipeline).__name__)
# Create and set logger
logger = createConsoleLogger(LoggerLevel.Debug)
setGlobalLogger(logger)
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# Optinal parameters for registration
# set True if you need
need_bigdepth = True
need_color_depth_map = True
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
color_pub = rospy.Publisher('color', Image, queue_size=10)
depth_pub = rospy.Publisher('depth', Image, queue_size=10)
ir_pub = rospy.Publisher('ir', Image, queue_size=10)
small_depth_pub = rospy.Publisher('small_depth', Image, queue_size=10)
rospy.init_node('projection', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
frames = listener.waitForNewFrame()
color = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
registration.apply(color,
depth,
undistorted,
registered,
bigdepth=bigdepth,
color_depth_map=color_depth_map)
color_image = CvBridge().cv2_to_imgmsg(color.asarray())
color_pub.publish(color_image)
depth_image = CvBridge().cv2_to_imgmsg(bigdepth.asarray(np.float32))
depth_pub.publish(depth_image)
ir_image = CvBridge().cv2_to_imgmsg(ir.asarray())
ir_pub.publish(ir_image)
small_depth_image = CvBridge().cv2_to_imgmsg(depth.asarray())
small_depth_pub.publish(small_depth_image)
rate.sleep()
listener.release(frames)
key = cv2.waitKey(delay=1)
if key == ord('q'):
break
device.stop()
device.close()
sys.exit(0)
class Kinect:
def __init__(self):
self.averageSpineX = 0
self.fn = Freenect2()
if self.fn.enumerateDevices() == 0:
sys.exit(47)
self.serial = self.fn.getDeviceSerialNumber(0)
types = 0
types |= FrameType.Color
types |= (FrameType.Ir | FrameType.Depth)
self.listener = SyncMultiFrameListener(types)
self.logger = createConsoleLogger(LoggerLevel.Debug)
self.device = self.fn.openDevice(self.serial)
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
self.undistorted = Frame(512, 424, 4)
self.registered = Frame(512, 424, 4)
def valueBounded(self, checkValue, absoluteValue):
if ((-absoluteValue <= checkValue >= absoluteValue)):
return True
else:
return False
def valueUnbounded(self, checkValue, absoluteValue):
if ((checkValue > absoluteValue) | (checkValue < -absoluteValue)):
return True
else:
return False
def valuePlusMinusDifferential(self, checkValue, testValue, Differential):
if((checkValue < testValue + Differential) & (checkValue > testValue - Differential)):
return True
else:
return False
def update(self, registration):
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
frames = self.listener.waitForNewFrame()
depth = frames["depth"]
color = frames["color"]
d = self.undistorted.asarray(dtype=np.float32)
registration.apply(color, depth, self.undistorted, self.registered)
self.listener.release(frames)
return d
# calculate the average and skeleton points of a depth array, those
# values plus the depth array
# calculate mean depth of depth array, used to find skeleton points.
def getMeanDepth(self, depth, rows, cols):
total = 0
sumDepth = 0
for row in range(rows):
for col in range(cols):
try:
offset = row + col * (rows)
except IndexError as e:
print e
#if depth[offset]['depth'] < DANGER_THRESHOLD:
total += 1
sumDepth += depth[offset]['depth']
return (sumDepth / total)
# calculate the skeleton points of the depth array
def getSkeleton(self, depthArray, average, rows, cols):
topY = 0
leftX = rows
bottomY = cols
rightX = 0
for d in depthArray:
#print depthArray[offset]
if (d['x'] > rightX):
rightX = d['x']
if (d['x'] < leftX):
leftX = d['x']
if (d['y'] < bottomY):
bottomY = d['y']
if (d['y'] > topY):
topY = d['y']
averageX = (leftX + rightX) / 2
returnValues = {'averageX': averageX,
'topY': topY,
'bottomY': bottomY,
'rightX': rightX,
'leftX': leftX}
return returnValues
def changeInX(self, spine):
print "changeInX"
if self.averageSpineX == 0:
self.averageSpineX = spine['averageX']
elif (self.valueBounded((self.averageSpineX - spine['averageX']), X_CHANGE_THRESHOLD)):
self.averageSpineX = ((self.averageSpineX + spine['averageX']) / 2)
else:
self.checkDrowning()
# Check to see if the difference between the averageSpineX and the last
# analyzed averageX is less below a threshold. If it is, the loop
# continues. If it runs for 20 seconds, it will set the drowning flag to
# true. If falsePositive gets to be too high, DORi will no longer
# assume the victim is a drowning risk
def checkDrowning(self):
print "checkDrowning"
drowningRisk = True
drowning = False
falsePositive = 0
# 20 seconds from start of def #
timeLimit = time.time() + DROWN_TIME
self.device.start()
while drowningRisk:
print 'checking...'
if time.time() > timeLimit:
drowningRisk = False
drowning = True
data = self.fullDataLoop()
if (self.valueUnbounded((self.averageSpineX - data['spine']['averageX']), X_CHANGE_THRESHOLD)):
falsePositive += 1
if falsePositive > 100:
drowningRisk = False
else:
continue
if drowning:
self.device.stop()
print "This guy is for sure drowning"
sys.exit(47)
self.device.stop()
def depthMatrixToPointCloudPos2(self, depthArray):
R, C = depthArray.shape
R -= KINECT_SPECS['cx']
R *= depthArray
R /= KINECT_SPECS['fx']
C -= KINECT_SPECS['cy']
C *= depthArray
C /= KINECT_SPECS['fy']
return np.column_stack((depthArray.ravel(), R.ravel(), C.ravel()))
def getBody(self, depthArray, average, rows, cols):
body = []
for d in depthArray:
if self.valuePlusMinusDifferential(d['depth'], average, BODY_DEPTH_THRESHOLD):
body.append(d)
return body
def pointRavel(self, unraveledArray, rows, cols):
raveledArray = []
d = unraveledArray.ravel()
for row in range(rows):
for col in range(cols):
try:
offset = row + col * (rows)
raveledArray.append({'x': row, 'y': col, 'depth': d[offset]})
except IndexError as e:
print e
return raveledArray
def fullDataLoop(self):
registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
deptharraytest = [{'x': 152, 'y': 100, 'depth': 400}, {'x': 300, 'y': 200, 'depth': 400},
{'x': 350, 'y': 150, 'depth': 400}, {'x': 400, 'y': 300, 'depth': 400},
{'x': 22, 'y': 10, 'depth': 400}, {'x': 144, 'y': 12, 'depth': 400}]
depthArray = self.update(registration)
rows, cols = depthArray.shape
d = self.pointRavel(depthArray, rows, cols)
m = self.getMeanDepth(d, rows, cols)
b = self.getBody(d, m, rows, cols)
s = self.getSkeleton(deptharraytest, m, cols, rows)
return {'spine' : s, 'meanDepth' : m}
def run(self, duration):
end = time.time() + duration
self.device.start()
registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
deptharraytest = [{'x':152, 'y':100, 'depth':400}, {'x':300, 'y':200, 'depth':400},
{'x': 350, 'y': 150, 'depth': 400},{'x':400, 'y':300, 'depth':400},
{'x': 22, 'y': 10, 'depth': 400},{'x':144, 'y':12, 'depth':400}]
while time.time() < end:
depthArray = self.update(registration)
rows, cols = depthArray.shape
d = self.pointRavel(depthArray, rows, cols)
m =self.getMeanDepth(d, rows, cols)
b = self.getBody(d, m, rows, cols)
s = self.getSkeleton(deptharraytest, m, cols, rows)
self.changeInX(s)
self.device.stop()
def execute(self):
self.device.start()
registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
d = self.update(registration)
shape = d.shape
m = self.getMeanDepth(d, 9)
b = self.getBody(d,m)
s = self.getSkeleton(d, m)
self.changeInX(s)
print 'depthArray' + str(len(d.ravel()))
print 'body' + str(len(b))
for body in b:
print "(" + str(body['x']) + "," + str(body['y']) + "): " + str(body['z'])
print m
self.device.stop()
def exit(self):
self.device.stop()
self.device.close()
class RunCamera(object):
# check if it needs to make a picture and save the picture
_img_save = False
_img_num = 0
_img_done = True
def __init__(self):
try:
from pylibfreenect2 import OpenGLPacketPipeline
self._pipeline = OpenGLPacketPipeline()
except:
try:
from pylibfreenect2 import OpenCLPacketPipeline
self._pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
self._pipeline = CpuPacketPipeline()
print("Packet pipeline:", type(self._pipeline).__name__)
pygame.init()
# Used to manage how fast the screen updates
self._clock = pygame.time.Clock()
# Set the width and height of the screen [width, height]
self._screen_width = 500
self._screen_heigth = 300
self._screen = pygame.display.set_mode((self._screen_width, self._screen_heigth), pygame.HWSURFACE|pygame.DOUBLEBUF|pygame.RESIZABLE, 32)
# Set the titel of the screen- window
pygame.display.set_caption("Kamera")
# Loop until the user clicks the close button.
self._done = False
# Kinect runtime object, we want only color and body frames
#self._kinect = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Color)
# Create and set logger
logger = createConsoleLogger(LoggerLevel.Debug)
setGlobalLogger(logger)
self._fn = Freenect2()
self._num_devices = self._fn.enumerateDevices()
if self._num_devices == 0:
print("No device connected!")
sys.exit(1)
self._serial = self._fn.getDeviceSerialNumber(0)
self._device = self._fn.openDevice(self._serial, pipeline=self._pipeline)
self._listener = SyncMultiFrameListener(
FrameType.Color | FrameType.Ir | FrameType.Depth)
# Register listeners
self._device.setColorFrameListener(self._listener)
self._device.setIrAndDepthFrameListener(self._listener)
self._device.start()
# NOTE: must be called after device.start()
self._registration = Registration(self._device.getIrCameraParams(),
self._device.getColorCameraParams())
self._undistorted = Frame(512, 424, 4)
self._registered = Frame(512, 424, 4)
# Optinal parameters for registration
# set True if you need
self._need_bigdepth = False
self._need_color_depth_map = False
# back buffer surface for getting Kinect color frames, 32bit color, width and height equal to the Kinect color frame size
#self._frame_surface = pygame.Surface((self._kinect.color_frame_desc.Width, self._kinect.color_frame_desc.Height), 0, 32)
self._bigdepth = Frame(1920, 1082, 4) if self._need_bigdepth else None
self._color_depth_map = np.zeros((424, 512), np.int32).ravel() if self._need_color_depth_map else None
self._frame_surface = pygame.Surface((self._registered.width, self._registered.height), 0, 32)
def draw_color_frame(self, np_frame, target_surface):
target_surface.lock()
#address = self._kinect.surface_as_array(target_surface.get_buffer())
#ctypes.memmove(address, frame.ctypes.data, frame.size)
address = get_address_from_array(target_surface.get_buffer())
ctypes.memmove(address, np_frame.ctypes.data, np_frame.size)
del address
target_surface.unlock()
def run(self):
while not self._done:
# --- Main event loop
for event in pygame.event.get(): # User did something
if event.type == pygame.QUIT: # If user clicked close
self._done = True # Flag that we are done so we exit this loop
elif event.type == pygame.VIDEORESIZE: # window resized
self._screen = pygame.display.set_mode(event.dict['size'],
pygame.HWSURFACE|pygame.DOUBLEBUF|pygame.RESIZABLE, 32)
# --- Getting frames and drawing
#if self._kinect.has_new_color_frame():
# frame = self._kinect.get_last_color_frame()
# self.draw_color_frame(frame, self._frame_surface)
# frame = None
frames = self._listener.waitForNewFrame()
color_frame = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
self._registration.apply(color_frame, depth, self._undistorted, self._registered, bigdepth=self._bigdepth, color_depth_map=self._color_depth_map)
color_arr_view = color_frame.asarray()[:,:,0:3]
test_surf = pygame.surfarray.make_surface(color_arr_view)
#self.draw_color_frame(color_frame.asarray(), self._frame_surface)
self._frame_surface = test_surf
# --- When the flag of Saving image is unlocked, so do it
if RunCamera._img_save == True:
print("entered if statement for image saving")
# First to lock it, or it will save too many picture
RunCamera._img_save = False
# Folder and names of pictures
# or we can just use other names of th picture..
name = "pic\\test-" + str(RunCamera._img_num) + ".jpg"
# Save the image
pygame.image.save(self._frame_surface, name)
print "\n \nWe just saved the " + name + "!\n\nLock save-image-processing...\n\n===============================================\n"
RunCamera._img_num = RunCamera._img_num + 1
# Tell wether all be done
RunCamera._img_done = True
# --- copy back buffer surface pixels to the screen, resize it if needed and keep aspect ratio
h_to_w = float(self._frame_surface.get_height()) / self._frame_surface.get_width()
# --- Screen's width and height
target_width = self._screen.get_width()
target_height = int(h_to_w * target_width )
surface_to_draw = pygame.transform.scale(self._frame_surface, (target_width, target_height))
self._screen.blit(surface_to_draw, (0,0))
surface_to_draw = None
pygame.display.update()
# --- Go ahead and update the screen with what we've drawn.
pygame.display.flip()
# --- Limit to 30 frames per second, try 60 if 30 runs smoothly
self._clock.tick(30)
self._listener.release(frames)
# Close our Kinect sensor, close the window and quit.
#self._kinect.close()
#self._device.stop()
self._device.close()
pygame.quit()