一个坐标转换

VINS-Fusion中IMU用的是惯导坐标系：前x,右y,下z,重力向量G{0.0, 0.0, 9.8}，相机使用的坐标系是，右x,下y,前z

公司同事标定好的数据,将坐标系统一为相机坐标系，鄙人觉得这不甚优雅，所以要给他算回来，顺手记录一下运算过程。

// 左目使用结构外参
// 右目通过左目结构外参和双目标定外参计算得到body_R_cam0 << -1.0, 0.0, 0.0,0.0, 1.0, 0.0,0.0, 0.0, -1.0;body_t_cam0 << 0.05143, -0.00453, -0.01503;RIC.push_back(body_R_cam0);TIC.push_back(body_t_cam0);cv::FileNode stereoNode = stereoFS["stereo_params"];double roll = static_cast< double > (stereoNode["Rx"]);double pitch = static_cast< double > (stereoNode["Ry"]);double yaw = static_cast< double > (stereoNode["Rz"]);double tx = static_cast< double > (stereoNode["Tx"]) * 0.001;double ty = static_cast< double > (stereoNode["Ty"]) * 0.001;double tz = static_cast< double > (stereoNode["Tz"]) * 0.001;Eigen::Matrix3d Rx, Ry, Rz;Rx << 1.0, 0.0, 0.0,0.0, cos(roll), -sin(roll),0.0, sin(roll), cos(roll);Ry << cos(pitch), 0.0, sin(pitch),0.0, 1.0, 0.0,-sin(pitch), 0.0, cos(pitch);Rz << cos(yaw), -sin(yaw), 0.0,sin(yaw), cos(yaw), 0.0,0.0, 0.0, 1.0;Eigen::Matrix3d R_rl = Rz * Ry * Rx;Eigen::Vector3d t_rl(tx, ty, tz);body_R_cam1 = body_R_cam0 * R_rl.inverse();body_t_cam1 = body_R_cam0 * (-R_rl.inverse() * t_rl) + body_t_cam0;RIC.push_back(body_R_cam1);TIC.push_back(body_t_cam1);

---sn: IB41SZ00880base: 6.0093750000000000e+01bxf: 2.2715767578125000e+04stereo_params:Tx: -6.0080211639404297e+01Ty: -3.6239957809448242e-01Tz: -2.9354727268218994e-01Rx: -4.1563804261386395e-03Ry: -8.8217593729496002e-03Rz: 9.5129925757646561e-03

计算过程

Rx = [1.0,        0,         0;0,   cos(roll), -sin(roll);0,   sin(roll),  cos(roll)]= [1.0,        0,         0;0,   0.999991, 0.004156;0,  -0.004156, 0.999991]Ry = [cos(pitch),  0, sin(pitch);0,           1,        0;-sin(pitch),  0, cos(pitch)]= [0.999961,  0, -0.008822;0,         1,        0;0.008822,  0,  0.999961]Rz = [cos(yaw), -sin(yaw), 0;sin(yaw),  cos(yaw), 0;0,               0,  1]= [0.999955, -0.009513, 0;0.009513,  0.999955, 0;0,               0,  1]R_rl = Rz * Ry * Rx
t_rl = np.array([-0.060080211639404297, -0.00036239957809448242, -0.00029354727268218994])

body_R_cam1 = body_R_cam0 * R_rl.inverse();
body_t_cam1 = body_R_cam0 * (-R_rl.inverse() * t_rl) + body_t_cam0;

or

body_R_cam1 = body_R_cam0 @ np.linalg.inv(R_rl)
body_t_cam1 = body_R_cam0 @ (-np.linalg.inv(R_rl) @ t_rl) + body_t_cam0

R_rl =
[[ 0.99991584 -0.0094761  -0.00886071][ 0.00951248  0.99994646  0.00407226][ 0.00882164 -0.00415621  0.99995245]]t_rl =
[-0.06008021 -0.0003624  -0.00029355]原始坐标系中的右目外参：
body_R_cam1 =
[[-0.99991584 -0.00951248 -0.00882164][-0.0094761   0.99994646 -0.00415621][ 0.00886071 -0.00407226 -0.99995245]]
body_t_cam1 =
[-0.00865119 -0.00473817 -0.01479266]------>坐标系转化 将imu更新回惯导坐标系
坐标系转换矩阵：从右x,下y,前z → 前x,右y,下z
T_convert = np.array([[0,  0, 1],[-1, 0, 0],[0, -1, 0]])

# 转换左目外参
body_R_cam0_new = T_convert @ body_R_cam0_old @ T_convert.T
body_t_cam0_new = T_convert @ body_t_cam0_old# 转换右目外参
body_R_cam1_new = T_convert @ body_R_cam1_old @ T_convert.T
body_t_cam1_new = T_convert @ body_t_cam1_old

body_T_cam0 (4x4) =
[[-1.       0.       0.      -0.01503][ 0.      -1.       0.      -0.05143][ 0.       0.       1.       0.00453][ 0.       0.       0.       1.     ]]body_T_cam1 (4x4) =
[[-0.99995245 -0.00886071  0.00407226 -0.01479266][ 0.00882164 -0.99991584 -0.00951248  0.00865119][ 0.00415621 -0.0094761   0.99994646  0.00473817][ 0.          0.          0.          1.        ]]