1.dlib检测人脸

2.保存彩色信息（png）和深度信息（npy）

3.点云显示

#coding = utf-8
import pyrealsense2 as rs
import cv2
import numpy as np
import dlib
import pcl

def find_martrix_min(data_matrix):
    new_data=[]
    for i in range(len(data_matrix)):
        new_data.append(min(data_matrix[i]))
    return min(new_data)

def find_martrix_max(data_matrix):
    new_data=[]
    for i in range(len(data_matrix)):
        new_data.append(max(data_matrix[i]))
    return max(new_data)

#广度优先搜索
def floodfill(image,seed,threshold_min,threshold_max):
    Q=[]
    g = np.zeros((len(image),len(image[0])))
    Q.append([seed[0],seed[1]])
    g[seed[0],seed[1]] = 1
    while len(Q)>0:
        [x, y] = Q.pop(0)
        x1, y1 = x - 1, y
        x2, y2 = x, y - 1
        x3, y3 = x + 1, y
        x4, y4 = x, y + 1

        if x1>=0 and y1>=0 and x1<len(image) and y1<len(image[0]) and g[x1][y1] == 0 and image[x1][y1]>threshold_min and image[x1][y1] <threshold_max:
            g[x1][y1] = image[x1][y1]
            #g[x1][y1] = 255
            Q.append([x1, y1])
        if x2>=0 and y2>=0 and x2<len(image) and y2<len(image[0]) and g[x2][y2] == 0 and image[x2][y2]>threshold_min  and image[x2][y2] <threshold_max:
            g[x2][y2] = image[x2][y2]
            #g[x2][y2] = 255
            Q.append([x2, y2])
        if x3>=0 and y3>=0 and x3<len(image) and y3<len(image[0]) and g[x3][y3] == 0 and image[x2][y2]>threshold_min and image[x3][y3] <threshold_max:
            g[x3][y3] = image[x3][y3]
            #g[x3][y3] = 255
            Q.append([x3, y3])
        if x4>=0 and y4>=0 and x4<len(image) and y4<len(image[0]) and g[x4][y4] == 0 and image[x2][y2]>threshold_min and image[x4][y4]  <threshold_max:
            g[x4][y4] = image[x4][y4]
            #g[x4][y4] = 255
            Q.append([x4, y4])
    return Matrix_normalization(g,threshold_min,threshold_max)

#矩阵归一化
def Matrix_normalization(Matrix_image,depth_min,depth_max):
    # depth_min = img_depth[seed[1],seed[0]]-10
    # depth_max = img_depth[seed[1],seed[0]]+80
    print('depth_min: ',depth_min, ' depth_max: ',depth_max)

    for h in range(len(Matrix_image)):
        for w in range(len(Matrix_image[0])):
            Matrix_image[h,w]= (Matrix_image[h,w] - depth_min)/(depth_max-depth_min)*255
    print("Normalization the Matrix_image to 0-255")
    return Matrix_image


def depth_to_3d(color_image,depth_image):
    depth = cv2.imread(depth_image)
    rgb = cv2.imread(color_image)
    B, G, R = cv2.split(rgb)

    cloud = pcl.PointCloud_PointXYZRGBA()  # 存储图像的点云
    rows = len(depth)
    cols = len(depth[0])
    pointcloud = []

    # camera_factor = 1
    # camera_cx = 0
    # camera_cy = 0
    camera_fx = 615.896
    camera_fy = 615.804

    for m in range(0, rows):
        for n in range(0, cols):
            d = depth[m][n][0]
            if d == 0:
                pass
            else:
                z = float(d)
                x = n * z / camera_fx
                y = m * z / camera_fy
                # points = [x, y, z,255<<16|255<<8|255]
                # points = [x, y, z, R[m][n] << 16 | G[m][n] << 8 | B[m][n]]
                points = [x, y, z, R[m][n] << 16 | G[m][n] << 8 | B[m][n]]
                pointcloud.append(points)
    # 由于pcl库不会直接识别列表格式的点云数据，所以我们需要使用numpy库进行数据格式转换，并将点云保存pcd文件中。
    pointcloud = np.array(pointcloud, dtype=np.float32)
    cloud.from_array(pointcloud)
    pcl.save(cloud, "cloud.pcd", format='pcd')

