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"

FOV IR Camera di Kinect

La camera infrarossa del Kinect (che viene usata per calcolare la mappa di distanza) ha una FOV (Field of View) di 58.5°x45.6° per una risoluzione di 640x480

Tradotto in radianti (prendendo come origine degli assi il centro dell'immagine) si ha che
alfa (angolo lungo l'asse maggiore) = +/- 0.510588 rad
beta (angolo lungo l'asse minore= +/- 0.397935 rad

dividendo per la risoluzione in pixel si hanno del dimensioni di ogni pixel
delta_alfa = 0.00159534
delta_beta = 0.001658052

Per calcolare la reale dimesione di ogni pixel, considerando un caso semplice ovvero una immagine piatta e ortogonale al sensore, le misure reali si possono calcolare come
dis_c = distanza del punto centrale dell'immagine
x_reale = dis_c * tg(alfa)
y_reale = dis_c * tg(beta)

ad una distanza di 80 cm la finestra di kinect risulta essere di 130x81 cm ovvero circa 1 metro quadrato.


tradotto in Python
----------------------------------------------------------
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")
              t=t+1
              alfa = alfa + delta_a
beta = beta - delta_b
----------------------------------------------------------

Per verificare che i calcoli siano corretti ho ripreso un oggetto di dimensioni conosciute ovvero un libro di 292x205 mm ad una distanza di 1342 mm (centro immagine)

Immagine RGB

CloudP Point in Meshlab

CloudP Point in Meshlab

Come si deve le dimensioni misurate risultano essere 288mmx198mm a conferma di come i calcoli siano corretti

Problemi di Ubuntu 14.04 con Virtualbox

Cercando di dare un'occhiata alle novita' di Ubuntu 14.04, ho provato ad installare Ubuntu 14.04 in Virtualbox. Di solito non ci sono problemi tra Virtualbox ed Ubuntu ma in questo caso il risultato finale e' quello in immagine, ovvero una finestra microscopica
Installare le estensioni di Virtualbox non ha migliorato la situazione

Ho risolto il problema facendo l'upgrade di Virtualbox (alla versione 4.3.10) e reinstallando ex-novo le estensioni di Virtualbox. In ogni caso per far girare decentemente Ubuntu 14.04 ho dovuto assegnare meta' della Ram (1.4 Gb) ed abilitare 128Mb di Ram video con accelerazione 3D

Accelerometro su Kinect

Kinect e' fornita di un accelerometro a 3 assi che secondo quando riportato da IFixIt dovrebbe essere un modello KXSD9 di Kionix

Per leggere i dati dell'accelorometro in Python si puo' usare il seguente script (ripreso da qui)
Attenzione: per funzionare e' necessario utilizzare PyUsb alla versione unstable 1.0 e non quella stable 0.4 che normalmente e' pacchettizzata per Linux
Prima e' quindi necessario scaricare il pacchetto unstable da GitHub
(installarea con il classico python setup.py install)
Lo script e' un demo anche per muovere il motore di tilt del Kinect
------------------------------------------------------
import usb.core
import usb.util
import sys
import time


# find our device
dev = usb.core.find(idVendor=0x045e, idProduct=0x02B0)
# was it found?
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')


# set the active configuration. With no arguments, the first
# configuration will be the active one
dev.set_configuration()

# (bmRequestType, bmRequestType, bmRequest, wValue, wIndex, nBytes)
ret = dev.ctrl_transfer(0xC0, 0x10, 0x0, 0x0, 1)

print hex(ret[0])   # should return 0x22 but dont know why ?
# ???
ret = dev.ctrl_transfer(0x40, 0x6, 0x1, 0x0, [])

time.sleep(1.5)
# head up!
ret = dev.ctrl_transfer(0x40, 0x31, 0xfff0, 0x0, [])
time.sleep(1.5)
# bring head down
ret = dev.ctrl_transfer(0x40, 0x31, 0xffd0, 0x0, [])
time.sleep(1.5)
# up!
ret = dev.ctrl_transfer(0x40, 0x31, 0xfff0, 0x0, [])
time.sleep(1.5)
# down!
ret = dev.ctrl_transfer(0x40, 0x31, 0xffd0, 0x0, [])
time.sleep(1.5)
# up!
ret = dev.ctrl_transfer(0x40, 0x31, 0xfff0, 0x0, [])


while True:
    # Get accel data
    ret = dev.ctrl_transfer(0xC0, 0x32, 0x0, 0x0, 10)
    #print map(hex, ret)
    # bytes 0 & 1 are always zero
    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
    print x, "\t", y, "\t", z
    time.sleep(0.5)
------------------------------------------------------
(attenzione l'asse x dello script in  Python coincide con l'asse Z della codifica Microsoft mentre l'asse z dello script in Python corrisponde con quello X della codifica Microsoft)

Leggendo i dati si ha che non e' presente un sensore di azimuth per cui non e' possibile avere una orientazione nello spazio kinect. In pratica non e' possibile distinguere la rotazione sull'asse Y

Sito Microsoft

L'asse Z Microsoft mostra la rotazione destra/sinistra (mettendo di fronte al kinect appoggiato sul tavolo) e ruotando di 90° verso sinistra mostra valori positivi di 858 unita' mentre ruotando a destra di -797 unita'

L'asse X mostra la rotazione con valori di +779 unita' per posizione a 90° con i sensori che guardano lo zenith mentre di -865 unita' con i sensori che guardano nadir

Se si continua la rotazione oltre i 90° i valori tornano a decrescere verso il valore 0. Per capire se il sensore sta guardando avanti od indietro (ovvero se e' appoggiato alla sua basetta od e' appeso alla sua basetta) vengono usati i valori dell'asse Y Microsoft (y Python) che sono positivi se il Kinect guarda avanti e negativi se guarda indietro

i valori riportati sono espressi come accelerazioni in funzione dell'accelerazione di gravita'

Per calcolare l'accelerazione reale misurata la regola sembra essere

(misura/819)*9.18 m/sec*sec

Sembra quindi che i valori letti debbano essere confinati tra 0 ed 819 mentre le mie letture eccedono questo valore. Se si calcola il valore medio tra la misura maggiore e minore si ha che
asse Z (858+797)/2 = 827
asse X (779+865)/2=822
quindi considerando l'incertezza (vedi sotto) c'e' solo da calcolare un piccolo offset dello zero ed i valori coincidono con quelli di fabbrica

Mantenendo fermo il kinect ho effettuato oltre 14200 misure ed ho ottenuto una deviazione standard dei dati nei vari assi come segue

x = +/- 6.9 unita'

y = +/- 7.45 unita'

z = +/- 21.7 unita'

Per riportare i valori di inclinazione in gradi si puo' usare il seguente calcolo
-------------------------------------------------------------------------
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)
-------------------------------------------------------------------------
Di seguito una immagine con l'orientazione ed il valore degli angoli in relazione a diverse orientazioni

RGB con PyOpenNi Kinect

Non si tratta di una funzione molto bene documentata (anche perche' non presente nella cartella degli esempi) ma con PyOpenNi e' possibile acquisire immagini RBG da Kinect

il codice di riferimento (sostanzialmente autoesplicativo) e' il seguente
----------------------------------------------

from openni import *
from PIL import Image

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("rgb.jpg")

ctx.wait_one_update_all(depth)
de = Image.fromstring('L',(640,480),depth.get_raw_depth_map_8())
de.save("depth.jpg")

Kinect con SimpleCV

Un metodo alternatico a PyOpenNi per interagire con Kinect da Python e' utilizzare SimpleCV

Per l'installazione si puo' procedere come segue

sudo apt-get install libopencv-*
sudo apt-get isntall python-opencv
sudo apt-get install python-numpy
sudo apt-get install python-pygame
sudo apt-get install python-setuptools

si scarica la libreria Simple CV di GitHub al seguente link e si installa com

python setup.py install

se tutto e' andato a buon fine sono sufficiente le poche righe sottostanti per avere l'acquisizione dell'immagine RGB e di profondita' da Kinect

------------------------------------------------------
from SimpleCV import *

cam=Kinect()


depth = cam.getDepth()
depth.save('depth.jpg')
rgb = cam.getImage()
rgb.save('rgb.jpg')

Allineamento RGB/IR su Kinect

Sovrapponendo le immagini RGB e di profondita' di Kinect si vede chiaramente, indipendentemente se l'oggetto e' in primo piano o sullo sfondo

Leggendo questo link, si osserva che i parametri di calibrazioni delle camere sono codificate (e diversi) all'interno di ogni Kinect in funzione di come sono state montate e calibrate in fabbrica
Usando la versione completa di OpenNi (o l'SDK Microsoft) sono presenti specifiche funzioni che effettuano la fusione delle immagini dai due sensori come depth.GetAlternativeViewPointCap().SetViewPoint(image);