realsense d435i相机所有内外参获得

注意：imu加速度计accel的频率是100或200hz，陀螺仪gyro的频率为200或400，代码中填入其他频率是跑不通的，会报错

通过librealsense的sdk直接获取相机内外参的代码如下：

import math

import numpy as np
import pyrealsense2 as rs


# ---------------------------------------------------------------------------
# 内参：视频流（深度 / 彩色 / 红外）
# ---------------------------------------------------------------------------
def print_video_intrinsics(name, vsp):
    """打印视频流内参。vsp 为 video_stream_profile。"""
    if vsp is None:
        print(f"[WARN] {name}: 流未启用，跳过")
        return

    intr = vsp.get_intrinsics()
    K = np.array(
        [
            [intr.fx, 0, intr.ppx],
            [0, intr.fy, intr.ppy],
            [0, 0, 1.0],
        ]
    )
    fov_x = 2 * math.degrees(math.atan(intr.width / (2 * intr.fx)))
    fov_y = 2 * math.degrees(math.atan(intr.height / (2 * intr.fy)))

    print(f"\n=== {name} 内参 ===")
    print(f"  分辨率        : {intr.width} x {intr.height}")
    print(f"  焦距 fx, fy   : {intr.fx:.4f}, {intr.fy:.4f}")
    print(f"  主点 ppx, ppy : {intr.ppx:.4f}, {intr.ppy:.4f}")
    print(f"  畸变模型      : {intr.model}")
    print(f"  畸变系数      : {list(intr.coeffs)}")
    print(f"  内参矩阵 K    :\n{K}")
    print(f"  视场角 FOV    : x={fov_x:.2f}°, y={fov_y:.2f}°")


# ---------------------------------------------------------------------------
# 内参：IMU 流（加速度计 / 陀螺仪）
# ---------------------------------------------------------------------------
def print_motion_intrinsics(name, msp):
    """打印 IMU 内参。msp 为 motion_stream_profile。"""
    if msp is None:
        print(f"[WARN] {name}: 流未启用，跳过")
        return

    mintr = msp.get_motion_intrinsics()
    data = np.array(mintr.data)          # 新版 SDK 是 3x4，旧版是 3x3
    if data.size == 12:                  # [3x3 尺度/轴对齐 | 3x1 偏置]
        data = data.reshape(3, 4)
        scale_axis = data[:, :3]
        bias = data[:, 3]
    else:
        scale_axis = data.reshape(3, 3)
        bias = np.zeros(3)

    # 单位：加速度计 m/s^2，陀螺仪 rad/s
    unit = "m/s^2" if msp.stream_type() == rs.stream.accel else "rad/s"

    print(f"\n=== {name} 内参（IMU）===")
    print(f"  [说明] IMU 没有 fx/fy/ppx 那种相机内参；")
    print(f"         它的内参 = 尺度/轴对齐矩阵 + 偏置 + 噪声/偏置方差")
    print(f"  尺度/轴对齐矩阵 (3x3):\n{scale_axis}")
    print(f"  偏置向量 (3x1)      : {bias}")
    print(f"  噪声方差 noise_variances ({unit}^2): {list(mintr.noise_variances)}")
    print(f"  偏置方差 bias_variances  ({unit}^2): {list(mintr.bias_variances)}")


# ---------------------------------------------------------------------------
# 外参：两个流坐标系之间的变换
# ---------------------------------------------------------------------------
def rotation_matrix_to_euler(R):
    """3x3 旋转矩阵 -> (roll, pitch, yaw)，单位度。

    采用 ZYX 内旋约定：R = Rz(yaw) @ Ry(pitch) @ Rx(roll)
    roll 绕 X 轴、pitch 绕 Y 轴、yaw 绕 Z 轴。
    """
    sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
    if sy > 1e-6:
        roll = np.arctan2(R[2, 1], R[2, 2])
        pitch = np.arctan2(-R[2, 0], sy)
        yaw = np.arctan2(R[1, 0], R[0, 0])
    else:  # 万向锁退化（pitch ≈ ±90°）
        roll = np.arctan2(-R[1, 2], R[1, 1])
        pitch = np.arctan2(-R[2, 0], sy)
        yaw = 0.0
    return np.degrees([roll, pitch, yaw])


