D435类

import cv2
import time
import math
import numpy as np
import pyrealsense2 as rs


class D435():
    def __init__(self, cam_id):
        pi = math.pi
        self.cam_id = cam_id
        self.pipeline = rs.pipeline()
        self.config = rs.config()



    def enable_cam(self):#直接开
        self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 60)
        self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 60)

        align_to = rs.stream.color
        self.align = rs.align(align_to)
        self.pipeline.start(self.config)



    def enable_with_id(self):#用id开
        print(f'启动的摄像头序列号：{self.cam_id}')
        self.config.enable_device(self.cam_id)
        self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 60)
        self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 60)
        align_to = rs.stream.color
        self.align = rs.align(align_to)
        self.pipeline.start(self.config)



    def get_align_frame(self):
        frames = self.pipeline.wait_for_frames()
        aligned_frames = self.align.process(frames)
        self.depth_frame = aligned_frames.get_depth_frame()
        self.color_image = aligned_frames.get_color_frame()
        self.color_frame = np.asanyarray(self.color_image.get_data())
        return self.color_frame



    def get_3d_coord(self,pos):
        depth_intrin = self.depth_frame.profile.as_video_stream_profile().intrinsics  # 获取深度参数（像素坐标系转相机坐标系会用到）
        color_intrin = self.color_image.profile.as_video_stream_profile().intrinsics  # 获取相机内参
        x = pos[0]
        y = pos[1]
        dis = self.depth_frame.get_distance(x,y)
        print ('depth:',dis)       # 深度单位是m
        camera_coordinate = rs.rs2_deproject_pixel_to_point(depth_intrin, pos, dis)
        x_cam = float(camera_coordinate[2])
        y_cam = float(camera_coordinate[0])
        z_cam = float(camera_coordinate[1])
        self.cam_coord = [x_cam,y_cam,z_cam]
        print("cam_coord:",self.cam_coord)
        print("")
        return self.cam_coord





    def rotation_matrix(self,x, y, z):  # 旋转矩阵：3*3
        Rx = np.array([[1, 0, 0], [0, math.cos(x), -math.sin(x)], [0, math.sin(x), math.cos(x)]])
        Ry = np.array([[math.cos(y), 0, math.sin(y)], [0, 1, 0], [-math.sin(y), 0, math.cos(y)]])
        Rz = np.array([[math.cos(z), -math.sin(z), 0], [math.sin(z), math.cos(z), 0], [0, 0, 1]])
        R = Rx @ Ry @ Rz
        return R



    def cam2world_main(self,xc, yc, zc):
        RT = self.cam_extrin(0.0, 0.0, 0.0, 0.0, 0.0, -0.4)
        #
        matrix_c = np.array([xc, yc, zc, 1]).T
        matrix_w = RT @ matrix_c
        xw, yw, zw = matrix_w[0], matrix_w[1], matrix_w[2]
        return [xw, yw, zw]







    # 用于根据位姿计算变换矩阵
    def cam_extrin(self,x, y, z, Tx, Ty, Tz):  # 相机外参：4*4
        pi = math.pi
        thetaX = x / 180 * pi
        thetaY = y / 180 * pi
        thetaZ = z / 180 * pi
        R = self.rotation_matrix(thetaX, thetaY, thetaZ)
        t = np.array([[Tx], [Ty], [Tz]])
        RT = np.column_stack([R, t])  # 列合并
        RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
        return RT


# if __name__ == "__main__":
#     cam = D435()
#     cam.cam_init()
#     try:
#         while True:
#             align,depth,color = cam.get_align_frame()
#             cv2.imshow("color",color)
#             cv2.waitKey(20)
#     finally:
#         # Stop streaming
#         cam.pipeline.stop()


if __name__ == "__main__":
    cam = D435("117222071004")
    cam.enable_with_id()
    while True:
        color = cam.get_align_frame()
        cv2.imshow("color", color)
        mid_pos = (320,240)
        cam.get_3d_coord(mid_pos)
        cv2.circle(color,mid_pos,5,(255,0,0),-1)
        # cv2.imshow("color",color)
        cv2.waitKey(10)