obtain_sensor2top函数， sensor → ego_s → global → ego_lidar → lidar

obtain_sensor2top

def obtain_sensor2top(nusc,
                      sensor_token,
                      l2e_t,
                      l2e_r_mat,
                      e2g_t,
                      e2g_r_mat,
                      sensor_type='lidar'):
    """Obtain the info with RT matric from general sensor to Top LiDAR.

    Args:
        nusc (class): Dataset class in the nuScenes dataset.
        sensor_token (str): Sample data token corresponding to the
            specific sensor type.
        l2e_t (np.ndarray): Translation from lidar to ego in shape (1, 3).
        l2e_r_mat (np.ndarray): Rotation matrix from lidar to ego
            in shape (3, 3).
        e2g_t (np.ndarray): Translation from ego to global in shape (1, 3).
        e2g_r_mat (np.ndarray): Rotation matrix from ego to global
            in shape (3, 3).
        sensor_type (str): Sensor to calibrate. Default: 'lidar'.

    Returns:
        sweep (dict): Sweep information after transformation.
    """
    sd_rec = nusc.get('sample_data', sensor_token)
    cs_record = nusc.get('calibrated_sensor',
                         sd_rec['calibrated_sensor_token'])
    pose_record = nusc.get('ego_pose', sd_rec['ego_pose_token'])
    data_path = str(nusc.get_sample_data_path(sd_rec['token']))
    if os.getcwd() in data_path:  # path from lyftdataset is absolute path
        data_path = data_path.split(f'{os.getcwd()}/')[-1]  # relative path
    sweep = {
        'data_path': data_path,
        'type': sensor_type,
        'sample_data_token': sd_rec['token'],
        'sensor2ego_translation': cs_record['translation'],
        'sensor2ego_rotation': cs_record['rotation'],
        'ego2global_translation': pose_record['translation'],
        'ego2global_rotation': pose_record['rotation'],
        'timestamp': sd_rec['timestamp']
    }
    l2e_r_s = sweep['sensor2ego_rotation']
    l2e_t_s = sweep['sensor2ego_translation']
    e2g_r_s = sweep['ego2global_rotation']
    e2g_t_s = sweep['ego2global_translation']

    # obtain the RT from sensor to Top LiDAR
    # sweep->ego->global->ego'->lidar
    l2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrix
    e2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrix
    R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ (
        np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)
    T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ (
        np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)
    T -= e2g_t @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T
                  ) + l2e_t @ np.linalg.inv(l2e_r_mat).T
    sweep['sensor2lidar_rotation'] = R.T  # points @ R.T + T
    sweep['sensor2lidar_translation'] = T
    return sweep

这个函数的作用是：计算从任意传感器坐标系到顶部 LiDAR 坐标系的刚体变换（旋转矩阵 R 和平移向量 T）。

坐标系变换链

sensor → ego_s → global → ego_lidar → lidar
之所以要经过 global，是因为传感器帧和 LiDAR 帧的采集时间不同，车辆位姿（ego pose）也不同，必须借助全局坐标系"对齐"。

关键变量含义
变量 含义
l2e_r_s_mat, l2e_t_s 当前传感器时刻：sensor → ego 的旋转/平移
e2g_r_s_mat, e2g_t_s 当前传感器时刻：ego → global 的旋转/平移
e2g_r_mat, e2g_t 参考 LiDAR 时刻：ego → global 的旋转/平移
l2e_r_mat, l2e_t 参考 LiDAR 时刻：lidar → ego 的旋转/平移

数学推导

点坐标约定为行向量，变换写作 p' = p @ R.T + t。
正向链（sensor → global）：
p_global = p_sensor @ l2e_r_s.T @ e2g_r_s.T
+ l2e_t_s @ e2g_r_s.T + e2g_t_s
逆向链（global → lidar）：
p_lidar = (p_global - e2g_t) @ inv(e2g_r).T @ inv(l2e_r).T - l2e_t @ inv(l2e_r).T
令 M = inv(e2g_r_mat).T @ inv(l2e_r_mat).T，合并得：
R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ M

T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ M

• e2g_t @ M
• l2e_t @ inv(l2e_r_mat).T
  这正好对应代码里的两行计算。
  一句话总结：函数通过"传感器→全局→LiDAR"的两段变换链，消除了不同传感器采集时刻车辆位姿差异的影响，最终输出可以直接将传感器点云对齐到 LiDAR 坐标系的变换矩阵。

逐步推导

