lunedì 21 aprile 2014

Programma completo per Kinect

Utilizzando tutte le esperienze precedenti (1,2) ho scritto questo programmino che salva in un colpo solo tutti i parametri di Kinect

lo script deve essere lanciato con una stringa argomento che diventa il nome del progetto.
per esempio
./acquisizione test

genera poi i file
test_angolo.txt
test_rgb.jpg
test_cloud.asc
test_cloud.txt

nel file _angolo.txt sono salvati gli angolo di pitch, roll, la distanza al centro dell'immagine ed il numero di punti della nuvola che risultano corretti (non tutti i pixel di una ascquisizione con il laser risultano corretti, valori attorno al 70% sono gia' ottimali

nel file _rgb.rgb viene salvata una fotografia della scansione. Attenzione: in alcuni casi ci sono problemi di sincronia per cui questa immagine puo' risultare tagliata o mescolata al frame precedente

nel file _cloud.asc sono riportati i valori in x,y,z in millimetri della scansione gia' pronti per essere inclusi in Meshlab o Cloudcompare

il file _cloud.txt e' l'acquisizione della nuvola dei punti pura senza elaborazione e serve nel caso ci siano problemi al punto precedente

il ritardo iniziale serve solo a dare il tempo all'operatore di mettersi in posizione con il kinect
--------------------------------------
#!/usr/bin/python
import usb.core
import usb.util

import sys
import time
import math

from openni import *
from PIL import Image
import numpy as np
import pickle

print "10 secondi alla misura"
time.sleep(5)
print "5 secondi alla misura"
time.sleep(5)
print "Inizio misura"

stringa = sys.argv[1]
print "Progetto : "+stringa

dev = usb.core.find(idVendor=0x045e, idProduct=0x02B0)
if dev is None:
    raise ValueError('Device not found')
for cfg in dev:
    sys.stdout.write("Configuration #"+str(cfg.bConfigurationValue) + '\n')
    for intf in cfg:
        sys.stdout.write('\tInterface #' + \
                         str(intf.bInterfaceNumber) + \
                         '\t, Alternate setting ' + \
                         str(intf.bAlternateSetting) + \
                         '\n')
        sys.stdout.write("\tEndpoints:\n")
        for ep in intf:
            sys.stdout.write('\t\t' + \
                             str(ep.bEndpointAddress) + \
                             '\n')

dev.set_configuration()


ret = dev.ctrl_transfer(0xC0, 0x10, 0x0, 0x0, 1)
ret = dev.ctrl_transfer(0x40, 0x6, 0x1, 0x0, [])


#calcola l'angolo
ret = dev.ctrl_transfer(0xC0, 0x32, 0x0, 0x0, 10)
x = (ret[2] << 8) | ret[3]
x = (x + 2**15) % 2**16 - 2**15     # convert to signed 16b
y = (ret[4] << 8) | ret[5]
y = (y + 2**15) % 2**16 - 2**15     # convert to signed 16b
z = (ret[6] << 8) | ret[7]
z = (z + 2**15) % 2**16 - 2**15     # convert to signed 16b
pitch = math.atan2(y,x)*(180/3.1415926)
roll = math.atan2(y,math.sqrt((x*x)+(z*z)))*(180/3.1415926)
if (z <0):
roll = 90+(90-roll)

#immagine rgb e profondita'
ctx = Context()
ctx.init()
depth = DepthGenerator()
rgb = ImageGenerator()
depth.create(ctx)
rgb.create(ctx)
depth.set_resolution_preset(RES_VGA)
depth.fps = 30
rgb.set_resolution_preset(RES_VGA)
rgb.fps = 30
ctx.start_generating_all()
ctx.wait_one_update_all(rgb)
im = Image.fromstring('RGB',(640,480),rgb.get_raw_image_map())
im.save(stringa+"_rgb.jpg")
print "RGB salvata"
#ctx.wait_one_update_all(depth)
#de = Image.fromstring('L',(640,480),depth.get_raw_depth_map_8())
#de.save(stringa+"_depth.jpg")
#print "Depth salvata"

time.sleep(0.5)
#mappa_punti
ctx.wait_one_update_all(depth)
depthMap = depth.map
depthMap2 = np.array(depthMap)
f = open(stringa+"_cloud.txt","w")
pickle.dump(depthMap2,f)
f.close()

f = open(stringa+"_cloud.txt")
data = pickle.load(f)
f.close()

g = open(stringa+"_cloud.asc","w")
t = 0
d = 0
alfa = -0.5105088 # angolo lungo x
delta_a = 0.00159534 # delta alfa
beta = 0.397935 # angolo lungo beta
delta_b = 0.001658052 # delta beta
dis_c = data[153600] # distanza del punto centrale 320x240
print "Distanza media "+str(dis_c) + " mm"
for y in range(0,480):
alfa = -0.5105088
for x in range (0,640):
              if (data[t] !=0):
g.write(str(dis_c*math.tan(alfa))+","+str(dis_c*math.tan(beta))+","+str(data[t])+"\n")
d = d + 1
              t=t+1
              alfa = alfa + delta_a
beta = beta - delta_b
g.close()
perc = (d/307200.0)*100
print "Nuvola dei punti salvati"
print "Punti validi : " + str(round(perc,1)) + "%"

out = open(stringa + "_angolo.txt","w")
out.write("Pitch: "+str(round(pitch,1))+", Roll : "+str(round(roll,1))+", Distanza : "+str(dis_c)+" mm, Punti validi :"+str(round(perc,1))+"% \n")
out.close()
print "Angoli salvati"