# todo 现在是边界有深度信息，面没有深度信息
# todo 个别线段深度信息有误
# 用 卷积去彩虹， 卷积核的作用是时间轴上的边界重叠位置，计算重合时刻的视差，
# 计算的结果仅仅是边沿， todo 将视差向右传播，方法是？？（直到另外一个极值？）
''' 伪代码：
frame2 [0:240,0:320] 左右眼照片，320x240
cv2.cvtColor(imgL,cv2.COLOR_BGR2GRAY) 灰度化
cv2.addWeighted() 融合左右眼算法，
# 将shift=1 shift=99 堆叠成3D 阵列
# 将3D 转换为2D，选择极值，并记录层数
# 将2D转换为深度图，
# 将深度图转换为色彩图
'''
############## from
# https://blog.csdn.net/youcans/article/details/121309503 图像的平移
# https://blog.csdn.net/u013044310/article/details/80407220 二维array叠加成三维array
import cv2
import numpy as np
import time
start = time.time()
dx, dy = 0, -1 # dx 向右偏移量, dy=-3 固定偏移量
# dx=-3 窗外的塔吊重合
# dx=-5 办公室远处的绿植重合
# dx=-99 距离摄像头15cm的鼻子重合
DATA3D =np.zeros((30,240,320), dtype=np.uint8) # 存储 所有的视差融合图 高 x 宽 x 视差（像素），每个单元格里是：左右眼差异
DATA3D2 =np.zeros((30,240,320), dtype=np.uint8) # 存储 视差融合点
DATA2D=np.zeros((240,320), dtype=np.uint8 ) # 存储 高 x 宽 ，每个单元格里是： 融合后最小值的视差（像素）
# print(DATA3D.shape)
###########################################################################
kernel = np.array(( # 卷积核，用于查找左右眼重合点
[-1, -1, 1, 1, 0],
[-1, -1, 1, 0, -1],
[-1, -1, 0, -1, -1],
[-1, 0, 1, -1, -1],
[ 0, 1, 1, -1, -1]),
dtype="float32") / 1
############## CHUSEI摄像头初始化和获取图片，分割图片。 开始 ############################
import usb.core
cam=cv2.VideoCapture(700)
#cam=cv2.VideoCapture(cv2.CAP_DSHOW) #### cv2.CAP_DSHOW = 700 ，需要import usb， 而不是import usb.core？？
dev = usb.core.find(idVendor=0x18e3, idProduct=0x5031)
dev.set_configuration() #### 去除这个语句也可以使用？？
# simulate the SET_CUR sequence
dev.ctrl_transfer(0x21,0x01,0x0800,0x0600,[0x50,0xff]) # bEndpointAddress 0x21
dev.ctrl_transfer(0x21,0x01,0x0f00,0x0600,[0x00,0xf6])
dev.ctrl_transfer(0x21,0x01,0x0800,0x0600,[0x25,0x00])
dev.ctrl_transfer(0x21,0x01,0x0800,0x0600,[0x5f,0xfe])
dev.ctrl_transfer(0x21,0x01,0x0f00,0x0600,[0x00,0x03])
dev.ctrl_transfer(0x21,0x01,0x0f00,0x0600,[0x00,0x02])
dev.ctrl_transfer(0x21,0x01,0x0f00,0x0600,[0x00,0x12])
dev.ctrl_transfer(0x21,0x01,0x0f00,0x0600,[0x00,0x04])
dev.ctrl_transfer(0x21,0x01,0x0800,0x0600,[0x76,0xc3])
dev.ctrl_transfer(0x21,0x01,0x0a00,0x0600,[4,0x00])
k=0
while cam .isOpened():
end = time.time()
seconds = end - start # 处理一帧所用的时间
start = end
fps = 1 / seconds # 一秒钟可以处理多少帧
fps = "%.2f fps"%fps
# print(fps)
ret0, frame0 = cam .read()
####################################################################
frame2 = cv2.resize(frame0, (640, 240), interpolation=cv2.INTER_CUBIC) ## 拉伸画面 2*width,height
imgL = frame2 [0:240,0:320] ## 2个窗口，分别显示左右眼
imgR = frame2 [0:240,320:640]
# print (imgL.shape)
#########################################################
imgL_gray=cv2.cvtColor(imgL,cv2.COLOR_BGR2GRAY)
## rows, cols, ch = imgL.shape # for 彩色图片
rows, cols = imgL_gray.shape
## print (imgL_gray.dtype)
## imgL_gray=imgL_gray.astype(np.int16)
imgR_gray=cv2.cvtColor(imgR,cv2.COLOR_BGR2GRAY)
cv2.imshow('imgR_gray',imgR_gray)
## imgR_gray=imgR_gray.astype(np.int16)
for i in range(2,31):
# print(i)
MAT = np.float32([[1, 0, -2*i], [0, 1, dy]]) # 构造平移变换矩阵 ,[dx， dy]像素移动
imgL_gray_shift = cv2.warpAffine(imgL_gray, MAT, (cols, rows)) # 仿射变换，移动图片。默认为黑色填充
# imgL_gray_shift = cv2.warpAffine(imgL_gray, MAT, (cols, rows), borderValue=(255,255,255)) # 白色填充
# cv2.imshow('imgL_gray_shift',imgL_gray_shift)
###########################################################################################
# imgMerge=np.subtract(imgR_gray ,imgL_gray_shift ) # 2图相减，
imgMerge=cv2.absdiff(imgR_gray ,imgL_gray_shift )
# print(imgMerge)
# imgMergeAbs=np.abs( cv2.addWeighted(imgR_gray ,1,imgL_gray_shift,-1,0 ) ) # 取绝对值，与不取绝对值相比，没有任何差异
DATA3D[i-2,:,:] = imgMerge # 堆叠。最好一开始申请一段连续的空间，数据搬运工作量少，比较节约cpu时间，
# print( DATA3D.dtype ) # 一直保持了uint8
# print( DATA3D.shape )
# 从另外一个维度切片，做卷积，求重合点
for i in range(0,240):
probe2D= DATA3D[:,i,:]
imgProbe2DMin = cv2.filter2D(probe2D, -1, kernel)
DATA3D2[:,i,:] = imgProbe2DMin
#############################################
imgTemp = DATA3D[:,100,:] *20 ## 显示 未经卷积处理过的3D矩阵的剖面， *20 表示增强信号，否则看不到
cv2.imshow( 'imgTemp', imgTemp )
imgTemp1 = DATA3D2[:,100,:] *20 ## 显示 已经卷积处理过的3D矩阵的剖面， *20 表示增强信号，否则看不到
cv2.imshow( 'imgTemp1', imgTemp1 )
# 求极值
DATA2D = np.argmax( DATA3D2,axis = 0 ).astype(np.uint8) * 9 # np.argmin( ) 返回了最小值的位置
# DATA2D = np.argmin( DATA3D2,axis = 0 ).astype(np.uint8) * 9 # np.argmin( ) 返回了最小值的位置
# DATA2D = pow( np.argmax( DATA3D2,axis = 0 ) * 2000, 0.5 ) .astype(np.uint8) # np.argmin( ) 返回了最小值的位置
# debug 《《《《《 程序 会返回全黑！！
# debug 《《《《《 程序会将uint8 自动更改为 int64
# print(DATA2D.dtype)
# print( DATA2D.shape )
imgColor = cv2.applyColorMap(DATA2D, cv2.COLORMAP_JET) # COLORMAP_JET = 2, 蓝到红
# COLORMAP_RAINBOW = 4,红到蓝
'''
cv2.putText(
imgMerge, # 图片
fps, # 添加的文字
(5,50 ), # 位置
cv2.FONT_HERSHEY_SIMPLEX, # 字体
0.75, # 字体大小
(0, 0, 255), # 字体颜色
2) # 字体粗细
'''
cv2.imshow( 'imgColor', imgColor ) # 如果 DATA2D 声明的时候没有指定数据类型，或者在计算过程中数据类型改变，可以在这里指定为 uint8
######### 键盘响应 ，兼计时器 ########################################3
k = cv2.waitKey(3) & 0xFF #等待键盘输入,间隔 xx us
if (k == ord('q')) | (k == 27 ) :
break
cam.release()
cv2.destroyAllWindows()
