深度摄像头depth坐标转化成rgb坐标通过cuda优化问题

本文链接：https://blog.csdn.net/a8491087/article/details/85165886

首先，这个问题很难。

如果你先不知道标定的参数旋转和平移，以及参数之间运算的话，先别看了。

搞懂了坐标转化问题再来看。

这里面其实就是三个坐标转化，但分了好多步。

上代码。

https://github.com/mashevictor/shuanghecuda.git

核心代码是这个，我用的奥比中光摄像头，然后只能在windows里面标定。标定我看有很大的随机性，如果不标定个几十次，无法找到真正的参数的。

#-*-coding:utf-8-*-
import pycuda.driver as drv
import pycuda.tools
import pycuda.autoinit
from pycuda.compiler import SourceModule
from primesense import openni2
from primesense import _openni2 as c_api
import numpy as np
import scipy.misc as scm
import matplotlib.pyplot as p
import cv2
from itertools import chain
np.set_printoptions(threshold=np.inf)
np.set_printoptions(suppress=True)
dist ='/home/victor/software/OpenNI-Linux-x64-2.3/Redist'
mod = SourceModule \
(
"""
#include<stdio.h>
#define INDEX(a, b) a*320+b
#define fx 597.092
#define Cx 396.422
#define fy 513.418
#define Cy 205.821

__global__ void Depthuv2Depthworld(float *dest,float *nimabi,float *r_img)
{
unsigned int idx = threadIdx.x+(blockIdx.x*(blockDim.x*blockDim.y));
unsigned int a = idx/320;
unsigned int b = idx%320;
dest[INDEX(a, b)] = 0*nimabi[INDEX(a, b)]+(a-Cx)*r_img[INDEX(a, b)]/fx;
}

__global__ void Depthuv2Depthworld2(float *dest2,float *nimabi,float *r_img)
{
unsigned int idx = threadIdx.x+(blockIdx.x*(blockDim.x*blockDim.y));
unsigned int a = idx/320;
unsigned int b = idx%320;
dest2[INDEX(a, b)] =0*nimabi[INDEX(a, b)]+(b-Cy)*r_img[INDEX(a, b)]/fy;
}

__global__ void Depthworld2Rgbworld(float *dest,float *r_img, float *g_img, float *b_img)
{

unsigned int idx = threadIdx.x+(blockIdx.x*(blockDim.x*blockDim.y));

unsigned int a = idx/320;
unsigned int b = idx%320;
dest[INDEX(a, b)] =(0.998003*r_img[INDEX(a, b)]+0.00241323*g_img[INDEX(a, b)]+0.0631272*b_img[INDEX(a, b)]);
}

__global__ void Depthworld2Rgbworld2(float *dest2,float *r_img, float *g_img, float *b_img)
{

unsigned int idx =threadIdx.x+(blockIdx.x*(blockDim.x*blockDim.y));

unsigned int a = idx/320;
unsigned int b = idx%320;
dest2[INDEX(a, b)] = (-0.00354764*r_img[INDEX(a, b)]+0.999834*g_img[INDEX(a, b)]+0.0178643*b_img[INDEX(a, b)]);
}

__global__ void Depthworld2Rgbworld3(float *dest3,float *r_img, float *g_img, float *b_img)
{

unsigned int idx = threadIdx.x+(blockIdx.x*(blockDim.x*blockDim.y));

unsigned int a = idx/320;
unsigned int b = idx%320;
dest3[INDEX(a, b)] = (-0.0630737*r_img[INDEX(a, b)]-0.0180526*g_img[INDEX(a, b)]+0.997846*b_img[INDEX(a, b)]);
}

#define k2_fx 571.71
#define k2_Cx 296.04
#define k2_fy 496.645
#define k2_Cy 295.57

__global__ void Pingyi2Rgbuv(float *dest,float *r_img,float *b_img)
{
unsigned int idx = threadIdx.x+(blockIdx.x*(blockDim.x*blockDim.y));
unsigned int a = idx/320;
unsigned int b = idx%320;
dest[INDEX(a, b)] = r_img[INDEX(a, b)]*k2_fx/b_img[INDEX(a, b)]+k2_Cx;
}

__global__ void Pingyi2Rgbuv2(float *dest2,float *g_img,float *b_img)
{
unsigned int idx = threadIdx.x+(blockIdx.x*(blockDim.x*blockDim.y));
unsigned int a = idx/320;
unsigned int b = idx%320;
dest2[INDEX(a, b)] =g_img[INDEX(a, b)]*k2_fy/b_img[INDEX(a, b)]+k2_Cy;
}

"""
)
def han(*arr):
r_img=np.array(arr).astype(np.float32)
dest=r_img
zeros_shuzu=np.zeros(76800,)
Depthuv2Depthworld = mod.get_function("Depthuv2Depthworld")
Depthuv2Depthworld(drv.Out(dest),drv.In(zeros_shuzu),drv.In(r_img),block=(1024, 1, 1), grid=(75, 1, 1))
diyige=np.array(dest)
return diyige

