SLAM_kitti数据集求相机cam2到IMU的变换矩阵

秒速五厘米 2022-11-19 04:29 392阅读 0赞

# 功能说明: #

功能: 求cam2相机坐标系(3D)到IMU坐标系(3D)的变换Tbc (输出, 即T\_body\_cam2)

输入:

*  T\_velo\_imu: IMU到激光雷达的变换矩阵
 *  T\_cam\_velo: 激光雷达到cam0的变换矩阵
 *  R\_i\_rect: 从cam0到cami的旋转矩阵
 *  P\_i\_rect: 由cami的内参, 和 cami相对于cam0的位移组成的投影矩阵(3D->2D)

输出：

*  T\_imu\_cam2\_new: cam2**相机**坐标系(3D)到IMU坐标系(3D)的变换
 *  T\_imu\_cam2: cam2**图像**坐标系(2D)到IMU坐标系(3D)的变换

公式来源: 论文 **Vision meets robotics: the KITTI dataset**

*  y为图像i中的像素点2D坐标，x为IMU坐标系下的3D坐标
 *  ![20210414165602538.png][]
 *  ![20210414165620429.png][]
 *  T\_cam2\_imu = P\_2\_rect\_real \* R\_0\_rect \* T\_cam\_velo \* T\_velo\_imu
 *  T\_cam2\_imu\_new = T\_cam2\_cam0 \* T\_cam\_velo \* T\_velo\_imu

# 实现: #

