右手系转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));
}