右手系转Unity左手系坐标系

右手系转Unity左手系坐标系

inline Sophus::SE3d TransformRightPoseToLeftUnity(const Sophus::SE3d &pose)
{
    Eigen::Matrix4d T_wl_wr, T_br_bl;
    T_wl_wr.setZero();
    T_wl_wr(0, 0) = 1.0;
    T_wl_wr(1, 2) = 1.0;
    T_wl_wr(2, 1) = 1.0;
    T_wl_wr(3, 3) = 1.0;

    T_br_bl.setZero();
    T_br_bl(0, 0) = 1.0;
    T_br_bl(1, 1) = -1.0;
    T_br_bl(2, 2) = 1.0;
    T_br_bl(3, 3) = 1.0;

    Eigen::Matrix4d T_wl_bl_mat = T_wl_wr * pose.matrix() * T_br_bl;
    Eigen::Quaterniond q(T_wl_bl_mat.block<3, 3>(0, 0));
    q.normalize();
    return Sophus::SE3d(q, T_wl_bl_mat.block<3, 1>(0, 3));
}