Exemple #1
0
    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'
Exemple #2
0
    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()
Exemple #5
0
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()
Exemple #7
0
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'
Exemple #9
0
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
Exemple #10
0
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)
Exemple #13
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()
Exemple #14
0
def num_devices():
    return fn.num_devices(get_ctx())
Exemple #15
0
def is_kinect_available():
    context = freenect.init()
    return freenect.num_devices(context) > 0
Exemple #16
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:
Exemple #20
0
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"
Exemple #22
0
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):