intel realsense計算指定像素真實深度與像素座標系轉相機座標系

intel realsense深度轉真實距離與像素座標系轉相機座標系

1. 深度轉真實距離

1. 1初始化配置

import pyrealsense2 as rs
# 相機配置
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, WIDTH, HEIGHT, rs.format.z16, 60)
config.enable_stream(rs.stream.color, WIDTH, HEIGHT, rs.format.rgb8, 60)

profile = pipeline.start(config)
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
# 獲取相機內參
intr = color_frame.profile.as_video_stream_profile().intrinsics
camera_parameters = {'fx': intr.fx, 'fy': intr.fy,
                     'ppx': intr.ppx, 'ppy': intr.ppy,
                     'height': intr.height, 'width': intr.width,
                     'depth_scale': profile.get_device().first_depth_sensor().get_depth_scale()
                     }
# 保存內參到本地
with open('./intrinsics.json', 'w') as fp:
    json.dump(camera_parameters, fp)
# 圖像對齊
align_to = rs.stream.color
align = rs.align(align_to)

1.2 獲取圖像

• depth_intrin 在下一步座標系轉換用到

frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)

aligned_depth_frame = aligned_frames.get_depth_frame()
# 深度參數，像素座標系轉相機座標系用到
depth_intrin = aligned_depth_frame.profile.as_video_stream_profile().intrinsics
color_frame = aligned_frames.get_color_frame()

# 深度圖
d = np.asanyarray(aligned_depth_frame.get_data())
# 彩色圖
image_np = np.asanyarray(color_frame.get_data())
# 輸入像素的x和y計算真實距離
dis = aligned_depth_frame.get_distance(x, y)

2. 像素座標系轉相機座標系

• depth_intrin 從上一步獲取
• x 像素點的x
• y 像素點的y
• dis 上一步計算的真實距離

rs2_deproject_pixel_to_point輸入的dis與輸出的距離是一樣的，改變的只是x與y

camera_coordinate = rs.rs2_deproject_pixel_to_point(intrin=depth_intrin, pixel=[x, y], depth=dis)