Ra_2_b 可以是 a坐标系到b坐标系之间的转换;也是 b坐标系上的点,经过 Ra_2_b矩阵 投影到 a 坐标系上的点;
还可以是 a 坐标系下, b坐标系的姿态!
外参定义:b坐标系下的点 经过 T 后,转换到 C坐标系 下的矩阵!故 外参 = Tc_2_b
1、calib_imu_to_velo.txt
calib_time: 25-May-2012 16:47:16
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
激光雷达到IMU的转换:
IMU_2_V =([[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]])
2、calib_velo_to_cam.txt
calib_time: 15-Mar-2012 11:45:23
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
delta_f: 0.000000e+00 0.000000e+00
delta_c: 0.000000e+00 0.000000e+00
相机到激光雷达的转换:
V_2_C = ([[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]])
3、calib_cam_to_cam.txt
calib_time: 09-Jan-2012 14:00:15
corner_dist: 9.950000e-02
S_00: 1.392000e+03 5.120000e+02
K_00: 9.799200e+02 0.000000e+00 6.900000e+02 0.000000e+00 9.741183e+02 2.486443e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_00: -3.745594e-01 2.049385e-01 1.110145e-03 1.379375e-03 -7.084798e-02
R_00: 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 //旋转矩阵
T_00: -9.251859e-17 8.326673e-17 -7.401487e-17 //平移矩阵
S_rect_00: 1.241000e+03 3.760000e+02 //图像尺寸
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_rect_00: 7.188560e+02 0.000000e+00 6.071928e+02 0.000000e+00 0.000000e+00 7.188560e+02 1.852157e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
S_01: 1.392000e+03 5.120000e+02
K_01: 9.903522e+02 0.000000e+00 7.020000e+02 0.000000e+00 9.855674e+02 2.607319e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_01: -3.712084e-01 1.978723e-01 -3.709831e-05 -3.440494e-04 -6.724045e-02
R_01: 9.993440e-01 1.814887e-02 -3.134011e-02 -1.842595e-02 9.997935e-01 -8.575221e-03 3.117801e-02 9.147067e-03 9.994720e-01 //旋转矩阵
T_01: -5.370000e-01 5.964270e-03 -1.274584e-02 //平移矩阵
S_rect_01: 1.241000e+03 3.760000e+02
R_rect_01: 9.996568e-01 -1.110284e-02 2.372712e-02 1.099810e-02 9.999292e-01 4.539964e-03 -2.377585e-02 -4.277453e-03 9.997082e-01
P_rect_01: 7.188560e+02 0.000000e+00 6.071928e+02 -3.861448e+02 0.000000e+00 7.188560e+02 1.852157e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
S_02: 1.392000e+03 5.120000e+02
K_02: 9.601149e+02 0.000000e+00 6.947923e+02 0.000000e+00 9.548911e+02 2.403547e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_02: -3.685917e-01 1.928022e-01 4.069233e-04 7.247536e-04 -6.276909e-02
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
S_rect_02: 1.241000e+03 3.760000e+02
R_rect_02: 9.999191e-01 1.228161e-02 -3.316013e-03 -1.228209e-02 9.999246e-01 -1.245511e-04 3.314233e-03 1.652686e-04 9.999945e-01
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
S_03: 1.392000e+03 5.120000e+02
K_03: 9.049931e+02 0.000000e+00 6.957698e+02 0.000000e+00 9.004945e+02 2.389820e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_03: -3.735725e-01 2.066816e-01 -6.133284e-04 -1.193269e-04 -7.600861e-02
R_03: 9.995578e-01 1.656369e-02 -2.469315e-02 -1.663353e-02 9.998582e-01 -2.625576e-03 2.464616e-02 3.035149e-03 9.996916e-01
T_03: -4.738786e-01 5.991982e-03 -3.215069e-03
S_rect_03: 1.241000e+03 3.760000e+02
R_rect_03: 9.998092e-01 -9.354781e-03 1.714961e-02 9.382303e-03 9.999548e-01 -1.525064e-03 -1.713457e-02 1.685675e-03 9.998518e-01
P_rect_03: 7.188560e+02 0.000000e+00 6.071928e+02 -3.372877e+02 0.000000e+00 7.188560e+02 1.852157e+02 2.369057e+00 0.000000e+00 0.000000e+00 1.000000e+00 4.915215e-03
相机之间转换的参数:
C_2_C1 = np.mat([[9.993440e-01, 1.814887e-02, -3.134011e-02, -5.370000e-01],
[-1.842595e-02, 9.997935e-01, -8.575221e-03, 5.964270e-03],
[3.117801e-02, 9.147067e-03, 9.994720e-01, -1.274584e-02],
[0,0,0,1]])
C_2_C1_ = np.mat([[1, 0, 0, 0.537150653267924],
[0, 1, 0, 0,],
[0, 0, 1, 0,],
[0, 0, 0, 1]])
这里需要注意:两个相机之间的参数有一个配置文件给出的不准确,需要从VINS程序里面的配置文件摘取出来
import numpy as np;
from numpy import *
IMU_2_V =np.mat([[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]])
V_2_C = np.mat([[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]])
C_2_C1 = np.mat([[9.993440e-01, 1.814887e-02, -3.134011e-02, -5.370000e-01],
[-1.842595e-02, 9.997935e-01, -8.575221e-03, 5.964270e-03],
[3.117801e-02, 9.147067e-03, 9.994720e-01, -1.274584e-02],
[0,0,0,1]])
C_2_C1_ = np.mat([[1, 0, 0, 0.537150653267924],
[0, 1, 0, 0,],
[0, 0, 1, 0,],
[0, 0, 0, 1]])
IMU_2_C0 = V_2_C*IMU_2_V
IMU_2_C0 = IMU_2_C0.I
#####################################
IMU_2_C1 = C_2_C1_.I*V_2_C*IMU_2_V
IMU_2_C1 = IMU_2_C1.I
print("Tb_2_c0:")
print(mat(IMU_2_C0))
print("Tb_2_c1:")
print(mat(IMU_2_C1))