def __init__(self):

        if not KinGeo.ctx:
            KinGeo.ctx = fn.init()

        self.ctx = fn.init()
        # self.device = fn.open_device(KinGeo.ctx, 1)
        # assert self.device is not None
        self.last_access = 0.  # time of last kinect depth map access
        self._frame_time = 1. / 30.  # float of min time in seconds per frame
        self._depth_arr = None  # holds numpy array of
        self._pc_timestamp = 0.
Beispiel #2
0
    def __init__(self):
        """
        Initializes Kinect handler
        """

        if not KinGeo.ctx:
            KinGeo.ctx = fn.init()

        self.ctx = fn.init()
        # self.device = fn.open_device(KinGeo.ctx, 1)
        # assert self.device is not None
        self.last_access = 0.  # time of last kinect depth map access
        self._frame_time = 1. / KinGeo.default_max_frq  # min time in s per frame
        self._depth_arr = None  # holds numpy array of
        self._pc_timestamp = 0.
        self._points_arr_timestamp = 0.
Beispiel #3
0
    def __init__(self, dummy=False, mirror=True):
        self.__class__._instances.append(weakref.proxy(self))
        self.id = next(self._ids)
        self.resolution = (640, 480)
        self.dummy = dummy
        self.mirror = mirror

        if self.dummy == False:
            print("looking for kinect...")
            self.ctx = freenect.init()
            self.dev = freenect.open_device(self.ctx, self.id)
            print(self.id)
            freenect.close_device(
                self.dev)  # TODO Test if this has to be done!

            self.angle = None
            self.depth = freenect.sync_get_depth(
                index=self.id, format=freenect.DEPTH_MM
            )[0]  # get the first Depth frame already (the first one takes much longer than the following)
            self.filtered_depth = None
            print("kinect initialized")
        else:
            print(
                "dummy mode. get_frame() will return a synthetic depth frame, other functions may not work"
            )
Beispiel #4
0
def loop(processimg):
	if not use_webcam:
		ctx = freenect.init()
		dev = freenect.open_device(ctx, 0)
		freenect.set_tilt_degs(dev, 10)
		freenect.close_device(dev)
		
	
	cv2.namedWindow('processing')
	for k, v in params.iteritems():
		cv2.createTrackbar(k, 'processing', v, 255, onchange(k))
	
	runonce = True
	while runonce:
		#runonce = False
		if imgpath != "":
			img = cv2.imread(imgpath)
		else:
			img = getimg()

		cv2.imshow('processing', cv2.resize(processimg(img), (width, height)))
		char = cv2.waitKey(10)
		if char == 27:
			break
		elif char == ord('p'):
			for k, v in params.iteritems():
				print "%s: %d" % (k, v)
		#freenect.set_tilt_degs(dev, pid)
	cv2.destroyAllWindows()
Beispiel #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()
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()
Beispiel #8
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'
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()
Beispiel #10
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()
Beispiel #11
0
    def __init__(self):
        # hard coded class attributes for KinectV1's native resolution
        self.name = 'kinect_v1'
        self.depth_width = 320
        self.depth_height = 240
        self.color_width = 640
        self.color_height = 480

        self.id = 0
        self.device = None
        self.depth = None
        self.color = None
        logger.warning(
            'Two kernels cannot access the Kinect at the same time. '
            'This will lead to a sudden death of the kernel. '
            'Be sure no other kernel is running before you initialize a KinectV1 object.'
        )

        logger.info("looking for kinect...")
        ctx = freenect.init()
        self.device = freenect.open_device(ctx, self.id)
        print(self.id)
        freenect.close_device(self.device)  # TODO Test if this has to be done!
        # get the first Depth frame already (the first one takes much longer than the following)
        self.depth = self.get_frame()
        logger.info("KinectV1 initialized.")
Beispiel #12
0
	def run(self):
		try:
			self.ctx = freenect.init()
			self.dev = freenect.open_device(self.ctx, 0)

			freenect.set_depth_mode(self.dev, freenect.RESOLUTION_MEDIUM, freenect.DEPTH_11BIT)
			freenect.set_depth_callback(self.dev, self._depth_cb)
			freenect.set_video_mode(self.dev, freenect.RESOLUTION_MEDIUM, freenect.VIDEO_RGB)
			freenect.set_video_callback(self.dev, self._video_cb)

			self.video_started = False
			self.depth_started = False

			while self.keep_running:
				with self.lock:
					if self.led_update is not None:
						freenect.set_led(self.dev, self.led_update)
						self.led_update = None
					self.update_streams()
					if not self.video_started and not self.depth_started:
						self.update_cond.wait()
						continue
					self.update.clear()
					if not self.keep_running:
						break
				freenect.base_runloop(self.ctx, self._body)
		finally:
			with self.lock:
				for k in self.depth_consumers.keys() + self.video_consumers.keys():
					k.put(StreamerDied("The Kinect streamer died"))
				self.depth_consumers = {}
				self.video_consumers = {}
				self.update_streams()
			freenect.close_device(self.dev)
			freenect.shutdown(self.ctx)
