iCub_Python_Lib/YARP_image_processing.py at master · tfietzek/iCub_Python_Lib · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
"""
Created on Thu Apr 13 15:16:32 2018

@author: Torsten Fietzek

library for YARP image processing functions
"""

import time

import numpy as np
import yarp


####################################################
# Initialization of the eye images
def define_eye_imgs():
    '''
    Initialization of both eye images

    return: right_eye_yarp_image    -- yarp image for the right eye image
            right_eye_img_array     -- np array for the right eye image
            left_eye_yarp_image     -- yarp image for the left eye image
            left_eye_img_array      -- np array for the left eye image
    '''
    # Create np array to receive the image and the YARP image wrapped around it
    left_eye_img_array = np.ones((240, 320, 3), np.uint8)
    left_eye_yarp_image = yarp.ImageRgb()
    left_eye_yarp_image.resize(320, 240)

    right_eye_img_array = np.ones((240, 320, 3), np.uint8)
    right_eye_yarp_image = yarp.ImageRgb()
    right_eye_yarp_image.resize(320, 240)

    left_eye_yarp_image.setExternal(
        left_eye_img_array.data, left_eye_img_array.shape[1], left_eye_img_array.shape[0])
    right_eye_yarp_image.setExternal(
        right_eye_img_array.data, right_eye_img_array.shape[1], right_eye_img_array.shape[0])

    return right_eye_yarp_image, right_eye_img_array, left_eye_yarp_image, left_eye_img_array


####################################################
# Initialization of one eye image
def define_eye_img():
    '''
    Initialization of one eye image

    return: right_eye_yarp_image    -- yarp image for the eye image
            right_eye_img_array     -- np array for the eye image
    '''
    # Create np array to receive the image and the YARP image wrapped around it
    eye_img_array = np.ones((240, 320, 3), np.uint8)

    eye_yarp_image = yarp.ImageRgb()
    eye_yarp_image.resize(320, 240)
    eye_yarp_image.setExternal(
        eye_img_array.data, eye_img_array.shape[1], eye_img_array.shape[0])

    return eye_yarp_image, eye_img_array


####################################################
# read and convert robot eye images
def read_robot_eyes(port_right_eye, port_left_eye, right_eye_yarp_image, left_eye_yarp_image, right_eye_img_array, left_eye_img_array):
    '''
    read and convert both robot eye images

    params: port_right_eye          -- input port from the right eye camera
            port_left_eye           -- input port from the left eye camera
            right_eye_yarp_image    -- yarp image for the right eye image
            left_eye_yarp_image     -- yarp image for the left eye image
            right_eye_img_array     -- np array for the right eye image
            left_eye_img_array      -- np array for the left eye image

    return: right_eye_img_array     -- np array now containing the right eye image
            left_eye_img_array      -- np array now containing the left eye image
    '''
    # Read the images from the robot cameras
    port_left_eye.read(left_eye_yarp_image)
    port_left_eye.read(left_eye_yarp_image)
    port_right_eye.read(right_eye_yarp_image)
    port_right_eye.read(right_eye_yarp_image)

    if left_eye_yarp_image.getRawImage().__int__() != left_eye_img_array.__array_interface__['data'][0]:
        print("read() reallocated my left_eye_yarp_image!")
    if right_eye_yarp_image.getRawImage().__int__() != right_eye_img_array.__array_interface__['data'][0]:
        print("read() reallocated my right_eye_yarp_image!")

    return right_eye_img_array, left_eye_img_array


####################################################
# read and convert robot eye image
def read_robot_eye(port_eye, eye_yarp_image, eye_img_array):
    '''
    read and convert one robot eye image

    params: port_eye        -- input port from the eye camera
            eye_yarp_image  -- yarp image for the eye image
            eye_img_array   -- np array for the eye image
    return: eye_img_array   -- np array now containing the eye image
    '''
    # Read the images from the robot camera
    # time_transfer = time.time()
    port_eye.read(eye_yarp_image)
    port_eye.read(eye_yarp_image)
    time.sleep(0.03)

    if eye_yarp_image.getRawImage().__int__() != eye_img_array.__array_interface__['data'][0]:
        print("read() reallocated my yarp_image!")

    # time_transfer = time.time() - time_transfer
    # print('time transfer image from iCub:', round(time_transfer, 4))

    return eye_img_array


####################################################
# show image on screen (iCub simulator)
def show_image(screen_port, image):
    '''
    show image on the screen in the iCub-simulator

    params: screen_port     -- input port of the screen
            image           -- image to be shown on the screen
    '''
    # show the image on the screen
    # time_transfer = time.time()
    screen_port.write(image)
    time.sleep(0.03)
    # time_transfer = time.time() - time_transfer
    # print('time transfer image to screen:', round(time_transfer, 4))


####################################################
# read image from world cam (iCub simulator)
def read_image(scene_cam):
    '''
    retrieve image from the world camera in the iCub-simulator

    return: image   -- return image from the world camera

    '''
    scene_cam['port'].read(scene_cam['y_img'])
    scene_cam['port'].read(scene_cam['y_img'])

    if scene_cam['y_img'].getRawImage().__int__() != scene_cam['np_img'].__array_interface__['data'][0]:
        print("read() reallocated my scene_cam['y_img']!")

    return scene_cam['np_img'].copy()