def print_extrinsics(name_from, name_to, p_from, p_to):
    """打印 p_from -> p_to 的外参（旋转 + 平移 + 4x4 变换矩阵 + 欧拉角）。"""
    if p_from is None or p_to is None:
        print(f"[WARN] {name_from} -> {name_to}: 流未启用，跳过")
        return

    ext = p_from.get_extrinsics_to(p_to)
    R = np.array(ext.rotation).reshape(3, 3)
    t = np.array(ext.translation).reshape(3, 1)
    T = np.vstack([np.hstack([R, t]), [0, 0, 0, 1.0]])

    roll, pitch, yaw = rotation_matrix_to_euler(R)
    # "to" 在 "from" 坐标系中的位置（get_extrinsics_to 平移的逆）
    pos_to_in_from = (-R.T @ t).ravel()

    print(f"\n=== 外参 {name_from} -> {name_to} ===")
    print(f"  旋转矩阵 R :\n{R}")
    print(f"  旋转角 (roll/pitch/yaw) : {roll:.4f}° / {pitch:.4f}° / {yaw:.4f}°")
    print(f"  平移向量 t (P_to = R·P_from + t) : {t.ravel()}")
    print(f"  [{name_to} 在 {name_from} 坐标系中的位置] : {pos_to_in_from}")
    print(f"  变换矩阵 T :\n{T}")


def main():
    pipeline = rs.pipeline()
    config = rs.config()

    # 启用所有需要读取内/外参的流
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
    config.enable_stream(rs.stream.infrared, 1, 640, 480, rs.format.y8, 30)
    config.enable_stream(rs.stream.infrared, 2, 640, 480, rs.format.y8, 30)
    config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, 200)
    config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, 200)

    try:
        profile = pipeline.start(config)
    except Exception as e:
        print(f"[ERROR] 无法启动相机，请检查连接: {e}")
        return

    # 各流的 profile（用于读内参 + 求外参）
    p_depth = profile.get_stream(rs.stream.depth)
    p_color = profile.get_stream(rs.stream.color)
    p_ir1 = profile.get_stream(rs.stream.infrared, 1)
    p_ir2 = profile.get_stream(rs.stream.infrared, 2)
    p_accel = profile.get_stream(rs.stream.accel)
    p_gyro = profile.get_stream(rs.stream.gyro)

    # 视频流内参
    print_video_intrinsics("深度 Depth", p_depth.as_video_stream_profile() if p_depth else None)
    print_video_intrinsics("彩色 Color", p_color.as_video_stream_profile() if p_color else None)
    print_video_intrinsics("红外 IR1", p_ir1.as_video_stream_profile() if p_ir1 else None)
    print_video_intrinsics("红外 IR2", p_ir2.as_video_stream_profile() if p_ir2 else None)

    # IMU 内参
    print_motion_intrinsics("加速度计 Accel", p_accel.as_motion_stream_profile() if p_accel else None)
    print_motion_intrinsics("陀螺仪 Gyro", p_gyro.as_motion_stream_profile() if p_gyro else None)

    # 外参（D435i 常用组合）
    print_extrinsics("深度 Depth", "彩色 Color", p_depth, p_color)
    print_extrinsics("深度 Depth", "红外 IR1", p_depth, p_ir1)
    print_extrinsics("红外 IR1", "红外 IR2", p_ir1, p_ir2)
    print_extrinsics("红外 IR2", "红外 IR1", p_ir2, p_ir1)
    print_extrinsics("陀螺仪 Gyro", "加速度计 Accel", p_gyro, p_accel)
    print_extrinsics("深度 Depth", "陀螺仪 Gyro", p_depth, p_gyro)
    print_extrinsics("深度 Depth", "加速度计 Accel", p_depth, p_accel)
    print_extrinsics("彩色 Color", "陀螺仪 Gyro", p_color, p_gyro)

    pipeline.stop()


if __name__ == "__main__":
    main()