camera_to_car
car_to_camera
car坐标系原点在后轴中心，camera原点在前挡风玻璃上。
z ↑
|
| y ← (+)
| /
| /
|/
+------→ x (+)
右手定则验证：x(食指)→，y(中指)←，z(拇指)↑
car坐标系
| x (+)
|
y(+) |
<-----+
camera坐标系
z
/
/
/ x
+-------->
|
|
| y
import numpy as np
RT_cam2car =np.array([[ 0.00866422, 0.01209645, 0.9998893, 2.112719 ],
[-0.99986316, 0.01419647, 0.00849225, 0.013116],
[-0.01409217, -0.99982605, 0.0122178, 1.227401 ],
[0, 0, 0, 1]])
RT_car2cam = np.array([[ 0.00866422, -0.99986316, -0.01409217, 0.01210588],
[ 0.01209645, 0.01419647, -0.99982605, 1.20144489],
[ 0.9998893 , 0.00849225, 0.0122178 , -2.12759264],
[0,0,0,1]])
rot = RT_car2cam[:3, :3]
tran = RT_car2cam[:3,3]
print(rot)
print("=====inv")
print(rot.T)
print()
rot_inv = np.linalg.inv(rot)
print(rot_inv)
=====inv
[[ 0.00866422 0.01209645 0.9998893 ]
[-0.99986316 0.01419647 0.00849225]
[-0.01409217 -0.99982605 0.0122178 ]]
[[ 0.00866422 0.01209645 0.99988929]
[-0.99986316 0.01419647 0.00849225]
[-0.01409217 -0.99982606 0.0122178 ]]
rot_car2cam = RT_car2cam[:3, :3]的取逆或者转置就是RT_cam2car。
观察RT_car2cam数值部分的trans[0.01210588, 1.20144489, -2.12759264]. 以cam为坐标系，car原点在cam坐标系下的坐标trans。cam的z向前，car坐标系是后轴中心，cam在前挡风玻璃，所以差距 -2.12759264米。两个坐标系之间的转换。
先旋转再平移 P_cam = rot_car2cam * P_car + trans_car2cam;
比如已知rot_car2cam，trans_car2cam，需要求car坐标系下cam的坐标。就是令P_cam为（0,0,0）求P_car
P_car = rot_car2cam.T * (P_cam - trans_car2cam)
p_cam = np.array([0,0,0])
pcam_car = rot.T @ (p_cam.reshape(3,1)- tran[:, None])
#array([[2.11271901],
[0.013116 ],
[1.227401 ]])
可以看到这几个数值刚好是RT_cam2car的trans
坐标变换还可以先平移再旋转， P_cam = rot * (P_car - trans)
这里trans必须是car坐标系下的。rot是car2cam。trans就是我们刚刚求得的pcam_car。我们用P_car为0,0,0代入，求得p_cam, 就是在cam坐标系下car原点坐标
p_cam = rot @ (p_car - trans)
print(p_cam)
[[ 0.01210589]
[ 1.20144489]
[-2.12759266]]
可以看到打印的数值p_cam就是RT_car2cam里面的tran。
camera_to_car
trans 2.112719， 0.013116， 1.227401 表示的是相机原点在car坐标系下的位置
相机坐标系的x和car坐标系的y是反方向，所以可以看到第一列只有-0.99986316靠近1， cos0°=1， cos-180°=-1
rot是在car坐标系下相机坐标系与car坐标系夹角
第一列 [0.00866422, -0.99986316, -0.01409217] → 相机 x 轴在车身坐标系的方向
第二列 [0.01209645, 0.01419647, -0.99982605] → 相机 y 轴在车身坐标系的方向
第三列 [0.9998893, 0.00849225, 0.0122178] → 相机 z 轴在车身坐标系的方向
对于任意相机坐标下的点 $P_{cam}$：
$$
P_{ego} = R \cdot P_{cam} + t
$$
[2.112719, 0.013116, 1.227401]反过来，把 ego 坐标下点变到相机坐标：
$$
P_{cam} = R^T \cdot (P_{ego} - t)
$$
现在rot
tensor([[-0.0228, -0.9997, 0.0022],
[ 0.0247, -0.0028, -0.9997],
[ 0.9994, -0.0227, 0.0247]], device='cuda:0')
trans
tensor([[2.5222, 0.1295, 1.1364],
[2.5222, 0.1295, 1.1364],
[2.5222, 0.1295, 1.1364]], device='cuda:0')
根据上面可以得知rot是car2camera旋转矩阵， trans是camera原点在car坐标系下。那么pcar转camera需要：
$$
\boxed{
p_{cam} = R , (p_{car} - t)
}
$$
p_car：点在 car 坐标系下t_cam^car：camera 原点在 car 坐标系下👉 点相对于 camera 原点的向量（仍在 car 坐标系）是：
$$
p_{car} - t_{cam}^{car}
$$
因为 rots 是 car → camera 的旋转矩阵：
$$
\boxed{
p_{cam} = R_{car \to cam} \cdot (p_{car} - t_{cam}^{car})
}
$$
$$
\boxed{
p_{cam} = R , (p_{car} - t)
}
$$
其中：
R = rotst = transarray([[-0.00700055, -0.9999651 , 0.00456505],
[-0.00865496, -0.0045044 , -0.9999524 ],
[ 0.999938 , -0.00703972, -0.00862313]], dtype=float32)
array([ 0.029477, 1.225619, -1.87224 ], dtype=float32)
旋转和平移矩阵， 这个是表示car和camera哪个到哪个的转换
这是一个方向判断题，而且你给的这组 R + t 数值本身就已经“暴露了答案”。我一步一步带你反推它到底表示 car → camera，还是 camera → car。
这组
R, t表示：
👉 car（ego）坐标系 → camera 坐标系 的变换
也就是：
$$
\boxed{
P_{cam} = R \cdot P_{car} + t
}
$$
下面我告诉你 为什么可以非常确定。
你的 R 是：
R =
[[-0.0070, -0.99997, 0.00456],
[-0.00865, -0.00450, -0.99995],
[ 0.99994, -0.00704, -0.00862]]
如果这是 car → cam：
[ 0.999938, -0.00704, -0.00862 ]
这表示：
car 的 X 轴（前方）
在 camera 的 Z 轴（前方）上几乎是 1
📌 也就是说：
👉 这正是前视相机的典型外参
cam_x ≈ -car_y
cam_y ≈ -car_z
而标准定义通常是：
| Camera | Car |
|---|---|
| x 右 | y 左 |
| y 下 | z 上 |
| z 前 | x 前 |
👉 刚好是一个右手系重排 + 符号翻转
t = [0.029477, 1.225619, -1.87224]
如果公式是：
$$
P_{cam} = R \cdot P_{car} + t
$$
那么：
t = car 原点，在 camera 坐标系下的位置
这数值意味着：
👉 非常符合前视相机安装位置
如果这是 camera → car：
$$
P_{car} = R \cdot P_{cam} + t
$$
那么：
👉 不成立
你可以直接跑这一段：
import numpy as np
R = np.array([...])
t = np.array([0.029477, 1.225619, -1.87224])
# car 原点
p_car = np.zeros(3)
# 转到 camera
p_cam = R @ p_car + t
print(p_cam)
结果就是：
car 原点在 camera 坐标系下的位置
如果这个点看起来：
那就再次验证：
👉 car → cam
记住公式：
R_cam2car = R.T
t_cam2car = -R.T @ t
$$
P_{car} = R^T (P_{cam} - t)
$$
这组 R, t 表示：
car（ego）→ camera 的坐标变换
如果你愿意，下一步我可以帮你：
points -= trans; points = rots @ points你现在是要 用它投影到图像，还是 反投影到 BEV？
对的，本质上确实存在两种“写法”，但这里有一个非常容易混淆、必须说清楚的点：
不是你想怎么写就怎么写，而是由
👉R和t的“物理含义”决定你“先减 t 还是后加 t”
我给你系统地理一遍，把这个坑一次性填平。
坐标相加相减必须在同一个坐标系下
| 写法 | 公式 | t 的含义 |
|---|---|---|
| 先平移再旋转 | p_cam = R (p_car - t) |
camera 原点在 car 坐标系下 |
| 先旋转再平移 | p_cam = R p_car + t |
car 原点在 camera 坐标系下 |
👉 两种都对，但前提完全不同
你明确说过：
rots：car → cameratrans：相机原点在 car 坐标系下✔️ 那么 只能 用这一种：
$$
\boxed{
p_{cam} = R (p_{car} - t)
}
$$
👉 先平移，再旋转
R p + t（关键直觉）t 是什么？
$$
t = O_{cam}^{car}
$$
也就是：
用 car 坐标系表示的 camera 原点
这一步对应的就是：
p_car → p_car - O_cam
当你的 t 定义成：
$$
t = O_{car}^{cam}
$$
也就是：
car 原点在 camera 坐标系下的位置
这种是 OpenCV / 标定工具 / 投影公式里最常见的定义。
此时公式才是：
$$
\boxed{
p_{cam} = R p_{car} + t
}
$$
如果你现在有的是：
$$
p_{cam} = R (p_{car} - t)
$$
想改写成：
$$
p_{cam} = R p_{car} + t'
$$
那么：
$$
\boxed{
t' = - R t
}
$$
t_new = -rots @ trans
因为：
trans 存成 cam → car👉 只要语义自洽，结果就对
t 表示“谁的原点在哪个坐标系”
就要先把点减到那个原点
points -= trans # 移到 camera 原点
points = rots @ points # car → cam
✔️ 完全正确
如果你愿意，下一步我可以帮你：
R (p - t) ↔ R p + t 统一整理你更想继续哪一个？
如果rot和trans都是车辆转相机那么应该是这样：
rot （rot的列应该是camera）
array([[-0.00700055, -0.9999651 , 0.00456505],
[-0.00865496, -0.0045044 , -0.9999524 ],
[ 0.999938 , -0.00703972, -0.00862313]], dtype=float32)
tran（在camera坐标系下车辆原点表示）
array([ 0.029477, 1.225619, -1.87224 ], dtype=float32)
已知rot和tran是car转camera， array([[-0.00700055, -0.9999651 , 0.00456505],
[-0.00865496, -0.0045044 , -0.9999524 ],
[ 0.999938 , -0.00703972, -0.00862313]], dtype=float32)
array([ 0.029477, 1.225619, -1.87224 ], dtype=float32)
现在用先平移后旋转和先旋转后平移举例
明白，我们来用你给的实际数据做个具体示例，同时对应两种写法：
你的数据：
import numpy as np
rots = np.array([[-0.00700055, -0.9999651 , 0.00456505],
[-0.00865496, -0.0045044 , -0.9999524 ],
[ 0.999938 , -0.00703972, -0.00862313]], dtype=np.float32)
trans = np.array([ 0.029477, 1.225619, -1.87224 ], dtype=np.float32)
假设我们有一个点在 car 坐标系下：
p_car = np.array([1.0, 2.0, 3.0], dtype=np.float32)
适用条件：
trans表示 camera 原点在 car 坐标系下
公式：
$$
p_{cam} = R \cdot (p_{car} - t_{cam}^{car})
$$
Python 实现：
p_cam1 = rots @ (p_car - (-rots.T @ trans)) # 先计算camera原点在car下
# 或者直接先计算t_cam_in_car
t_cam_in_car = -rots.T @ trans
p_cam1 = rots @ (p_car - t_cam_in_car)
适用条件：
trans表示 car 原点在 camera 坐标系下（这是 OpenCV 外参常见写法）
公式：
$$
p_{cam} = R \cdot p_{car} + t
$$
Python 实现：
p_cam2 = rots @ p_car + trans
我们代入你的数据计算：
# 先旋转再平移
p_cam2 = rots @ p_car + trans
# 先平移再旋转
t_cam_in_car = -rots.T @ trans
p_cam1 = rots @ (p_car - t_cam_in_car)
print("先旋转后平移:", p_cam2)
print("先平移后旋转:", p_cam1)
✅ 结果会一样，因为公式完全等价，只要 t 的语义对应正确：
$$
p_{cam} = R (p_{car} - O_{cam}^{car}) = R p_{car} + t
$$
| 方法 | 公式 | t 的语义 |
|---|---|---|
| 先平移再旋转 | p_cam = R @ (p_car - t_cam_in_car) |
camera 原点在 car 坐标系下 |
| 先旋转再平移 | p_cam = R @ p_car + trans |
car 原点在 camera 坐标系下（OpenCV 外参标准） |
两种方式数学等价，但必须明确 t 的定义，不然坐标会出错。
我可以帮你画一个 car ↔ camera 的示意图，同时标出 两种方式对应的平移箭头，这样公式直观可视化，不容易再混淆。
你希望我画吗？
t_cam_in_car = -rots.T @ trans详解
好的，我们详细拆解这个公式：
$$
\mathbf{t}_{\text{cam}}^{\text{car}} = - R^\top , \mathbf{t}
$$
这里的意思是 求相机原点在车身坐标系下的坐标，前提是你已知：
我们一步步讲清楚每个符号和操作的物理意义。
标准外参（OpenCV 风格）通常写作：
$$
p_{\text{cam}} = R , p_{\text{car}} + t
$$
现在问题是：我们想知道 camera 原点在 car 坐标系下的位置，记作 $O_{\text{cam}}^{\text{car}}$。
$$
p_{\text{cam}} = 0
$$
$$
0 = R , O_{\text{cam}}^{\text{car}} + t
$$
从公式：
$$
0 = R , O_{\text{cam}}^{\text{car}} + t
$$
两边减 t：
$$
-R , O_{\text{cam}}^{\text{car}} = t
$$
两边乘 $R^{-1}$（旋转矩阵正交， $R^{-1} = R^\top$）：
$$
O_{\text{cam}}^{\text{car}} = - R^\top , t
$$
✅ 这就是公式的来源。
| 步骤 | 操作 | 意义 |
|---|---|---|
| $R^\top , t$ | 将 car 原点在 camera 下的位置，旋转回 car 坐标系 | 先旋转回车身坐标系方向 |
| 前面加负号 (-) | 因为我们要的是 camera 原点相对于 car 原点 | 让向量从 car → cam 变成 cam → car |
import numpy as np
rots = np.array([[-0.00700055, -0.9999651 , 0.00456505],
[-0.00865496, -0.0045044 , -0.9999524 ],
[ 0.999938 , -0.00703972, -0.00862313]], dtype=np.float32)
trans = np.array([0.029477, 1.225619, -1.87224], dtype=np.float32)
# 相机原点在 car 坐标系下
t_cam_in_car = -rots.T @ trans
print(t_cam_in_car)
rots.T = $R^\top$-rots.T @ trans = $- R^\top t = O_{\text{cam}}^{\text{car}}$trans 表示：car 原点在 camera 坐标系下的位置R.T-Car coord (O_car) -------> Camera coord (O_cam)
\ ^
\ |
\ t |
\ |
-------------- O_cam in car
t = O_car 在 camera 下的坐标-R.T @ t = O_cam 在 car 下的坐标如果你愿意，我可以帮你画一张 箭头+坐标系示意图，把这个 -R.T @ t 直观画出来，让你脑子里一秒理解。
你想让我画吗？
总结所以都是没有毛病的，一般而言车辆转相机就是需要知道rot，trans用先旋转再平移，注意平移相加减都需要在同坐标系下，先旋转就是把车辆坐标转到相机坐标，再平移tran（相机坐标系下car原点表示）
Pcam = rot*Pcar + trans, 这里rot和trans都是需要在camera坐标系下：
rot和trans的数值类似如下：
array([[-0.00700055, -0.9999651 , 0.00456505],
[-0.00865496, -0.0045044 , -0.9999524 ],
[ 0.999938 , -0.00703972, -0.00862313]], dtype=float32)
array([ 0.029477, 1.225619, -1.87224 ], dtype=float32)
但是非要用先平移再旋转，就得知道rot（car-->camera坐标系）和trans(car坐标系下camera原点的表示)。
比如开头的camera_to_car
trans我们直接取[ 2.112719, 0.013116, 1.227401 ]
rot的话我们取逆，就是rotT
就可以用Pcam=rot（Pcar-trans）
已知rot是car转camera的旋转矩阵，tran_veh是car坐标系下camera原点坐标，现在需要求在相机坐标系下，car原点坐标
rotation_vector0，1,2 和translation_vector0，1,2都是car转camera
明白了，你现在的语义是：
rots = car → camera 的旋转矩阵 $R$tran_veh = camera 原点在 car 坐标系下的位置 $O_{\text{cam}}^{\text{car}}$你想求 car 原点在 camera 坐标系下的位置 $O_{\text{car}}^{\text{cam}}$。
标准旋转平移公式：
$$
p_{\text{cam}} = R \cdot (p_{\text{car}} - O_{\text{cam}}^{\text{car}})
$$
代入：
$$
O_{\text{car}}^{\text{cam}} = R \cdot (0 - O_{\text{cam}}^{\text{car}}) = - R , O_{\text{cam}}^{\text{car}}
$$
✅ 就是你要的结果。
lidar2cam_param1 = camera_param[CAM]
rmat = np.linalg.inv(lidar2cam_param1['rmat']) #这里取逆是因为.pt里面存的rot是camera2body2，rotation_vector0需要body2 to camera
R_vec = cv2.Rodrigues(rmat)[0]
lidar2cam_param1['rotation_vector0'], lidar2cam_param1['rotation_vector1'],lidar2cam_param1['rotation_vector2'] = R_vec[0][0],R_vec[1][0],R_vec[2][0]
tvec = np.array([[0,0,0]]).T - np.dot(rmat,lidar2cam_param1['tran_veh'][...,np.newaxis])
lidar2cam_param1['translation_vector0'], lidar2cam_param1['translation_vector1'],lidar2cam_param1['translation_vector2'] = tvec[0,0],tvec[1][0],tvec[2][0]
训练的xml里面rotation_vector0和translation_vector0都是 body2 to cam
此内容由惯性聚合(RSS阅读器)自动聚合整理，仅供阅读参考。 原文来自 — 版权归原作者所有。