python 手动对齐d435相机的rgb与depth，非官方align_processing()函数

（代码制作不易，免费提供大家使用，希望换一个关注，和点赞）

废话不多说，代码如下（参考了很多资料，最终凑出了代码QAQ）。

 获取相机参数，也可以手动输入相机参数（若相机无法标定可以尝试）

import cv2
import numpy as np
import pyrealsense2 as rs

pipe = rs.pipeline()
cfg = rs.config()

cfg.enable_stream(rs.stream.depth,640,480,rs.format.z16,30)
cfg.enable_stream(rs.stream.color,640,480,rs.format.bgr8,30)
cfg.enable_stream(rs.stream.infrared,1,640,480,rs.format.y8,30)

profile = pipe.start(cfg)

depth_profile = profile.get_stream(rs.stream.depth)
color_profile = profile.get_stream(rs.stream.color)

depth_intr = depth_profile.as_video_stream_profile().get_intrinsics()
color_intr = color_profile.as_video_stream_profile().get_intrinsics()
extrinsics_depth_to_color = depth_profile.get_extrinsics_to(color_profile)
#也可以手动输入参数
'''
#print(color_intr)
color_intr.fx=np.array(5.990818971121990e+02)
color_intr.fy=np.array(6.007770207896575e+02)
color_intr.ppx=np.array(3.360454215338955e+02)
color_intr.ppy=np.array(2.400705427417924e+02)

#print(color_intr)
depth_intr.fx=np.array(3.904567900216258e+02)
depth_intr.fy=np.array(3.913375769950008e+02)
depth_intr.ppx=np.array(3.237127035243132e+02)
depth_intr.ppy=np.array(2.377210640958379e+02)
#print(depth_intr)
#extrinsics_color_to_ir1.rotation=np.array([ 1,0,0,0,1,0,0,0,1])
extrinsics_depth_to_color.rotation=np.array([ 0.99925857,  0.00553705 , 0.03810057,
 -0.00437968 , 0.99952842 ,-0.03039341,
-0.03825089 , 0.030204 ,   0.99881159])

extrinsics_depth_to_color.translation=np.array([0.024382348683330505, -0.008719076611654965, -0.003561121659189037])
#print(extrinsics_color_to_ir1)
'''

导入视频流，参加一个黑色图片，用于存放对齐后深度信息

while True:
    #ret, frame = cap.read()
    stream_frame = pipe.wait_for_frames()
    depth_frame = stream_frame.get_depth_frame()
    color_frame = stream_frame.get_color_frame()
    #ired1_frame=stream_frame.get_infrared_frame()

    colorizer = rs.colorizer()
    d1 = np.asanyarray(colorizer.colorize(depth_frame).get_data())
    c = np.asanyarray(colorizer.colorize(color_frame).get_data())
    #ir = np.asanyarray(colorizer.colorize(ired1_frame).get_data())
    cv2.imshow('color_frame',c)
    cv2.imshow('depth_frame',d1)
    #cv2.imshow('ired1_frame',i)

    #d2=np.zeros((640,480),dtype=int)
    d2=np.zeros((480,640,3),dtype=np.uint8)
    d2[:]=[0,0,0]
    #print(d2)
    #cv2.imshow('aligned_depth_frame',d2)

通过嵌套for循环对深度图遍历，映射出rgb的深度值

#遍历深度图像素点坐标（i,j）做过降低延迟处理
    for i in range(320):
        for j in range(240):
            #print(i,j)
            dis = depth_frame.get_distance(i*2, j*2)        # 获取该像素点对应的深度
            #print(dis)
            depth_coordinate = rs.rs2_deproject_pixel_to_point(depth_intr, [i*2,j*2], dis)#获取depth空间坐标
            #print(depth_coordinate)
            color_coordinate = rs.rs2_transform_point_to_point(extrinsics_depth_to_color,  depth_coordinate)#空间坐标变化
            #print(color_coordinate)
            color_pixels = rs.rs2_project_point_to_pixel(color_intr,color_coordinate)#获取rgb像素点坐标
            #开始映射
            x=int(color_pixels[0])
            y=int(color_pixels[1])
            #print(x,y)
            #print(d1[i][j])
            if x>=0 and x<640 and y>=0 and y<480:
                d2[y][x]=d1[j*2][i*2]
                d2[y-1][x-1]=d1[j*2][i*2]
                #continue

    #获取对齐的深度图        
    cv2.imshow('aligned_depth_frame',d2)

    key = cv2.waitKey(1)
    if key == 27:         # 按esc键退出
        print('esc break...')
        #cap.release()
        cv2.destroyAllWindows()
        break
    if key == ord(' '):   # 按空格键保存
        #filename = str(time.time())[:10] + ".jpg"
        #cv2.imwrite("color"+filename, frame1)
        #cv2.imwrite("infrared"+filename, frame2)
        break

最终效果如图所示