织梦企业网站模板,wordpress建哪些网站,简述网站开发的主要阶段,教育培训学校网站建设策划有两种方式可以实现两个坐标系的转换。 车身坐标系下一个点p_car#xff0c;需要转换到相机坐标系下#xff0c;旋转矩阵R_car2Cam#xff0c;平移矩阵T_car2Cam。点p_car在相机坐标系下记p_cam. 方法1#xff1a;先旋转再平移 p_cam T_car2Cam * p_car T_car2Cam 需要注… 有两种方式可以实现两个坐标系的转换。 车身坐标系下一个点p_car需要转换到相机坐标系下旋转矩阵R_car2Cam平移矩阵T_car2Cam。点p_car在相机坐标系下记p_cam. 方法1先旋转再平移 p_cam T_car2Cam * p_car T_car2Cam 需要注意的是这里的平移矩阵T_car2Cam是车身坐标系原点在相机坐标系下面的坐标 用具体数值表示如下 import numpy as nprot np.array([[ 4.6776832e-04, -9.9942386e-01, -3.3936482e-02],[ 3.1350527e-02, 3.3934463e-02, -9.9893224e-01],[ 9.9950832e-01, -5.9665786e-04, 3.1348340e-02]])tran np.array([0.106073, 1.411398, -1.971551])p_veh np.array([3, 6, 8]).reshape(3, 1)
p_cam_1 rot.dot(p_veh) tran.reshape(3,1)
print(p_cam_1)#[[-6.16055871]#[-6.28240156]#[ 1.27418073]] 方法2先平移再旋转 在方法1中 是把p经过旋转转到相机坐标系 再加上平移向量这个平移向量就是相机坐标系的所以可以直接相加减。 同样的我们这里的方法2是需要先平移就要求平移的向量是车身坐标系下的才能相加减。就是我们如果现在需要先平移那么就要求这个平移向量是在车身坐标系下的才能直接相加减。 即需要知道相机坐标原点相对于车身坐标系。 如图就是把左边的相机坐标系原点0,0,0先平移到车身坐标系的原点(0,0,0)处注意平移后的还是在相机坐标系下现在tran np.array([0.106073, 1.411398, -1.971551])是相机坐标系下car坐标系原点的位置。 就是在相机坐标系下一个点Q0.106073, 1.411398, -1.971551我现在就要把我相机坐标系原点挪到这个Q点。就是Q点在新相机坐标系下是(0, 0, 0). 所以之前的点n转到以Q点为原点的新相机坐标系需要经过n-0.106073, 1.411398, -1.971551 比如之前的原点(0,0,0)转到新平移后的Q为原点相机坐标系下是(0,0,0)-0.106073, 1.411398, -1.971551 相机坐标系原点平移到车身坐标系原点这个时候还差一步旋转。旋转注意这个时候的旋转矩阵是需要相机坐标系到车身坐标系的因为我们现在点都在相机坐标系下只是平移了需要相机到车身的旋转矩阵直接R_car2Cam取个逆就是。 Cam_0_to_car inv(R_car2Cam) * (Cam_0 - T_car2Cam) import numpy as npR_car2Cam np.array([[ 4.6776832e-04, -9.9942386e-01, -3.3936482e-02],[ 3.1350527e-02, 3.3934463e-02, -9.9893224e-01],[ 9.9950832e-01, -5.9665786e-04, 3.1348340e-02]])T_car2Cam np.array([0.106073, 1.411398, -1.971551])p_car np.array([3, 6, 8]).reshape(3, 1)
p_cam_1 R_car2Cam.dot(p_car) T_car2Cam.reshape(3,1)
print(p_cam_1)#旋转矩阵的逆等于其转置
Cam_0_to_car (np.dot(R_car2Cam.T, (np.array([[0, 0, 0]]).T - T_car2Cam[..., np.newaxis])).T).reshape(3,1)
p_cam_2 R_car2Cam.dot(p_car - Cam_0_to_car)
print(p_cam_2)
输出
[[-6.16055871][-6.28240156][ 1.27418073]]
[[-6.16055872][-6.28240152][ 1.27418082]] RT矩阵的逆 import numpy as npR_car2Cam np.array([[ 4.6776832e-04, -9.9942386e-01, -3.3936482e-02],[ 3.1350527e-02, 3.3934463e-02, -9.9893224e-01],[ 9.9950832e-01, -5.9665786e-04, 3.1348340e-02]])T_car2Cam np.array([0.106073, 1.411398, -1.971551])p_car np.array([3, 6, 8]).reshape(3, 1)
p_cam_1 R_car2Cam.dot(p_car) T_car2Cam.reshape(3,1)
print(p_cam_1)Cam_0_to_car (np.dot(R_car2Cam.T, (np.array([[0, 0, 0]]).T - T_car2Cam[..., np.newaxis])).T).reshape(3,1)
p_cam_2 R_car2Cam.dot(p_car - Cam_0_to_car)
print(p_cam_2)rt np.eye(4)
rt[:3, :3] R_car2Cam
rt[:3, -1] T_car2Cam
rt_1 np.linalg.inv(rt)print(rt)
print(rt)
print(inv rt)
print(rt_1)print(Cam_0_to_car)
print(Cam_0_to_car) [[-6.16055871][-6.28240156][ 1.27418073]]
[[-6.16055872][-6.28240152][ 1.27418082]]
rt
[[ 4.6776832e-04 -9.9942386e-01 -3.3936482e-02 1.0607300e-01][ 3.1350527e-02 3.3934463e-02 -9.9893224e-01 1.4113980e00][ 9.9950832e-01 -5.9665786e-04 3.1348340e-02 -1.9715510e00][ 0.0000000e00 0.0000000e00 0.0000000e00 1.0000000e00]]
inv rt
[[ 4.67768326e-04 3.13505285e-02 9.99508364e-01 1.92628402e00][-9.99423904e-01 3.39344610e-02 -5.96657759e-04 5.69405203e-02][-3.39364847e-02 -9.98932217e-01 3.13483392e-02 1.47529553e00][ 0.00000000e00 0.00000000e00 0.00000000e00 1.00000000e00]]
Cam_0_to_car
[[1.92628394][0.05694051][1.47529556]] 这里求相机坐标原点在车身坐标系下的坐标 其实用RT矩阵再求逆就可以得到。