def loop(processimg):
    if not use_webcam:
        ctx = freenect.init()
        dev = freenect.open_device(ctx, 0)
        freenect.set_tilt_degs(dev, 10)
        freenect.close_device(dev)

    cv2.namedWindow('processing')
    for k, v in params.iteritems():
        cv2.createTrackbar(k, 'processing', v, 255, onchange(k))

    runonce = True
    while runonce:
        #runonce = False
        if imgpath != "":
            img = cv2.imread(imgpath)
        else:
            img = getimg()

        cv2.imshow('processing', cv2.resize(processimg(img), (width, height)))
        char = cv2.waitKey(10)
        if char == 27:
            break
        elif char == ord('p'):
            for k, v in params.iteritems():
                print "%s: %d" % (k, v)
        #freenect.set_tilt_degs(dev, pid)
    cv2.destroyAllWindows()
Beispiel #14
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')
Beispiel #15
0
    def __init__(self, kinectid=1):
        self.kinectid = kinectid
        self.rgb = None
        self.depth = None
        self.keep_running = True

        # init Kenect
        self.context = freenect.init()
        self.device = freenect.open_device(self.context, self.kinectid)
        self.init = freenect.set_depth_mode(self.device, freenect.RESOLUTION_MEDIUM, freenect.DEPTH_REGISTERED)
Beispiel #16
0
 def start(self, degs=10):
     self.ctx = freenect.init()
     if not self.ctx:
         freenect.error_open_device()
     self.dev = freenect.open_device(self.ctx, 0)
     if not self.dev:
         freenect.error_open_device()
     freenect.set_tilt_degs(self.dev, -degs)
     freenect.set_tilt_degs(self.dev, degs)
     self.intialised == True
     print('kinect Started')