def han2(*arr):
r_img=np.array(arr).astype(np.float32)
dest2=r_img
zeros_shuzu=np.zeros(76800,)
Depthuv2Depthworld2 = mod.get_function("Depthuv2Depthworld2")
Depthuv2Depthworld2(drv.Out(dest2),drv.In(zeros_shuzu),drv.In(r_img),block=(1024, 1, 1), grid=(75, 1, 1))
return dest2

def pingyihan(*arr):
r_img=np.array(arr).astype(np.float32)
dest=r_img
zeros_shuzu=np.zeros(76800,)
Pingyi2Rgbuv = mod.get_function("Pingyi2Rgbuv")
Pingyi2Rgbuv(drv.Out(dest),drv.In(zeros_shuzu),drv.In(r_img),block=(1024, 1, 1), grid=(75, 1, 1))
diyige=np.array(dest)
return diyige

def pingyihan2(*arr):
r_img=np.array(arr).astype(np.float32)
dest2=r_img
zeros_shuzu=np.zeros(76800,)
Pingyi2Rgbuv2 = mod.get_function("Pingyi2Rgbuv2")
Pingyi2Rgbuv2(drv.Out(dest2),drv.In(zeros_shuzu),drv.In(r_img),block=(1024, 1, 1), grid=(75, 1, 1))
return dest2

openni2.initialize(dist)
if (openni2.is_initialized()):
print("openNI2 initialized")
else:
print("openNI2 not initialized")
dev = openni2.Device.open_any()
depth_stream = dev.create_depth_stream()
depth_stream.set_video_mode(c_api.OniVideoMode(pixelFormat=c_api.OniPixelFormat.ONI_PIXEL_FORMAT_DEPTH_1_MM, resolutionX=320, resolutionY=240, fps=30))
depth_stream.set_mirroring_enabled(False)
depth_stream.start()
def get_depth():
dmap = np.fromstring(depth_stream.read_frame().get_buffer_as_uint16(),dtype=np.uint16).reshape(240,320)
d4d = np.uint8(dmap.astype(float) *255/ 2**12-1)
d4d = cv2.cvtColor(d4d,cv2.COLOR_GRAY2RGB)
d4d = 255 - d4d
return dmap, d4d
s=0
done = False
while not done:
key = cv2.waitKey(1)
key = cv2.waitKey(1) & 255
if key == 27:
print("\tESC key detected!")
done = True
elif chr(key) =='s':
print("\ts key detected. Saving image and distance map {}".format(s))
cv2.imwrite("ex1_"+str(s)+'.png', d4d)
np.savetxt("ex1dmap_"+str(s)+'.out',dmap)
dmap,d4d = get_depth()
dmap2=dmap.reshape(76800,)
print('-----------------------')
print(dmap2)
zuobiao=[]
for d in range(dmap.shape[0]):
   for e in range(dmap.shape[1]):
       x=np.array([d,e,1])
       zuobiao.append(x)
