1.前言
相机d435i 从背面看相机
从左往右：
彩色镜头，左红外镜头，红外激光发生器，右红外镜头
深度镜头默认与与左红外镜头重合，深度镜头的内参就左红外镜头。
当采用对齐至彩色图的深度图时，深度镜头与彩色镜头重合，深度镜头的内参就是彩色镜头。
2.代码及参数
对齐彩色图的深度图
#2025825
"""
这里参考csdn: https://blog.csdn.net/acanab/article/details/138762830?spm=1001.2014.3001.5506
读取 对齐的彩色镜头的深度帧 到 彩色帧的 外参数，
demo03
读取 未对齐的彩色镜头的深度帧 到 彩色帧的 外参数，
"""
import pyrealsense2 as rs
import numpy as np
import cv2
#配置深度和视频流
#创建一个pipeline对象，用于管理数据流，以及一个config对象，用于配置相机流。
pipeline = rs.pipeline()
config = rs.config()
#配置深度和颜色流的参数，包括分辨率、格式和帧率。
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
# config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30) #蓝色变红色， 黄色变蓝色
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) #正常肉眼颜色
config.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 30) # 第一个红外摄像头 左#启用红外摄像头流，索引为1，分辨率为1280x720，格式为Y8，帧率为30fps。
config.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 30) # 第二个红外摄像头，右
# Start streaming
#根据配置启动相机流
conf = pipeline.start(config)
align = rs.align(rs.stream.color) # 这个函数用于 将深度图像 与彩色图像对齐
# align = rs.align(rs.stream.depth) #2025.3.13尝试将 彩色图 与深度图对应,该方法没有用
depth_sensor = conf.get_device().first_depth_sensor()
depth_sensor.set_option(rs.option.emitter_enabled, 1) # 0关闭红外激光,1开启
try:
while True:
# Wait for a coherent pair of frames: depth and color
# 等待直到有新的帧可用，并分别获取深度帧和颜色帧。如果任一帧丢失，则跳过当前循环。
frames = pipeline.wait_for_frames()
# 对齐彩色和深度图
aligned_frames = align.process(frames) # 获取对齐帧，将深度框与颜色框对齐
aligned_depth_frame = aligned_frames.get_depth_frame() # 获取深度帧
aligned_color_frame = aligned_frames.get_color_frame() # 获取对齐帧中的的color帧
# 对齐的两个红外图像
aligned_infrared_frame_1 = aligned_frames.get_infrared_frame(1)
aligned_infrared_frame_2 = aligned_frames.get_infrared_frame(2)
# Validate that both frames are valid
if not aligned_depth_frame or not aligned_color_frame:
continue
# 获取图像，realsense刚启动的时候图像会有一些失真，我们保存第100帧图片。
for i in range(1000):
aligned_depth_frame = aligned_frames.get_depth_frame() # 获取深度帧
aligned_color_frame = aligned_frames.get_color_frame() # 获取对齐帧中的的color帧
# 对齐的两个红外图像
aligned_infrared_frame_1 = aligned_frames.get_infrared_frame(1)
aligned_infrared_frame_2 = aligned_frames.get_infrared_frame(2)
# 获取内参
dprofile = aligned_depth_frame.get_profile()
cprofile = aligned_color_frame.get_profile()
lprofile = aligned_infrared_frame_1.get_profile()
rprofile = aligned_infrared_frame_2.get_profile()
cvsprofile = rs.video_stream_profile(cprofile)
dvsprofile = rs.video_stream_profile(dprofile)
lvsprofile = rs.video_stream_profile(lprofile)
rvsprofile = rs.video_stream_profile(rprofile)
color_intrin = cvsprofile.get_intrinsics()
print(f"color_intrin={color_intrin}")
depth_intrin = dvsprofile.get_intrinsics()
print(f"depth_intrin={depth_intrin}")
ir_left_intrin = lvsprofile.get_intrinsics()
print(f"ir_left_intrin={ir_left_intrin}")
ir_right_intrin = rvsprofile.get_intrinsics()
print(f"ir_right_intrin={ir_right_intrin}")
# 外参
extrin_d2c = dprofile.get_extrinsics_to(cprofile)
print(f"extrin_d2c = {extrin_d2c}")
extrin_c2d = cprofile.get_extrinsics_to(dprofile)
print(f"extrin_c2d = {extrin_c2d}")
extrin_c2c = cprofile.get_extrinsics_to(cprofile)
print(f"extrin_c2c = {extrin_c2c}")
extrin_d2d = dprofile.get_extrinsics_to(dprofile)
print(f"extrin_d2d = {extrin_d2d}")
#红外摄像头
extrin_l2r = lprofile.get_extrinsics_to(rprofile)
print(f"extrin_l2r = {extrin_l2r}")
extrin_r2l = rprofile.get_extrinsics_to(lprofile)
print(f"extrin_r2l = {extrin_r2l}")
extrin_r2r = rprofile.get_extrinsics_to(rprofile)
print(f"extrin_r2r = {extrin_r2r}")
extrin_l2l = lprofile.get_extrinsics_to(lprofile)
print(f"extrin_l2l = {extrin_l2l}")
#彩色镜头与红外两个镜头
extrin_c2l = cprofile.get_extrinsics_to(lprofile)
print(f"extrin_c2l = {extrin_c2l}")
extrin_l2c = lprofile.get_extrinsics_to(cprofile)
print(f"extrin_l2c = {extrin_l2c}")
extrin_c2r = cprofile.get_extrinsics_to(rprofile)
print(f"extrin_c2r = {extrin_c2r}")
extrin_r2c = rprofile.get_extrinsics_to(cprofile)
print(f"extrin_r2c = {extrin_r2c}")
#深度镜头与红外两个镜头
extrin_d2l = dprofile.get_extrinsics_to(lprofile)
print(f"extrin_d2l = {extrin_d2l}")
extrin_l2d = lprofile.get_extrinsics_to(dprofile)
print(f"extrin_l2d = {extrin_l2d}")
extrin_d2r = dprofile.get_extrinsics_to(rprofile)
print(f"extrin_d2r = {extrin_d2r}")
extrin_r2d = rprofile.get_extrinsics_to(dprofile)
print(f"extrin_r2d = {extrin_r2d}")
# 提取旋转矩阵和平移向量
rotation_matrix = extrin_d2c.rotation # 3×3 旋转矩阵
translation_vector = extrin_d2c.translation # 3×1 平移向量
print(f"旋转矩阵: {rotation_matrix}")
print(f"平移向量: {translation_vector}")
"""
extrin_l2r.rotation # 长度为 9 的 list，按行主序展开的 3×3 旋转矩阵
extrin_l2r.translation # 长度为 3 的 list，平移向量 [tx, ty, tz]（单位：米）
rotation 是把 lprofile 坐标系 中的 3D 点旋转到 rprofile 坐标系 的旋转矩阵（按行展开）。
translation 是 rprofile 原点 在 lprofile 坐标系 中的坐标。
"""
# # 使用外参将深度坐标系的点转换到彩色坐标系
# depth_point = [x, y, z] # 深度坐标系中的点
# color_point = np.dot(rotation_matrix, depth_point) + translation_vector
#在退出循环或出现异常时，停止相机流。这确保了即使在脚本异常结束时，相机资源也能被正确释放。
finally:
# Stop streaming
pipeline.stop()
"""
demo01:未对齐的彩色镜头和深度镜头相对位姿关系， 深度镜头转换到彩色镜头坐标系的位姿变换矩阵
旋转矩阵: [0.9999870657920837, -0.005001777317374945, 0.00089955487055704, 0.004999833647161722, 0.9999852180480957, 0.0021501965820789337, -0.0009102963376790285, -0.002145671285688877, 0.9999972581863403]
平移向量: [0.014798061922192574, 4.7961682867025957e-05, 8.807632548268884e-05]
demo02 #对齐彩色图的深度图
color_intrin=[ 1280x720 p[648.297 368.739] f[910.241 909.903] Inverse Brown Conrady [0 0 0 0 0] ]
depth_intrin=[ 1280x720 p[648.297 368.739] f[910.241 909.903] Inverse Brown Conrady [0 0 0 0 0] ]
#由于此时深度相机与坐标系与彩色相机坐标系重合， 因此 深度相机为彩色相机， 因此 深度相机内参等于彩色相机内参（所以我之前使用的彩色相机内参是错误的！！！！，是未对齐的深度相机内参
ir_left_intrin=[ 1280x720 p[640.84 356.49] f[646.526 646.526] Brown Conrady [0 0 0 0 0] ]
ir_right_intrin=[ 1280x720 p[640.84 356.49] f[646.526 646.526] Brown Conrady [0 0 0 0 0] ]
extrin_d2c = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1] #由于采用的是 对齐彩色图的深度图， 与彩色图， 因此两个图坐标系其实已经重合 因此R为单位矩阵，t为0
translation: [0, 0, 0]
extrin_c2d = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [-0, -0, -0]
extrin_c2c = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_d2d = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_l2r = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [-0.0500014, 0, 0] #左相机 → 右相机 的刚体变换 , 右相机原点位于左相机坐标系中 X 轴负方向 5.00 cm 处。 #说明左相机x轴正方向是朝向 左边（背对着看相机）
extrin_r2l = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0.0500014, 0, 0] #右相机 → 左相机 的刚体变换 , 左相机原点位于右相机坐标系中 X 轴正方向 5.00 cm 处。 #说明右相机x轴正方向是朝向 左边（背对着看相机）
extrin_r2r = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_l2l = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_c2l = rotation: [0.999987, 0.00499983, -0.000910296, -0.00500178, 0.999985, -0.00214567, 0.000899555, 0.0021502, 0.999997] #旋转角度小，几乎无旋转
translation: [-0.0147977, -0.000122138, -7.45026e-05] #左相机原点在 彩色相机坐标系 中的坐标， 左相机原点位于彩色相机坐标系中 X 轴负方向 1.48 cm 处， #说明彩色相机x轴正方向是朝向 左边（背对着看相机） （眼睛正面看（相当于看镜面），彩色相机X轴正方向是 朝水平右边）
extrin_l2c = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
translation: [0.0147981, 4.79617e-05, 8.80763e-05] #彩色相机原点在 左相机坐标系 中的坐标， 彩色相机原点位于左相机坐标系中 X 轴正方向 1.48 cm 处， #说明左相机x轴正方向是朝向 左边（背对着看相机） （眼睛正面看（相当于看镜面），左相机X轴正方向是 朝水平右边）
extrin_c2r = rotation: [0.999987, 0.00499983, -0.000910296, -0.00500178, 0.999985, -0.00214567, 0.000899555, 0.0021502, 0.999997]
translation: [-0.0647991, -0.000122138, -7.45026e-05]
extrin_r2c = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
translation: [0.0647988, -0.000202134, 0.000133055] #彩色相机 → 右红外相机的变换，右相机原点在 彩色相机坐标系 中的坐标，彩色相机位于 右红外相机X轴正方向 约 6.48 cm 处 # #说明右相机x轴正方向是朝向 左边（背对着看相机）
#X 方向 位移 ≈ 6.48 cm，是彩色-左红外间距（1.48 cm）的 3 倍多，正好对应 RealSense 双目基线 ≈ 5 cm。
extrin_d2l = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0] #深度坐标系 → 左红外坐标系 的变换， 深度坐标系与左红外坐标系完全重合 ！！！！！！（这个在后面也有证明，即在为对齐的深度图，和彩色图的外参中，t(x,y,z)=(1.48,0,0)cm
extrin_l2d = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
translation: [0.0147981, 4.79617e-05, 8.80763e-05] #左红外坐标系 → 深度坐标系 的变换, 这里： 由于彩色相机与深度相机重合，因此 深度相机原点 在左相机坐标系X轴正方形1.48cm 处。
# #SDK/固件 bug：RealSense 在某些固件版本或配置下，会把 extrinsics_d2l 全部置零（单位阵 + 零平移），而 extrinsics_l2d 实际给出的是 L→C（左红外→彩色） 的数据，导致张冠李戴。
# #深度坐标系在驱动里被强行绑定到左红外，因此 SDK 直接返回“零变换”作为 D→L，但反向却保留了正确的外参。
#extrin_d2r = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
#translation: [-0.0500014, 0, 0] #深度坐标系 → 右红外坐标系的变换， 深度坐标系与左红外坐标系完全重合， 因此 右相机原点 在深度相机坐标系的X轴反方向5cm处
#extrin_r2d = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
#translation: [0.0647988, -0.000202134, 0.000133055] #右红外坐标系 → 深度坐标系的变换， 由于彩色相机与深度相机坐标系重合， 深度相机原点 在右相机坐标系X轴正方向6.48cm 处
#注意这里有矛盾： 可能是SDK内部设定的原因
1.深度相机坐标系与左相机坐标系重合x 换一个说法：左相机坐标系与深度相机坐标系重合
2. 深度相机坐标系与彩色相机坐标系重合
旋转矩阵: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
平移向量: [0.0, 0.0, 0.0]
"""未对齐彩色图的深度图
#2025825
"""
这里参考csdn: https://blog.csdn.net/acanab/article/details/138762830?spm=1001.2014.3001.5506
demo03
读取 未对齐的彩色镜头的深度帧 到 彩色帧的 外参数，
"""
import pyrealsense2 as rs
import numpy as np
import cv2
#配置深度和视频流
#创建一个pipeline对象，用于管理数据流，以及一个config对象，用于配置相机流。
pipeline = rs.pipeline()
config = rs.config()
#配置深度和颜色流的参数，包括分辨率、格式和帧率。
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
# config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30) #蓝色变红色， 黄色变蓝色
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) #正常肉眼颜色
config.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 30) # 第一个红外摄像头 左#启用红外摄像头流，索引为1，分辨率为1280x720，格式为Y8，帧率为30fps。
config.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 30) # 第二个红外摄像头，右
# Start streaming
#根据配置启动相机流
conf = pipeline.start(config)
# align = rs.align(rs.stream.color) # 这个函数用于 将深度图像 与彩色图像对齐
# ######align = rs.align(rs.stream.depth) #2025.3.13尝试将 彩色图 与深度图对应,该方法没有用
depth_sensor = conf.get_device().first_depth_sensor()
depth_sensor.set_option(rs.option.emitter_enabled, 1) # 0关闭红外激光,1开启
try:
while True:
# Wait for a coherent pair of frames: depth and color
# 等待直到有新的帧可用，并分别获取深度帧和颜色帧。如果任一帧丢失，则跳过当前循环。
frames = pipeline.wait_for_frames()
# # 对齐彩色和深度图
# aligned_frames = align.process(frames) # 获取对齐帧，将深度框与颜色框对齐
# aligned_depth_frame = aligned_frames.get_depth_frame() # 获取深度帧
# aligned_color_frame = aligned_frames.get_color_frame() # 获取对齐帧中的的color帧
# # 对齐的两个红外图像
# aligned_infrared_frame_1 = aligned_frames.get_infrared_frame(1)
# aligned_infrared_frame_2 = aligned_frames.get_infrared_frame(2)
# #未对齐
#未对齐的彩色和深度图
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
#未对齐的两个红外图像
# 获取红外图像
infrared_frame_1 = frames.get_infrared_frame(1)
infrared_frame_2 = frames.get_infrared_frame(2)
# Validate that both frames are valid
if not depth_frame or not color_frame:
continue
# 获取图像，realsense刚启动的时候图像会有一些失真，我们保存第100帧图片。
for i in range(1000):
depth_frame = frames.get_depth_frame() # 获取深度帧
color_frame = frames.get_color_frame() # 获取对齐帧中的的color帧
# 对齐的两个红外图像
infrared_frame_1 = frames.get_infrared_frame(1)
infrared_frame_2 = frames.get_infrared_frame(2)
# 获取内参
dprofile = depth_frame.get_profile()
cprofile = color_frame.get_profile()
lprofile = infrared_frame_1.get_profile()
rprofile = infrared_frame_2.get_profile()
cvsprofile = rs.video_stream_profile(cprofile)
dvsprofile = rs.video_stream_profile(dprofile)
lvsprofile = rs.video_stream_profile(lprofile)
rvsprofile = rs.video_stream_profile(rprofile)
color_intrin = cvsprofile.get_intrinsics()
print(f"color_intrin={color_intrin}")
depth_intrin = dvsprofile.get_intrinsics()
print(f"depth_intrin={depth_intrin}")
ir_left_intrin = lvsprofile.get_intrinsics()
print(f"ir_left_intrin={ir_left_intrin}")
ir_right_intrin = rvsprofile.get_intrinsics()
print(f"ir_right_intrin={ir_right_intrin}")
# 外参
extrin_d2c = dprofile.get_extrinsics_to(cprofile)
print(f"extrin_d2c = {extrin_d2c}")
extrin_c2d = cprofile.get_extrinsics_to(dprofile)
print(f"extrin_c2d = {extrin_c2d}")
extrin_c2c = cprofile.get_extrinsics_to(cprofile)
print(f"extrin_c2c = {extrin_c2c}")
extrin_d2d = dprofile.get_extrinsics_to(dprofile)
print(f"extrin_d2d = {extrin_d2d}")
#红外摄像头
extrin_l2r = lprofile.get_extrinsics_to(rprofile)
print(f"extrin_l2r = {extrin_l2r}")
extrin_r2l = rprofile.get_extrinsics_to(lprofile)
print(f"extrin_r2l = {extrin_r2l}")
extrin_r2r = rprofile.get_extrinsics_to(rprofile)
print(f"extrin_r2r = {extrin_r2r}")
extrin_l2l = lprofile.get_extrinsics_to(lprofile)
print(f"extrin_l2l = {extrin_l2l}")
#彩色镜头与红外两个镜头
extrin_c2l = cprofile.get_extrinsics_to(lprofile)
print(f"extrin_c2l = {extrin_c2l}")
extrin_l2c = lprofile.get_extrinsics_to(cprofile)
print(f"extrin_l2c = {extrin_l2c}")
extrin_c2r = cprofile.get_extrinsics_to(rprofile)
print(f"extrin_c2r = {extrin_c2r}")
extrin_r2c = rprofile.get_extrinsics_to(cprofile)
print(f"extrin_r2c = {extrin_r2c}")
#深度镜头与红外两个镜头
extrin_d2l = dprofile.get_extrinsics_to(lprofile)
print(f"extrin_d2l = {extrin_d2l}")
extrin_l2d = lprofile.get_extrinsics_to(dprofile)
print(f"extrin_l2d = {extrin_l2d}")
extrin_d2r = dprofile.get_extrinsics_to(rprofile)
print(f"extrin_d2r = {extrin_d2r}")
extrin_r2d = rprofile.get_extrinsics_to(dprofile)
print(f"extrin_r2d = {extrin_r2d}")
# 提取旋转矩阵和平移向量
rotation_matrix = extrin_d2c.rotation # 3×3 旋转矩阵
translation_vector = extrin_d2c.translation # 3×1 平移向量
print(f"旋转矩阵: {rotation_matrix}")
print(f"平移向量: {translation_vector}")
"""
extrin_l2r.rotation # 长度为 9 的 list，按行主序展开的 3×3 旋转矩阵
extrin_l2r.translation # 长度为 3 的 list，平移向量 [tx, ty, tz]（单位：米）
rotation 是把 lprofile 坐标系 中的 3D 点旋转到 rprofile 坐标系 的旋转矩阵（按行展开）。
translation 是 rprofile 原点 在 lprofile 坐标系 中的坐标。
"""
# # 使用外参将深度坐标系的点转换到彩色坐标系
# depth_point = [x, y, z] # 深度坐标系中的点
# color_point = np.dot(rotation_matrix, depth_point) + translation_vector
#在退出循环或出现异常时，停止相机流。这确保了即使在脚本异常结束时，相机资源也能被正确释放。
finally:
# Stop streaming
pipeline.stop()
"""
demo01:
旋转矩阵: [0.9999870657920837, -0.005001777317374945, 0.00089955487055704, 0.004999833647161722, 0.9999852180480957, 0.0021501965820789337, -0.0009102963376790285, -0.002145671285688877, 0.9999972581863403]
平移向量: [0.014798061922192574, 4.7961682867025957e-05, 8.807632548268884e-05]
demo02
color_intrin=[ 1280x720 p[648.297 368.739] f[910.241 909.903] Inverse Brown Conrady [0 0 0 0 0] ]
depth_intrin=[ 1280x720 p[648.297 368.739] f[910.241 909.903] Inverse Brown Conrady [0 0 0 0 0] ]
ir_left_intrin=[ 1280x720 p[640.84 356.49] f[646.526 646.526] Brown Conrady [0 0 0 0 0] ]
ir_right_intrin=[ 1280x720 p[640.84 356.49] f[646.526 646.526] Brown Conrady [0 0 0 0 0] ]
extrin_d2c = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1] #由于采用的是 对齐彩色图的深度图， 与彩色图， 因此两个图坐标系其实已经重合 因此R为单位矩阵，t为0
translation: [0, 0, 0]
extrin_c2d = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [-0, -0, -0]
extrin_c2c = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_d2d = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_l2r = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [-0.0500014, 0, 0] #左相机 → 右相机 的刚体变换 , 右相机原点位于左相机坐标系中 X 轴负方向 5.00 cm 处。 #说明左相机x轴正方向是朝向 左边（背对着看相机）
extrin_r2l = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0.0500014, 0, 0] #右相机 → 左相机 的刚体变换 , 左相机原点位于右相机坐标系中 X 轴正方向 5.00 cm 处。 #说明右相机x轴正方向是朝向 左边（背对着看相机）
extrin_r2r = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_l2l = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_c2l = rotation: [0.999987, 0.00499983, -0.000910296, -0.00500178, 0.999985, -0.00214567, 0.000899555, 0.0021502, 0.999997] #旋转角度小，几乎无旋转
translation: [-0.0147977, -0.000122138, -7.45026e-05] #左相机原点在 彩色相机坐标系 中的坐标， 左相机原点位于彩色相机坐标系中 X 轴负方向 1.48 cm 处， #说明彩色相机x轴正方向是朝向 左边（背对着看相机） （眼睛正面看（相当于看镜面），彩色相机X轴正方向是 朝水平右边）
extrin_l2c = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
translation: [0.0147981, 4.79617e-05, 8.80763e-05] #彩色相机原点在 左相机坐标系 中的坐标， 彩色相机原点位于左相机坐标系中 X 轴正方向 1.48 cm 处， #说明左相机x轴正方向是朝向 左边（背对着看相机） （眼睛正面看（相当于看镜面），左相机X轴正方向是 朝水平右边）
extrin_c2r = rotation: [0.999987, 0.00499983, -0.000910296, -0.00500178, 0.999985, -0.00214567, 0.000899555, 0.0021502, 0.999997]
translation: [-0.0647991, -0.000122138, -7.45026e-05]
extrin_r2c = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
translation: [0.0647988, -0.000202134, 0.000133055] #彩色相机 → 右红外相机的变换，右相机原点在 彩色相机坐标系 中的坐标，彩色相机位于 右红外相机X轴正方向 约 6.48 cm 处 # #说明右相机x轴正方向是朝向 左边（背对着看相机）
#X 方向 位移 ≈ 6.48 cm，是彩色-左红外间距（1.48 cm）的 3 倍多，正好对应 RealSense 双目基线 ≈ 5 cm。
extrin_d2l = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0] #深度坐标系 → 左红外坐标系 的变换， 深度坐标系与左红外坐标系完全重合 ！！！！！！（这个在后面也有证明，即在为对齐的深度图，和彩色图的外参中，t(x,y,z)=(1.48,0,0)cm
extrin_l2d = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
translation: [0.0147981, 4.79617e-05, 8.80763e-05] #左红外坐标系 → 深度坐标系 的变换, 这里： 由于彩色相机与深度相机重合，因此 深度相机原点 在左相机坐标系X轴正方形1.48cm 处。
# #SDK/固件 bug：RealSense 在某些固件版本或配置下，会把 extrinsics_d2l 全部置零（单位阵 + 零平移），而 extrinsics_l2d 实际给出的是 L→C（左红外→彩色） 的数据，导致张冠李戴。
# #深度坐标系在驱动里被强行绑定到左红外，因此 SDK 直接返回“零变换”作为 D→L，但反向却保留了正确的外参。
#extrin_d2r = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
#translation: [-0.0500014, 0, 0] #深度坐标系 → 右红外坐标系的变换， 深度坐标系与左红外坐标系完全重合， 因此 右相机原点 在深度相机坐标系的X轴反方向5cm处
#extrin_r2d = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
#translation: [0.0647988, -0.000202134, 0.000133055] #右红外坐标系 → 深度坐标系的变换， 由于彩色相机与深度相机坐标系重合， 深度相机原点 在右相机坐标系X轴正方向6.48cm 处
#注意这里有矛盾： 可能是SDK内部设定的原因
1.深度相机坐标系与左相机坐标系重合x 换一个说法：左相机坐标系与深度相机坐标系重合
2. 深度相机坐标系与彩色相机坐标系重合
旋转矩阵: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
平移向量: [0.0, 0.0, 0.0]
"""
"""
demo03
color_intrin=[ 1280x720 p[648.297 368.739] f[910.241 909.903] Inverse Brown Conrady [0 0 0 0 0] ]
depth_intrin=[ 1280x720 p[640.84 356.49] f[646.526 646.526] Brown Conrady [0 0 0 0 0] ]
ir_left_intrin=[ 1280x720 p[640.84 356.49] f[646.526 646.526] Brown Conrady [0 0 0 0 0] ]
ir_right_intrin=[ 1280x720 p[640.84 356.49] f[646.526 646.526] Brown Conrady [0 0 0 0 0] ]
#深度相机与两个红外相机的 内参一致
extrin_d2c = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
translation: [0.0147981, 4.79617e-05, 8.80763e-05] #深度坐标系 → 彩色相机坐标系的变换，彩色相机原点在 深度坐标系 中的坐标：彩色相机原点位于深度相机坐标系中 X 轴正方向 1.48 cm 处
extrin_c2d = rotation: [0.999987, 0.00499983, -0.000910296, -0.00500178, 0.999985, -0.00214567, 0.000899555, 0.0021502, 0.999997]
translation: [-0.0147977, -0.000122138, -7.45026e-05] #彩色相机坐标系 → 深度坐标系的变换，深度坐标系原点在 彩色相机坐标系 中的坐标：深度相机原点位于彩色相机坐标系中 X 轴负方向 1.48 cm 处
#深度坐标系与彩色相机坐标系之间 仅有约 1.48 cm 的 X 向偏移 和 < 0.5° 的微小旋转
extrin_c2c = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_d2d = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_l2r = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [-0.0500014, 0, 0]
extrin_r2l = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0.0500014, 0, 0]
extrin_r2r = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_l2l = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_c2l = rotation: [0.999987, 0.00499983, -0.000910296, -0.00500178, 0.999985, -0.00214567, 0.000899555, 0.0021502, 0.999997]
translation: [-0.0147977, -0.000122138, -7.45026e-05]
extrin_l2c = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
translation: [0.0147981, 4.79617e-05, 8.80763e-05]
extrin_c2r = rotation: [0.999987, 0.00499983, -0.000910296, -0.00500178, 0.999985, -0.00214567, 0.000899555, 0.0021502, 0.999997]
translation: [-0.0647991, -0.000122138, -7.45026e-05]
extrin_r2c = rotation: [0.999987, -0.00500178, 0.000899555, 0.00499983, 0.999985, 0.0021502, -0.000910296, -0.00214567, 0.999997]
translation: [0.0647988, -0.000202134, 0.000133055]
extrin_d2l = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
extrin_l2d = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0, 0, 0]
## 说明 深度相机与左相机 重合
extrin_d2r = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [-0.0500014, 0, 0]
extrin_r2d = rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1]
translation: [0.0500014, 0, 0]
旋转矩阵: [0.9999870657920837, -0.005001777317374945, 0.00089955487055704, 0.004999833647161722, 0.9999852180480957, 0.0021501965820789337, -0.0009102963376790285, -0.002145671285688877, 0.9999972581863403]
平移向量: [0.014798061922192574, 4.7961682867025957e-05, 8.807632548268884e-05]
"""读取镜头内参：
由于上述代码获取的相机内参仅包含小数点后三位，因此换一种参数读取方式。
#2025825
"""
读取深度相机内参 小数点许多位
"""
"""
参考1：
https://blog.csdn.net/weixin_42776565/article/details/137652882?spm=1001.2014.3001.5506
其获取的d405相机内参被我用于 生成m18k 蘑菇点云
"""
import pyrealsense2 as rs
import numpy as np
import cv2
#配置深度和视频流
#创建一个pipeline对象，用于管理数据流，以及一个config对象，用于配置相机流。
pipeline = rs.pipeline()
config = rs.config()
#配置深度和颜色流的参数，包括分辨率、格式和帧率。
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
# config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30) #蓝色变红色， 黄色变蓝色
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) #正常肉眼颜色
config.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 30) # 第一个红外摄像头 左#启用红外摄像头流，索引为1，分辨率为1280x720，格式为Y8，帧率为30fps。
config.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 30) # 第二个红外摄像头，右
# Start streaming
#根据配置启动相机流
conf = pipeline.start(config)
# align = rs.align(rs.stream.color) # 这个函数用于 将深度图像 与彩色图像对齐
# ######align = rs.align(rs.stream.depth) #2025.3.13尝试将 彩色图 与深度图对应,该方法没有用
depth_sensor = conf.get_device().first_depth_sensor()
depth_sensor.set_option(rs.option.emitter_enabled, 1) # 0关闭红外激光,1开启
try:
while True:
# Wait for a coherent pair of frames: depth and color
# 等待直到有新的帧可用，并分别获取深度帧和颜色帧。如果任一帧丢失，则跳过当前循环。
frames = pipeline.wait_for_frames()
# # 对齐彩色和深度图
# aligned_frames = align.process(frames) # 获取对齐帧，将深度框与颜色框对齐
# aligned_depth_frame = aligned_frames.get_depth_frame() # 获取深度帧
# aligned_color_frame = aligned_frames.get_color_frame() # 获取对齐帧中的的color帧
# # 对齐的两个红外图像
# aligned_infrared_frame_1 = aligned_frames.get_infrared_frame(1)
# aligned_infrared_frame_2 = aligned_frames.get_infrared_frame(2)
# #未对齐
#未对齐的彩色和深度图
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
#未对齐的两个红外图像
# 获取红外图像
infrared_frame_1 = frames.get_infrared_frame(1)
infrared_frame_2 = frames.get_infrared_frame(2)
# Validate that both frames are valid
if not depth_frame or not color_frame:
continue
# 获取图像，realsense刚启动的时候图像会有一些失真，我们保存第100帧图片。
for i in range(1000):
depth_frame = frames.get_depth_frame() # 获取深度帧
color_frame = frames.get_color_frame() # 获取对齐帧中的的color帧
# 对齐的两个红外图像
infrared_frame_1 = frames.get_infrared_frame(1)
infrared_frame_2 = frames.get_infrared_frame(2)
# Get device product line for setting a supporting resolution
profile = pipeline.get_active_profile()
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print(f"depth_scale={depth_scale}")
# Get the depth sensor's depth stream profile and extract intrinsic parameters
#深度相机
# depth_stream = profile.get_stream(rs.stream.depth)
# intrinsics = depth_stream.as_video_stream_profile().get_intrinsics()
#彩色相机
# color_stream = profile.get_stream(rs.stream.color)
# intrinsics = color_stream.as_video_stream_profile().get_intrinsics()
# infrared 没有左右红外 相机
infrared_stream = profile.get_stream(rs.stream.infrared)
intrinsics = infrared_stream.as_video_stream_profile().get_intrinsics()
# Print intrinsics
print("Width:", intrinsics.width)
print("Height:", intrinsics.height)
print("PPX:", intrinsics.ppx)
print("PPY:", intrinsics.ppy)
print("FX:", intrinsics.fx)
print("FY:", intrinsics.fy)
print("Distortion Model:", intrinsics.model)
print("Coefficients:", intrinsics.coeffs)
#在退出循环或出现异常时，停止相机流。这确保了即使在脚本异常结束时，相机资源也能被正确释放。
finally:
# Stop streaming
pipeline.stop()
"""
深度流
depth_scale=0.0010000000474974513
Width: 1280
Height: 720
PPX: 640.8402099609375
PPY: 356.4901123046875
FX: 646.5264282226562
FY: 646.5264282226562
Distortion Model: distortion.brown_conrady
Coefficients: [0.0, 0.0, 0.0, 0.0, 0.0]
#右红外相机
#左红外相机
Width: 1280
Height: 720
PPX: 640.8402099609375
PPY: 356.4901123046875
FX: 646.5264282226562
FY: 646.5264282226562
Distortion Model: distortion.brown_conrady
Coefficients: [0.0, 0.0, 0.0, 0.0, 0.0]
#彩色相机
Width: 1280
Height: 720
PPX: 648.2967529296875
PPY: 368.73944091796875
FX: 910.24072265625
FY: 909.9026489257812
Distortion Model: distortion.inverse_brown_conrady
Coefficients: [0.0, 0.0, 0.0, 0.0, 0.0]
如果 coeffs 非零，你会看到
coeffs: [0.123, -0.456, 0.001, 0.002, 0.000]
顺序固定：
[k1, k2, p1, p2, k3]（对应 Brown-Conrady 模型）。
深度流的内参：
depth_scale=0.0010000000474974513
Width: 640
Height: 480
PPX: 320.5041198730469
PPY: 237.89407348632812
FX: 387.9158630371094
FY: 387.9158630371094
Distortion Model: distortion.brown_conrady
Coefficients: [0.0, 0.0, 0.0, 0.0, 0.0]
网上找到，d435i深度相机，彩色 相机内参
原文链接：https://blog.csdn.net/doudou2weiwei/article/details/139934114
Depth Intrinsics: [ 640x480 p[322.02 236.768] f[391.288 391.288] Brown Conrady [0 0 0 0 0] ]
Color Intrinsics: [ 640x480 p[328.884 240.065] f[606.697 606.756] Inverse Brown Conrady [0 0 0 0 0] ]
其中：640x480是像素宽度x像素高度； p[322.02 236.768]为主点（镜头光轴与图像平面的交点）的坐标； f[391.288 391.288]为焦距参数；Brown Conrady [0 0 0 0 0]分别表示畸变模型与畸变系数
对比了两个同一型号相机，在640*480 分辨率下 深度流的内参， 发现不一样，cx,cy 差了2， fx,fy也差了4,
网上找的 d435相机内参 与d453i有区别
https://blog.csdn.net/ZNC1998/article/details/139124353?spm=1001.2014.3001.5506
[384.77294921875, 0, 324.17236328125]
[0, 384.77294921875, 236.48226928710938]
[0, 0, 1]
这些参数是RealSense深度相机的深度传感器配置信息。以下是对这些参数的解释：
depth_scale: 深度比例因子，用于将深度数据转换为实际距离。例如，深度图中的每个像素值乘以这个比例因子，即可得到实际距离（单位通常是米）。
Width: 深度图像的宽度，这里是1280像素。
Height: 深度图像的高度，这里是720像素。
PPX: 深度相机的主点X坐标，这里是640.8402099609375。
PPY: 深度相机的主点Y坐标，这里是356.4901123046875。
FX: 深度相机的焦距X，这里是646.5264282226562。
FY: 深度相机的焦距Y，这里是646.5264282226562。
Distortion Model: 畸变模型，这里是Brown-Conrady模型，用于校正镜头畸变。
Coefficients: 畸变系数，这里是五个系数，都为0，表示没有畸变。
这些参数在使用RealSense深度相机进行深度数据处理和三维重建时非常重要，它们可以帮助你将深度图像转换为实际的三维坐标，并校正图像畸变。
"""