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")
class KinectCamera(Camera):
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 getFrames(self):
frames = self.listener.waitForNewFrame()
self.registration.apply(frames['color'], frames['depth'], self.undistorted, self.registered)
frames['registered'] = self.registration
return frames
def stop(self):
self.device.stop()
self.device.close()
class Kinect:
# Kinects's intrinsic parameters based on v2 hardware (estimated).
CameraParams = {
"cx": 254.878,
"cy": 205.395,
"fx": 365.456,
"fy": 365.456,
"k1": 0.0905474,
"k2": -0.26819,
"k3": 0.0950862,
"p1": 0.0,
"p2": 0.0,
}
# Kinect's physical orientation in the real world.
CameraPosition = {
"x": -4, # distance of kinect sensor from wall.
"y":
0, # actual position in meters of kinect sensor relative to the viewport's center.
"z": 0, # height in meters of actual kinect sensor from the floor.
"roll":
0, # angle in degrees of sensor's roll (used for INU input - trig function for this is commented out by default).
"azimuth": 0, # sensor's yaw angle in degrees.
"elevation": 0, # sensor's pitch angle in degrees.
}
DEPTH_SHAPE = (424, 512)
N_DEPTH_PIXELS = DEPTH_SHAPE[0] * DEPTH_SHAPE[1]
COLOR_SHAPE = (360, 640, 3)
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 hasNextFrame(self):
return True
def close(self):
self.device.stop()
self.device.close()
def get_frames(self):
self._update()
return self.frames
def _update(self):
if self.listener.hasNewFrame() or len(self.frames) == 0:
_frames = self.listener.waitForNewFrame()
if self.need_color:
# get the data, then throw out the 4th channel
color = _frames['color'].asarray()[:, :, :3]
h, w, d = self.COLOR_SHAPE
color = cv2.resize(color, (w, h))
self.frames['color'] = color
# print('color:' +str(_frames['color'].timestamp))
if self.need_depth:
depth = _frames['depth'].asarray() / 4500
self.frames['depth'] = depth
# print('depth:' + str(_frames['depth'].timestamp))
if self.need_ir:
ir = _frames['ir'].asarray() / 65535.
self.frames['ir'] = ir
# print('ir:' + str(_frames['ir'].timestamp))
# register the franes if we need to
if self.registration and self.need_color and self.need_color:
c, d, u, r = _frames['color'], _frames[
'depth'], self.undistorted, self.registered
self.registration.apply(c, d, u, r)
self.frames['undistorted'] = self.undistorted.asarray(
np.float32)
self.frames['registered'] = self.registered.asarray(np.uint8)
# we NEED to explicitly deallocate the frames from the listener
self.listener.release(_frames)
def makePointCloud(self, undistorted, registered=None):
# convert depth frame to point cloud
pts = self.depthMatrixToPointCloudPos(undistorted)
# # Kinect sensor real-world orientation compensation.
pts = self.applyCameraMatrixOrientation(pts)
# if supplied with registered color image, figure out color of each point
if registered is not None:
colors = self.registered2points(registered)
else:
# else default to white RBGA
colors = (1.0, 1.0, 1.0, 1.0)
pts = pts.astype(np.float32)
return (pts, colors)
def depthMatrixToPointCloudPos(self, z, scale=1000):
# calculate the real-world xyz vertex coordinates from the raw depth data matrix.
C, R = np.indices(z.shape)
R = np.subtract(R, self.CameraParams['cx'])
R = np.multiply(R, z)
R = np.divide(R, self.CameraParams['fx'] * scale)
C = np.subtract(C, self.CameraParams['cy'])
C = np.multiply(C, z)
C = np.divide(C, self.CameraParams['fy'] * scale)
return np.column_stack((z.ravel() / scale, R.ravel(), -C.ravel()))
def applyCameraMatrixOrientation(self, pt):
# 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.
#rotatePoints(1, 2, CameraPosition['roll']) #rotate on the Y&Z plane # Disabled because most tripods don't roll. If an Inertial Nav Unit is available this could be used)
if self.CameraPosition['elevation'] != 0:
rotatePoints(0, 2,
self.CameraPosition['elevation']) #rotate on the X&Z
if self.CameraPosition['azimuth'] != 0:
rotatePoints(0, 1,
self.CameraPosition['azimuth']) #rotate on the X&Y
# Apply offsets for height and linear position of the sensor (from viewport's center)
if any([
self.CameraPosition['x'], self.CameraPosition['y'],
self.CameraPosition['z']
]):
pt[:] += np.float_([
self.CameraPosition['x'], self.CameraPosition['y'],
self.CameraPosition['z']
])
return pt
def registered2points(self, registered):
# Format the color registration map from an 2D image of BGRA-pixels to a 1D list of RGB pixels
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 colors
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
face_process_frame = True
bbox = None
track_bbox = None
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)
face_bbox = None
new_track_bbox = None
face_locations = face_recognition.face_locations(
c, number_of_times_to_upsample=0, model="cnn")
face_encodings = face_recognition.face_encodings(c, face_locations)
for face_encoding in face_encodings:
matches = face_recognition.compare_faces([known_faces[target]],
face_encoding, 0.6)
if len(matches) > 0 and matches[0]:
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 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 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
os.makedirs(colorDir)
bigdepthDir = os.path.join(outputDir, bigdepthDir)
if not os.path.exists(bigdepthDir):
os.makedirs(bigdepthDir)
for x in range(0, framesToCapture):
frames = listener.waitForNewFrame()
frameNum = "%04d" % (x)
color = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
registration.apply(color, depth, undistorted, registered,
enable_filter=need_enable_filter,
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.imwrite(os.path.join(outputDir, "ir" + frameNum + ".png"), ir.asarray() / 65535.)
cv2.imwrite(os.path.join(outputDir, "depth" + frameNum + ".png"), depth.asarray() / 4500.)
# Convert from RGBX or BGRX to RGB or BGR
colorArray = color.asarray()
print(colorArray.shape)
trimColor = np.zeros( (np.size(colorArray,0), np.size(colorArray,1), 3) )
print(trimColor.shape)
trimColor[:,:,0] = colorArray[:,:,0]
class Kinect():
''' Kinect object, it uses pylibfreenect2 as interface to get the frames.
The original example was taken from the pylibfreenect2 github repository, at:
https://github.com/r9y9/pylibfreenect2/blob/master/examples/selective_streams.py '''
def __init__(self,
enable_rgb,
enable_depth,
need_bigdepth,
need_color_depth_map,
K=None,
D=None):
''' Init method called upon creation of Kinect object '''
# according to the system, it loads the correct pipeline
# and prints a log for the user
try:
from pylibfreenect2 import OpenGLPacketPipeline
self.pipeline = OpenGLPacketPipeline()
except:
try:
from pylibfreenect2 import OpenCLPacketPipeline
self.pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
self.pipeline = CpuPacketPipeline()
rospy.loginfo(color.BOLD + color.YELLOW + '-- PACKET PIPELINE: ' +
str(type(self.pipeline).__name__) + ' --' + color.END)
self.enable_rgb = enable_rgb
self.enable_depth = enable_depth
self.K = K
self.D = D
# creates the freenect2 device
self.fn = Freenect2()
# if no kinects are plugged in the system, it quits
self.num_devices = self.fn.enumerateDevices()
if self.num_devices == 0:
rospy.loginfo(color.BOLD + color.RED +
'-- ERROR: NO DEVICE CONNECTED!! --' + color.END)
sys.exit(1)
# otherwise it gets the first one available
self.serial = self.fn.getDeviceSerialNumber(0)
self.device = self.fn.openDevice(self.serial, pipeline=self.pipeline)
# defines the streams to be acquired according to what the user wants
types = 0
if self.enable_rgb:
types |= FrameType.Color
if self.enable_depth:
types |= (FrameType.Ir | FrameType.Depth)
self.listener = SyncMultiFrameListener(types)
# Register listeners
if self.enable_rgb:
self.device.setColorFrameListener(self.listener)
if self.enable_depth:
self.device.setIrAndDepthFrameListener(self.listener)
if self.enable_rgb and self.enable_depth:
self.device.start()
else:
self.device.startStreams(rgb=self.enable_rgb,
depth=self.enable_depth)
# NOTE: must be called after device.start()
if self.enable_depth:
self.registration = Registration(
self.device.getIrCameraParams(),
self.device.getColorCameraParams())
# last number is bytes per pixel
self.undistorted = Frame(512, 424, 4)
self.registered = Frame(512, 424, 4)
# Optinal parameters for registration
self.need_bigdepth = need_bigdepth
self.need_color_depth_map = need_color_depth_map
if self.need_bigdepth:
self.bigdepth = Frame(1920, 1082, 4)
else:
self.bigdepth = None
if self.need_color_depth_map:
self.color_depth_map = np.zeros((424, 512), np.int32).ravel()
else:
self.color_depth_map = None
def acquire(self):
''' Acquisition method to trigger the Kinect to acquire new frames. '''
# acquires a frame only if it's new
frames = self.listener.waitForNewFrame()
if self.enable_rgb:
self.color = frames["color"]
self.color_new = cv2.resize(self.color.asarray(),
(int(1920 / 1), int(1080 / 1)))
# The image obtained has a fourth dimension which is the alpha value
# thus we have to remove it and take only the first three
self.color_new = self.color_new[:, :, 0:3]
# correct distortion of camera
self.correct_distortion()
if self.enable_depth:
# these only have one dimension, we just need to convert them to arrays
# if we want to perform detection on them
self.depth = frames["depth"]
self.depth_new = cv2.resize(self.depth.asarray() / 4500.,
(int(512 / 1), int(424 / 1)))
self.registration.undistortDepth(self.depth, self.undistorted)
self.undistorted_new = cv2.resize(
self.undistorted.asarray(np.float32) / 4500.,
(int(512 / 1), int(424 / 1)))
self.ir = frames["ir"]
self.ir_new = cv2.resize(self.ir.asarray() / 65535.,
(int(512 / 1), int(424 / 1)))
if self.enable_rgb and self.enable_depth:
self.registration.apply(self.color,
self.depth,
self.undistorted,
self.registered,
bigdepth=self.bigdepth,
color_depth_map=self.color_depth_map)
# RGB + D
self.registered_new = self.registered.asarray(np.uint8)
if self.need_bigdepth:
self.bigdepth_new = cv2.resize(
self.bigdepth.asarray(np.float32),
(int(1920 / 1), int(1082 / 1)))
#cv2.imshow("bigdepth", cv2.resize(self.bigdepth.asarray(np.float32), (int(1920 / 1), int(1082 / 1))))
if self.need_color_depth_map:
#cv2.imshow("color_depth_map", self.color_depth_map.reshape(424, 512))
self.color_depth_map_new = self.color_depth_map.reshape(
424, 512)
# do this anyway to release every acquired frame
self.listener.release(frames)
def stop(self):
''' Stop method to close device upon exiting the program '''
rospy.loginfo(color.BOLD + color.RED + '\n -- CLOSING DEVICE... --' +
color.END)
self.device.stop()
self.device.close()
def correct_distortion(self):
''' Method to correct distortion using camera calibration parameters '''
if self.K is not None and self.D is not None:
h, w = self.color_new.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
self.K, self.D, (w, h), 1, (w, h))
# undistort
self.RGBundistorted = cv2.undistort(self.color_new, self.K, self.D,
None, newcameramtx)
# crop the image
x, y, w, h = roi
self.RGBundistorted = self.RGBundistorted[y:y + h, x:x + w]
self.RGBundistorted = cv2.flip(self.RGBundistorted, 0)
self.RGBundistorted = self.RGBundistorted[0:900, 300:1800]
#self.RGBundistorted = self.RGBundistorted[0:900, 520:1650]
else:
rospy.loginfo(color.BOLD + color.RED +
'-- ERROR: NO CALIBRATION LOADED!! --' + color.END)
self.RGBundistorted = self.color_new
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 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 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()
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)
if (bearing >= 0) and (bearing <= 180):
if ( heading > bearing and heading < (bearing+180)):
print("right")
turnRight()
print (heading)
#time.sleep(0.5)
else:
print("Left")
turnLeft()
print (heading)
#time.sleep(0.5)
moveForward()
frames = listener.waitForNewFrame()
depth = frames["depth"]
registration.apply(depth,registered)
cv2.imshow("depth", depth.asarray() / 4500.)
#Comment the coming lines
print('center value is:',depth.asarray(np.float32).item((212,256)))
print('down1 value is:',depth.asarray(np.float32).item((270,256)))
'''print('up1 value is:',depth.asarray(np.float32).item((150,256)))
print('up2 value is:',depth.asarray(np.float32).item((100,256)))
print('down2 value is:',depth.asarray(np.float32).item((350,256)))
print('right1 value is:',depth.asarray(np.float32).item((212,300)))
print('right2 value is:',depth.asarray(np.float32).item((212,350)))
print('left1 value is:',depth.asarray(np.float32).item((212,200)))
print('left2 value is:',depth.asarray(np.float32).item((212,150)))'''
def publishRGBD(self):
fn = Freenect2()
num_devices = fn.enumerateDevices()
#Exit if device is not found
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=self.pipeline)
types = 0
if self.enable_rgb:
types |= FrameType.Color
if self.enable_depth:
types |= (FrameType.Ir | FrameType.Depth)
listener = SyncMultiFrameListener(types)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
if self.enable_rgb and self.enable_depth:
device.start()
else:
device.startStreams(rgb=self.enable_rgb, depth=self.enable_depth)
# Should be transformed to manually calibrated values.
if self.enable_depth:
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
while not rospy.is_shutdown():
frames = listener.waitForNewFrame()
if self.enable_rgb:
color = frames["color"]
if self.enable_depth:
ir = frames["ir"]
depth = frames["depth"]
if self.enable_rgb and self.enable_depth:
registration.apply(color, depth, undistorted, registered)
elif enable_depth:
registration.undistortDepth(depth, undistorted)
if self.enable_depth:
#cv2.imshow("undistorted", undistorted.asarray(np.float32) / 4500.)
self.publishDepth(undistorted)
if self.enable_rgb and self.enable_depth:
#cv2.imshow("registered", registered.asarray(np.uint8))
self.publishColor(registered)
listener.release(frames)
device.stop()
device.close()
sys.exit(0)
def 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 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()
def run(self):
pipeline = CpuPacketPipeline()
print("Packet pipeline:", type(pipeline).__name__)
enable_rgb = True
enable_depth = True
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
types = 0
if enable_rgb:
types |= FrameType.Color
if enable_depth:
types |= (FrameType.Ir | FrameType.Depth)
listener = SyncMultiFrameListener(types)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
if enable_rgb and enable_depth:
device.start()
else:
device.startStreams(rgb=enable_rgb, depth=enable_depth)
# NOTE: must be called after device.start()
if enable_depth:
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
count = 0
while True:
mutex = Lock()
mutex.acquire()
frames = listener.waitForNewFrame()
if enable_rgb:
color = frames["color"]
if enable_depth:
ir = frames["ir"]
depth = frames["depth"]
if enable_rgb and enable_depth:
registration.apply(color, depth, undistorted, registered)
elif enable_depth:
registration.undistortDepth(depth, undistorted)
if enable_depth:
cv2.imshow("ir", ir.asarray() / 65535.)
nameir = self.makename("ir", count)
#cv2.imwrite(nameir, ir.asarray() / 65535.)
cv2.imshow("depth", depth.asarray() / 4500.)
named = self.makename("depth", count)
#cv2.imwrite(named, depth.asarray() / 4500.)
cv2.imshow("undistorted",
undistorted.asarray(np.float32) / 4500.)
nameu = self.makename("undistorted", count)
#cv2.imwrite(nameu, undistorted.asarray(np.float32) / 4500.)
if enable_rgb:
cv2.imshow(
"color",
cv2.resize(color.asarray(),
(int(1920 / 3), int(1080 / 3))))
namec = self.makename("color", count)
#cv2.imwrite(namec,cv2.resize(color.asarray(),(int(1920 / 3), int(1080 / 3))))
if enable_rgb and enable_depth:
cv2.imshow("registered", registered.asarray(np.uint8))
namer = self.makename("registered", count)
#cv2.imwrite(namer,registered.asarray(np.uint8))
mutex.release()
count = count + 1
listener.release(frames)
key = cv2.waitKey(delay=1)
if key == ord('q'):
break
device.stop()
device.close()
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()
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)
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()
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"]
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))))
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
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)
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:
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)
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")
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"]
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(
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)
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)
imgray = cv2.flip(imgray, 1)
if len(sys.argv) > 1:
if sys.argv[1] == "test" and frame_i < num_frames:
np.save("test_frames/" + str(frame_i) + ".npy",
depth_frame.asarray(np.float32))
frame_i += 1
#begin edge detection
def main(lata, longa, latb, longb):
lata = conv.convert_gps(float(lata))
longa = conv.convert_gps(float(longa))
latb = conv.convert_gps(float(latb))
longb = conv.convert_gps(float(longb))
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 list for gps location of obstacles
###############################################################
# 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
while True:
frames = listener.waitForNewFrame()
#time.sleep(5)
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.)
#time.sleep(2)
cv2.imshow("depth", depth.asarray() / 4500.)
#Comment the coming lines
print('center value is:', depth.asarray(np.float32).item((212, 256)))
print('down1 value is:', depth.asarray(np.float32).item((270, 256)))
'''print('up1 value is:',depth.asarray(np.float32).item((150,256)))
print('up2 value is:',depth.asarray(np.float32).item((100,256)))
print('down2 value is:',depth.asarray(np.float32).item((350,256)))
print('right1 value is:',depth.asarray(np.float32).item((212,300)))
print('right2 value is:',depth.asarray(np.float32).item((212,350)))
print('left1 value is:',depth.asarray(np.float32).item((212,200)))
print('left2 value is:',depth.asarray(np.float32).item((212,150)))'''
x = depth.asarray(np.float32)
y = np.logical_and(np.greater(x, 1200), np.less(x, 1500))
#print(np.extract(y, x))
no_of_pixels = np.count_nonzero(np.extract(y, x))
print(no_of_pixels)
#get gps coordinate at this location########
'''
assumingthegps received is lat,long variables:
obs.append([lat,long])
this is to be retrieved in the path planning code.
'''
if no_of_pixels > 14000: #approx distance from the camera = 1.5 m
print('big Obstacle found, stop!!!')
obs.append(findPosition())
elif no_of_pixels > 8000: #approx distance from the camera = 1.5 m
print('small Obstacle found!!')
obs.append(findPosition())
#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))
listener.release(frames)
#time.sleep(20)
key = cv2.waitKey(delay=1) & 0xFF
if key == ord('q'):
break
#######################################################
target(latb, longb, lata, longa)
#stage3.py main fnc
#######################################################
device.stop()
device.close()
