Kitti stereo数据集位姿转换与像素投影到世界坐标系问题咨询

核心错误梳理 #

1. 位姿矩阵方向理解错误
   Kitti官方提供的3×4矩阵T_i作用是将第i帧相机坐标系下的点转换到第0帧（默认世界坐标系），满足公式 P_w_kitti = T_i @ P_cam_i（P_cam_i为i帧相机系下的齐次点，P_w_kitti为第0帧世界系下的齐次点），本身就是相机到世界的变换，不需要额外求逆。你的初始理解完全反向。
2. 逆变换计算后未生效
   就算按照你的错误理解做了逆变换的R、t计算，后续代码又把M的旋转、平移分量赋值回了原始的Rc、tc，等于逆变换逻辑完全没有生效，属于低级逻辑错误。
3. 坐标系转换方法错误
   零散修改旋转矩阵元素会破坏旋转矩阵的正交性，导致旋转结果完全失真，正确做法是通过整体的坐标系变换矩阵做统一转换。

正确转换流程 #

你需要的目标世界坐标系为：x轴向右、y轴向上、-z轴向前，和Kitti默认世界坐标系（x向右、y向下、z向前）的差异仅为y、z轴方向翻转，因此只需要做一次固定的坐标系映射即可。

示例代码如下：

import numpy as np

# M为从txt文件读取的单行12个浮点数值
# 第一步：将原始矩阵补为4x4齐次变换矩阵，作用：i帧相机系 -> Kitti默认第0帧世界系
T_cam2kitti = np.vstack([np.reshape(M, (3, 4)), [0, 0, 0, 1]])

# 第二步：定义Kitti世界系到目标CG世界系的固定变换矩阵
T_kitti2cg = np.array([
    [1, 0, 0, 0],
    [0, -1, 0, 0],
    [0, 0, -1, 0],
    [0, 0, 0, 1]
])

# 第三步：得到i帧相机系到目标CG世界系的变换矩阵
T_cam2cg = T_kitti2cg @ T_cam2kitti

# 若需要做世界系点到i帧图像的重投影，求逆得到CG世界系到i帧相机系的变换，配合内参使用即可
T_cg2cam = np.linalg.inv(T_cam2cg)
proj_mat = camera_intrinsic @ T_cg2cam[:3, :]

内容的提问来源于stack exchange，提问作者Kosheleva Elena