一个坐标转换

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

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

cpp 复制代码

// 左目使用结构外参
// 右目通过左目结构外参和双目标定外参计算得到
        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);

cpp 复制代码

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

计算过程

cpp 复制代码

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])

cpp 复制代码

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

python 复制代码

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

cpp 复制代码

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]])

python 复制代码

# 转换左目外参
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

cpp 复制代码

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.        ]]