def _identifyDevice(self):
try:
global freenect
import freenect
self.a = freenect.init()
if freenect.num_devices(self.a) == 0:
kinect = False
elif freenect.num_devices(self.a) > 1:
self._initError('Multiple Kinect1s attached. Unsure how to handle')
else:
kinect = True
except ImportError:
kinect = False
try:
global FN2
import pylibfreenect2 as FN2
if FN2.Freenect2().enumerateDevices() == 1:
kinect2 = True
elif FN2.Freenect2().enumerateDevices() > 1:
self._initError('Multiple Kinect2s attached. Unsure how to handle')
else:
kinect2 = False
except ImportError:
kinect2 = False
if kinect and kinect2:
self._initError('Kinect1 and Kinect2 attached. Unsure how to handle')
elif not kinect and not kinect2:
self._initError('No depth sensor attached')
elif kinect:
self.device = 'kinect'
else:
self.device = 'kinect2'
def run(self):
try:
ctx = freenect.init()
for index in xrange(freenect.num_devices(ctx)):
self.devs.append(freenect.open_device(ctx, index))
for device_num, dev in enumerate(self.devs):
for stream_type in ('video', 'depth'):
self.producers[device_num, stream_type] = \
KinectProducer(dev, device_num, stream_type, self)
self.initialized.set()
while self.keep_running:
while not self.command_q.empty():
self.command_q.get()()
if self._should_runloop():
freenect.base_runloop(ctx, self._body)
else:
self.command_q.get()()
finally:
with self.lock:
self.keep_running = False
for producer in self.producers.itervalues():
producer.stop()
self.producers = {}
freenect.shutdown(ctx)
self.devs = []
self.initialized.set()
def start():
ser = serial.Serial(
'/dev/ttyUSB0') #initialization of serial communication
global test_cases, ser, global_depth_map, DOOR_FLAG, DOOR_COUNT
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
test_cases = [True, True, True]
while True:
global_depth_map = get_depth() #returns the depth frame
contoursright = contours_return(global_depth_map, -10)
contoursleft = contours_return(global_depth_map, 10)
door_detection(contoursright, contoursleft, test_cases)
if DOOR_FLAG:
door_movement(global_depth_map)
else:
regular_movement(global_depth_map)
cv2.imshow('final', global_depth_map)
if cv2.waitKey(1) != -1:
ser.write('\x35')
ser.close()
break
cv2.destroyAllWindows()
def start():
global XAXIS
global YAXIS
global TEST_CASES
global DIRECTION
global GLOBAL_DEPTH_MAP
plt.ion()
plt.figure()
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
DIRECTION = 0
for i in xrange(5):
initial_map = get_depth()
while True:
GLOBAL_DEPTH_MAP = get_depth() #returns the depth frame
back_movement(GLOBAL_DEPTH_MAP)
contoursright = contours_return(GLOBAL_DEPTH_MAP, -10)
contoursleft = contours_return(GLOBAL_DEPTH_MAP, 10)
door_detection(contoursright, contoursleft)
if DOOR_FLAG:
door_movement()
else: regular_movement()
cv2.imshow('final', GLOBAL_DEPTH_MAP)
if cv2.waitKey(1) != -1:
SERIALDATA.write('\x35')
SERIALDATA.close()
break
cv2.destroyAllWindows()
def start():
global XAXIS
global YAXIS
global TEST_CASES
global DIRECTION
global GLOBAL_DEPTH_MAP
plt.ion()
plt.figure()
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
DIRECTION = 0
for i in xrange(5):
initial_map = get_depth()
while True:
GLOBAL_DEPTH_MAP = get_depth() #returns the depth frame
back_movement(GLOBAL_DEPTH_MAP)
contoursright = contours_return(GLOBAL_DEPTH_MAP, -10)
contoursleft = contours_return(GLOBAL_DEPTH_MAP, 10)
door_detection(contoursright, contoursleft)
if DOOR_FLAG:
door_movement()
else:
regular_movement()
cv2.imshow('final', GLOBAL_DEPTH_MAP)
if cv2.waitKey(1) != -1:
SERIALDATA.write('\x35')
SERIALDATA.close()
break
cv2.destroyAllWindows()
def start():
ser = serial.Serial('/dev/ttyUSB0') #initialization of serial communication
global test_cases, ser, global_depth_map, DOOR_FLAG, DOOR_COUNT
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
test_cases = [True, True, True]
while True:
global_depth_map = get_depth() #returns the depth frame
contoursright = contours_return(global_depth_map, -10)
contoursleft = contours_return(global_depth_map, 10)
door_detection(contoursright, contoursleft, test_cases)
if DOOR_FLAG:
door_movement(global_depth_map)
else: regular_movement(global_depth_map)
cv2.imshow('final', global_depth_map)
if cv2.waitKey(1) != -1:
ser.write('\x35')
ser.close()
break
cv2.destroyAllWindows()
def initkinect():
#set up kinect camera
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
if not dev:
freenect.error_open_device()
return [ctx, dev]
print('kinect setup complete')
def _identifyDevice(self):
try:
global freenect
import freenect
self.a = freenect.init()
if freenect.num_devices(self.a) == 0:
kinect = False
elif freenect.num_devices(self.a) > 1:
self._initError(
'Multiple Kinect1s attached. Unsure how to handle')
else:
kinect = True
except ImportError:
kinect = False
try:
global rs
import pyrealsense2 as rs
ctx = rs.context()
if len(ctx.devices) == 0:
realsense = False
if len(ctx.devices) > 1:
self._initError(
'Multiple RealSense devices attached. Unsure how to handle'
)
else:
realsense = True
except ImportError:
realsense = False
if kinect and realsense:
self._initError(
'Kinect1 and RealSense devices attached. Unsure how to handle')
elif not kinect and not realsense:
self._initError('No depth sensor attached')
elif kinect:
self.device = 'kinect'
else:
self.device = 'realsense'
def depth_view():
import matplotlib.pyplot as plt
fn_ctx = fn.init()
fn_dev = fn.open_device(fn_ctx, fn.num_devices(fn_ctx) - 1)
fn.set_tilt_degs(fn_dev, 0)
fn.close_device(fn_dev)
x = np.arange(0, 256, 1)
zeros = np.zeros_like(x)
fig = plt.figure()
ax = fig.add_subplot(111)
view1, = ax.plot(x, zeros, '-k', label='black')
view2, = ax.plot(x, zeros, '-r', label='red')
np_array = np.array
np_max = np.max
view1_sety = view1.set_ydata
view2_sety = view2.set_ydata
ax_relim = ax.relim
ax_autoscale_view = ax.autoscale_view
while True:
dep = get_depth(False)
cv2.imshow('raw', dep)
dep = cv2.medianBlur(dep, 3)
bin = __get_bins(dep)
clean = copy(bin)
clean = __pipeline(clean)
bin[:2] = 0
clean *= np_max(bin)
view1_sety(bin)
view2_sety(clean)
ax_relim()
ax_autoscale_view()
im = fig2img(fig)
graph = np_array(im)
# dep = remove_background(dep, 100)
dep = isolate_depths(dep)
# dep = convert_to_bw(dep)
cv2.imshow('depth', dep)
cv2.imshow('graph', graph)
# show_image('all', frame, masked_frame, depth, video)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
break
def temp_test():
fn_ctx = fn.init()
fn_dev = fn.open_device(fn_ctx, fn.num_devices(fn_ctx) - 1)
fn.set_tilt_degs(fn_dev, 0)
fn.close_device(fn_dev)
while True:
dep = get_depth()
dep *= (dep * 1.3).astype(np.uint8)
print("{}\t,\t{}".format(np.min(dep), np.max(dep)))
cv2.imshow('depth', dep)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
fn.sync_stop()
break
def keyPressEvent(self, ev):
global paused, depth_arr
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
# SPACEBAR to 'pause' and 'unpause'
if (ev.key() == 32 and paused):
t.start(10)
paused = False
elif (ev.key() == 32):
t.stop()
paused = True
# 'S' key to save curr frame to .pcd
elif (ev.key() == 83):
background = AsyncWrite(depth_arr)
background.start()
def set_tilt():
TILT_MAX = 30
TILT_STEP = 10
TILT_START = 0
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
if not dev:
freenect.error_open_device()
print "Starting TILT Cycle"
for tilt in xrange(TILT_START, TILT_MAX + TILT_STEP, TILT_STEP):
print "Setting TILT: ", tilt
freenect.set_led(dev, 6)
freenect.set_tilt_degs(dev, tilt)
time.sleep(3)
freenect.set_tilt_degs(dev, 0)
def density_plot():
fn_ctx = fn.init()
fn_dev = fn.open_device(fn_ctx, fn.num_devices(fn_ctx) - 1)
fn.set_tilt_degs(fn_dev, 0)
fn.close_device(fn_dev)
show_image = cv2.imshow
waitkey = cv2.waitKey
ravel = np.ravel
countbin = np.bincount
length = 256
nums = np.arange(0, length, 1)
zero = np.zeros_like(nums)
import matplotlib.pyplot as plt
import matplotlib.animation as animation
fig, ax = plt.subplots()
line, = ax.plot(nums, zero)
ax.set_ylim(0, 10000)
ax.set_xlim(0, 256)
set_y_data = line.set_ydata
def update(data):
set_y_data(data)
return line,
def get_dep():
dep = get_depth()
dep = cv2.medianBlur(dep, 3, dep)
dep = ravel(dep)
# dep = medfilt(dep, 21).astype(np.uint8)
return dep
def data_gen():
while True:
yield countbin(get_dep(), minlength=length)
ani = animation.FuncAnimation(fig, update, data_gen)
plt.show()
cv2.destroyAllWindows()
fn.sync_stop()
def num_devices():
return fn.num_devices(get_ctx())
def is_kinect_available():
context = freenect.init()
return freenect.num_devices(context) > 0
def main():
rospy.init_node("camera")
rospy.loginfo("Starting Camera Node")
rospy.on_shutdown(shutdown_hook)
# Set initial camera angle
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
r = rospy.Rate(PUBLISH_RATE)
cv_bridge = CvBridge()
video_pub = rospy.Publisher('/camera/video', Image, queue_size=10)
depth_pub = rospy.Publisher('/camera/depth', Image, queue_size=10)
# Uses a different encoding for april tags
camera_image_pub = rospy.Publisher('/camera/image_raw',
Image,
queue_size=10)
camera_info_pub = rospy.Publisher('/camera/camera_info',
CameraInfo,
queue_size=10)
# Kinect manually calibrated using this http://people.csail.mit.edu/peterkty/teaching/Lab4Handout_Fall_2016.pdf
camera_info = CameraInfo()
hd = Header()
camera_info.height = 480
camera_info.width = 640
camera_info.distortion_model = "plumb_bob"
camera_info.D = [
0.16966890693679473, -0.32564392755677646, 0.0014273722857157428,
-0.0007780067287402459, 0.0
]
camera_info.K = [
522.7790149706918, 0.0, 317.5941836796907, 0.0, 523.5195539902463,
255.18973237498545, 0.0, 0.0, 1.0
]
camera_info.P = [
532.5130615234375, 0.0, 317.01325625466416, 0.0, 0.0, 535.176025390625,
255.7121671461573, 0.0, 0.0, 0.0, 1.0, 0.0
]
camera_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
while not rospy.is_shutdown():
video = get_video()
depth = get_depth()
video_msg = cv_bridge.cv2_to_imgmsg(video, encoding="passthrough")
depth_msg = cv_bridge.cv2_to_imgmsg(depth, encoding="passthrough")
cam_img_msg = cv_bridge.cv2_to_imgmsg(video, encoding="bgr8")
camera_info.header = video_msg.header
video_pub.publish(video_msg)
depth_pub.publish(depth_msg)
camera_image_pub.publish(cam_img_msg)
camera_info_pub.publish(camera_info)
r.sleep()
# -*- coding: utf-8 -*-
# @Time : 2018/1/8 10:55
# @Author : play4fun
# @File : multiple_kinects.py
# @Software: PyCharm
"""
multiple_kinects.py:
"""
import freenect
context = freenect.init()
num_devices = freenect.num_devices(context)
device1 = freenect.open_device(context, 0)
device2 = freenect.open_device(context, 1)
print('device1', device1)
print('device2', device2)
while True:
depth_frames = [freenect.sync_get_depth(i) for i in range(num_devices)]
video_frames = [freenect.sync_get_video(i) for i in range(num_devices)]
freenect.sync_stop()
# To run it asynchronously, you'll need to open the devices manually and call freenect.runloop() from multiple threads.
#!/usr/bin/env python
import freenect
import time
import sys
TILT_MAX = 20
TILT_STEP = 02
TILT_START = -35
if len(sys.argv) >= 2:
if sys.argv[1]: TILT_MAX = int(sys.argv[1])
if sys.argv[2]: TILT_STEP = int(sys.argv[2])
if sys.argv[2]: TILT_START = int(sys.argv[3])
ctx = freenect.init()
print "Number of kinects: %d\n" % freenect.num_devices(ctx)
dev = []
for devIdx in range(0, freenect.num_devices(ctx)):
print "opening device %d" % devIdx
dev.append(freenect.open_device(ctx, devIdx))
if not dev:
freenect.error_open_device()
print "Starting TILT Cycle"
for tilt in xrange(TILT_START, TILT_MAX + TILT_STEP, TILT_STEP):
for sensor in dev:
print "Setting TILT: ", tilt
freenect.set_tilt_degs(sensor, tilt)
time.sleep(1)
print "Starting TILT Cycle"
time.sleep(0.1)
def back_movement(z_back):
if z_back[0:479, 200:439].mean() > 200 or z_back[
0:479, 0:199].mean() > 200 or z_back[0:479, 440:639].mean() > 200:
ser.write('\x50')
time.sleep(3)
plt.ion()
plt.figure()
a = []
b = []
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
test_cases = [True, True, True]
#for i in xrange(5):
# z = get_depth()
#left_mean = z[0:479,0:319].mean()
#right_mean = z[0:479,320:639].mean()
#if left_mean > right_mean:
# wall = 1
#else: wall = 0
for i in xrange(5):
zp = get_depth()
wall = 0
while True:
def main_vision(load):
# inits
fn_ctx = fn.init()
fn_dev = fn.open_device(fn_ctx, fn.num_devices(fn_ctx) - 1)
fn.set_tilt_degs(fn_dev, 0)
fn.close_device(fn_dev)
key_point = KeyPoints(150)
predictor = prediction(ModelPath)
preds = []
# optimization
t0 = 0.0
t1 = 0.0
fps = 0.0
total_fps = 0.0
frames = 0
kp_speed = key_point._get_kp_speedup()
draw_speed = key_point._get_draw_speedup()
proc_speed = key_point._get_process_speedup()
cvtColor = cv2.cvtColor
BGR2RGB = cv2.COLOR_BGR2RGB
get_kp = key_point.get_key_points
draw_kp = key_point.draw_key_points
process_image = key_point.__process_image
show_image = cv2.imshow
wait_for_key = cv2.waitKey
copy_thing = copy.copy
num_features = key_point.get_num_features()
arr_shape = np.shape
shape_check = (num_features, 32)
ravel = np.ravel
append_pred = preds.append
get_time = time.time
current_class = 0
if load:
brain = predictor.load_brain()
pred_speed = predictor.get_pred_speed()
predict = predictor.predict
else:
add_speed = predictor.get_add_speed()
add_data = predictor.add_data
get_length = predictor.get_data_length
if load:
net = Neural_Net(predictor.brain.getPoint(), np.vstack(predictor.brain.getData()), 4800 * 2, num_features)
nl_predict = net.predict
nl_speed = net.get_neural_speed()
# mainLoop
while True:
t0 = get_time()
# Get a fresh frame
depth = get_depth()
frame = get_video()
show_image('Raw Image', cvtColor(frame, BGR2RGB))
# Process Depth Image
# depth = remove_background(depth, 25)
depth = remove_background_percent(depth, .5, 50)
depth = convert_to_bw(depth)
mask = make_mask(depth)
# Process Image
frame = cvtColor(frame, BGR2RGB)
video = copy_thing(frame)
frame = process_image(frame, proc_speed)
# Make Masked Frame
masked_frame = copy_thing(frame)
masked_frame[mask] = 0
# Process Key Points
kp, des = get_kp(masked_frame, kp_speed)
video = draw_kp(video, kp, True, speedup=draw_speed)
# Predict current
if (load) and (des is not None) and (arr_shape(des) == shape_check):
pred = predict(ravel(des), pred_speed)
append_pred(pred)
print(pred)
print(nl_predict([ravel(des)], nl_speed))
# Add object description to data set
if (not load) and (des is not None) and (arr_shape(des) == shape_check):
add_data(add_speed, np.ravel(des), current_class)
print('Current Class and Length:\t%i\t%i' % (get_length(), current_class))
t1 = get_time()
fps = (1 / (t1 - t0))
total_fps += fps
frames += 1
print('%.2f FPS' % fps)
show_image('masked image', masked_frame)
show_image('depth', depth)
show_image('key points', video)
# show_image('all', frame, masked_frame, depth, video)
if wait_for_key(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
if load:
break
print('Current Class: %i\nn : Next Class\nr : Continue Current Class\nq : Quit' % (current_class))
inp = raw_input()
if inp == 'n':
current_class += 1
elif inp == 'q':
break
# print(np.mean(preds))
cv2.destroyAllWindows()
print('Average FPS: %.2f' % (total_fps / frames))
fn.sync_stop()
if not load:
predictor.create_brain()
main_vision(True)
#!/usr/bin/env python
import freenect
import time
import sys
TILT_MAX = 20
TILT_STEP = 02
TILT_START = -35
if len(sys.argv) >= 2:
if sys.argv[1]: TILT_MAX = int(sys.argv[1])
if sys.argv[2]: TILT_STEP = int(sys.argv[2])
if sys.argv[2]: TILT_START = int(sys.argv[3])
ctx = freenect.init()
print "Number of kinects: %d\n" % freenect.num_devices(ctx)
dev = []
for devIdx in range(0, freenect.num_devices(ctx)):
print "opening device %d" % devIdx
dev.append(freenect.open_device(ctx, devIdx))
if not dev:
freenect.error_open_device()
print "Starting TILT Cycle"
for tilt in xrange(TILT_START, TILT_MAX+TILT_STEP, TILT_STEP):
for sensor in dev:
print "Setting TILT: ", tilt
freenect.set_tilt_degs(sensor, tilt)
time.sleep(1)
print "Starting TILT Cycle"
def init():
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 0)
freenect.close_device(dev)
def return_mean(a):
"""
* Function Name: return_mean
* Input: Depth Frame or any other matrix.
* Output: Returns mean of the frame
* Logic: It reduces the noise and calculates the mean of the pixel values in the frame using mean() function
* Example Call: return_mean(a)
"""
median = cv2.medianBlur(a,5)
mediane = cv2.medianBlur(a,5)
rect = mediane[0:479, 0:639]
mean = rect.mean()
return mean
ctx = freenect.init() #initialize freenect
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1) #open Kinect Device
freenect.set_tilt_degs(dev, 30) #Tilts kinect to 30 degrees
time.sleep(1)
freenect.set_tilt_degs(dev, 0) #Tilts kinect to 0 degree
time.sleep(1)
freenect.close_device(dev) #closes Kinect
while(True):
a = get_depth() #gets the depth data from Kinect
mean = return_mean(a) #returns the mean of the depth data
if mean > 240:
ser.write("\x38") #if the front area has more depth than the threshold than the robot will move forward
else:
while(return_mean(get_depth())<242):
