def convert_depth(depth1, depth2):
depth1 = depth1.astype(float)
depth1[depth1 == 0] = np.nan
depth1XYZ_tmp = PlotRGBD_3D.depth2XYZ(depth1, True, False)
depth2 = depth2.astype(float)
depth2[depth2 == 0] = np.nan
depth2XYZ_tmp = PlotRGBD_3D.depth2XYZ(depth2, True, False)
# Reshape the depth maps
dshape = depth1XYZ_tmp.shape
depth1XYZ_tmp2 = np.array([depth1XYZ_tmp[i,j,:] for i in range(dshape[0])
for j in range(dshape[1])
if not any(np.isnan(depth1XYZ_tmp[i,j,:]))])
depth2XYZ_tmp2 = np.array([depth2XYZ_tmp[i,j,:] for i in range(dshape[0])
for j in range(dshape[1])
if not any(np.isnan(depth2XYZ_tmp[i,j,:]))])
# Downsample the depth maps for speed
depth1XYZ = np.array([depth1XYZ_tmp2[i,:] for i in range(depth1XYZ_tmp2.shape[0])
if i % 10 == 0])
depth2XYZ = np.array([depth2XYZ_tmp2[i,:] for i in range(depth2XYZ_tmp2.shape[0])
if i % 10 == 0])
return(depth1XYZ, depth2XYZ)
Acumulative=np.eye(3)
bcumulative=np.zeros((1,3))
framepixels=np.zeros((totalframes))
filename="pictures/test"
for i in range(totalframes):
img2=read_data.rgbData[read_data.pairedData[startframe+i][0]][0]
depth2=read_data.depthData[read_data.pairedData[startframe+i][1]][0]
img=cv.imread(img2)
b,g,r=cv.split(img)
img=cv.merge([r,g,b])
depth=cv.imread(depth2,0)
XYZ=PlotRGBD_3D.depth2XYZ(depth,True,False)
inds=(XYZ[:,:,0]==XYZ[:,:,1])
inds=inds*(XYZ[:,:,1]==XYZ[:,:,2])
poscol=np.dstack((XYZ,img))
poscol=poscol[inds==0]
poscol[:,(0,1,2)]=np.dot(Acumulative,poscol[:,(0,1,2)].T).T+bcumulative
if(i<(totalframes-1)):
Acumulative=np.dot(Atransforms[i+1,:,:],Acumulative)
bcumulative=bcumulative+btransforms[i+1,:]
XYZRGB=np.row_stack((XYZRGB,poscol))
framepixels[i]=poscol.shape[0]
fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
