6DOF位姿投影至3DOF位姿

double getYaw(const Eigen::Matrix3d& rotation){
    Eigen::Matrix<double, 3, 1> direction = rotation * Eigen::Matrix<double, 3, 1>::UnitX();
    return atan2(direction.y(), direction.x());
}

    Eigen::Matrix3d tmp_R = location_T_world_base_link.block<3, 3>(0, 0);
    double yaw = getYaw(tmp_R);
    cout << "yaw " << yaw << endl;

    Eigen::AngleAxisd rollAngle(AngleAxisd(0, Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(AngleAxisd(0, Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(AngleAxisd(yaw, Vector3d::UnitZ()));

    tmp_R = yawAngle * pitchAngle * rollAngle;

    location_T_world_base_link.block<3, 3>(0, 0) = tmp_R;
    location_T_world_base_link(2, 3) = 0;