class realsense:
    def __init__(self):
        self.pipeline = rs.pipeline()
        self.config = rs.config()
        self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)
        self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15)
        # Start streaming
        self.pipe_profile = self.pipeline.start(self.config)
        #深度对齐彩色
        self.align_to_color = rs.align(rs.stream.color)

    #显示对齐后的彩色图像和深度图像
    def show(self):

        while True:
            self.frames = self.pipeline.wait_for_frames()
            self.align_frames = self.align_to_color.process(self.frames)

            self.depth_frame = self.align_frames.get_depth_frame()
            self.color_frame = self.align_frames.get_color_frame()

            self.depth_image = np.asanyarray(self.depth_frame.get_data())
            self.color_image = np.asanyarray(self.color_frame.get_data())
            self.depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(self.depth_image, alpha=0.03), cv2.COLORMAP_JET)

            cv2.circle(self.color_image, (200, 200), 8, [255, 0, 255], thickness=-1)
            cv2.circle(self.depth_colormap, (200, 200), 8, [255, 0, 255], thickness=-1)

            cv2.imshow('depth_frame', self.depth_colormap)
            cv2.imshow('color_frame', self.color_image)
            key = cv2.waitKey(1)
            # Press esc or 'q' to close the image window
            if key & 0xFF == ord('q') or key == 27:
                cv2.destroyAllWindows()
                break

    #获取内参外参矩阵
    def get_intrin_extrin(self):
        self.frames = self.pipeline.wait_for_frames()

        self.depth_frame = self.frames.get_depth_frame()
        self.color_frame = self.frames.get_color_frame()
        # 获取内参
        self.dprofile = self.depth_frame.get_profile()
        self.cprofile = self.color_frame.get_profile()

        self.cvsprofile = rs.video_stream_profile(self.cprofile)
        self.dvsprofile = rs.video_stream_profile(self.dprofile)

        self.color_intrin = self.cvsprofile.get_intrinsics()
        print("color_intrin",self.color_intrin)
        self.depth_intrin = self.dvsprofile.get_intrinsics()
        print("depth_intrin",self.depth_intrin)

        # 从深度到彩色图像坐标系的变换矩阵
        self.extrin = self.dprofile.get_extrinsics_to(self.cprofile)
        print("extrin: ", self.extrin)

        self.depth_sensor = self.pipe_profile.get_device().first_depth_sensor()
        self.depth_scale = self.depth_sensor.get_depth_scale()
        print('depth_scale', self.depth_scale)

    def save_image(self,color =True, depth = True, if_return =False):
        # 延迟100帧

        for i in range(100):
            self.frames = self.pipeline.wait_for_frames()
            self.align_frames = self.align_to_color.process(self.frames)
            self.depth_frame = self.align_frames.get_depth_frame()
            self.color_frame = self.align_frames.get_color_frame()

        self.depth_image = np.asanyarray(self.depth_frame.get_data())
        self.color_image = np.asanyarray(self.color_frame.get_data())

        #类似热图,但是将深度信息归一化了,人脸深度信息相对太细微不明显,只能作为可视化,不实际使用
        self.depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(self.depth_image, alpha=0.03), cv2.COLORMAP_JET)
        self.deep_image=cv2.cvtColor(self.depth_colormap, cv2.COLOR_RGB2GRAY)

        if color == True:
            cv2.imwrite('color_image.jpg', self.color_image)
        if depth == True:
            cv2.imwrite('depth_image.jpg', self.depth_image)
            cv2.imwrite('depth_colormap.jpg', self.depth_colormap)
            cv2.imwrite('deep_image.jpg', self.deep_image)
            #使用原始的深度文件，保存为矩阵。
            np.save('depth_image.npy',self.depth_image)

        if if_return == True:
            #实际使用的彩色图像和深度文件
            return self.color_image,self.depth_image

    def stop(self):
        self.pipeline.stop()


if __name__ =="__main__":

    detector = dlib.get_frontal_face_detector()
    predictor = dlib.shape_predictor('shape_predictor_68_face_landmarks.dat')

    #A=realsense()
    #A.get_intrin_extrin()
    # img_rd,img_depth = A.save_image(color =True, depth = True, if_return =True)


    # 读取图像和深度文件
    img_rd = cv2.imread("color_image.jpg")
    img_depth = np.load("depth_image.npy")

    # cv2.imshow('color_frame', img_rd)
    # key = cv2.waitKey(0)
    # # Press esc or 'q' to close the image window
    # if key & 0xFF == ord('q') or key == 27:
    #     cv2.destroyAllWindows()


    # #生成 Dlib 的图像窗口
    # win = dlib.image_window()
    # win.set_image(img_depth)

    faces = detector(img_rd,1)
    print("Number of faces detected: {}".format(len(faces)))

    # #Dlib图像窗口显示
    # win.add_overlay(faces)
    # dlib.hit_enter_to_continue()

    #中心点
    seed=[]
    for i,d in enumerate(faces):
        seed=[(d.left()+d.right())//2,(d.top()+d.bottom())//2]
    print("seed",seed)
    print(img_depth[seed[1],seed[0]])

    #根据中心点广度搜索， 为了防止中心点不是最近的点，将 -10到+80的点归入人脸
    mask = floodfill(img_depth,[seed[1],seed[0]],img_depth[seed[1],seed[0]]-10,img_depth[seed[1],seed[0]]+80)

    #mask show
    # cv2.imshow('mask', mask)
    # key = cv2.waitKey(0)
    # # Press esc or 'q' to close the image window
    # if key & 0xFF == ord('q') or key == 27:
    #     cv2.destroyAllWindows()

    cv2.imwrite('mask.png', mask)
    #彩色图和深度图转为3D点云
    depth_to_3d('color_image.jpg','mask.png')

    #pcl点云图显示
    import pcl.pcl_visualization
    cloud = pcl.load_XYZRGB('cloud.pcd')
    visual = pcl.pcl_visualization.CloudViewing()
    visual.ShowColorCloud(cloud, b'cloud')
    flag = True
    while flag:
        flag != visual.WasStopped()
    end