已知相机到车的rt 4x4矩阵,求pitch和yaw角度

[ 0.01236276, -0.03470584, 0.9993211, 2.052727 ]
[ -0.99991938, 0.00246558, 0.01245579, -0.0054 ]
[ -0.00289619, -0.99939453, -0.03467256, 1.4633291 ]
[ 0.0, 0.0, 0.0, 1.0 ]

图片

图片

最终结论

这个相机:

pitch ≈ +2.0°
表示:pitch角度正,相机向下。右手大拇指向左,和y轴方向一样,四指向下,向下为正。
相机略微向下俯视(自动驾驶非常常见)

import numpy as np

R = np.array([
    [ 0.01236276, -0.03470584,  0.9993211 ],
    [-0.99991938,  0.00246558,  0.01245579],
    [-0.00289619, -0.99939453, -0.03467256]
])

forward = R[:, 2]

pitch = np.arcsin(-forward[2]) * 180 / np.pi

print(pitch)
#1.986°

图片

pitch = asin(-f_z)
yaw   = atan2(f_y, f_x)
posted @ 2026-05-29 09:52  无左无右  阅读(12)  评论(0)    收藏  举报