右手系转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));
}原文地址：https://blog.csdn.net/wenwang88/article/details/140723041
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！