第一步：sensor → ego
p_ego = p_sensor @ l2e_r_s_mat.T + l2e_t_s
第二步：ego → global
p_global = p_ego @ e2g_r_s_mat.T + e2g_t_s
代入展开
把第一步的 p_ego 代入第二步：
p_global = (p_sensor @ l2e_r_s_mat.T + l2e_t_s) @ e2g_r_s_mat.T + e2g_t_s
矩阵乘法分配律展开括号：
p_global = p_sensor @ l2e_r_s_mat.T @ e2g_r_s_mat.T ← 旋转部分
+ l2e_t_s @ e2g_r_s_mat.T ← t_s2e 也要跟着旋转
+ e2g_t_s ← 再加 ego→global 的平移

逆向链（global → lidar），先转到ego，再转lidar
已知正向变换（ego → global）：
p_global = p_ego @ e2g_r_mat.T + e2g_t
要求逆变换，即从 p_global 解出 p_ego：
p_ego = (p_global - e2g_t) @ inv(e2g_r_mat).T
第一步：global → ego（参考 LiDAR 时刻）
p_ego = (p_global - e2g_t) @ inv(e2g_r_mat).T
注意这里用的是 LiDAR 时刻的 ego pose，所以是 e2g_r_mat 和 e2g_t，不是传感器时刻的。
第二步：ego → lidar
已知正向 lidar → ego：
p_ego = p_lidar @ l2e_r_mat.T + l2e_t
同样求逆：
p_lidar = (p_ego - l2e_t) @ inv(l2e_r_mat).T

代入展开
把第一步代入第二步：
p_lidar = (p_ego - l2e_t) @ inv(l2e_r_mat).T
= ((p_global - e2g_t) @ inv(e2g_r_mat).T - l2e_t) @ inv(l2e_r_mat).T
继续展开括号：
p_lidar = (p_global - e2g_t) @ inv(e2g_r_mat).T @ inv(l2e_r_mat).T
- l2e_t @ inv(l2e_r_mat).T

令 M = inv(e2g_r_mat).T @ inv(l2e_r_mat).T，整理得：
p_lidar = p_global @ M
- e2g_t @ M ← e2g_t 也要经过完整的逆旋转
- l2e_t @ inv(l2e_r_mat).T ← l2e_t 只需经过 lidar→ego 的逆旋转
这正好对应代码里 T 的计算：
T -= e2g_t @ (inv(e2g_r_mat).T @ inv(l2e_r_mat).T) + l2e_t @ inv(l2e_r_mat).T

最后再来一次完整推导：

把正向链的 p_global 代入逆向链，完整展开。
正向链结果：
p_global = p_sensor @ l2e_r_s_mat.T @ e2g_r_s_mat.T
+ l2e_t_s @ e2g_r_s_mat.T + e2g_t_s
逆向链：
p_lidar = p_global @ M - e2g_t @ M - l2e_t @ inv(l2e_r_mat).T
其中 M = inv(e2g_r_mat).T @ inv(l2e_r_mat).T

