-
Notifications
You must be signed in to change notification settings - Fork 1
/
2.retrieveMiDataCluster.py
149 lines (123 loc) · 6.18 KB
/
2.retrieveMiDataCluster.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
#!/usr/bin/python3
import argparse
import roypy
import time
import queue
from api.sample_camera_info import print_camera_info
from roypy_sample_utils import CameraOpener, add_camera_opener_options
from roypy_platform_utils import PlatformHelper
import numpy as np
import matplotlib.pyplot as plt
import mpldatacursor
class MyListener(roypy.IDepthDataListener):
# inicilizo contador de frame
frame = 0
# constructor
def __init__(self, q, directorio, clase_objeto, objeto, n_captura):
super(MyListener, self).__init__()
self.queue = q
self.directorio = directorio
self.claseObjeto = clase_objeto
self.objeto = objeto
self.nCaptura = n_captura
def onNewData(self, data):
# lista eje z y lista tridimensional para valores z y xyz
zvalues = []
zvalues_conf = []
xyzvalues = []
xyzvalues_conf = []
# bucle for que recorre todos los puntos
for i in range(data.getNumPoints()):
xyzvalues.append([data.getX(i), -data.getY(i), -data.getZ(i)]) # obtiene xyz
zvalues.append(data.getZ(i)) # obtiene z
if data.getDepthConfidence(i) > 0: # filtrado valor confianza
# añade a la lista los puntos del eje z y la lista 3D
zvalues_conf.append(data.getZ(i))
xyzvalues_conf.append([data.getX(i), -data.getY(i), -data.getZ(i)]) # xyz filtrado por confianza > 0
# se convierte la lista de valores en array vectorizado
zarray = np.asarray(zvalues)
zarray_conf = np.asarray(zvalues_conf)
xyzarray = np.asarray(xyzvalues)
xyzarray_conf = np.asarray(xyzvalues_conf)
# guarda los array tridimensionales en el directorio indicado en formato .txt
np.savetxt(f"./{self.directorio}/xyz/{self.claseObjeto}/{self.objeto}_C{self.nCaptura}_F{self.frame}.txt", xyzarray) # original
np.savetxt(f"./{self.directorio}/xyzConf/{self.claseObjeto}/{self.objeto}_C{self.nCaptura}_F{self.frame}.txt", xyzarray_conf) # conf
# array reformateado sin modificar sus datos para crear imagen RGBD
# el numero de elementos por fila es desconocido y por columna segun el ancho de la profundida de imagen
p = zarray.reshape(-1, data.width)
# añade a la cola el array p
self.queue.put(p)
# incrementa el contador frame
self.frame += 1
def paint(self, data):
"""Called in the main thread, with data containing one of the items that was added to the
queue in onNewData."""
# crea la imagen de profundidad con los datos crudos
fig, ax = plt.subplots()
ax.imshow(data, interpolation='none')
mpldatacursor.datacursor(hover=True, bbox=dict(alpha=1, fc='w'))
plt.imshow(data)
# guarda la imagen en el directorio indicado
plt.savefig(f"./{self.directorio}/ImagenesProfundidad(2D)/{self.claseObjeto}/{self.objeto}_C{self.nCaptura}_F{self.frame - 1}.png", bbox_inches='tight')
# pausa necesaria para asegurar algunas imagenes
plt.pause(0.05)
def main():
# Wrapper que inicializa llamadas necesarias por windows para operar camaras UVC (USB video class)
PlatformHelper()
# valores por defecto
seconds = 5
mode = "MODE_9_5FPS_2000"
directorio = "MiDataCluster"
clase_objeto = "mesa"
objeto = "MesaB"
n_captura = 1
# añade a la cámara la interfaz para actuar con los args dados por consola y activa y encience la cámara con
# la configuracion deseada
parser = argparse.ArgumentParser(usage=__doc__)
add_camera_opener_options(parser)
parser.add_argument("-s", "--seconds", type=int, default=seconds, help="duracion de la captura")
parser.add_argument("-m", "--mode", type=str, default=mode, help="modo de capturacion: "
"MODE_9_5FPS_2000, "
"MODE_9_10FPS_1000, "
"MODE_9_15FPS_700, "
"MODE_9_25FPS_450, "
"MODE_5_35FPS_600, "
"MODE_5_45FPS_500")
parser.add_argument("-d", "--directorio", type=str, default=directorio, help="ruta del directorio a guardar los resultados")
parser.add_argument("-c", "--clase_objeto", type=str, default=clase_objeto, help="clase del objeto a capturar")
parser.add_argument("-o", "--objeto", type=str, default=objeto, help="nombre del objeto a capturar")
parser.add_argument("-n", "--n_captura", type=int, default=n_captura, help="numero de captura")
options = parser.parse_args()
opener = CameraOpener(options)
cam = opener.open_camera()
# Seleccion de modo de captura deseado
cam.setUseCase(options.mode)
# presenta por consola la informacion escogida en la camara
print_camera_info(cam)
print("isConnected", cam.isConnected())
print("getFrameRate", cam.getFrameRate())
# usamos la cola para sincronizar el callback con el hilo principal, el dibujo debe de ir en el hilo principal
# inicializa la cola, asocia a la camara un listener y comienza a captar datos
q = queue.Queue()
l = MyListener(q, options.directorio, options.clase_objeto, options.objeto, options.n_captura)
cam.registerDataListener(l)
cam.startCapture()
# crea un bucle que corra un tiempo (por defecto 5 segundos)
process_event_queue(q, l, options.seconds)
cam.stopCapture()
def process_event_queue(q, painter, seconds):
# crea un bucle que correra la cantiad de tiempo indicada
t_end = time.time() + seconds
while time.time() < t_end:
try:
# obtiene item de la cola
# se bloquea hasta obtener un item o haber pasado 5 segundos
item = q.get(True, 5)
except queue.Empty:
# cuando se pasa el tiempo
break
else:
# crea imagen
painter.paint(item)
if __name__ == "__main__":
main()