| 69 | } |
| 70 | |
| 71 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d& R) { |
| 72 | Eigen::Vector3d n = R.col(0); |
| 73 | Eigen::Vector3d o = R.col(1); |
| 74 | Eigen::Vector3d a = R.col(2); |
| 75 | |
| 76 | Eigen::Vector3d ypr(3); |
| 77 | double y = atan2(n(1), n(0)); |
| 78 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); |
| 79 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); |
| 80 | ypr(0) = y; |
| 81 | ypr(1) = p; |
| 82 | ypr(2) = r; |
| 83 | |
| 84 | return ypr / M_PI * 180.0; |
| 85 | } |
| 86 | |
| 87 | template <typename Derived> |
| 88 | static Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr2R(const Eigen::MatrixBase<Derived>& ypr) { |
nothing calls this directly
no outgoing calls
no test coverage detected