Beispiel #17
0
    def __init__(self, calibration_file='config/calibration.frame', debug=False):
        try:
            self.device = kinect.open_device(kinect.init(), 0)
            kinect.set_tilt_degs(self.device, -90)
            kinect.close_device(self.device)
        except Exception as e:
            logging.error('No Kinect detected')
            self.device = None

        with open(calibration_file) as calibration_file:
            self.calibration_frame = np.load(calibration_file)
        '''
Beispiel #18
0
 def __init__(self, cam=-1):
     # Initialize freenect and get the context
     print 'Initalize kinect'
     context = freenect.init()
     # Open the device and get the device
     print 'Open device'
     self.kinect = freenect.open_device(context, 0)
     # Turn the led off
     print 'Turning the led off'
     freenect.set_led(self.kinect, freenect.LED_OFF)
     # Close the device
     print 'Closing the device'
     freenect.close_device(self.kinect)
Beispiel #19
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
Beispiel #20
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
Beispiel #21
0
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 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() 
Beispiel #23
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()
Beispiel #24
0
def loop(processimg):
	ctx = freenect.init()
	dev = freenect.open_device(ctx, 0)
	freenect.set_tilt_degs(dev, 10)
	freenect.close_device(dev)
	while True:
		#img = processimg(np.array(getimg_webcam()))
		#gotimg, img = c.read()
		img = getimg_irkinect()
		#gotimg = True
		#if not gotimg:
		#	print 'stopped'
		#	break

		cv2.imshow('processing', processimg(img))
		if cv2.waitKey(10) == 27:
			break
	cv2.destroyAllWindows()
Beispiel #25
0
def set_kinect_angle(angle, device_index=0):
    # Clamp angle to [-30, 30]
    angle = min(angle, max(angle, -30), 30)
    print "Setting Kinect angle to", angle

    # We have to stop the synchronous runloop to interact with the device.
    freenect.sync_stop()

    # Open the device
    ctx = freenect.init()
    dev = freenect.open_device(ctx, device_index)

    # Set angle
    freenect.set_tilt_degs(dev, angle)

    # Shutdown context, allowing synchronous runloop to start
    freenect.shutdown(ctx)

    return angle
Beispiel #26
0
    def __init__(self,
                 devno=0,
                 resolution=RESOLUTION_MEDIUM,
                 depth_mode=DEPTH_11BIT,
                 video_mode=VIDEO_RGB):
        self._ctx = freenect.init()
        if not self._ctx:
            raise KinectError("Cannot connect to device.")
        self.devno = devno
        self._dev = freenect.open_device(self._ctx, self.devno)
        if not self._dev:
            freenect.shutdown(self._ctx)
            raise KinectError("Cannot open device.")

        self._depth_callback = None
        self._video_callback = None

        self._resolution = resolution
        self.depth_mode = depth_mode
        self.video_mode = video_mode
Beispiel #27
0
    def __init__(self,
                 devno=0,
                 resolution=RESOLUTION_MEDIUM,
                 depth_mode=DEPTH_11BIT,
                 video_mode=VIDEO_RGB):
        self._ctx = freenect.init()
        if not self._ctx:
            raise KinectError("Cannot connect to device.")
        self.devno = devno
        self._dev = freenect.open_device(self._ctx, self.devno)
        if not self._dev:
            freenect.shutdown(self._ctx)
            raise KinectError("Cannot open device.")

        self._depth_callback = None
        self._video_callback = None

        self._resolution = resolution
        self.depth_mode = depth_mode
        self.video_mode = video_mode
    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'
Beispiel #29
0
    def run(self):
        try:
            self.ctx = freenect.init()
            self.dev = freenect.open_device(self.ctx, 0)

            freenect.set_depth_mode(self.dev, freenect.RESOLUTION_MEDIUM,
                                    freenect.DEPTH_11BIT)
            freenect.set_depth_callback(self.dev, self._depth_cb)
            freenect.set_video_mode(self.dev, freenect.RESOLUTION_MEDIUM,
                                    freenect.VIDEO_RGB)
            freenect.set_video_callback(self.dev, self._video_cb)

            self.video_started = False
            self.depth_started = False

            while self.keep_running:
                with self.lock:
                    if self.led_update is not None:
                        freenect.set_led(self.dev, self.led_update)
                        self.led_update = None
                    self.update_streams()
                    if not self.video_started and not self.depth_started:
                        self.update_cond.wait()
                        continue
                    self.update.clear()
                    if not self.keep_running:
                        break
                freenect.base_runloop(self.ctx, self._body)
        finally:
            with self.lock:
                for k in self.depth_consumers.keys(
                ) + self.video_consumers.keys():
                    k.put(StreamerDied("The Kinect streamer died"))
                self.depth_consumers = {}
                self.video_consumers = {}
                self.update_streams()
            freenect.close_device(self.dev)
            freenect.shutdown(self.ctx)
Beispiel #30
0
#!/usr/bin/env python
import freenect
import cv2
import frame_convert2
import numpy as np

cv2.namedWindow('Depth')
cv2.namedWindow('RGB')
keep_running = True

mdev = freenect.open_device(freenect.init(),0)
freenect.set_depth_mode(mdev,freenect.RESOLUTION_MEDIUM,freenect.DEPTH_REGISTERED)

lower = 0
upper  = 100

def display_depth(dev, data, timestamp):
    global keep_running,lower,upper
    data = 200 * np.logical_and(data > lower, data < upper)
    data = data.astype(np.uint8)
    canny = cv2.Canny(data, 100, 255)
    cv2.imshow('Depth',data)
    cv2.imshow('RGB',canny)
    lower+=10
    upper+=10
    if upper>1900:
	lower = 0
	upper = 100
    if cv2.waitKey(10) == 27:
        keep_running = False
import sys
import math
import os
from cv2 import imwrite
from opencv.cv import *
from opencv import adaptors
from opencv.highgui import *
from time import time
from freenect import sync_get_depth as get_depth, sync_get_video as get_video, init
import numpy as np

from rx_config import *
from timing_stats import *

#initialize the camera
ctx = init()

# Grab an initial frame to get the video size
global depth, rgb
(depth,_), (rgb,_) = get_depth(), get_video()

rgbFrameSize = cvGetSize(rgb)
depthSize = cvGetSize(depth)
dwnFrameSize = cvSize(rgbFrameSize.width / 2, rgbFrameSize.height / 2)
dwnDepthSize = cvSize(depthSize.width / 2, depthSize.height / 2)

print 'rgbSize = %d %d' % (rgbFrameSize.width, rgbFrameSize.height)
print 'depthSize = %d %d' % (depthSize.width, depthSize.height)


# Allocate processing chain image buffers the same size as
Beispiel #32
0
#% 
ipython /home/m/Dropbox/maze/libfreenect-goodies/demo_freenect.py
#% 
import freenect
k=freenect.init()
Beispiel #33
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)
    print(".")
    global i, working
    if not working:
        working = True
        depths[i] = depth
        filtered_depth = np.median(depths, axis=0)
        pointclouds.append(point_cloud(filtered_depth))
        timestamps.append(timestamp)
        i += 1
        i = i % n
        working = False

if __name__ == "__main__":
    parser = OptionParser()

    parser.add_option("-o", "--output", dest="output", help="Output data in .npz format to OUTPUTFILE", metavar="OUTPUTFILE")

    (options, args) = parser.parse_args()

    context = freenect.init()
    device = freenect.open_device(context, 0)
    freenect.set_depth_mode(device, freenect.RESOLUTION_MEDIUM, freenect.DEPTH_MM)

    try:
        print("Capturing...")
        freenect.runloop(dev=device, depth=depth_callback)
    except KeyboardInterrupt:
        if options.output:
            print("Dumping...")
            np.savez_compressed(options.output, tangoPointclouds=pointclouds, tangoPointcloudTimestamps=timestamps)
Beispiel #35
0
def get_ctx():
    global _ctx
    if not _ctx:
        _ctx = fn.init()
    return _ctx
import cv2
import frame_convert2
import numpy as np
from apriltag import apriltag

cv2.namedWindow('Video')
print('Press ESC in window to stop')


def get_video():
    return frame_convert2.video_cv(
        freenect.sync_get_video_with_res(
            resolution=freenect.RESOLUTION_HIGH)[0])


freenect.init()
freenect.sync_get_video_with_res(resolution=freenect.RESOLUTION_HIGH)
freenect.sync_set_autoexposure(False)
freenect.sync_set_whitebalance(False)

detector = apriltag("tag36h11", threads=4, decimate=2.0)

object_points = np.array(
    [[-0.1, -0.1, 0.0], [0.1, -0.1, 0.0], [0.1, 0.1, 0.0], [-0.1, 0.1, 0.0]],
    dtype="double")
camera_matrix = np.array([[1.09194704e+03, 0.00000000e+00, 6.79986322e+02],
                          [0.00000000e+00, 1.09300427e+03, 5.09471427e+02],
                          [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]],
                         dtype="double")

dist_coeffs = np.array(
Beispiel #37
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()
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:
import numpy as np
import time
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import freenect

ctx=freenect.init()
time.sleep(5)
z=freenect.sync_get_depth()[0]
print z
x=[]
y=[]
zi=[]

for i in range(0,479,5):
    for j in range(0,639,5):
        x.append(i)
        y.append(j)
        zi.append(z[i][j])

#print z[480][640]

#x, y, z = zip(*z)
print len(x)
print len(y)
print len(zi)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(x,y,zi)
plt.show()
Beispiel #40
0

def body(dev, ctx):
    global last_time
    global led
    global tilt
    if not keep_running:
        raise freenect.Kill
    if led > 6 and tilt > 30:
        return
    freenect.set_led(dev, led)
    freenect.set_tilt_degs(dev, tilt)
    print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
    time.sleep(1)
    led = led + 1
    tilt = tilt + 5


def handler(signum, frame):
    global keep_running
    keep_running = False


print('Press Ctrl-C to stop')
dev = freenect.open_device(freenect.init(), 0)
while True:
    body(dev, 0)

signal.signal(signal.SIGINT, handler)
#freenect.runloop(body=body)
Beispiel #41
0
def is_kinect_available():
    context = freenect.init()
    return freenect.num_devices(context) > 0
Beispiel #42
0
import numpy as np
import time
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import freenect

ctx = freenect.init()
time.sleep(5)
z = freenect.sync_get_depth()[0]
print z
x = []
y = []
zi = []

for i in range(0, 479, 5):
    for j in range(0, 639, 5):
        x.append(i)
        y.append(j)
        zi.append(z[i][j])

#print z[480][640]

#x, y, z = zip(*z)
print len(x)
print len(y)
print len(zi)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(x, y, zi)
plt.show()
Beispiel #43
0
GREEN = (0,255,0)
RED = (255,0,0)
YELLOW = (255,255,0)

pygame.init() #Initiates pygame
xSize,ySize = 640,480 #Sets size of window
screen = pygame.display.set_mode((xSize,ySize),pygame.RESIZABLE) #creates main surface

#KINECT

depth_raw=None

num_to_track=1000 #Number of points to use to determine where the closest 'thing' is
x_grid,y_grid=np.ogrid[0:xSize,0:ySize]

ctx=freenect.init() #Start up the kinect
dev=freenect.open_device(ctx,freenect.LED_OFF) #Pointer to the device itself used for led handling
freenect.set_led(dev,0) #Turn led off
freenect.close_device(dev) #Release the kinect

#CONTROLLER

r_small=15
r_medium=75
r_large=125

r_central_button=50
z_main_action=50
z_switch_controller=50

huge_motion_limit=40