Ejemplo n.º 1
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"
            )
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()
Ejemplo n.º 3
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()
Ejemplo n.º 6
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.")
Ejemplo n.º 7
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()
Ejemplo n.º 8
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)
Ejemplo n.º 9
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()
Ejemplo n.º 10
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()
Ejemplo n.º 11
0
def get_device(dev_num=0):
    if dev_num in _devices:
        return _devices[dev_num]

    dev = fn.open_device(get_ctx(), dev_num)
    _devices[dev_num] = dev
    return dev
Ejemplo n.º 12
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')
Ejemplo n.º 13
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)
Ejemplo n.º 14
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')
Ejemplo n.º 15
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)
        '''
Ejemplo n.º 16
0
Archivo: bbg.py Proyecto: spawnedc/bbg
 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)
Ejemplo n.º 17
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
Ejemplo n.º 18
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
Ejemplo n.º 19
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)
Ejemplo n.º 20
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() 
Ejemplo n.º 21
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()
Ejemplo n.º 22
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()
Ejemplo n.º 23
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
Ejemplo n.º 24
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
Ejemplo n.º 25
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
Ejemplo n.º 26
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)
# -*- 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.
Ejemplo n.º 28
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)
Ejemplo n.º 29
0
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"
for tilt in xrange(TILT_MAX + TILT_STEP, TILT_START, (TILT_STEP * -1)):
    for sensor in dev:
        print "Setting TILT: ", tilt
        freenect.set_tilt_degs(sensor, tilt)
Ejemplo n.º 30
0
        keep_running = False

def display_rgb(dev, data, timestamp):
    global keep_running
    data = data[:, :, ::-1]  # RGB -> BGR
    cv2.imshow('RGB', data)
    if cv2.waitKey(10) == 27:
        keep_running = False


def body(*args):
    if not keep_running:
        raise freenect.Kill
        cv2.destroyAllWindows()


print('Press ESC in window to stop')

ctx = freenect.init()
devPtr = freenect.open_device(ctx, 0)
#freenect.set_video_mode(devPtr, freenect.VIDEO_IR_8BIT, freenect.RESOLUTION_MEDIUM)
#ir, data = freenect.sync_get_video(format=freenect.VIDEO_IR_8BIT)
#print ir
#print data

freenect.runloop(depth=display_depth,
                 video=display_rgb,
                 body=body,
                 dev=devPtr
                 )
Ejemplo n.º 31
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
Ejemplo n.º 32
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
active_area_limit=50
Ejemplo n.º 33
0
import freenect
import sys

ctx = freenect.init()
dev = freenect.open_device(ctx, 0)
freenect.set_tilt_degs(dev, float(sys.argv[1]))
Ejemplo n.º 34
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()
Ejemplo n.º 35
0
#!/usr/bin/python2.7
import freenect
import cv2
import numpy as np
import time
from usb.core import find as finddev

dev1 = finddev(idVendor=0x045e, idProduct=0x02ae)
dev2 = finddev(idVendor=0x045e, idProduct=0x02b0)
dev3 = finddev(idVendor=0x045e, idProduct=0x02ad)
dev1.reset()
dev2.reset()
ctx = freenect.init()
mdev = freenect.open_device(ctx, 0)
freenect.shutdown(ctx)
print(ctx)
print(mdev)
freenect.sync_stop()


def applyCustomColorMap(im_gray):

    lut = np.zeros((256, 1, 3), dtype=np.uint8)

    lut[:, 0, 0] = [
        0, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 0, 50,
        50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 0, 50, 50, 50,
        50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 0, 50, 50, 50, 50, 50,
        50, 50, 50, 50, 50, 50, 50, 52, 49, 51, 0, 48, 48, 48, 46, 47, 47, 43,
        45, 45, 45, 44, 44, 43, 43, 43, 0, 46, 45, 46, 45, 50, 50, 50, 51, 55,
        56, 56, 56, 62, 61, 61, 0, 61, 68, 67, 67, 68, 73, 73, 73, 73, 78, 79,
        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:
Ejemplo n.º 37
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)
Ejemplo n.º 38
0
def doloop():

    #Series of commands to do pointer operations on the kinect (motor, led, accelerometer)
    ctx = init()  #Initiates device
    mdev = open_device(ctx, 0)  #Opens the device for commands
    set_led(mdev, 1)  #Sets LED to green
    close_device(
        mdev)  #Closes device. Device must be closed immediately after usage

    #Mean filter caches
    yList = [0, 0, 0, 0, 0, 0]
    xList = [0, 0, 0, 0, 0, 0]

    #Sets color tuples
    RED = (255, 0, 0)
    BLUE = (0, 0, 255)
    TEAL = (0, 200, 100)
    BLACK = (0, 0, 0)

    #Sets the size of the screen
    xSize = 640
    ySize = 480

    done = False  #Main while loop bool counter
    pygame.init()  #Initiates pygame
    screen = pygame.display.set_mode(
        (xSize, ySize), pygame.RESIZABLE)  #Creates the pygame window
    screen.fill(BLACK)  #Fills the window black

    #Initiates the xTempPos and yTempPos values so that the point will remain stationary
    #if the minimum value is larger than 600
    xTempPos = xSize / 2
    yTempPos = ySize / 2

    global depth, rgb  #Makes the depth and rgb variables global

    while not done:
        screen.fill(BLACK)  #Makes the pygame window black after each iteration

        # Get a fresh frame
        (depth, _) = get_depth()
        (rgb, _) = get_video()

        minVal = np.min(depth)  #This is the minimum value from the depth image
        minPos = np.argmin(
            depth)  #This is the raw index of the minimum value above
        xPos = np.mod(minPos, xSize)  #This is the x component of the raw index
        yPos = minPos // xSize  #This is the y component of the raw index

        #This is the mean filter process
        """
        A mean filter works by collecting values in a cache list and taking the mean of them
        to determine the final value. It works in this case to decrease the amount of
        volatility the minimum position experiences to get a smoother display with a more
        consistent value. My computer works smoothly with a 5 bit cache where as a faster
        computer may need a larger cache and a slower computer may need a smaller cache
        """
        xList.append(xPos)
        del xList[0]
        xPos = int(mean(xList))
        yList.append(yPos)
        del yList[0]
        yPos = int(mean(yList))
        """
        This if statement says that if the minimum value is below 600 to store the minimum
        positions in xTempPos and yTempPos and to make the dot color red. Also if the minimum
        value is larger than 600, xPos and yPos become the last stored minimum and maximum
        positions. It also changes the color to purple
        """
        if minVal < 600:
            xTempPos = xPos
            yTempPos = yPos
            COLOR = cv.RGB(255, 0, 0)
        else:
            xPos = xTempPos
            yPos = yTempPos
            COLOR = cv.RGB(100, 0, 100)

        #cv.Circle(rgb, (xPos, yPos), 2, COLOR, 40) #draws a circle of a certain color at minimum position

        #cv.ShowImage('Image',rgb) #Shows the image
        cv.WaitKey(5)  #Keyboard interupt
        """
        The if statement below sets up the virtual joystick by basically breaking the pygame
        window into four parts. A dot representing the minimum position is drawn on the window
        and the corresponding button based on the position is "pressed". The quarter of the
        window in which the button "pressed" corresponds to turns teal after being "pressed"

        Top Right   : A
        Bottom Right: B
        Bottom Left : Y
        Top Right   : X
        """
        if xPos <= xSize / 2 and yPos <= ySize / 2:
            command = 'A'
            rect1 = pygame.Rect((xSize / 2, 0), (xSize / 2, ySize / 2))
            pygame.draw.rect(screen, TEAL, rect1)
        elif xPos <= xSize / 2 and yPos > ySize / 2:
            command = 'B'
            rect1 = pygame.Rect((xSize / 2, ySize / 2), (xSize / 2, ySize / 2))
            pygame.draw.rect(screen, TEAL, rect1)
        elif xPos > xSize / 2 and yPos <= ySize / 2:
            command = 'X'
            rect1 = pygame.Rect((0, 0), (xSize / 2, ySize / 2))
            pygame.draw.rect(screen, TEAL, rect1)
        else:
            command = 'Y'
            rect1 = pygame.Rect((0, ySize / 2), (xSize / 2, ySize / 2))
            pygame.draw.rect(screen, TEAL, rect1)
        pygame.draw.line(
            screen, BLUE, (xSize / 2, ySize / 2),
            (xSize - xPos,
             yPos))  #Draws a line from the middle to the minimum position
        pygame.draw.circle(screen, RED, (xSize - xPos, yPos),
                           10)  #Draws the circle on pygame window
        pygame.display.flip()  #Displays the processed pygame window
        print command, minVal  #Prints the "pressed" button and the minimum value
        for e in pygame.event.get():  #Itertates through current events
            if e.type is pygame.QUIT:  #If the close button is pressed, the while loop ends
                done = True
Ejemplo n.º 39
0
 
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"
for tilt in xrange(TILT_MAX+TILT_STEP, TILT_START, (TILT_STEP * -1)):
    for sensor in dev:
        print "Setting TILT: ", tilt
        freenect.set_tilt_degs(sensor, tilt)
Ejemplo n.º 40
0
    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)
Ejemplo n.º 41
0
#!/usr/bin/env python

import freenect
import time

ctx = freenect.init()
device = freenect.open_device(ctx, 0)
freenect.set_tilt_degs(device, 25)
time.sleep(0.8)
freenect.set_tilt_degs(device, 5)
time.sleep(0.8)
freenect.set_tilt_degs(device, 25)
time.sleep(0.8)
freenect.set_tilt_degs(device, 5)
time.sleep(0.8)
freenect.set_tilt_degs(device, 25)
time.sleep(0.8)
freenect.set_tilt_degs(device, 5)
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):
Ejemplo n.º 43
0
def main():
    grip_dir = os.environ['GRIP_DATA']
    global g_model
    g_model = IO.load(os.path.join(grip_dir, 'aam.new.io'))[1]

    global g_predictor, reference_3d, geo_vs, geo_vts, rect
    rect = None
    pred_fn = os.path.join(grip_dir, 'pred.new.io')
    g_predictor = Face.load_predictor(pred_fn)  #, cutOff=15)
    reference_shape = g_predictor['ref_shape']
    size = reference_shape.shape[0]
    geo_vs = np.zeros((size, 3), dtype=np.float32)
    geo_vs[:size, :2] = reference_shape
    geo_vts = np.zeros((size, 2), dtype=np.float32)
    geo_vts[:size] = reference_shape + 0.5
    geo_ts = np.array([[1, 0, 0, 0], [0, 1, 0, 1000], [0, 0, 1, 0]],
                      dtype=np.float32)
    geo_fs = Face.triangulate_2D(reference_shape)
    geo_bs = []
    for p0, p1, p2 in geo_fs:
        geo_bs.append((p0, p1))
        geo_bs.append((p1, p2))
        geo_bs.append((p2, p0))
    reference_3d = np.zeros((reference_shape.shape[0], 3), dtype=np.float32)
    reference_3d[:, :2] = reference_shape * [100, 100]

    img_vs = np.array([[0, 0, 0], [640, 0, 0], [640, 480, 0], [0, 480, 0]],
                      dtype=np.float32)
    img_vts = np.array([[0, 1], [1, 1], [1, 0], [0, 0]], dtype=np.float32)
    img_fs = np.array([[0, 1, 2, 3]], dtype=np.int32)
    img_ts = np.array([[1, 0, 0, 0], [0, 1, 0, 1000], [0, 0, 1, 0]],
                      dtype=np.float32)

    geo_mesh = GLMeshes(names=['geo_mesh'],
                        verts=[geo_vs],
                        faces=[geo_fs],
                        transforms=[geo_ts],
                        bones=[geo_bs],
                        vts=[geo_vts])
    img_mesh = GLMeshes(names=['img_mesh'],
                        verts=[img_vs],
                        faces=[img_fs],
                        transforms=[img_ts],
                        bones=[None],
                        vts=[img_vts])
    kinect = freenect.init()
    tilt, roll = 0, 0

    if 1:
        kdev = freenect.open_device(kinect, 0)
        freenect.set_led(kdev, 0)  # turn off LED
        freenect.set_tilt_degs(kdev, 25)
        kstate = freenect.get_tilt_state(kdev)
        freenect.update_tilt_state(kdev)
        tilt_angle, tilt_status = kstate.tilt_angle, kstate.tilt_status
        ax, ay, az = kstate.accelerometer_x, kstate.accelerometer_y, kstate.accelerometer_z
        #bottom facing down: (85, 743, 369, 52, 0)
        #right side down: (916, 71, 96, 112, 0)
        #front side down: (52, 63, -863, -128, 0)
        freenect.close_device(kdev)
        y_axis = np.array((ax, ay, az), dtype=np.float32)
        y_axis = y_axis / np.linalg.norm(y_axis)
        roll = np.degrees(np.arctan2(ax, ay))
        tilt = -np.degrees(np.arctan2(az, (ax**2 + ay**2)**0.5))

    fovX = 62.0
    pan_tilt_roll = (0, tilt, roll)
    tx_ty_tz = (0, 1000, 6000)
    P = Calibrate.composeP_fromData((fovX, ), (pan_tilt_roll), (tx_ty_tz), 0)

    global g_camera_rays, g_camera_mat
    h, w = 480 // 2, 640 // 2
    coord, pix_coord = make_coords(h, w)
    #P = np.eye(3,4,dtype=np.float32)
    #P[0,0] = P[1,1] = 2.0
    k1, k2 = 0, 0
    g_camera_mat = Calibrate.makeMat(P, (k1, k2), [w, h])
    K, RT, P, ks, T, wh = g_camera_mat
    coord_undist = coord.copy()
    Calibrate.undistort_points_mat(coord.reshape(-1, 2), g_camera_mat,
                                   coord_undist.reshape(-1, 2))
    g_camera_rays = np.dot(coord_undist,
                           RT[:2, :3])  # ray directions (unnormalized)
    g_camera_rays -= np.dot([-K[0, 2], -K[1, 2], K[0, 0]], RT[:3, :3])
    g_camera_rays /= (np.sum(g_camera_rays**2, axis=-1)**0.5).reshape(
        h, w, 1)  # normalized ray directions
    names = ['kinect']
    vs = [np.zeros((h * w, 3), dtype=np.float32)]
    ts = [np.eye(3, 4, dtype=np.float32)]
    vts = [pix_coord * (1.0 / w, 1.0 / h)]
    faces = [make_faces(h, w)]
    mats = None
    geom_mesh = GLMeshes(names=names,
                         verts=vs,
                         faces=faces,
                         transforms=ts,
                         vts=vts)
    layers = {
        'geom_mesh': geom_mesh,
        'geo_mesh': geo_mesh,
        'img_mesh': img_mesh
    }
    QGLViewer.makeViewer(layers=layers,
                         mats=mats,
                         callback=cb,
                         timeRange=(0, 10000))