代入展开 p_global @ M：
p_global @ M = (p_sensor @ l2e_r_s_mat.T @ e2g_r_s_mat.T
+ l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ M
分配律展开：
= p_sensor @ l2e_r_s_mat.T @ e2g_r_s_mat.T @ M ← 旋转部分 → 就是 R

• (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ M ← T 的前部分
  所以完整的 p_lidar = p_sensor @ R + T，其中：

R = l2e_r_s_mat.T @ e2g_r_s_mat.T @ M
T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ M ← 代码第一行

• e2g_t @ M ← 代码 -= 的第一项
• l2e_t @ inv(l2e_r_mat).T ← 代码 -= 的第二项
  T 前部分的直觉：l2e_t_s @ e2g_r_s_mat.T + e2g_t_s 正是传感器时刻 ego 原点在 global 坐标系中的位置，再经过 M 变换到 LiDAR 坐标系。后两项则是减去 LiDAR 时刻 ego 原点的贡献，本质是"两个时刻车辆位置之差"。

行向量和列向量

列向量：p' = R p + T
行向量：p' = p R^T + T
行向量 p′=pRT+T
列向量 p′=Rp+T


✅ 一句话记忆
👉 如果点是 (N,3)：
必须用：points @ R.T + T
👉 如果点是 (3,N)：
必须用：R @ points + T

其实不用这么复杂，用4*4RT矩阵：

def obtain_sensor2top222(nusc, sensor_token, l2e_t, l2e_r_mat, e2g_t, e2g_r_mat, sensor_type='lidar'):
    sd_rec = nusc.get('sample_data', sensor_token)
    cs_record = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token'])
    pose_record = nusc.get('ego_pose', sd_rec['ego_pose_token'])
    data_path = str(nusc.get_sample_data_path(sd_rec['token']))
    if os.getcwd() in data_path:
        data_path = data_path.split(f'{os.getcwd()}/')[-1]

    sweep = {
        'data_path': data_path,
        'type': sensor_type,
        'sample_data_token': sd_rec['token'],
        'sensor2ego_translation': cs_record['translation'],
        'sensor2ego_rotation': cs_record['rotation'],
        'ego2global_translation': pose_record['translation'],
        'ego2global_rotation': pose_record['rotation'],
        'timestamp': sd_rec['timestamp'],
    }

    l2e_r_s_mat = Quaternion(sweep['sensor2ego_rotation']).rotation_matrix
    e2g_r_s_mat = Quaternion(sweep['ego2global_rotation']).rotation_matrix

    def make_rt(r, t):
        M = np.eye(4)
        M[:3, :3] = r
        M[:3, 3] = np.array(t).flatten()
        return M

    # sweep->ego->global->ego'->lidar（列向量：最后一步在最左）
    RT = (np.linalg.inv(make_rt(l2e_r_mat, l2e_t))
          @ np.linalg.inv(make_rt(e2g_r_mat, e2g_t))
          @ make_rt(e2g_r_s_mat, sweep['ego2global_translation'])
          @ make_rt(l2e_r_s_mat, sweep['sensor2ego_translation']))

    sweep['sensor2lidar_rotation'] = RT[:3, :3]   # points @ R.T + T
    sweep['sensor2lidar_translation'] = RT[:3, 3]
    return sweep

实测结果一样
sweep['sensor2lidar_rotation']
array([[ 0.99995012, 0.00730543, 0.00681137],
[-0.00694924, 0.01901527, 0.99979504],
[ 0.00717441, -0.9997925 , 0.01906509]])
sweep_tmp['sensor2lidar_rotation']
array([[ 0.99995012, 0.00730543, 0.00681137],
[-0.00694924, 0.01901527, 0.99979504],
[ 0.00717441, -0.9997925 , 0.01906509]])
sweep_tmp['sensor2lidar_translation']
array([ 0.00072265, 0.60818175, -0.31034774])
sweep['sensor2lidar_translation']
array([ 0.00072265, 0.60818175, -0.31034774])

用行向量：

用行向量约定 p' = p @ A + b，对应的 4×4 齐次矩阵为：

M = [[A, 0],
[b, 1]]
变换链直接矩阵连乘，逆变换用 np.linalg.inv，最后从结果矩阵拆出 R 和 T。

def obtain_sensor2top(nusc, sensor_token, l2e_t, l2e_r_mat, e2g_t, e2g_r_mat, sensor_type='lidar'):
    sd_rec = nusc.get('sample_data', sensor_token)
    cs_record = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token'])
    pose_record = nusc.get('ego_pose', sd_rec['ego_pose_token'])
    data_path = str(nusc.get_sample_data_path(sd_rec['token']))
    if os.getcwd() in data_path:
        data_path = data_path.split(f'{os.getcwd()}/')[-1]

    sweep = {
        'data_path': data_path,
        'type': sensor_type,
        'sample_data_token': sd_rec['token'],
        'sensor2ego_translation': cs_record['translation'],
        'sensor2ego_rotation': cs_record['rotation'],
        'ego2global_translation': pose_record['translation'],
        'ego2global_rotation': pose_record['rotation'],
        'timestamp': sd_rec['timestamp'],
    }

    l2e_r_s_mat = Quaternion(sweep['sensor2ego_rotation']).rotation_matrix
    e2g_r_s_mat = Quaternion(sweep['ego2global_rotation']).rotation_matrix

    def make_rt(r, t):
        M = np.eye(4)
        M[:3, :3] = r
        M[3, :3] = np.array(t).flatten()
        return M

    # sweep->ego->global->ego'->lidar
    RT = (make_rt(l2e_r_s_mat.T, sweep['sensor2ego_translation'])
          @ make_rt(e2g_r_s_mat.T, sweep['ego2global_translation'])
          @ np.linalg.inv(make_rt(e2g_r_mat.T, e2g_t))
          @ np.linalg.inv(make_rt(l2e_r_mat.T, l2e_t)))

    sweep['sensor2lidar_rotation'] = RT[:3, :3].T  # points @ R.T + T
    sweep['sensor2lidar_translation'] = RT[3, :3]
    return sweep