zed 2i相机坐标系与内参、外参

相应内外参如下：

相机型号 : ZED 2i
序列号 : 31474187
固件版本 : 相机 1523 / 传感器 778

=== 左相机 Left 内参 ===
分辨率 : 1280 x 720
焦距 fx, fy : 536.4066, 536.4066
主点 cx, cy : 642.2694, 351.1544
内参矩阵 K :

\[536.40655518 0. 642.26940918

0. 536.40655518 351.15435791

0. 0. 1. \]

视场角 FOV : h=100.07°, v=67.74°, d=107.71°
畸变系数 : $0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0$

=== 右相机 Right 内参 ===
分辨率 : 1280 x 720
焦距 fx, fy : 536.4066, 536.4066
主点 cx, cy : 642.2694, 351.1544
内参矩阵 K :

\[536.40655518 0. 642.26940918

0. 536.40655518 351.15435791

0. 0. 1. \]

视场角 FOV : h=100.07°, v=67.74°, d=107.71°
畸变系数 : $0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0$

=== 加速度计 Accelerometer 参数 ===
$说明$ IMU 没有 fx/fy/cx/cy 那种相机内参；
它的参数 = 采样率 + 量程 + 分辨率 + 噪声密度 + 随机游走
采样率 : 400.0 Hz
量程 : -78.4800033569336 ~ 78.4800033569336 m/s²
分辨率 : 0.002395019633695483 m/s²
噪声密度 : 0.0004400000034365803 m/s²/√Hz
随机游走 : 0.02019999921321869 m/s²/s/√Hz

=== 陀螺仪 Gyroscope 参数 ===
$说明$ IMU 没有 fx/fy/cx/cy 那种相机内参；
它的参数 = 采样率 + 量程 + 分辨率 + 噪声密度 + 随机游走
采样率 : 400.0 Hz
量程 : -1000.0 ~ 1000.0 °/s
分辨率 : 0.030517578125 °/s
噪声密度 : 0.004991999827325344 °/s/√Hz
随机游走 : 0.042399998754262924 °/s/s/√Hz

=== 磁力计 Magnetometer 参数 ===
$说明$ IMU 没有 fx/fy/cx/cy 那种相机内参；
它的参数 = 采样率 + 量程 + 分辨率 + 噪声密度 + 随机游走
采样率 : 50.0 Hz
量程 : -2500.0 ~ 2500.0 µT
分辨率 : 0.30000001192092896 µT
噪声密度 : 0.07000000029802322 µT/√Hz
随机游走 : N/A

=== 气压计 Barometer 参数 ===
$说明$ IMU 没有 fx/fy/cx/cy 那种相机内参；
它的参数 = 采样率 + 量程 + 分辨率 + 噪声密度 + 随机游走
采样率 : 25.0 Hz
量程 : 300.0 ~ 1100.0 hPa
分辨率 : 0.009999999776482582 hPa
噪声密度 : 0.00039999998989515007 hPa/√Hz
随机游走 : N/A

=== 外参左相机 Left -> 右相机 Right ===
旋转矩阵 R :

\[1. 0. 0.

0. 1. 0.

0. 0. 1.\]

旋转角 (roll/pitch/yaw) : 0.0000° / -0.0000° / 0.0000°
平移向量 t = $右相机 Right 在左相机 Left 系下的位置$ : $0.11990686 0. 0.$
（坐标变换：P_左相机 Left = R · P_右相机 Right + t）

