-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathicub_detYOLO.py
executable file
·160 lines (118 loc) · 4.59 KB
/
icub_detYOLO.py
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
152
153
154
155
156
157
158
159
160
import yarp
import numpy
import cv2 as cv
from ultralytics import YOLO
import numpy as np
import math
from std_msgs.msg import Float64MultiArray
import rospy
model = YOLO("/home/diego/Escritorio/Plataforma-evaluacion-iCub-ELO308/runs/detect/train2/weights/last.pt")
rospy.init_node('icub_node', anonymous=True)
pub = rospy.Publisher('/datos_sensor2', Float64MultiArray, queue_size=10)
yarp.Network.init()
def calcular_diametro_desde_area(area):
radio = math.sqrt(area / math.pi)
diametro = 2 * radio
return diametro
focal_length_x = 215.483
focal_length_y = 214.935
optical_center_x = 174.868
optical_center_y = 105.63
K = np.array([[focal_length_x, 0, optical_center_x],
[0, focal_length_y, optical_center_y],
[0, 0, 1]])
theta = (50)/(math.pi*180)
Rot_m = np.array([[math.cos(theta), 0, math.sin(theta)],
[0, 1, 0],
[-math.sin(theta), 0, math.cos(theta)]])
trans_v = np.array([0.693799, 0.000005, -0.338206])
T = np.array([[math.cos(theta), 0, math.sin(theta), 0.693799],
[0, 1, 0, 0.000005],
[-math.sin(theta), 0, math.cos(theta), -0.338206],
[0, 0, 0, 1]])
T_inverse = np.linalg.inv(T)
# prepare a property object
props = yarp.Property()
props.put("device","remote_controlboard")
props.put("local","/client/head")
props.put("remote","/icubSim/head")
headDriver = yarp.PolyDriver(props)
#query motor control interfaces
iPos = headDriver.viewIPositionControl()
iEnc = headDriver.viewIEncoders()
#retrieve number of joints
jnts=iPos.getAxes()
#retrieve number of joints
jnts=iPos.getAxes()
print('Controlling', jnts, 'joints')
# read encoders
encs=yarp.Vector(jnts)
iEnc.getEncoders(encs.data())
# store as home position
home=yarp.Vector(jnts, encs.data())
#initialize a new tmp vector identical to encs
tmp=yarp.Vector(jnts)
tmp.set(0, tmp.get(0)-40)
iPos.positionMove(tmp.data())
'''
output_port = yarp.Port()
output_port.open("/python-image-port")
yarp.Network.connect("/python-image-port", "/view01")
output_port.write(yarp_image)
'''
# Create a port and connect it to the iCub simulator virtual camera
input_port = yarp.Port()
input_port.open("/python-image-port")
yarp.Network.connect("/icubSim/cam/right/rgbImage:o", "/cam1")
yarp.Network.connect("/icubSim/cam/right/rgbImage:o", "/python-image-port")
img_array = numpy.zeros((240, 320, 3), dtype=numpy.uint8)
yarp_image = yarp.ImageRgb()
yarp_image.resize(320, 240)
yarp_image.setExternal(img_array, img_array.shape[1], img_array.shape[0])
input_port.read(yarp_image)
while(1):
yarp_image = yarp.ImageRgb()
yarp_image.resize(320, 240)
yarp_image.setExternal(img_array, img_array.shape[1], img_array.shape[0])
input_port.read(yarp_image)
img_rgb = cv.cvtColor(img_array, cv.COLOR_BGR2RGB)
#----------YOLO implementation-----------#
results = model.track(img_rgb, conf=0.5, device=0, persist=True)
annotated_frame = results[0].plot()
for result in results:
boxes = result.boxes.cpu().numpy() # get boxes on cpu in numpy
for box in boxes: # iterate boxes
r = box.xyxy[0].astype(int) # get corner points as int
x1 = int(r[0])
y1 = int(r[1])
x2 = int(r[2])
y2 = int(r[3])
# Calcular el centro del bounding box
center_x = int((x1 + x2) / 2)
center_y = int((y1 + y2) / 2)
center_point = (center_x, center_y)
cv.circle(annotated_frame, center_point, 5, (0, 255, 255), -1) # Punto en el centro en rojo
w = x2 - x1
h = y2 - y1
z = (focal_length_x * 0.07) / w
y = ((center_x-optical_center_x)*z)/focal_length_x
x = ((center_y-optical_center_y)*z)/focal_length_y
object_coords = (x, y, z)
#coords_hom_robot = np.array([x, y, z, 1])
coords_hom_robot = np.array([x, y, z])
coords_robot_cam_rot = np.dot(Rot_m, coords_hom_robot)
coords_robot_cam = coords_robot_cam_rot + trans_v
#coords_robot_cam = np.dot(T_inverse, coords_hom_robot)
#coords_robot_cam = np.dot(coords_hom_robot, T)
x_final = round(coords_robot_cam[0], 2)
y_final = round(coords_robot_cam[1], 2)
z_final = round(coords_robot_cam[2], 2)
datos = np.array([z_final, y_final, x_final])
pub.publish(Float64MultiArray(data=datos))
cv.putText(annotated_frame, f"Coordenadas: ({y_final}, {z_final}, {x_final})", (50,50),cv.FONT_HERSHEY_SIMPLEX,0.5,(255,0,0), 1)
cv.imshow("Imagen recibida", annotated_frame)
print(f"({z_final}, {y_final}, {x_final})")
if cv.waitKey(1) & 0xFF == ord('q'):
break
cv.destroyAllWindows()
iPos.positionMove(home.data())