相机变换要注意的事项

往往在使用公开数据集的时候，会有多个传感器，比如kitti,和nuscenes都是有多个传感器的，每个传感器都有相对于某坐标系的pose信息，
比如对于kitti,
有
T_cam_velo, T_imu_velo等.

对于nuscenes,也有 lidar2ego, camera2ego等，
在这些信息都有时候，在计算复合pose的时候，就要注意了.

下面以 nuscenes为例，kitti的也是类似.

假设我们拿到了, ego2w, lidar2ego以及 camfront2ego 的pose信息了，
这里的pose指的就是从一个坐标系变到另外一个坐标系要经历的变换. 通常是用左乘,

比如说现在要算 lidar2w的pose,以及lidar2camfront的pose时，实际上是这样算的

 lidar2w = np.dot(ego2w, lidar2ego)

 lidar2camfront = np.dot(inverse_pose(camfront2ego), lidar2ego)

def inverse_pose(pose):
    """
    pose: 4x4,
    """
    inv_pose = np.eye(4)
    rvec = pose[:3,:3]
    tvec = pose[:3,3].reshape(3,1)
    inv_rvec = rvec.T
    t = - np.dot(inv_rvec, tvec)
    inv_pose[:3, :3] = inv_rvec
    inv_pose[:3, [3]] = t
    return inv_pose

只要记住是左乘 进行复合运算的，出错的可能性就不大.