def __init__(self,
frametypes=FrameType.Color | FrameType.Depth,
registration=True):
# open the device and start the listener
num_devices = fn.enumerateDevices()
assert num_devices > 0, "Couldn't find any devices"
serial = fn.getDeviceSerialNumber(0)
self.device = fn.openDevice(serial, pipeline=pipeline)
self.listener = SyncMultiFrameListener(frametypes)
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
self.device.start()
# NOTE: must be called after device.start()
if registration:
ir = self.device.getIrCameraParams()
color = self.device.getColorCameraParams()
self.registration = Registration(ir, color)
# create these here so we don't have to every time we call update()
self.undistorted = Frame(512, 424, 4)
self.registered = Frame(512, 424, 4)
# which kinds of frames are we going to be reading?
# will be true or false
self.need_color = frametypes & FrameType.Color
self.need_depth = frametypes & FrameType.Depth
self.need_ir = frametypes & FrameType.Ir
# initialize our frames
self.frames = {}
# ensure that we shut down smoothly
atexit.register(self.close)
def __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 mainSnapshot(options=None):
pipeline = CudaKdePacketPipeline()
fn = Freenect2()
device, listener, registration = initCamera(fn, pipeline)
warmUp(listener)
undistorted = Frame(FRAME_WIDTH, FRAME_HEIGHT, 4)
registered = Frame(FRAME_WIDTH, FRAME_HEIGHT, 4)
result = None
while True:
frames = listener.waitForNewFrame()
color = frames["color"]
depth = frames["depth"]
registration.apply(color, depth, undistorted, registered)
# kinect flips X axis
regArray = registered.asarray(np.uint8)
regArray = np.flip(regArray, 1)
imgRGB = cv2.cvtColor(regArray, cv2.COLOR_RGBA2RGB)
ret, buf = cv2.imencode(".jpg", imgRGB)
if ret == True:
data = base64.b64encode(buf)
result = {
'width': FRAME_WIDTH,
'height': FRAME_HEIGHT,
'data': data
}
break
else:
print 'fail'
listener.release(frames)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
shutdownCamera(device)
return result
def __init__(self): self.pipeline = OpenCLPacketPipeline() fn = Freenect2() self.device = fn.openDefaultDevice(pipeline=self.pipeline) self.listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) self.device.start() self.registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) self.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4)
def 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 captureKinect(device, serial,listener, timestamp, dID):
ts = timestamp
dataID = dID
camera="Kinect"
filename = dataID+"_"+ts+"_"+camera
firmware = device.getFirmwareVersion()
firmware = str(firmware).split("'")[1]
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
frames = listener.waitForNewFrame()
color = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
registration.apply(color, depth, undistorted, registered,bigdepth=None,color_depth_map=None)
colorExposure = color.exposure
depthExposure = depth.exposure
irExposure = ir.exposure
colorGain = color.gain
depthGain = depth.gain
irGain = ir.gain
#key = cv2.waitKey(5000)
cv2.imwrite('DataSet/'+filename+"_color.jpg", color.asarray())
cv2.imwrite('DataSet/'+filename+"_depth.jpg", depth.asarray()%256)
cv2.imwrite('DataSet/'+filename+"_RGBD.jpg", registered.asarray(np.uint8))
listener.release(frames)
with open("KinectDeviceID.txt") as file:
deviceID = file.read()
process = subprocess.Popen("lsusb -v -d "+deviceID+"|grep \""+deviceID+"\"", stdout=subprocess.PIPE, shell=True)
usb = process.stdout.readline()
usb = str(usb).split("'")[1]
usb = usb.split(':')[0]
#print(usb)
process.communicate()
SensorDetails=[]
SensorDetails.append(str(serial).split("'")[1])
SensorDetails.append(firmware)
SensorDetails.append("NA")
SensorDetails.append(usb)
SensorDetails.append("Color: "+str(colorExposure)+" Depth: "+str(depthExposure)+" IR: "+str(irExposure))
SensorDetails.append("Color: "+str(colorGain)+" Depth: "+str(depthGain)+" IR: "+str(irGain))
with open("kinect.txt",'w') as file:
for item in SensorDetails:
file.write(item+"\n")
def test_frame():
frame = Frame(512, 424, 4)
assert frame.width == 512
assert frame.height == 424
assert frame.bytes_per_pixel == 4
assert frame.exposure == 0
assert frame.gain == 0
assert frame.gamma == 0
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 _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])
def mainSegment(options = None):
print '--------------------------------------------'
print options
counter = 0
pipeline = CudaKdePacketPipeline()
fn = Freenect2()
device, listener, registration = initCamera(fn, pipeline)
warmUp(listener)
minVal = computeBackground(listener, registration)
undistorted = Frame(FRAME_WIDTH, FRAME_HEIGHT, 4)
net = bp.newNet()
t0 = time.clock()
counterOld = counter
while True:
frames = listener.waitForNewFrame()
depth = frames["depth"]
registration.undistortDepth(depth, undistorted)
# kinect flips X axis
flipDepthFrame(undistorted)
d = np.copy(undistorted.asarray(np.float32))
alpha = subtraction(minVal, d)
if (options and options.get('display')):
cv2.imshow('Contour', alpha)
result = bp.process(options, net, scale(d), alpha, registration,
undistorted, device)
if (options and options.get('display')):
if cv2.waitKey(1) & 0xFF == ord('q'):
break
listener.release(frames)
counter = counter + 1
if counter % 10 == 0:
t1 = time.clock()
print '{:.3f} images/sec'.format((counter - counterOld)/(t1-t0))
t0 = t1
counterOld = counter
#print(result)
yield json.dumps(result)
shutdownCamera(device);
def computeBackground(listener, registration):
count = 0
undistorted = Frame(FRAME_WIDTH, FRAME_HEIGHT, 4)
minVal = np.full((FRAME_HEIGHT, FRAME_WIDTH), MAX_FLOAT32, dtype=np.float32)
while count < 60:
frames = listener.waitForNewFrame()
depth = frames["depth"]
registration.undistortDepth(depth, undistorted)
# kinect flips X axis
flipDepthFrame(undistorted)
d = undistorted.asarray(np.float32)
zeros = d.size - np.count_nonzero(d)
#print('Input:zeros:' + str(zeros) + ' total:' + str(d.size))
d[d == 0.] = MAX_FLOAT32
minVal = np.minimum(minVal, d)
#print ('Minval: zeros:' + str(minVal[minVal == MAX_FLOAT32].size) +
# ' total:' + str(minVal.size))
count = count + 1
listener.release(frames)
return inpaint(minVal)
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 __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 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 __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)
device = fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# Optinal parameters for registration
# set True if you need
need_bigdepth = False
need_color_depth_map = False
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
while True:
frames = listener.waitForNewFrame()
color = frames["color"]
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
SIZE_AVG_FILTER = 5
class VideoStreamer:
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_process_frame = True
bbox = None
track_bbox = None
while True:
timer = cv2.getTickCount()
frames = listener.waitForNewFrame()
color = frames["color"]
# 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)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
# select image to use
types = FrameType.Color | FrameType.Depth
listener = SyncMultiFrameListener(types)
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# use preset calibration parameters
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
while True:
frames = listener.waitForNewFrame()
color = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
registration.apply(color, depth, undistorted, registered)
### image processing ###
img = cv2.resize(color.asarray(), (int(1920 / 3), int(1080 / 3)))
img = cv2.medianBlur(img, 5)
grayscale = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
depth = depth.asarray()
depth = cv2.medianBlur(depth, 5)
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 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:
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
listener = SyncMultiFrameListener(
FrameType.Color | FrameType.Ir | FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# Optinal parameters for registration
# set True if you need
need_bigdepth = False
need_color_depth_map = False
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
while True:
frames = listener.waitForNewFrame()
color = frames["color"]
ir = frames["ir"]
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()
# 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)
bigdepth = Frame(1920, 1082, 4)
color_depth_map = np.zeros((424, 512), np.int32).ravel()
rows = 424
cols = 512
ll = 0.
ul = 4500.
rowStr = ''
distance = 0.
#newDepthImg = [[0] * cols for i in range(rows)]
newBigDepthArr = np.zeros([1082, 1920], dtype=float)
newBigDcrop = np.zeros([420, 420], dtype=float)
newDepthArr = np.zeros([rows, cols], dtype=float)
newDepthImg = np.zeros([360, 640], dtype=float)
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
need_enable_filter = True # Filter out pixels not visible to both cameras.
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
framesToCapture = 1
outputDir = "output"
sceneName = "my_room"
size = 224 * SCALE_FACTOR
THICKNESS = 2
FPS = 30
FRAME_WIDTH = 512
FRAME_HEIGHT = 424
TARGET_WIDTH = size + 2 * THICKNESS
TARGET_HEIGHT = size + 2 * THICKNESS
CAPTURE_MODE = False
IMG_SAVE_PATH = "/Users/Daocun/Desktop/CS231A/project/data/"
frame_count = 0
img_count = 1 # used when naming the training samples
undistorted = Frame(FRAME_WIDTH, FRAME_HEIGHT, 4)
registered = Frame(FRAME_WIDTH, FRAME_HEIGHT, 4)
while True:
frames = listener.waitForNewFrame()
output_text.update()
if enable_rgb:
color = frames["color"]
if enable_depth:
ir = frames["ir"]
depth = frames["depth"]
if enable_depth:
depth = depth.asarray()
depth = util.segment_hand(depth)
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)
# Optional parameters for registration
# set True if needed
need_bigdepth = False
need_color_depth_map = False
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
while True:
frames = listener.waitForNewFrame()
color = frames["color"]
def find_and_track_kinect(name, tracker = "CSRT",
min_range = 0, max_range = 1700,
face_target_box = DEFAULT_FACE_TARGET_BOX,
res = (RGB_W, RGB_H),
video_out = True, debug = True):
known_faces = {}
for person in faces:
image = face_recognition.load_image_file(faces[person])
print(person)
face_encoding_list = face_recognition.face_encodings(image)
if len(face_encoding_list) > 0:
known_faces[person] = face_encoding_list[0]
else:
print("\t Could not find face for person...")
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline = pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Depth)
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
bigdepth = Frame(1920, 1082, 4)
trackerObj = None
face_count = 5
face_process_frame = True
bbox = None
face_bbox = None
track_bbox = None
while True:
timer = cv2.getTickCount()
person_found = False
frames = listener.waitForNewFrame()
color = frames["color"]
depth = frames["depth"]
registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth)
bd = np.resize(bigdepth.asarray(np.float32), (1080, 1920))
c = cv2.cvtColor(color.asarray(), cv2.COLOR_RGB2BGR)
__clean_color(c, bd, min_range, max_range)
if face_process_frame:
small_c = __crop_frame(c, face_target_box)
face_locations = face_recognition.face_locations(small_c, model="cnn")
face_encodings = face_recognition.face_encodings(small_c, face_locations)
for face_encoding in face_encodings:
matches = face_recognition.compare_faces(
[known_faces[name]], face_encoding, 0.6)
if len(matches) > 0 and matches[0]:
person_found = True
face_count += 1
(top, right, bottom, left) = face_locations[0]
left += face_target_box[0]
top += face_target_box[1]
right += face_target_box[0]
bottom += face_target_box[1]
face_bbox = (left, top, right, bottom)
person_found = True
break
face_process_frame = not face_process_frame
overlap_pct = 0
track_area = __bbox_area(track_bbox)
if track_area > 0 and face_bbox:
overlap_area = __bbox_overlap(face_bbox, track_bbox)
overlap_pct = min(overlap_area / __bbox_area(face_bbox),
overlap_area / __bbox_area(track_bbox))
if person_found and face_count >= FACE_COUNT and overlap_pct < CORRECTION_THRESHOLD:
bbox = (face_bbox[0],
face_bbox[1],
face_bbox[2] - face_bbox[0],
face_bbox[3] - face_bbox[1])
trackerObj = __init_tracker(c, bbox, tracker)
face_count = 0
status = False
if trackerObj is not None:
status, trackerBBox = trackerObj.update(c)
bbox = (int(trackerBBox[0]),
int(trackerBBox[1]),
int(trackerBBox[0] + trackerBBox[2]),
int(trackerBBox[1] + trackerBBox[3]))
if bbox is not None:
track_bbox = bbox
fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer)
w = 0
h = 0
if status:
w = track_bbox[0] + int((track_bbox[2] - track_bbox[0])/2)
h = track_bbox[1] + int((track_bbox[3] - track_bbox[1])/2)
if (w < res[0] and w >= 0 and h < res[1] and h >= 0):
distanceAtCenter = bd[h][w]
__update_individual_position("NONE", track_bbox, distanceAtCenter, res)
if video_out:
cv2.line(c, (w, 0),
(w, int(res[1])), (0,255,0), 1)
cv2.line(c, (0, h), \
(int(res[0]), h), (0,255,0), 1)
__draw_bbox(True, c, face_target_box, (255, 0, 0), "FACE_TARGET")
__draw_bbox(status, c, track_bbox, (0, 255, 0), tracker)
__draw_bbox(person_found, c, face_bbox, (0, 0, 255), name)
c = __scale_frame(c, scale_factor = 0.5)
cv2.putText(c, "FPS : " + str(int(fps)), (100,50),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,0,255), 1)
if not status:
failedTrackers = "FAILED: "
failedTrackers += tracker + " "
cv2.putText(c, failedTrackers, (100, 80),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,142), 1)
cv2.imshow("color", c)
listener.release(frames)
key = cv2.waitKey(1) & 0xff
if key == ord('q'):
break
device.stop()
device.close()
class 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
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
h, w = 512, 424
FOVX = 1.232202 #horizontal FOV in radians
focal_x = device.getIrCameraParams().fx #focal length x
focal_y = device.getIrCameraParams().fy #focal length y
principal_x = device.getIrCameraParams().cx #principal point x
principal_y = device.getIrCameraParams().cy #principal point y
undistorted = Frame(h, w, 4)
registered = Frame(h, w, 4)
while True:
frames = listener.waitForNewFrame()
depth_frame = frames["depth"]
color = frames["color"]
registration.apply(color, depth_frame, undistorted, registered)
#convert image
color = registered.asarray(np.uint8)
color = cv2.flip(color, 1)
img = depth_frame.asarray(np.float32) / 4500.
imgray = np.uint8(depth_frame.asarray(np.float32) / 255.0)
#flip images
img = cv2.flip(img, 1)
class Tracker:
# X_POSITION = -0.11387496
# Z_POSITION = 1.3
X_POSITION = -0.185
Z_POSITION = 1.5
greenLower = (29, 86, 6)
greenUpper = (64, 255, 255)
greenLower = (27, 100, 50)
greenUpper = (60, 255, 255)
def __init__(self, use_kinect=False, basket=False, gui=True):
self.connected = False
self.max_radius = 100000
self.running = False
self.killed = False
self.gui = gui
self.basket = basket
self.use_kinect = use_kinect
# self.fig, self.ax = plt.subplots()
if self.use_kinect:
self.px = []
self.py = []
logger = createConsoleLogger(LoggerLevel.Error)
setGlobalLogger(logger)
while not self.connected:
# Bad, but needed
if self.use_kinect:
self.connect_kinect()
else:
self.connect_camera()
self.thread = threading.Thread(target=self.video_thread)
self.thread.start()
def connect_camera(self):
# Bad, but needed
self.camera = cv2.VideoCapture(1)
if not self.camera.isOpened():
print("Unable to connect to the camera, check again")
time.sleep(5)
else:
self.connected = True
def connect_kinect(self):
# Bad, but needed
self.fn = Freenect2()
num_devices = self.fn.enumerateDevices()
if num_devices == 0:
print("Kinect not found, check again")
self.connected = False
return
try:
self.pipeline = OpenGLPacketPipeline()
except:
self.pipeline = CpuPacketPipeline()
serial = self.fn.getDeviceSerialNumber(0)
self.device = self.fn.openDevice(serial, pipeline=self.pipeline)
self.listener = SyncMultiFrameListener(
FrameType.Color | FrameType.Ir | FrameType.Depth)
# Register listeners
self.device.setColorFrameListener(self.listener)
self.device.setIrAndDepthFrameListener(self.listener)
self.device.start()
print("started")
# NOTE: must be called after device.start()
self.registration = Registration(self.device.getIrCameraParams(),
self.device.getColorCameraParams())
print("registration")
self.undistorted = Frame(512, 424, 4)
self.registered = Frame(512, 424, 4)
self.connected = True
def video_thread(self):
print('Camera thread started')
need_bigdepth = False
need_color_depth_map = False
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
while not self.killed:
while self.running:
# (grabbed, frame) = self.camera.read()
frames = self.listener.waitForNewFrame()
frame = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
self.registration.apply(frame, depth, self.undistorted, self.registered,
bigdepth=bigdepth,
color_depth_map=color_depth_map)
# print(frame.width, frame.height, frame.bytes_per_pixel)
# print(ir.width, ir.height, ir.bytes_per_pixel)
# print(depth.width, depth.height, depth.bytes_per_pixel)
# print(frame.asarray().shape, ir.asarray().shape, depth.asarray().shape)
# color_image_array = frame.asarray()
color_image_array = self.registered.asarray(np.uint8)
hsv = cv2.cvtColor(color_image_array, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.greenLower, self.greenUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]),
int(M["m01"] / M["m00"]))
if radius > 10:
cv2.circle(color_image_array, (int(x), int(y)), int(radius),
(0, 255, 255), 2)
cv2.circle(color_image_array, center, 5, (0, 0, 255), -1)
x, y, z = self.registration.getPointXYZ(self.undistorted, y, x)
# print(x, y, z, end='\r')
if not math.isnan(y) and not math.isnan(z):
self.px.append(y)
self.py.append(z)
row = tuple(c[c[:, :, 1].argmin()][0])[1]
self.max_radius = min(self.max_radius, row)
if self.gui:
cv2.imshow("Frame", color_image_array)
cv2.imshow("Masked", mask)
self.listener.release(frames)
cv2.waitKey(1) & 0xFF
# self.ax.plot(self.px)
# self.ax.plot(self.py)
# plt.pause(0.01)
cv2.destroyAllWindows()
print("exit looping")
def start(self):
self.max_radius = 100000
if self.use_kinect:
self.px = []
self.py = []
self.running = True
def stop(self):
self.running = False
def kill(self):
self.killed = True
if self.use_kinect:
self.device.stop()
self.device.close()
def get_reward(self):
if self.use_kinect:
if not self.basket:
try:
peaks = peakutils.indexes(self.px)
except Exception as e:
print(e)
peaks = np.array([])
if peaks.any():
# for xx in peaks:
# t.ax.plot(xx, self.px[xx], 'ro')
return self.py[peaks[0]] * 100
else:
return 0
else:
print(np.mean(self.px), np.mean(self.py))
dist = distance(np.array([self.px, self.py]).T, self.X_POSITION, self.Z_POSITION)
print("distance", dist, len(self.px))
return dist * 100
else:
return self.max_radius
def 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 __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)