zuobiao=np.array(zuobiao)
xxx=han(dmap2)
xxx2=np.array(xxx)
yyy=han2(dmap2)
Tao=np.dstack((xxx2,yyy))
Fo=Tao.reshape(76800,2)
destzuizhong=np.c_[Fo,dmap2]
#print(destzuizhong)
#print('--------------------------ggggg')

#接下来是旋转运算，参数是R，
r_img = destzuizhong[:, 0].reshape(76800, order='F').astype(np.float32)
g_img = destzuizhong[:, 1].reshape(76800, order='F').astype(np.float32)
b_img = destzuizhong[:, 2].reshape(76800, order='F').astype(np.float32)
dest=r_img
dest2=r_img
dest3=r_img
Depthworld2Rgbworld = mod.get_function("Depthworld2Rgbworld")
Depthworld2Rgbworld(drv.Out(dest),drv.In(r_img),drv.In(g_img),drv.In(b_img),block=(1024, 1, 1), grid=(75, 1, 1))
#print ('11111111-------------------------')
#print (dest)
abc=np.array(dest)
r_img = destzuizhong[:, 0].reshape(76800, order='F').astype(np.float32)
g_img = destzuizhong[:, 1].reshape(76800, order='F').astype(np.float32)
b_img = destzuizhong[:, 2].reshape(76800, order='F').astype(np.float32)
Depthworld2Rgbworld2 = mod.get_function("Depthworld2Rgbworld2")
Depthworld2Rgbworld2(drv.Out(dest2),drv.In(r_img),drv.In(g_img),drv.In(b_img),block=(1024, 1, 1), grid=(75, 1, 1))
#print ('2222222-------------------------')
#print (dest2)
abc2=np.array(dest2)
r_img = destzuizhong[:, 0].reshape(76800, order='F').astype(np.float32)
g_img = destzuizhong[:, 1].reshape(76800, order='F').astype(np.float32)
b_img = destzuizhong[:, 2].reshape(76800, order='F').astype(np.float32)
Depthworld2Rgbworld3 = mod.get_function("Depthworld2Rgbworld3")
Depthworld2Rgbworld3(drv.Out(dest3),drv.In(r_img),drv.In(g_img),drv.In(b_img),block=(1024, 1, 1), grid=(75, 1, 1))
#print ('3333333-------------------------')
#print(abc)
#print(abc2)
#print (dest3)
#这里要理解abc.abc2.dest3分别对应的是x.y.z的数列。
#平移之后，记住这里得到的已经不是value的值了，而是坐标。所以对于第一个坐标转化的算法就不适应。

jieguopingyi=np.dstack((abc,abc2,dest3))+np.array([64.8816,-168.603,57.9951]).astype(np.float32)
jieguopingyi=jieguopingyi.reshape(76800,3)
pingyir_img = jieguopingyi[:, 0].reshape(76800, order='F').astype(np.float32)
pingyib_img = jieguopingyi[:, 2].reshape(76800, order='F').astype(np.float32)

lastdest=pingyir_img
lastdest2=pingyir_img
Pingyi2Rgbuv = mod.get_function("Pingyi2Rgbuv")
Pingyi2Rgbuv(drv.Out(lastdest),drv.In(pingyir_img),drv.In(pingyib_img),block=(1024, 1, 1), grid=(75, 1, 1))
lastabc=np.array(lastdest)
pingyig_img = destzuizhong[:, 1].reshape(76800, order='F').astype(np.float32)
pingyib_img = destzuizhong[:, 2].reshape(76800, order='F').astype(np.float32)
Pingyi2Rgbuv2 = mod.get_function("Pingyi2Rgbuv2")
Pingyi2Rgbuv2(drv.Out(lastdest2),drv.In(pingyig_img),drv.In(pingyib_img),block=(1024, 1, 1), grid=(75, 1, 1))
lastabc2=np.array(lastdest2)
tianbushengzhongni=np.dstack((lastabc,lastabc2))
print(tianbushengzhongni)
cv2.imshow('depth', d4d)
cv2.destroyAllWindows()
depth_stream.stop()
openni2.unload()
print ("Terminated")