# based on the paper :Vision meets robotics: the KITTI dataset (2013)
    # flies :   2011_10_03/calib_cam_to_cam.txt
    #               2011_10_03/calib_imu_to_velo.txt
    #               2011_10_03/calib_velo_to_cam.txt
    # formula (8) :
    #       将GPS/IMU坐标系下的点(3D), 变换到第i个图像平面(2D)公式
    #       y = P_i_rect * R_0_rect * T_cam_velo * T_velo_imu *x
    #           ep.  T_velo_imu means the transformation from imu to velo
    #           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
    #           T_cam_velo: 4x4
    #                       R: 7.967514e-03 -9.999679e-01 -8.462264e-04 -2.771053e-03 8.241710e-04 -9.999958e-01 9.999644e-01 7.969825e-03 -2.764397e-03
    #                       T: -1.377769e-02 -5.542117e-02 -2.918589e-01
    #           R_0_rect: 3x3 -- 最后一行和列加 0,0,0,1 --> 4x4
    #                       R_rect_00: 9.999454e-01 7.259129e-03 -7.519551e-03 -7.292213e-03 9.999638e-01 -4.381729e-03 7.487471e-03 4.436324e-03 9.999621e-01
    #           P_2_rect: 3x4
    #                      P_rect_02: 7.188560e+02 0.000000e+00 6.071928e+02 4.538225e+01 0.000000e+00 7.188560e+02 1.852157e+02 -1.130887e-01 0.000000e+00 0.000000e+00 1.000000e+00 3.779761e-03
    #                       令  P_rect_02 = (fx, 0,cx,delta1,
    #                                                          0, fy,cy, delta2,
    #                                                          0, 0, 1, delta3)
    # 求解过程:  分解出相机2对相机0的位移:
    #                       y = P_2_rect * x'   <=等价于=>  y = [K,0]* P_2_rect_real * x'       <==============
    #                       这里[K,0]为3x4相机内参矩阵；x' = R_0_rect * T_cam_velo * T_velo_imu *x
    # 则      P_2_rect_real = ( 1, 0, 0,  (delta1-delta3*cx)/fx,
    #                                               0, 1, 0, (delta2-delta3*cy)/fy,
    #                                               0, 0, 1,  delta3,
    #                                               0,0,0,     1)
    # so
    #       T_imu_cam2 = (P_2_rect_real  * R_0_rect * T_cam_velo * T_velo_imu)^(-1)
    
    import numpy as np
    #import cv2
    import torch
    
    # calculate the transformation from Cam2 cooridinate system(3d) to IMU cooridinate system(3d) based on kitti dataset.
    def getTbc():
        
        # the parameter of cam2
        fx = 7.188560e+02
        fy = 7.188560e+02
        cx = 6.071928e+02
        cy = 1.852157e+02
    
        delta1 = 4.538225e+01
        delta2 = -1.130887e-01
        delta3 = 3.779761e-03
        
        P_2_rect_real = np.mat(np.array([ 1, 0, 0,  (delta1-delta3*cx)/fx,
                                          0, 1, 0, (delta2-delta3*cy)/fy,
                                          0, 0, 1,  delta3,
                                          0,0,0, 1]).reshape([4,4]))
    
        R_0_rect = np.mat(np.array([ 9.999454e-01, 7.259129e-03, -7.519551e-03, 0,
                                    -7.292213e-03, 9.999638e-01, -4.381729e-03,0,
                                    7.487471e-03, 4.436324e-03, 9.999621e-01,0,
                                    0,0,0, 1]).reshape([4,4]))
    
        # [R,t]
        T_cam_velo= np.mat(np.array([ 7.967514e-03, -9.999679e-01, -8.462264e-04,-1.377769e-02, 
                                     -2.771053e-03, 8.241710e-04, -9.999958e-01,-5.542117e-02,
                                    9.999644e-01, 7.969825e-03, -2.764397e-03, -2.918589e-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_cam2_imu = P_2_rect_real  * R_0_rect * T_cam_velo * T_velo_imu
        Tbc = np.linalg.inv(T_cam2_imu)
        
        print("T_imu_cam2:", Tbc)
       # T_imu_cam2: [[ 0.00119669 -0.00924124  0.99995667  1.09838134]
       #             [-0.99997659 -0.00674881  0.00113434 -0.25914669]
       #             [ 0.00673803 -0.99993452 -0.0092491   0.74456586]
       #             [ 0.          0.          0.          1.        ]]
    
    
        # [R_02,t_02]  from cam00 to cam02   
        # R_02: 9.999788e-01 -5.008404e-03 -4.151018e-03 4.990516e-03 9.999783e-01 -4.308488e-03 4.172506e-03 4.287682e-03 9.999821e-01
        # T_02: 5.954406e-02 -7.675338e-04 3.582565e-03
        T_cam2_cam0= np.mat(np.array([ 9.999788e-01, -5.008404e-03, -4.151018e-03, 5.954406e-02,
                                       4.990516e-03, 9.999783e-01, -4.308488e-03, -7.675338e-04,
                                       4.172506e-03, 4.287682e-03, 9.999821e-01, 3.582565e-03,
                                       0,0,0,1]).reshape([4,4]))
    
        T_cam2_imu_new = T_cam2_cam0 * T_cam_velo * T_velo_imu
        Tbc_new = np.linalg.inv(T_cam2_imu_new)
        print("T_imu_cam2_new:\n", Tbc_new)
        # T_imu_cam2_new:
        #  [[ 0.00462418 -0.00906059  0.99994832  1.09837845]
        #  [-0.99980908 -0.01902944  0.00445111 -0.2595698 ]
        #  [ 0.01898812 -0.99977782 -0.00914685  0.74420066]
        #  [ 0.          0.          0.          1.        ]]
    
        #===check: P_2_rect should equal to  P * P_2_rect_real===
    
        # P = [K,0]
        P= np.mat(np.array([ fx, 0, cx,  0,
                             0, fy, cy, 0,
                             0, 0, 1,  0,
                             0,0,0, 1]).reshape([4,4]))
    
        P_2_rect = np.mat(np.array([ 7.188560e+02, 0.000000e+00, 6.071928e+02, 4.538225e+01,
                                     0.000000e+00, 7.188560e+02, 1.852157e+02, -1.130887e-01,
                                     0.000000e+00, 0.000000e+00, 1.000000e+00, 3.779761e-03]).reshape([3,4]))
        print("P_2_rect:\n", P_2_rect)
        print("P * P_2_rect_real:\n", P * P_2_rect_real)
        
        # P_2_rect:
        # [[ 7.188560e+02  0.000000e+00  6.071928e+02  4.538225e+01]
        # [ 0.000000e+00  7.188560e+02  1.852157e+02 -1.130887e-01]
        # [ 0.000000e+00  0.000000e+00  1.000000e+00  3.779761e-03]]
        # P * P_2_rect_real:
        # [[ 7.188560e+02  0.000000e+00  6.071928e+02  4.538225e+01]
        # [ 0.000000e+00  7.188560e+02  1.852157e+02 -1.130887e-01]
        # [ 0.000000e+00  0.000000e+00  1.000000e+00  3.779761e-03]
        # [ 0.000000e+00  0.000000e+00  0.000000e+00  1.000000e+00]]
        
        pass
    
    
    getTbc()

[20210414165602538.png]: /images/20221022/ef6af442698c401ab3a50203ebdb4231.png
[20210414165620429.png]: /images/20221022/8ec855f004d64f6194eaf318c47ccb92.png