KITTI数据集中cam0相机坐标系到IMU坐标系的变换

这里以2011_09_26序列为例。

追_光

722人浏览 · 2023-09-15 14:43:34

追_光 · 2023-09-15 14:43:34 发布

这里以2011_09_26序列为例

# coding=UTF-8  
# 功能: 求cam0相机坐标系(3D)到IMU坐标系(3D)的变换Tbc 

# 输入: 
#     T_velo_imu:  IMU到激光雷达的变换矩阵
#     T_cam_velo: 激光雷达到cam0的变换矩阵

# 输出：
#     T_imu_cam0: cam0相机坐标系(3D)到IMU坐标系(3D)的变换
#     T_cam0_imu =  T_cam_velo * T_velo_imu

# flies :   2011_09_26/calib_velo_to_cam.txt
#               2011_09_26/calib_imu_to_velo.txt

#           T_cam_velo: 4x4
#                       R: 7.533745e-03 -9.999714e-01 -6.166020e-04 1.480249e-02 7.280733e-04 -9.998902e-01 9.998621e-01 7.523790e-03 1.480755e-02
#                       T: -4.069766e-03 -7.631618e-02 -2.717806e-01

#           T_velo_imu: 4x4
#                       R: 9.999976e-01 7.553071e-04 -2.035826e-03 -7.854027e-04 9.998898e-01 -1.482298e-02 2.024406e-03 1.482454e-02 9.998881e-01
#                       T: -8.086759e-01 3.195559e-01 -7.997231e-01

import numpy as np

def getTbc():
    # [R,t]
    T_cam_velo= np.mat(np.array([ 7.533745e-03, -9.999714e-01, -6.166020e-04,-4.069766e-03, 
                                 1.480249e-02, 7.280733e-04,-9.998902e-01,-7.631618e-02,
                                9.998621e-01, 7.523790e-03,1.480755e-02,-2.717806e-01,
                                0,0,0,1]).reshape([4,4]))
    # [R,t]   
    T_velo_imu= np.mat(np.array([9.999976e-01, 7.553071e-04, -2.035826e-03, -8.086759e-01,
                                  -7.854027e-04, 9.998898e-01, -1.482298e-02,3.195559e-01,
                                  2.024406e-03, 1.482454e-02, 9.998881e-01, -7.997231e-01,
                                  0,0,0,1]).reshape([4,4]))
    
    T_cam0_imu =  T_cam_velo * T_velo_imu
    Tbc = np.linalg.inv(T_cam0_imu)
    
    print("T_imu_cam0:", Tbc)
    
    # ('T_imu_cam0:', matrix([[ 0.00831786,  0.0127777 ,  0.99988376,  1.08330198],
    #     [-0.99986477, -0.01408374,  0.00849768, -0.3098889 ],
    #     [ 0.01419068, -0.99981911,  0.01265882,  0.72991979],
    #     [ 0.        ,  0.        ,  0.        ,  1.        ]]))

    # R: [0.00831786,  0.0127777,   0.99988376,  
        #  -0.99986477, -0.01408374,  0.00849768, 
        #   0.01419068, -0.99981911,  0.01265882]   
    # T: [1.08330198, -0.3098889, 0.72991979]
    
    pass


getTbc()

参考链接