左相机 Left 在 右相机 Right 坐标系中的位置\] : \[-0.11990686 0. 0.

变换矩阵 T :

\[1. 0. 0. 0.11990686

0. 1. 0. 0.

0. 0. 1. 0.

0. 0. 0. 1. \]

=== 外参左相机 Left -> IMU ===
旋转矩阵 R :

\[ 0.99976206 0.01169387 -0.01841571

-0.0116109 0.99992198 0.00460632

0.01846814 -0.0043914 0.99981982\]

旋转角 (roll/pitch/yaw) : -0.2517° / -1.0582° / -0.6654°
平移向量 t = $IMU 在左相机 Left 系下的位置$ : $0.023061 -0.000217 -0.002$
（坐标变换：P_左相机 Left = R · P_IMU + t）

左相机 Left 在 IMU 坐标系中的位置\] : \[-2.30210978e-02 -6.14721674e-05 2.42532406e-03

变换矩阵 T :

\[ 9.99762058e-01 1.16938744e-02 -1.84157118e-02 2.30610017e-02

-1.16108973e-02 9.99921978e-01 4.60632052e-03 -2.17000023e-04

1.84681416e-02 -4.39140107e-03 9.99819815e-01 -2.00000009e-03

0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00\]

双目基线 baseline = 0.1199 m

注意：在运行代码与使用相机前，需要先安装zed2i相机的sdk，我的cuda是cu11.7，所以安装的sdk是ZED_SDK_Windows_cuda11.8_v4.2.5.exe，sdk链接如下：

我用夸克网盘给你分享了「ZED_SDK_Windows_cuda11.8_v4.2.5.exe」，点击链接或复制整段内容，打开「夸克网盘APP」即可获取。
/~52173aDuRT~:/
链接：https://pan.quark.cn/s/b46717dbb838?pwd=jdNr
提取码：jdNr

相机内外参直接提取的代码：

python 复制代码

"""打印 ZED2i 的相机内参 / IMU 参数 / 外参。

本文件是 realsense-435i-vio/realsense_calib_params.py 的 ZED2i 移植版。

ZED2i 与 D435i 的标定信息来源不同：
    RealSense：按流（depth/color/infrared/accel/gyro）分别取 profile；
    ZED2i   ：所有参数都在 zed.get_camera_information() 里一次性给出。

映射：
    视频流内参      -> camera_configuration.calibration_parameters.left_cam / right_cam
    IMU 参数        -> sensors_configuration.accelerometer_parameters / gyroscope_parameters
                       （采样率/量程/噪声密度/随机游走）
    外参            -> calibration_parameters.stereo_transform（左->右：右相机在左相机系下的位姿，t≈+baseline）
                       sensors_configuration.camera_imu_transform（左相机->IMU）
"""

import math

import numpy as np
import pyzed.sl as sl


# ---------------------------------------------------------------------------
# 视频流内参（左 / 右相机）
# ---------------------------------------------------------------------------
def print_video_intrinsics(name, cp):
    """打印单个相机内参。cp 为 sl.CameraParameters。"""
    if cp is None:
        print(f"[WARN] {name}: 参数不可用，跳过")
        return

    K = np.array([
        [cp.fx, 0, cp.cx],
        [0, cp.fy, cp.cy],
        [0, 0, 1.0],
    ])

    print(f"\n=== {name} 内参 ===")
    print(f"  分辨率        : {cp.image_size.width} x {cp.image_size.height}")
    print(f"  焦距 fx, fy   : {cp.fx:.4f}, {cp.fy:.4f}")
    print(f"  主点 cx, cy   : {cp.cx:.4f}, {cp.cy:.4f}")
    print(f"  内参矩阵 K    :\n{K}")
    print(f"  视场角 FOV    : h={cp.h_fov:.2f}°, v={cp.v_fov:.2f}°, d={cp.d_fov:.2f}°")
    print(f"  畸变系数      : {list(cp.disto)}")


# ---------------------------------------------------------------------------
# IMU 参数（加速度计 / 陀螺仪）
# ---------------------------------------------------------------------------
# sl.SENSORS_UNIT 枚举 -> 人类可读单位
_SENSOR_UNIT_STR = {
    sl.SENSORS_UNIT.M_SEC_2: "m/s²",
    sl.SENSORS_UNIT.DEG_SEC: "°/s",
    sl.SENSORS_UNIT.U_T: "µT",
    sl.SENSORS_UNIT.HPA: "hPa",
    sl.SENSORS_UNIT.CELSIUS: "°C",
    sl.SENSORS_UNIT.HERTZ: "Hz",
}


def _unit_str(sensor_unit):
    """把 sl.SENSORS_UNIT 枚举转成可读单位字符串。"""
    return _SENSOR_UNIT_STR.get(sensor_unit, str(sensor_unit))


def print_sensor_parameters(name, sp):
    """打印单个传感器参数。sp 为 sl.SensorParameters。"""
    if sp is None or not sp.is_available:
        print(f"\n=== {name} 参数 ===  (不可用)")
        return

    unit = _unit_str(sp.sensor_unit)
    # sensor_range 是 numpy.ndarray，形状 (2,)，[min, max]
    rng = sp.sensor_range

    print(f"\n=== {name} 参数 ===")
    print(f"  [说明] IMU 没有 fx/fy/cx/cy 那种相机内参；")
    print(f"         它的参数 = 采样率 + 量程 + 分辨率 + 噪声密度 + 随机游走")
    print(f"  采样率          : {sp.sampling_rate} Hz")
    print(f"  量程            : {rng[0]} ~ {rng[1]} {unit}")
    print(f"  分辨率          : {sp.resolution} {unit}")
    nd = "N/A" if math.isnan(sp.noise_density) else f"{sp.noise_density} {unit}/√Hz"
    rw = "N/A" if math.isnan(sp.random_walk) else f"{sp.random_walk} {unit}/s/√Hz"
    print(f"  噪声密度        : {nd}")
    print(f"  随机游走        : {rw}")


# ---------------------------------------------------------------------------
# 外参
# ---------------------------------------------------------------------------
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 transform_to_R_t(transform):
    """sl.Transform -> (R, t)。transform.m 为 4x4 numpy.ndarray（行主序）。

    布局（同 ZED SDK 文档）：
        [ R00 R01 R02 tx ]
        [ R10 R11 R12 ty ]
        [ R20 R21 R22 tz ]
        [ 0   0   0   1  ]
    """
    m = transform.m
    R = np.array(m[:3, :3], dtype=np.float64)
    t = np.array(m[:3, 3], dtype=np.float64)
    return R, t


def print_extrinsics(name_from, name_to, transform):
    """打印外参（旋转 + 平移 + 4x4 变换矩阵 + 欧拉角）。

    约定：`name_from -> name_to` 表示 `name_to` 在 `name_from` 坐标系下的位姿
    （机器人学里常见的 ``A -> B`` = B 在 A 系下的位姿，即 T_A_B）。
    平移向量 t 即 `name_to` 原点在 `name_from` 系中的位置，坐标变换为
        P_{name_from} = R · P_{name_to} + t
    """
    if transform is None:
        print(f"[WARN] {name_from} -> {name_to}: 参数不可用，跳过")
        return

    R, t = transform_to_R_t(transform)
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t

    roll, pitch, yaw = rotation_matrix_to_euler(R)
    # name_from 原点在 name_to 系中的位置（反变换 = -R^T·t）
    pos_from_in_to = (-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 = [{name_to} 在 {name_from} 系下的位置] : {t}")
    print(f"  （坐标变换：P_{name_from} = R · P_{name_to} + t）")
    print(f"  [{name_from} 在 {name_to} 坐标系中的位置] : {pos_from_in_to}")
    print(f"  变换矩阵 T :\n{T}")


def main():
    init = sl.InitParameters()
    init.coordinate_units = sl.UNIT.METER

    zed = sl.Camera()
    status = zed.open(init)
    if status != sl.ERROR_CODE.SUCCESS:
        print(f"[ERROR] 无法打开 ZED2i，请检查连接: {repr(status)}")
        return

    info = zed.get_camera_information()
    calib = info.camera_configuration.calibration_parameters
    sensors = info.sensors_configuration

    print(f"相机型号      : {info.camera_model}")
    print(f"序列号        : {info.serial_number}")
    print(f"固件版本      : 相机 {info.camera_configuration.firmware_version} / "
          f"传感器 {sensors.firmware_version}")

    # 视频流内参（左 / 右相机）
    print_video_intrinsics("左相机 Left", calib.left_cam)
    print_video_intrinsics("右相机 Right", calib.right_cam)

    # IMU 参数
    print_sensor_parameters("加速度计 Accelerometer", sensors.accelerometer_parameters)
    print_sensor_parameters("陀螺仪 Gyroscope", sensors.gyroscope_parameters)
    print_sensor_parameters("磁力计 Magnetometer", sensors.magnetometer_parameters)
    print_sensor_parameters("气压计 Barometer", sensors.barometer_parameters)

    # 外参
    # stereo_transform：左相机 -> 右相机（= 右相机在左相机系下的位姿），t ≈ +baseline
    print_extrinsics("左相机 Left", "右相机 Right", calib.stereo_transform)
    print_extrinsics("左相机 Left", "IMU", sensors.camera_imu_transform)

    print(f"\n双目基线 baseline = {calib.get_camera_baseline():.4f} m")
    print(f"[说明] ZED2i 的灰度/彩色共用左相机，无独立 RGB->IR 外参。")

    zed.close()


if __name__ == "__main__":
    main()