PS4 Stereo Camera su Linux

Visto che oramai si trovano anche a prezzi sui 20 euro ho provato la camera stereo di PS4

PS eye versione 1 e 2

Per usarla su PC si deve avere obbligatoriamente una porta USB3 e il convertitore (che costa in proporzione piu' della camera stessa ma si puo' anche autoprodurre) 

https://github.com/sieuwe1/PS4-eye-camera-for-linux-with-python-and-OpenCV

Dopo la connessione fisica deve essere modificato il firmware per usare il dispositivo come UVC

In fase di calibrazione ho avuto un errore di 0.024 con 40 immagini ed usando i file del pacchetto

Per vedere come funziona la camera ho un po' modificato il file GetDepth.py estraendo i dati di profondita' che sono contenuti nella matrice z_values

Come si vede dall'immagine sottostante c'e' una banda a sinistra che non riporta valori. Inoltre le profondita' variano da 0.3  a 2.6 m (valore massimo di profondita' restituito dal software nonostante le dimensioni della stanza siano ben maggiori)

I dati di profondita' sono molto variabili  ma se si mediano i valori accumulando le immagini (nel mio caso ho accumulato 100 immagini) l'errore si avvicina ad 1 cm

Differenza tra due mappe di profondita' consecutive

import numpy as np

import cv2

from matplotlib import pyplot as plt

import time

from datetime import datetime

#Load camera parameters

ret = np.load('param_ret.npy')

K = np.load('param_K.npy')

dist = np.load('param_dist.npy')

h,w = (800, 1264)

new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(K,dist,(w,h),1,(w,h))

mapx, mapy = cv2.initUndistortRectifyMap(K,dist, None ,new_camera_matrix,(w, h),cv2.CV_16SC2)

#cap = cv2.VideoCapture("/home/sieuwe/Desktop/vidz/full_2.avi")

#cap = cv2.VideoCapture("/home/sieuwe/Desktop/vidz/full_3.avi")

#cap = cv2.VideoCapture("/home/sieuwe/Desktop/vidz/full_1.avi")

cap = cv2.VideoCapture(2)

cap.set(cv2.CAP_PROP_FRAME_WIDTH, 3448)

cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 808)

kernel= np.ones((13,13),np.uint8)

#Stereo matcher settings

win_size = 5

min_disp = 10

max_disp = 16 * 2 + 10

num_disp = max_disp - min_disp # Needs to be divisible by 16

stereo = cv2.StereoSGBM_create(minDisparity= min_disp,

numDisparities = num_disp,

blockSize = 5,

uniquenessRatio = 10,

speckleWindowSize = 1000,

speckleRange = 10,

disp12MaxDiff = 25,

P1 = 8*3*win_size**2,#8*3*win_size**2,

P2 =32*3*win_size**2) #32*3*win_size**2)

sommatore=np.zeros((400,632),np.float32)

contatore = 0

limite = 100

def decode(frame):

left = np.zeros((800,1264,3), np.uint8)

right = np.zeros((800,1264,3), np.uint8)

for i in range(800):

left[i] = frame[i, 64: 1280 + 48]

right[i] = frame[i, 1280 + 48: 1280 + 48 + 1264]

return (left, right)

def get_distance(d):

return 30 * 10/d

while(1):

ret, frame = cap.read()

start = time.time()

right, left = decode(frame)

#Undistort images

#img_1_undistorted = cv2.undistort(left, K, dist, None, new_camera_matrix)

#img_2_undistorted = cv2.undistort(right, K, dist, None, new_camera_matrix)

#img_1_undistorted= cv2.remap(left,mapx,mapy, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)

#img_2_undistorted= cv2.remap(right,mapx,mapy, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)

#downsample image for higher speed

img_1_downsampled = cv2.pyrDown(cv2.cvtColor(left, cv2.COLOR_BGR2GRAY))

img_2_downsampled = cv2.pyrDown(cv2.cvtColor(right, cv2.COLOR_BGR2GRAY))

new_w, new_h = img_1_downsampled.shape

#compute stereo

disp = stereo.compute(img_1_downsampled,img_2_downsampled)

#denoise step 1

denoised = ((disp.astype(np.float32)/ 16)-min_disp)/num_disp

dispc= (denoised-denoised.min())*255

dispC= dispc.astype(np.uint8)

#denoise step 2

denoised= cv2.morphologyEx(dispC,cv2.MORPH_CLOSE, kernel)

#apply color map

disp_Color= cv2.applyColorMap(denoised,cv2.COLORMAP_OCEAN)

f = 0.3*w # 30cm focal length

Q = np.float32([[1, 0, 0, -0.5*new_w],

[0,-1, 0, 0.5*new_h], # turn points 180 deg around x-axis,

[0, 0, 0, f], # so that y-axis looks up

[0, 0, 1, 0]])

points = cv2.reprojectImageTo3D(disp, Q)

z_values = points[:,:,2]

print(np.size(z_values))

print(z_values.shape)

now = datetime.now()

np.save(now.strftime("%Y%m%d%H%M%S%f"),z_values)

cv2.imshow("Z",z_values)

sommatore = sommatore + z_values

contatore = contatore + 1

if (contatore == limite):

contatore = 0

sommatore = sommatore /limite

now = datetime.now()

np.save(now.strftime("%Y%m%d%H%M%S"),sommatore)

sommatore.fill(0)

z_values = z_values.flatten()

indices = z_values.argsort()

precentage = 25280

min_distance = np.mean(np.take(z_values,indices[0:precentage])) # takes the 30% lowest measuerements and gets the average distance from these.

avg_distance = np.mean(z_values) # averages all distances

max_distance = np.mean(np.take(z_values,indices[z_values.shape[0]-precentage:z_values.shape[0]])) # takes the 30% highest measuerements and gets the average distance from these.

#print(np.take(z_values,indices[z_values.shape[0]-precentage:z_values.shape[0]]))

#print(np.take(z_values,indices[:-100]))

#visualize

cv2.imshow("Depth", disp_Color)

left = cv2.resize(left,(632, 400))

color_depth = cv2.addWeighted(left,0.4,disp_Color,0.4,0)

end = time.time()

fps = 1 / (end-start)

cv2.putText(color_depth, "minimum: " + str(round(min_distance,1)),(5, 20),cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,0,255),2,cv2.LINE_AA)

cv2.putText(color_depth, "average: " + str(round(avg_distance,1)),(5, 40),cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,0,255),2,cv2.LINE_AA)

cv2.putText(color_depth, "maximum: " + str(round(max_distance,1)),(5, 60),cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,0,255),2,cv2.LINE_AA)

cv2.putText(color_depth, "FPS: " + str(round(fps)),(5, 80),cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,0,255),2,cv2.LINE_AA)

cv2.imshow("color & Depth", color_depth)

if cv2.waitKey(1) & 0xFF == ord('q'):

break

cap.release()

cv2.destroyAllWindows()